Skip to content
This repository has been archived by the owner on Aug 5, 2022. It is now read-only.

[Sensor] Initial implementation of the Generic Sensor API #353

Merged
merged 9 commits into from
Nov 14, 2016
Merged

[Sensor] Initial implementation of the Generic Sensor API #353

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
1608641
[Sensor] Initial implementation of the Generic Sensor API
jimmy-huang Oct 13, 2016
36efdc8
Fixed typos and updating API according to latest spec changes
jimmy-huang Nov 3, 2016
7c3f41a
Use a union to hold readings instead of array
jimmy-huang Nov 3, 2016
e252b6d
Fixed error handling based on feedback
jimmy-huang Nov 3, 2016
501ab09
Added auto calibration
jimmy-huang Nov 8, 2016
67f7297
Updating PR based on Geoff's comments
jimmy-huang Nov 8, 2016
abf1009
Rebased against master with ring buffer patch
jimmy-huang Nov 8, 2016
ff8154b
Fixed build fail on travis
jimmy-huang Nov 8, 2016
5b7ef1a
Rebase against master
jimmy-huang Nov 11, 2016
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
303 changes: 303 additions & 0 deletions arc/src/main.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,9 @@
#ifdef BUILD_MODULE_I2C
#include <i2c.h>
#endif
#ifdef BUILD_MODULE_SENSOR
#include <sensor.h>
#endif
#ifdef BUILD_MODULE_GROVE_LCD
#include <display/grove_lcd.h>
#endif
Expand Down Expand Up @@ -58,6 +61,14 @@ static struct device *glcd = NULL;
static char str[MAX_BUFFER_SIZE];
#endif

#ifdef BUILD_MODULE_SENSOR
static struct device *bmi160 = NULL;
static bool accel_poll = false;
static bool gyro_poll = false;
static double accel_last_value[3];
static double gyro_last_value[3];
#endif

// add strnlen() support for security since it is missing
// in Zephyr's minimal libc implementation
size_t strnlen(const char *str, size_t max_len) {
Expand Down Expand Up @@ -421,6 +432,288 @@ static void handle_glcd(struct zjs_ipm_message* msg)
}
#endif

#ifdef BUILD_MODULE_SENSOR
#ifdef DEBUG_BUILD
static inline int sensor_value_snprintf(char *buf, size_t len,
const struct sensor_value *val)
{
int32_t val1, val2;

switch (val->type) {
case SENSOR_VALUE_TYPE_INT:
return snprintf(buf, len, "%d", val->val1);
case SENSOR_VALUE_TYPE_INT_PLUS_MICRO:
if (val->val2 == 0) {
return snprintf(buf, len, "%d", val->val1);
}

/* normalize value */
if (val->val1 < 0 && val->val2 > 0) {
val1 = val->val1 + 1;
val2 = val->val2 - 1000000;
} else {
val1 = val->val1;
val2 = val->val2;
}

/* print value to buffer */
if (val1 > 0 || (val1 == 0 && val2 > 0)) {
return snprintf(buf, len, "%d.%06d", val1, val2);
} else if (val1 == 0 && val2 < 0) {
return snprintf(buf, len, "-0.%06d", -val2);
} else {
return snprintf(buf, len, "%d.%06d", val1, -val2);
}
case SENSOR_VALUE_TYPE_DOUBLE:
return snprintf(buf, len, "%f", val->dval);
default:
return 0;
}
}
#endif

static void send_sensor_data(enum sensor_channel channel,
union sensor_reading reading)
{
struct zjs_ipm_message msg;
msg.id = MSG_ID_SENSOR;
msg.type = TYPE_SENSOR_EVENT_READING_CHANGE;
msg.flags = 0;
msg.user_data = NULL;
msg.error_code = ERROR_IPM_NONE;
msg.data.sensor.channel = channel;
memcpy(&msg.data.sensor.reading, &reading, sizeof(union sensor_reading));
zjs_ipm_send(MSG_ID_SENSOR, &msg);
}

#define ABS(x) ((x) >= 0) ? (x) : -(x)

static double convert_sensor_value(const struct sensor_value *val)
{
int32_t val1, val2;
double result = 0;

switch (val->type) {
case SENSOR_VALUE_TYPE_INT:
result = val->val1;
break;
case SENSOR_VALUE_TYPE_INT_PLUS_MICRO:
if (val->val2 == 0) {
result = (double)val->val1;
break;
}

/* normalize value */
if (val->val1 < 0 && val->val2 > 0) {
val1 = val->val1 + 1;
val2 = val->val2 - 1000000;
} else {
val1 = val->val1;
val2 = val->val2;
}

if (val1 > 0 || (val1 == 0 && val2 > 0)) {
result = val1 + (double)val2 * 0.000001;
} else if (val1 == 0 && val2 < 0) {
result = (double)val2 * (-0.000001);
} else {
result = val1 + (double)val2 * (-0.000001);
}
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What happens in this block if val1 > 0 and val2 < 0, doesn't it do the wrong thing?

It kind of seems like this logic could be simplified to be more obviously correct. I'm still not sure what it's doing but will wait for your response to this before digging deeper. :)

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

if val1 > 0, then it falls into the first case, and if val1 > 0, then val2 can't be < 0 as the sign bit is in val1. But anyway, this is taken from the the zephyr sample how they checked.

break;
case SENSOR_VALUE_TYPE_DOUBLE:
result = val->dval;
break;
default:
ZJS_PRINT("convert_sensor_value: invalid type %d\n", val->type);
return 0;
}

