| /* |
| * 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_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 |
| } |