Loading...
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 | /*
* Copyright (c) 2016 Intel Corporation
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <zephyr.h>
#include <stdio.h>
#include <device.h>
#include <sensor.h>
#include <misc/printk.h>
#define MAX_TEST_TIME 15000
#define SLEEPTIME 300
/* uncomment next line for setting offsets manually */
/* #define PERFORM_MANUAL_CALIBRATION */
/* uncomment the next line for auto calibration */
/* #define PERFORM_AUTO_CALIBRATION */
#ifdef PERFORM_MANUAL_CALIBRATION
/*
* Offset map needed for manual accelerometer calibration. These values are
* deduced by holding the device perpendicular on one axis (usually that's Z if
* the device lies flat on the table) and compute the difference so that the
* values on the 3 axis look like this: X: 0, Y: 0; Z: 9.80665. Due to
* accelerometer noise, the values will vary around these values.
*
* For example if the accelerometer output, without offset compensation, is :
*
* Acc (m/s^2): X=-2.349435, Y=-0.488070, Z=11.158620
*
* then the offsets necessary to compensate the read values are:
* X = +2.349435, Y = +0.488070. Z = -1.351970
*/
static struct sensor_value accel_offsets[] = {
{2, 349435}, /* X */
{0, 488070}, /* Y */
{-1, -351970}, /* Z */
};
/*
* The same goes for gyro offsets but, in gyro's case, the values should
* converge to 0 (with the device standing still).
*/
static struct sensor_value gyro_offsets[] = {
{0, 3195}, /* X */
{0, 3195}, /* Y */
{0, -4260},/* Z */
};
static int manual_calibration(struct device *bmi160)
{
#if !defined(CONFIG_BMI160_ACCEL_PMU_SUSPEND)
/* set accelerometer offsets */
if (sensor_attr_set(bmi160, SENSOR_CHAN_ACCEL_XYZ,
SENSOR_ATTR_OFFSET, accel_offsets) < 0) {
return -EIO;
}
#endif
#if !defined(CONFIG_BMI160_GYRO_PMU_SUSPEND)
/* set gyroscope offsets */
if (sensor_attr_set(bmi160, SENSOR_CHAN_GYRO_XYZ,
SENSOR_ATTR_OFFSET, gyro_offsets) < 0) {
return -EIO;
}
#endif
return 0;
}
#endif
#ifdef PERFORM_AUTO_CALIBRATION
/*
* 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[] = {
{0, 0}, /* X */
{0, 0}, /* Y */
{9, 806650}, /* Z */
};
static int auto_calibration(struct device *bmi160)
{
#if !defined(CONFIG_BMI160_ACCEL_PMU_SUSPEND)
/* calibrate accelerometer */
if (sensor_attr_set(bmi160, SENSOR_CHAN_ACCEL_XYZ,
SENSOR_ATTR_CALIB_TARGET, acc_calib) < 0) {
return -EIO;
}
#endif
#if !defined(CONFIG_BMI160_GYRO_PMU_SUSPEND)
/*
* 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(bmi160, SENSOR_CHAN_GYRO_XYZ,
SENSOR_ATTR_CALIB_TARGET, NULL) < 0) {
return -EIO;
}
#endif
return 0;
}
#endif
/**
* @brief Helper function for printing a sensor value to a buffer
*
* @param buf A pointer to the buffer to which the printing is done.
* @param len Size of buffer in bytes.
* @param val A pointer to a sensor_value struct holding the value
* to be printed.
*
* @return The number of characters printed to the buffer.
*/
static inline int sensor_value_snprintf(char *buf, size_t len,
const struct sensor_value *val)
{
s32_t val1, val2;
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);
}
}
#if !defined(CONFIG_BMI160_GYRO_PMU_SUSPEND)
static void print_gyro_data(struct device *bmi160)
{
struct sensor_value val[3];
char buf_x[18], buf_y[18], buf_z[18];
if (sensor_channel_get(bmi160, SENSOR_CHAN_GYRO_XYZ, val) < 0) {
printk("Cannot read bmi160 gyro channels.\n");
return;
}
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]);
printk("Gyro (rad/s): X=%s, Y=%s, Z=%s\n", buf_x, buf_y, buf_z);
}
#endif
#if !defined(CONFIG_BMI160_ACCEL_PMU_SUSPEND)
static void print_accel_data(struct device *bmi160)
{
struct sensor_value val[3];
char buf_x[18], buf_y[18], buf_z[18];
if (sensor_channel_get(bmi160,
SENSOR_CHAN_ACCEL_XYZ, val) < 0) {
printk("Cannot read bmi160 accel channels.\n");
return;
}
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]);
printk("Acc (m/s^2): X=%s, Y=%s, Z=%s\n", buf_x, buf_y, buf_z);
}
#endif
static void print_temp_data(struct device *bmi160)
{
struct sensor_value val;
char buf[18];
if (sensor_channel_get(bmi160, SENSOR_CHAN_DIE_TEMP, &val) < 0) {
printk("Temperature channel read error.\n");
return;
}
sensor_value_snprintf(buf, sizeof(buf), &val);
printk("Temperature (Celsius): %s\n", buf);
}
static void test_polling_mode(struct device *bmi160)
{
s32_t remaining_test_time = MAX_TEST_TIME;
struct sensor_value attr;
#if defined(CONFIG_BMI160_ACCEL_ODR_RUNTIME)
/* set sampling frequency to 100Hz for accel */
attr.val1 = 100;
attr.val2 = 0;
if (sensor_attr_set(bmi160, SENSOR_CHAN_ACCEL_XYZ,
SENSOR_ATTR_SAMPLING_FREQUENCY, &attr) < 0) {
printk("Cannot set sampling frequency for accelerometer.\n");
return;
}
#endif
#if defined(CONFIG_BMI160_GYRO_ODR_RUNTIME)
/* set sampling frequency to 3200Hz for gyro */
attr.val1 = 3200;
attr.val2 = 0;
if (sensor_attr_set(bmi160, SENSOR_CHAN_GYRO_XYZ,
SENSOR_ATTR_SAMPLING_FREQUENCY, &attr) < 0) {
printk("Cannot set sampling frequency for gyroscope.\n");
return;
}
#endif
/* wait for the change to take effect */
k_sleep(SLEEPTIME);
/* poll the data and print it */
do {
if (sensor_sample_fetch(bmi160) < 0) {
printk("Sample update error.\n");
return;
}
#if !defined(CONFIG_BMI160_GYRO_PMU_SUSPEND)
print_gyro_data(bmi160);
#endif
#if !defined(CONFIG_BMI160_ACCEL_PMU_SUSPEND)
print_accel_data(bmi160);
#endif
print_temp_data(bmi160);
/* wait a while */
k_sleep(SLEEPTIME);
remaining_test_time -= SLEEPTIME;
} while (remaining_test_time > 0);
}
#ifdef CONFIG_BMI160_TRIGGER
static void trigger_hdlr(struct device *bmi160,
struct sensor_trigger *trigger)
{
if (trigger->type != SENSOR_TRIG_DELTA &&
trigger->type != SENSOR_TRIG_DATA_READY) {
return;
}
if (sensor_sample_fetch(bmi160) < 0) {
printk("Sample update error.\n");
return;
}
#if !defined(CONFIG_BMI160_GYRO_PMU_SUSPEND)
if (trigger->chan == SENSOR_CHAN_GYRO_XYZ) {
print_gyro_data(bmi160);
}
#endif
#if !defined(CONFIG_BMI160_ACCEL_PMU_SUSPEND)
if (trigger->chan == SENSOR_CHAN_ACCEL_XYZ) {
print_accel_data(bmi160);
}
#endif
}
static void test_anymotion_trigger(struct device *bmi160)
{
s32_t remaining_test_time = MAX_TEST_TIME;
struct sensor_value attr;
struct sensor_trigger trig;
/* set up anymotion trigger */
/*
* Set slope threshold to 0.1G (0.1 * 9.80665 = 4.903325 m/s^2).
* This depends on the chosen range. One cannot choose a threshold
* bigger than half the range. For example, for a 16G range, the
* threshold must not exceed 8G.
*/
attr.val1 = 0;
attr.val2 = 980665;
if (sensor_attr_set(bmi160, SENSOR_CHAN_ACCEL_XYZ,
SENSOR_ATTR_SLOPE_TH, &attr) < 0) {
printk("Cannot set anymotion slope threshold.\n");
return;
}
/*
* Set slope duration to 2 consecutive samples (after which the
* anymotion interrupt will trigger.
*
* Allowed values are from 1 to 4.
*/
attr.val1 = 2;
attr.val2 = 0;
if (sensor_attr_set(bmi160, SENSOR_CHAN_ACCEL_XYZ,
SENSOR_ATTR_SLOPE_DUR, &attr) < 0) {
printk("Cannot set anymotion slope duration.\n");
return;
}
/* enable anymotion trigger */
trig.type = SENSOR_TRIG_DELTA;
trig.chan = SENSOR_CHAN_ACCEL_XYZ;
if (sensor_trigger_set(bmi160, &trig, trigger_hdlr) < 0) {
printk("Cannot enable anymotion trigger.\n");
return;
}
printk("Anymotion test: shake the device to get anymotion events.\n");
do {
/* wait a while */
k_sleep(SLEEPTIME);
remaining_test_time -= SLEEPTIME;
} while (remaining_test_time > 0);
printk("Anymotion test: finished, removing anymotion trigger...\n");
if (sensor_trigger_set(bmi160, &trig, NULL) < 0) {
printk("Cannot remove anymotion trigger.\n");
return;
}
}
static void test_data_ready_trigger(struct device *bmi160)
{
s32_t remaining_test_time = MAX_TEST_TIME;
struct sensor_trigger trig;
/* enable data ready trigger */
trig.type = SENSOR_TRIG_DATA_READY;
trig.chan = SENSOR_CHAN_ACCEL_XYZ;
if (sensor_trigger_set(bmi160, &trig, trigger_hdlr) < 0) {
printk("Cannot enable data ready trigger.\n");
return;
}
printk("Data ready test:\n");
do {
/* wait a while */
k_sleep(SLEEPTIME);
remaining_test_time -= SLEEPTIME;
} while (remaining_test_time > 0);
printk("Data ready test: finished, removing data ready trigger...\n");
if (sensor_trigger_set(bmi160, &trig, NULL) < 0) {
printk("Cannot remove data ready trigger.\n");
return;
}
}
static void test_trigger_mode(struct device *bmi160)
{
struct sensor_value attr;
#if defined(CONFIG_BMI160_ACCEL_ODR_RUNTIME)
/* set sampling frequency to 100Hz for accel */
attr.val1 = 100;
attr.val2 = 0;
if (sensor_attr_set(bmi160, SENSOR_CHAN_ACCEL_XYZ,
SENSOR_ATTR_SAMPLING_FREQUENCY, &attr) < 0) {
printk("Cannot set sampling frequency for accelerometer.\n");
return;
}
#endif
#if defined(CONFIG_BMI160_GYRO_ODR_RUNTIME)
/* set sampling frequency to 100Hz for gyro */
attr.val1 = 100;
attr.val2 = 0;
if (sensor_attr_set(bmi160, SENSOR_CHAN_GYRO_XYZ,
SENSOR_ATTR_SAMPLING_FREQUENCY, &attr) < 0) {
printk("Cannot set sampling frequency for gyroscope.\n");
return;
}
#endif
test_anymotion_trigger(bmi160);
test_data_ready_trigger(bmi160);
}
#endif /* CONFIG_BMI160_TRIGGER */
extern u8_t pbuf[1024];
extern u8_t *pos;
void main(void)
{
struct device *bmi160;
#if defined(CONFIG_BMI160_ACCEL_RANGE_RUNTIME) ||\
defined(CONFIG_BMI160_GYRO_RANGE_RUNTIME)
struct sensor_value attr;
#endif
printk("IMU: Binding...\n");
bmi160 = device_get_binding(CONFIG_BMI160_NAME);
if (!bmi160) {
printk("Gyro: Device not found.\n");
return;
}
#if defined(CONFIG_BMI160_ACCEL_RANGE_RUNTIME)
/*
* Set accelerometer range to +/- 16G. Since the sensor API needs SI
* units, convert the range to m/s^2.
*/
sensor_g_to_ms2(16, &attr);
if (sensor_attr_set(bmi160, SENSOR_CHAN_ACCEL_XYZ,
SENSOR_ATTR_FULL_SCALE, &attr) < 0) {
printk("Cannot set accelerometer range.\n");
return;
}
#endif
#if defined(CONFIG_BMI160_GYRO_RANGE_RUNTIME)
/*
* Set gyro range to +/- 250 degrees/s. Since the sensor API needs SI
* units, convert the range to rad/s.
*/
sensor_degrees_to_rad(250, &attr);
if (sensor_attr_set(bmi160, SENSOR_CHAN_GYRO_XYZ,
SENSOR_ATTR_FULL_SCALE, &attr) < 0) {
printk("Cannot set gyro range.\n");
return;
}
#endif
#ifdef PERFORM_MANUAL_CALIBRATION
/* manually adjust accelerometer and gyro offsets */
if (manual_calibration(bmi160) < 0) {
printk("Manual calibration failed.\n");
return;
}
#endif
#ifdef PERFORM_AUTO_CALIBRATION
/* auto calibrate accelerometer and gyro */
if (auto_calibration(bmi160) < 0) {
printk("HW calibration failed.\n");
return;
}
#endif
printk("Testing the polling mode.\n");
test_polling_mode(bmi160);
printk("Testing the polling mode finished.\n");
#ifdef CONFIG_BMI160_TRIGGER
printk("Testing the trigger mode.\n");
test_trigger_mode(bmi160);
printk("Testing the trigger mode finished.\n");
#endif
}
|