return result;
}

static void process_accel_data(struct device *dev)
{
struct sensor_value val[3];
double dval[3];

if (sensor_channel_get(dev, SENSOR_CHAN_ACCEL_ANY, val) < 0) {
ZJS_PRINT("Cannot read accelerometer channels.\n");
return;
}

dval[0] = convert_sensor_value(&val[0]);
dval[1] = convert_sensor_value(&val[1]);
dval[2] = convert_sensor_value(&val[2]);

if (dval[0] == 0 && dval[1] == 0 && dval[2] == 0) {
// FIXME: BUG? why sometimes it reports 0, 0, 0 on all axes
return;
}

// set slope threshold to 0.1G (0.1 * 9.80665 = 4.903325 m/s^2)
double threshold = 0.980665;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Now that's some accurate gravity, right there. Is that calibrated to the second floor of JF1?

if (ABS(dval[0] - accel_last_value[0]) > threshold ||
ABS(dval[1] - accel_last_value[1]) > threshold ||
ABS(dval[2] - accel_last_value[2]) > threshold) {
union sensor_reading reading;
reading.x = dval[0];
reading.y = dval[1];
reading.z = dval[2];
send_sensor_data(SENSOR_CHAN_ACCEL_ANY, reading);
}

#ifdef DEBUG_BUILD
char buf_x[18], buf_y[18], buf_z[18];

sensor_value_snprintf(buf_x, sizeof(buf_x), &val[0]);
sensor_value_snprintf(buf_y, sizeof(buf_y), &val[1]);
sensor_value_snprintf(buf_z, sizeof(buf_z), &val[2]);
ZJS_PRINT("sending accel: X=%s, Y=%s, Z=%s\n", buf_x, buf_y, buf_z);
#endif
}

static void process_gyro_data(struct device *dev)
{
struct sensor_value val[3];
double dval[3];

if (sensor_channel_get(dev, SENSOR_CHAN_GYRO_ANY, val) < 0) {
ZJS_PRINT("Cannot read gyroscope channels.\n");
return;
}

dval[0] = convert_sensor_value(&val[0]);
dval[1] = convert_sensor_value(&val[1]);
dval[2] = convert_sensor_value(&val[2]);

if (dval[0] != gyro_last_value[0] ||
dval[1] != gyro_last_value[1] ||
dval[2] != gyro_last_value[2]) {
union sensor_reading reading;
reading.x = dval[0];
reading.y = dval[1];
reading.z = dval[2];
send_sensor_data(SENSOR_CHAN_GYRO_ANY, reading);
}

#ifdef DEBUG_BUILD
char buf_x[18], buf_y[18], buf_z[18];

sensor_value_snprintf(buf_x, sizeof(buf_x), &val[0]);
sensor_value_snprintf(buf_y, sizeof(buf_y), &val[1]);
sensor_value_snprintf(buf_z, sizeof(buf_z), &val[2]);
ZJS_PRINT("Sending gyro : X=%s, Y=%s, Z=%s\n", buf_x, buf_y, buf_z);
#endif
}

static void fetch_sensor(struct device *dev) {
if (sensor_sample_fetch(dev) < 0) {
ZJS_PRINT("failed to fetch sensor data\n");
return;
}

if (accel_poll) {
process_accel_data(dev);
}
if (gyro_poll) {
process_gyro_data(dev);
}
}

/*
* The values in the following map are the expected values that the
* accelerometer needs to converge to if the device lies flat on the table. The
* device has to stay still for about 500ms = 250ms(accel) + 250ms(gyro).
*/
struct sensor_value acc_calib[] = {
{SENSOR_VALUE_TYPE_INT_PLUS_MICRO, { {0, 0} } }, /* X */
{SENSOR_VALUE_TYPE_INT_PLUS_MICRO, { {0, 0} } }, /* Y */
{SENSOR_VALUE_TYPE_INT_PLUS_MICRO, { {9, 806650} } }, /* Z */
};

static bool auto_calibration(struct device *dev)
{
/* calibrate accelerometer */
if (sensor_attr_set(dev, SENSOR_CHAN_ACCEL_ANY,
SENSOR_ATTR_CALIB_TARGET, acc_calib) < 0) {
return false;
}

/*
* Calibrate gyro. No calibration value needs to be passed to BMI160 as
* the target on all axis is set internally to 0. This is used just to
* trigger a gyro calibration.
*/
if (sensor_attr_set(dev, SENSOR_CHAN_GYRO_ANY,
SENSOR_ATTR_CALIB_TARGET, NULL) < 0) {
return false;
}

return true;
}

