Repository: incubator-mynewt-core Updated Branches: refs/heads/master 8cf8f54dc -> cfe42df79
SensorAPI - BNO055: Add calibration support - Adding calibration support to driver and shell Project: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/repo Commit: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/commit/316c18ac Tree: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/tree/316c18ac Diff: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/diff/316c18ac Branch: refs/heads/master Commit: 316c18acef538bae7a22d30013aa616020f112c8 Parents: b3be6f0 Author: Vipul Rahane <[email protected]> Authored: Tue Feb 28 16:41:58 2017 -0800 Committer: Vipul Rahane <[email protected]> Committed: Tue Feb 28 16:43:22 2017 -0800 ---------------------------------------------------------------------- .../sensors/bno055/include/bno055/bno055.h | 90 +++++- hw/drivers/sensors/bno055/src/bno055.c | 315 ++++++++++++++++++- hw/drivers/sensors/bno055/src/bno055_priv.h | 10 +- hw/drivers/sensors/bno055/src/bno055_shell.c | 74 +++++ 4 files changed, 472 insertions(+), 17 deletions(-) ---------------------------------------------------------------------- http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/316c18ac/hw/drivers/sensors/bno055/include/bno055/bno055.h ---------------------------------------------------------------------- diff --git a/hw/drivers/sensors/bno055/include/bno055/bno055.h b/hw/drivers/sensors/bno055/include/bno055/bno055.h index b8ceb02..0529a2d 100644 --- a/hw/drivers/sensors/bno055/include/bno055/bno055.h +++ b/hw/drivers/sensors/bno055/include/bno055/bno055.h @@ -123,7 +123,6 @@ extern "C" { #define BNO055_MAG_CFG_PWR_MODE_SUSPEND (0x2 << 5) #define BNO055_MAG_CFG_PWR_MODE_FORCE_MODE (0x3 << 5) - struct bno055_cfg { uint8_t bc_opr_mode; uint8_t bc_pwr_mode; @@ -145,6 +144,27 @@ struct bno055_rev_info { uint16_t bri_sw_rev; }; +struct bno055_calib_info { + uint8_t bci_sys; + uint8_t bci_gyro; + uint8_t bci_accel; + uint8_t bci_mag; +}; + +struct bno055_sensor_offsets { + uint16_t bso_acc_off_x; + uint16_t bso_acc_off_y; + uint16_t bso_acc_off_z; + uint16_t bso_gyro_off_x; + uint16_t bso_gyro_off_y; + uint16_t bso_gyro_off_z; + uint16_t bso_mag_off_x; + uint16_t bso_mag_off_y; + uint16_t bso_mag_off_z; + uint16_t bso_acc_radius; + uint16_t bso_mag_radius; +}; + /** * Initialize the bno055. This function is normally called by sysinit. * @@ -197,6 +217,74 @@ int bno055_write8(uint8_t reg, uint8_t value); /** + * Writes a multiple bytes to the specified register + * + * @param The register address to write to + * @param The data buffer to write from + * + * @return 0 on success, non-zero error on failure. + */ +int +bno055_writelen(uint8_t reg, uint8_t *buffer, uint8_t len); + +/** + * Gets current calibration status + * + * @param Calibration info structure to fill up calib state + * @return 0 on success, non-zero on failure + */ +int +bno055_get_calib_status(struct bno055_calib_info *bci); + +/** + * Checks if bno055 is fully calibrated + * + * @return 0 on success, non-zero on failure + */ +int +bno055_is_calib(void); + +/** + * Reads the sensor's offset registers into a byte array + * + * @param byte array to return offsets into + * @return 0 on success, non-zero on failure + * + */ +int +bno055_get_raw_sensor_offsets(uint8_t *offsets); + +/** + * + * Reads the sensor's offset registers into an offset struct + * + * @param structure to fill up offsets data + * @return 0 on success, non-zero on failure + */ +int +bno055_get_sensor_offsets(struct bno055_sensor_offsets *offsets); + +/** + * + * Writes calibration data to the sensor's offset registers + * + * @param calibration data + * @return 0 on success, non-zero on success + */ +int +bno055_set_sensor_raw_offsets(uint8_t* calibdata, uint8_t len); + +/** + * + * Writes to the sensor's offset registers from an offset struct + * + * @param pointer to the offset structure + * @return 0 on success, non-zero on failure + */ +int +bno055_set_sensor_offsets(struct bno055_sensor_offsets *offsets); + +/** * Set operation mode for the bno055 sensor * * @param Operation mode for the sensor http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/316c18ac/hw/drivers/sensors/bno055/src/bno055.c ---------------------------------------------------------------------- diff --git a/hw/drivers/sensors/bno055/src/bno055.c b/hw/drivers/sensors/bno055/src/bno055.c index 1d06545..fb98197 100644 --- a/hw/drivers/sensors/bno055/src/bno055.c +++ b/hw/drivers/sensors/bno055/src/bno055.c @@ -136,6 +136,59 @@ bno055_write8(uint8_t reg, uint8_t value) } /** + * Writes a multiple bytes to the specified register + * + * @param The register address to write to + * @param The data buffer to write from + * + * @return 0 on success, non-zero error on failure. + */ +int +bno055_writelen(uint8_t reg, uint8_t *buffer, uint8_t len) +{ + int rc; + uint8_t payload[23] = { reg, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0}; + + struct hal_i2c_master_data data_struct = { + .address = MYNEWT_VAL(BNO055_I2CADDR), + .len = 1, + .buffer = payload + }; + + memcpy(&payload[1], buffer, len); + + /* Register write */ + rc = hal_i2c_master_write(MYNEWT_VAL(BNO055_I2CBUS), &data_struct, + OS_TICKS_PER_SEC / 10, 1); + if (rc) { + BNO055_ERR("I2C access failed at address 0x%02X\n", addr); +#if MYNEWT_VAL(BNO055_STATS) + STATS_INC(g_bno055stats, errors); +#endif + goto err; + } + + memset(payload, 0, sizeof(payload)); + data_struct.len = len; + rc = hal_i2c_master_write(MYNEWT_VAL(BNO055_I2CBUS), &data_struct, + OS_TICKS_PER_SEC / 10, len); + + if (rc) { + BNO055_ERR("Failed to read from 0x%02X:0x%02X\n", addr, reg); +#if MYNEWT_VAL(BNO055_STATS) + STATS_INC(g_bno055stats, errors); +#endif + goto err; + } + + return 0; +err: + return rc; +} + +/** * Reads a single byte from the specified register * * @param The register address to read from @@ -198,7 +251,9 @@ static int bno055_readlen(uint8_t reg, uint8_t *buffer, uint8_t len) { int rc; - uint8_t payload[8] = { reg, 0, 0, 0, 0, 0, 0, 0}; + uint8_t payload[23] = { reg, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0}; struct hal_i2c_master_data data_struct = { .address = MYNEWT_VAL(BNO055_I2CADDR), @@ -206,13 +261,8 @@ bno055_readlen(uint8_t reg, uint8_t *buffer, uint8_t len) .buffer = payload }; - if (len > 8) { - rc = SYS_EINVAL; - goto err; - } - /* Clear the supplied buffer */ - memset(buffer, 0, 8); + memset(buffer, 0, 22); /* Register write */ rc = hal_i2c_master_write(MYNEWT_VAL(BNO055_I2CBUS), &data_struct, @@ -225,7 +275,7 @@ bno055_readlen(uint8_t reg, uint8_t *buffer, uint8_t len) goto err; } - /* Read six bytes back */ + /* Read len bytes back */ memset(payload, 0, sizeof(payload)); data_struct.len = len; rc = hal_i2c_master_read(MYNEWT_VAL(BNO055_I2CBUS), &data_struct, @@ -247,7 +297,6 @@ err: return rc; } - /** * Setting operation mode for the bno055 sensor * @@ -685,7 +734,7 @@ bno055_find_reg(sensor_type_t type, uint8_t *reg) { int rc; - rc = 0; + rc = SYS_EOK; switch(type) { case SENSOR_TYPE_ACCELEROMETER: *reg = BNO055_ACCEL_DATA_X_LSB_ADDR; @@ -1035,6 +1084,248 @@ err: return rc; } +/** + * Gets current calibration status + * + * @param Calibration info structure to fill up calib state + * @return 0 on success, non-zero on failure + */ +int +bno055_get_calib_status(struct bno055_calib_info *bci) +{ + uint8_t status; + int rc; + + rc = bno055_read8(BNO055_CALIB_STAT_ADDR, &status); + if (rc) { + goto err; + } + + bci->bci_sys = (status >> 6) & 0x03; + bci->bci_gyro = (status >> 4) & 0x03; + bci->bci_accel = (status >> 2) & 0x03; + bci->bci_mag = status & 0x03; + + return 0; +err: + return rc; +} + +/** + * Checks if bno055 is fully calibrated + * + * @return 0 on success, non-zero on failure + */ +int +bno055_is_calib(void) +{ + struct bno055_calib_info bci; + int rc; + + rc = bno055_get_calib_status(&bci); + if (rc) { + goto err; + } + + if (bci.bci_sys< 3 || bci.bci_gyro < 3 || bci.bci_accel < 3 || bci.bci_mag < 3) { + goto err; + } + + return 0; +err: + return rc; +} + +/** + * Reads the sensor's offset registers into a byte array + * + * @param byte array to return offsets into + * @return 0 on success, non-zero on failure + * + */ +int +bno055_get_raw_sensor_offsets(uint8_t *offsets) +{ + uint8_t prev_mode; + int rc; + + rc = SYS_EOK; + if (!bno055_is_calib()) { + rc = bno055_get_opr_mode(&prev_mode); + if (rc) { + goto err; + } + + rc = bno055_set_opr_mode(BNO055_OPR_MODE_CONFIG); + if (rc) { + goto err; + } + + rc = bno055_readlen(BNO055_ACCEL_OFFSET_X_LSB_ADDR, offsets, + BNO055_NUM_OFFSET_REGISTERS); + if (rc) { + goto err; + } + + rc = bno055_set_opr_mode(prev_mode); + if (rc) { + goto err; + } + + return 0; + } +err: + return rc; +} + +/** + * + * Reads the sensor's offset registers into an offset struct + * + * @param structure to fill up offsets data + * @return 0 on success, non-zero on failure + */ +int +bno055_get_sensor_offsets(struct bno055_sensor_offsets *offsets) +{ + uint8_t payload[22]; + int rc; + + + rc = bno055_get_raw_sensor_offsets(payload); + if (rc) { + goto err; + } + + offsets->bso_acc_off_x = (payload[1] << 8) | payload[0]; + offsets->bso_acc_off_y = (payload[3] << 8) | payload[2]; + offsets->bso_acc_off_z = (payload[5] << 8) | payload[4]; + + offsets->bso_gyro_off_x = (payload[7] << 8) | payload[6]; + offsets->bso_gyro_off_y = (payload[9] << 8) | payload[8]; + offsets->bso_gyro_off_z = (payload[11] << 8) | payload[10]; + + offsets->bso_mag_off_x = (payload[13] << 8) | payload[12]; + offsets->bso_mag_off_y = (payload[15] << 8) | payload[14]; + offsets->bso_mag_off_z = (payload[17] << 8) | payload[16]; + + offsets->bso_acc_radius = (payload[19] << 8) | payload[18]; + offsets->bso_mag_radius = (payload[21] << 8) | payload[20]; + + return 0; +err: + return rc; +} + + +/** + * + * Writes calibration data to the sensor's offset registers + * + * @param calibration data + * @param calibration data length + * @return 0 on success, non-zero on success + */ +int +bno055_set_sensor_raw_offsets(uint8_t* calibdata, uint8_t len) +{ + uint8_t prev_mode; + int rc; + + if (len != 22) { + rc = SYS_EINVAL; + goto err; + } + + rc = bno055_get_opr_mode(&prev_mode); + if (rc) { + goto err; + } + + rc = bno055_set_opr_mode(BNO055_OPR_MODE_CONFIG); + if (rc) { + goto err; + } + + os_time_delay((25 * OS_TICKS_PER_SEC)/1000 + 1); + + rc = bno055_writelen(BNO055_ACCEL_OFFSET_X_LSB_ADDR, calibdata, len); + if (rc) { + goto err; + } + + rc = bno055_set_opr_mode(prev_mode); + if (rc) { + goto err; + } + + return 0; +err: + return rc; +} + +/** + * + * Writes to the sensor's offset registers from an offset struct + * + * @param pointer to the offset structure + * @return 0 on success, non-zero on failure + */ +int +bno055_set_sensor_offsets(struct bno055_sensor_offsets *offsets) +{ + uint8_t prev_mode; + int rc; + + rc = bno055_get_opr_mode(&prev_mode); + if (rc) { + goto err; + } + + rc = bno055_set_opr_mode(BNO055_OPR_MODE_CONFIG); + if (rc) { + goto err; + } + + os_time_delay((25 * OS_TICKS_PER_SEC)/1000 + 1); + + rc |= bno055_write8(BNO055_ACCEL_OFFSET_X_LSB_ADDR, (offsets->bso_acc_off_x) & 0x0FF); + rc |= bno055_write8(BNO055_ACCEL_OFFSET_X_MSB_ADDR, (offsets->bso_acc_off_x >> 8) & 0x0FF); + rc |= bno055_write8(BNO055_ACCEL_OFFSET_Y_LSB_ADDR, (offsets->bso_acc_off_y) & 0x0FF); + rc |= bno055_write8(BNO055_ACCEL_OFFSET_Y_MSB_ADDR, (offsets->bso_acc_off_y >> 8) & 0x0FF); + rc |= bno055_write8(BNO055_ACCEL_OFFSET_Z_LSB_ADDR, (offsets->bso_acc_off_z) & 0x0FF); + rc |= bno055_write8(BNO055_ACCEL_OFFSET_Z_MSB_ADDR, (offsets->bso_acc_off_z >> 8) & 0x0FF); + + rc |= bno055_write8(BNO055_GYRO_OFFSET_X_LSB_ADDR, (offsets->bso_gyro_off_x) & 0x0FF); + rc |= bno055_write8(BNO055_GYRO_OFFSET_X_MSB_ADDR, (offsets->bso_gyro_off_x >> 8) & 0x0FF); + rc |= bno055_write8(BNO055_GYRO_OFFSET_Y_LSB_ADDR, (offsets->bso_gyro_off_y) & 0x0FF); + rc |= bno055_write8(BNO055_GYRO_OFFSET_Y_MSB_ADDR, (offsets->bso_gyro_off_y >> 8) & 0x0FF); + rc |= bno055_write8(BNO055_GYRO_OFFSET_Z_LSB_ADDR, (offsets->bso_gyro_off_z) & 0x0FF); + rc |= bno055_write8(BNO055_GYRO_OFFSET_Z_MSB_ADDR, (offsets->bso_gyro_off_z >> 8) & 0x0FF); + + rc |= bno055_write8(BNO055_MAG_OFFSET_X_LSB_ADDR, (offsets->bso_mag_off_x) & 0x0FF); + rc |= bno055_write8(BNO055_MAG_OFFSET_X_MSB_ADDR, (offsets->bso_mag_off_x >> 8) & 0x0FF); + rc |= bno055_write8(BNO055_MAG_OFFSET_Y_LSB_ADDR, (offsets->bso_mag_off_y) & 0x0FF); + rc |= bno055_write8(BNO055_MAG_OFFSET_Y_MSB_ADDR, (offsets->bso_mag_off_y >> 8) & 0x0FF); + rc |= bno055_write8(BNO055_MAG_OFFSET_Z_LSB_ADDR, (offsets->bso_mag_off_z) & 0x0FF); + rc |= bno055_write8(BNO055_MAG_OFFSET_Z_MSB_ADDR, (offsets->bso_mag_off_z >> 8) & 0x0FF); + + rc |= bno055_write8(BNO055_ACCEL_RADIUS_LSB_ADDR, (offsets->bso_acc_radius) & 0x0FF); + rc |= bno055_write8(BNO055_ACCEL_RADIUS_MSB_ADDR, (offsets->bso_acc_radius >> 8) & 0x0FF); + + rc |= bno055_write8(BNO055_MAG_RADIUS_LSB_ADDR, (offsets->bso_mag_radius) & 0x0FF); + rc |= bno055_write8(BNO055_MAG_RADIUS_MSB_ADDR, (offsets->bso_mag_radius >> 8) & 0x0FF); + + rc |= bno055_set_opr_mode(prev_mode); + if (rc) { + goto err; + } + + return 0; +err: + return rc; +} + static void * bno055_sensor_get_interface(struct sensor *sensor, sensor_type_t type) { @@ -1064,8 +1355,8 @@ bno055_sensor_get_config(struct sensor *sensor, sensor_type_t type, cfg->sc_valtype = SENSOR_VALUE_TYPE_INT32; } - return (0); + return 0; err: - return (rc); + return rc; } http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/316c18ac/hw/drivers/sensors/bno055/src/bno055_priv.h ---------------------------------------------------------------------- diff --git a/hw/drivers/sensors/bno055/src/bno055_priv.h b/hw/drivers/sensors/bno055/src/bno055_priv.h index d98d3d0..b912a52 100644 --- a/hw/drivers/sensors/bno055/src/bno055_priv.h +++ b/hw/drivers/sensors/bno055/src/bno055_priv.h @@ -108,10 +108,10 @@ #define BNO055_PWR_MODE_ADDR 0X3E #define BNO055_SYS_TRIGGER_ADDR 0X3F -#define BNO055_SYS_TRIGGER_CLK_SEL (0x01 << 7) -#define BNO055_SYS_TRIGGER_RST_INT (0x01 << 6) -#define BNO055_SYS_TRIGGER_RST_SYS (0x01 << 5) -#define BNO055_SYS_TRIGGER_SELF_TEST (0x01) +#define BNO055_SYS_TRIGGER_CLK_SEL (0x01 << 7) +#define BNO055_SYS_TRIGGER_RST_INT (0x01 << 6) +#define BNO055_SYS_TRIGGER_RST_SYS (0x01 << 5) +#define BNO055_SYS_TRIGGER_SELF_TEST (0x01) #define BNO055_TEMP_SOURCE_ADDR 0X40 @@ -218,4 +218,6 @@ #define BNO055_GYRO_ANY_MOTION_THRES_ADDR 0X1E #define BNO055_GYRO_ANY_MOTION_SET_ADDR 0X1F +#define BNO055_NUM_OFFSET_REGISTERS 22 + #define BNO055_ID 0xA0 http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/316c18ac/hw/drivers/sensors/bno055/src/bno055_shell.c ---------------------------------------------------------------------- diff --git a/hw/drivers/sensors/bno055/src/bno055_shell.c b/hw/drivers/sensors/bno055/src/bno055_shell.c index 5a74dfd..6404245 100644 --- a/hw/drivers/sensors/bno055/src/bno055_shell.c +++ b/hw/drivers/sensors/bno055/src/bno055_shell.c @@ -96,12 +96,81 @@ bno055_shell_help(void) console_printf("\trev\n"); console_printf("\treset\n"); console_printf("\tpmode [0-normal | 1-lowpower | 2-suspend]\n"); + console_printf("\tsensor_offsets\n"); console_printf("\tdumpreg [addr]\n"); return 0; } static int +bno055_shell_cmd_sensor_offsets(int argc, char **argv) +{ + int i; + int rc; + struct bno055_sensor_offsets bso; + long val; + uint16_t offsetdata[11] = {0}; + char *tok; + + rc = 0; + if (argc > 3) { + return bno055_shell_err_too_many_args(argv[1]); + } + + /* Display the chip id */ + if (argc == 2) { + rc = bno055_get_sensor_offsets(&bso); + if (rc) { + console_printf("Read failed %d\n", rc); + goto err; + } + console_printf("Offsets:\n"); + console_printf(" \tacc \t | gyro\t | mag \t \n" + "\tx :0x%02X\t : 0x%02X\t : 0x%02X\t \n" + "\ty :0x%02X\t : 0x%02X\t : 0x%02X\t \n" + "\tz :0x%02X\t : 0x%02X\t : 0x%02X\t \n" + "\trad:0x%02X\t : \t : 0x%02X\t \n", + bso.bso_acc_off_x, bso.bso_mag_off_x, + bso.bso_gyro_off_x, bso.bso_acc_off_y, + bso.bso_mag_off_y, bso.bso_gyro_off_y, + bso.bso_acc_off_z, bso.bso_mag_off_z, + bso.bso_gyro_off_z, bso.bso_acc_radius, + bso.bso_mag_radius); + } else if (argc == 3) { + tok = strtok(argv[2], ":"); + i = 0; + do { + if (bno055_shell_stol(tok, 0, UINT16_MAX, &val)) { + return bno055_shell_err_invalid_arg(argv[2]); + } + offsetdata[i] = val; + tok = strtok(0, ":"); + } while(i++ < 11 && tok); + + bso.bso_acc_off_x = offsetdata[0]; + bso.bso_acc_off_y = offsetdata[1]; + bso.bso_acc_off_z = offsetdata[2]; + bso.bso_gyro_off_x = offsetdata[3]; + bso.bso_gyro_off_y = offsetdata[4]; + bso.bso_gyro_off_z = offsetdata[5]; + bso.bso_mag_off_x = offsetdata[6]; + bso.bso_mag_off_y = offsetdata[7]; + bso.bso_mag_off_z = offsetdata[8]; + bso.bso_acc_radius = offsetdata[9]; + bso.bso_mag_radius = offsetdata[10]; + + rc = bno055_set_sensor_offsets(&bso); + if (rc) { + goto err; + } + } + + return 0; +err: + return rc; +} + +static int bno055_shell_cmd_get_chip_id(int argc, char **argv) { uint8_t id; @@ -490,6 +559,11 @@ bno055_shell_cmd(int argc, char **argv) if (argc > 1 && strcmp(argv[1], "units") == 0) { return bno055_shell_units_cmd(argc, argv); } + + if (argc > 1 && strcmp(argv[1], "sensor_offsets") == 0) { + return bno055_shell_cmd_sensor_offsets(argc, argv); + } + return bno055_shell_err_unknown_arg(argv[1]); }