static void handle_sensor(struct zjs_ipm_message* msg)
{
uint32_t error_code = ERROR_IPM_NONE;

if (msg->type != TYPE_SENSOR_INIT && !bmi160) {
ZJS_PRINT("Grove LCD device not found.\n");
ipm_send_error_reply(msg, ERROR_IPM_OPERATION_FAILED);
return;
}

switch(msg->type) {
case TYPE_SENSOR_INIT:
if (!bmi160) {
bmi160 = device_get_binding("bmi160");

if (!bmi160) {
error_code = ERROR_IPM_OPERATION_FAILED;
ZJS_PRINT("failed to initialize BMI160 sensor\n");
} else {
if (!auto_calibration(bmi160)) {
ZJS_PRINT("failed to perform auto calibration\n");
}

accel_poll = gyro_poll = false;
DBG_PRINT("BMI160 sensor initialized\n");
}
}
break;
case TYPE_SENSOR_START:
if (msg->data.sensor.channel == SENSOR_CHAN_ACCEL_ANY) {
accel_poll = true;
} else if (msg->data.sensor.channel == SENSOR_CHAN_GYRO_ANY) {
gyro_poll = true;
} else {
ZJS_PRINT("invalid sensor channel\n");
error_code = ERROR_IPM_NOT_SUPPORTED;
}
break;
case TYPE_SENSOR_STOP:
if (msg->data.sensor.channel == SENSOR_CHAN_ACCEL_ANY) {
accel_poll = false;
} else if (msg->data.sensor.channel == SENSOR_CHAN_GYRO_ANY) {
gyro_poll = false;
} else {
ZJS_PRINT("invalid sensor channel\n");
error_code = ERROR_IPM_NOT_SUPPORTED;
}
break;
default:
ZJS_PRINT("unsupported sensor message type %lu\n", msg->type);
error_code = ERROR_IPM_NOT_SUPPORTED;
}

if (error_code != ERROR_IPM_NONE) {
ipm_send_error_reply(msg, error_code);
return;
}

ipm_send_reply(msg);
}
#endif

static void process_messages()
{
struct zjs_ipm_message* msg = msg_queue;
Expand All @@ -442,6 +735,11 @@ static void process_messages()
case MSG_ID_GLCD:
handle_glcd(msg);
break;
#endif
#ifdef BUILD_MODULE_SENSOR
case MSG_ID_SENSOR:
handle_sensor(msg);
break;
#endif
case MSG_ID_DONE:
return;
Expand Down Expand Up @@ -482,6 +780,11 @@ void main(void)
tick_count = 0;
}
tick_count += SLEEP_TICKS;
#endif
#ifdef BUILD_MODULE_SENSOR
if (accel_poll || gyro_poll) {
fetch_sensor(bmi160);
}
#endif
task_sleep(SLEEP_TICKS);
}
Expand Down
30 changes: 30 additions & 0 deletions samples/Accelerometer.js
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,30 @@
// Copyright (c) 2016, Intel Corporation.

// Test code to use the Accelerometer (subclass of Generic Sensor) API
// to communicate with the BMI160 inertia sensor on the Arduino 101
// and obtaining information about acceleration applied to the X, Y and Z axis
console.log("Accelerometer test...");


var sensor = new Accelerometer({
includeGravity: false, // true is not supported, will throw error
frequency: 50
});

sensor.onchange = function(event) {
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@jimmy-huang : Some changes were made for the Sensor Reading
Now

accleromterSensor.onchange = function () {
console.log("acceleration (m/s^2): " +
        " x=" + accleromterSensor.reading.x +
        " y=" + accleromterSensor.reading.y +
        " z=" + accleromterSensor.reading.z);
}

just a heads up, you can make this changes in subsequent PR also, upto you.

console.log("acceleration (m/s^2): " +
" x=" + event.reading.x +
" y=" + event.reading.y +
" z=" + event.reading.z);
};

sensor.onstatechange = function(event) {
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

onstatechange is also removed

console.log("state: " + event);
};

sensor.onerror = function(event) {
console.log("error: " + event.error.name +
" - " + event.error.message);
};

sensor.start();
27 changes: 27 additions & 0 deletions samples/Gyroscope.js
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,27 @@
// Copyright (c) 2016, Intel Corporation.

// Test code to use the Gyroscope (subclass of Generic Sensor) API
// to communicate with the BMI160 inertia sensor on the Arduino 101
// and monitor the rate of rotation around the the X, Y and Z axis
console.log("Gyroscope test...");


var sensor = new Gyroscope();

sensor.onchange = function(event) {
console.log("rotation (rad/s): " +
" x=" + event.reading.x +
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please see the comment above for accelerometer

" y=" + event.reading.y +
" z=" + event.reading.z);
};

sensor.onstatechange = function(event) {
console.log("state: " + event);
};

sensor.onerror = function(event) {
console.log("error: " + event.error.name +
" - " + event.error.message);
};

sensor.start();
Loading