Repository: incubator-mynewt-core Updated Branches: refs/heads/sensors_branch 40b9b6b51 -> 30b53c064
SensorAPI:BNO055-Handle scaling using UNIT_SEL reg 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/30b53c06 Tree: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/tree/30b53c06 Diff: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/diff/30b53c06 Branch: refs/heads/sensors_branch Commit: 30b53c064284042da9063927edc28898b57523af Parents: 40b9b6b Author: Vipul Rahane <[email protected]> Authored: Thu Feb 16 16:46:05 2017 -0800 Committer: Vipul Rahane <[email protected]> Committed: Thu Feb 16 16:53:41 2017 -0800 ---------------------------------------------------------------------- .../sensors/bno055/include/bno055/bno055.h | 17 ++++ hw/drivers/sensors/bno055/src/bno055.c | 100 +++++++++++++++---- hw/drivers/sensors/bno055/src/bno055_priv.h | 11 ++ hw/drivers/sensors/bno055/src/bno055_shell.c | 58 +++++++++++ 4 files changed, 167 insertions(+), 19 deletions(-) ---------------------------------------------------------------------- http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/30b53c06/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 09fa3ec..8aade64 100644 --- a/hw/drivers/sensors/bno055/include/bno055/bno055.h +++ b/hw/drivers/sensors/bno055/include/bno055/bno055.h @@ -166,6 +166,23 @@ bno055_set_pwr_mode(uint8_t mode); uint8_t bno055_get_pwr_mode(void); +/** + * Setting units for the bno055 sensor + * + * @param power mode for the sensor + * @return 0 on success, non-zero on failure + */ +int +bno055_set_units(uint8_t val); + +/** + * Read current power mode of the sensor + * + * @return mode + */ +uint8_t +bno055_get_units(void); + #ifdef __cplusplus } #endif http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/30b53c06/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 bcc880d..014473f 100644 --- a/hw/drivers/sensors/bno055/src/bno055.c +++ b/hw/drivers/sensors/bno055/src/bno055.c @@ -104,6 +104,7 @@ static const struct sensor_driver g_bno055_sensor_driver = { static uint8_t g_bno055_opr_mode; static uint8_t g_bno055_pwr_mode; +static uint8_t g_bno055_units; /** * Writes a single byte to the specified register @@ -262,19 +263,23 @@ bno055_set_opr_mode(uint8_t mode) { int rc; + rc = bno055_write8(BNO055_OPR_MODE_ADDR, BNO055_OPERATION_MODE_CONFIG); + if (rc) { + goto err; + } + + os_time_delay(OS_TICKS_PER_SEC/1000 * 19); + rc = bno055_write8(BNO055_OPR_MODE_ADDR, mode); if (rc) { goto err; } /* Refer table 3-6 in the datasheet for the delay values */ - if (mode == BNO055_OPERATION_MODE_CONFIG) { - os_time_delay(OS_TICKS_PER_SEC/1000 * 19); - } else { - os_time_delay(OS_TICKS_PER_SEC/1000 * 7); - } + os_time_delay(OS_TICKS_PER_SEC/1000 * 7); g_bno055_opr_mode = mode; + return 0; err: return rc; @@ -314,6 +319,39 @@ bno055_get_pwr_mode(void) } /** + * Setting units for the bno055 sensor + * + * @param power mode for the sensor + * @return 0 on success, non-zero on failure + */ +int +bno055_set_units(uint8_t val) +{ + int rc; + + rc = bno055_write8(BNO055_UNIT_SEL_ADDR, val); + if (rc) { + goto err; + } + + g_bno055_units = val; + return 0; +err: + return rc; +} + +/** + * Read current power mode of the sensor + * + * @return mode + */ +uint8_t +bno055_get_units(void) +{ + return g_bno055_units; +} + +/** * Read current operational mode of the sensor * * @return mode @@ -479,7 +517,7 @@ bno055_config(struct bno055 *bno055, struct bno055_cfg *cfg) } if (id != BNO055_ID) { - os_time_delay(OS_TICKS_PER_SEC/2); + os_time_delay(OS_TICKS_PER_SEC/1000 * 100); rc = bno055_get_chip_id(&id); if (rc) { @@ -503,11 +541,8 @@ bno055_config(struct bno055 *bno055, struct bno055_cfg *cfg) goto err; } - /* Wait for about 100 ms */ - os_time_delay(OS_TICKS_PER_SEC/1000 * 100); - /* Set to normal power mode */ - rc = bno055_write8(BNO055_PWR_MODE_ADDR, BNO055_POWER_MODE_NORMAL); + rc = bno055_set_pwr_mode(BNO055_POWER_MODE_NORMAL); if (rc) { goto err; } @@ -529,6 +564,14 @@ bno055_config(struct bno055 *bno055, struct bno055_cfg *cfg) goto err; } + /* Setting units and data output format */ + rc = bno055_set_units(BNO055_ACC_UNIT_MS2 | BNO055_ANGRATE_UNIT_DPS | + BNO055_EULER_UNIT_DEG | BNO055_TEMP_UNIT_DEGC | + BNO055_DO_FORMAT_ANDROID); + if (rc) { + goto err; + } + /* Overwrite the configuration data. */ memcpy(&bno055->cfg, cfg, sizeof(*cfg)); @@ -637,6 +680,11 @@ bno055_get_vector_data(void *datastruct, int type) struct sensor_accel_data *sad; struct sensor_euler_data *sed; uint8_t reg; + uint8_t units; + float acc_div; + float gyro_div; + float euler_div; + int rc; memset (payload, 0, 6); @@ -657,6 +705,12 @@ bno055_get_vector_data(void *datastruct, int type) y = ((int16_t)payload[2]) | (((int16_t)payload[3]) << 8); z = ((int16_t)payload[4]) | (((int16_t)payload[5]) << 8); + units = bno055_get_units(); + + acc_div = units & BNO055_ACC_UNIT_MG ? 1.0:100.0; + gyro_div = units & BNO055_ANGRATE_UNIT_RPS ? 900.0:16.0; + euler_div = units & BNO055_EULER_UNIT_RAD ? 16.0:900.0; + /** * Convert the value to an appropriate range (section 3.6.4) */ @@ -671,25 +725,25 @@ bno055_get_vector_data(void *datastruct, int type) case SENSOR_TYPE_GYROSCOPE: sad = datastruct; /* 1rps = 900 LSB */ - sad->sad_x = ((double)x)/900.0; - sad->sad_y = ((double)y)/900.0; - sad->sad_z = ((double)z)/900.0; + sad->sad_x = ((double)x)/gyro_div; + sad->sad_y = ((double)y)/gyro_div; + sad->sad_z = ((double)z)/gyro_div; break; case SENSOR_TYPE_EULER: sad = datastruct; /* 1 degree = 16 LSB */ - sed->sed_h = ((double)x)/16.0; - sed->sed_r = ((double)y)/16.0; - sed->sed_p = ((double)z)/16.0; + sed->sed_h = ((double)x)/euler_div; + sed->sed_r = ((double)y)/euler_div; + sed->sed_p = ((double)z)/euler_div; break; case SENSOR_TYPE_ACCELEROMETER: case SENSOR_TYPE_LINEAR_ACCEL: case SENSOR_TYPE_GRAVITY: sad = datastruct; /* 1m/s^2 = 100 LSB */ - sad->sad_x = ((double)x)/100.0; - sad->sad_y = ((double)y)/100.0; - sad->sad_z = ((double)z)/100.0; + sad->sad_x = ((double)x)/acc_div; + sad->sad_y = ((double)y)/acc_div; + sad->sad_z = ((double)z)/acc_div; break; default: BNO055_ERR("Not supported sensor type: %d\n", type); @@ -712,9 +766,17 @@ int bno055_get_temp(int8_t *temp) { int rc; + uint8_t units; + uint8_t div; rc = bno055_read8(BNO055_TEMP_ADDR, (uint8_t *)temp); + units = bno055_get_units(); + + div = units & BNO055_TEMP_UNIT_DEGF ? 2 : 1; + + *temp = *temp/div; + return rc; } http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/30b53c06/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 97f4280..8bb0ea1 100644 --- a/hw/drivers/sensors/bno055/src/bno055_priv.h +++ b/hw/drivers/sensors/bno055/src/bno055_priv.h @@ -100,7 +100,18 @@ /* Unit selection register */ #define BNO055_UNIT_SEL_ADDR 0X3B + #define BNO055_DATA_SELECT_ADDR 0X3C +#define BNO055_ACC_UNIT_MS2 0x0 +#define BNO055_ACC_UNIT_MG 0x1 +#define BNO055_ANGRATE_UNIT_DPS (0 << 1) +#define BNO055_ANGRATE_UNIT_RPS (1 << 1) +#define BNO055_EULER_UNIT_DEG (0 << 2) +#define BNO055_EULER_UNIT_RAD (1 << 2) +#define BNO055_TEMP_UNIT_DEGC (0 << 4) +#define BNO055_TEMP_UNIT_DEGF (1 << 4) +#define BNO055_DO_FORMAT_WINDOWS (0 << 7) +#define BNO055_DO_FORMAT_ANDROID (1 << 7) /* Mode registers */ #define BNO055_OPR_MODE_ADDR 0X3D http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/30b53c06/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 f370655..4e0b3f7 100644 --- a/hw/drivers/sensors/bno055/src/bno055_shell.c +++ b/hw/drivers/sensors/bno055/src/bno055_shell.c @@ -159,6 +159,7 @@ bno055_shell_cmd_read(int argc, char **argv) struct sensor_quat_data *sqd; struct sensor_euler_data *sed; struct sensor_accel_data *sad; + int8_t *temp; int type; char tmpstr[13]; @@ -211,6 +212,15 @@ bno055_shell_cmd_read(int argc, char **argv) console_printf("r:%s ", sensor_ftostr(sed->sed_r, tmpstr, 13)); console_printf("p:%s\n", sensor_ftostr(sed->sed_p, tmpstr, 13)); + } else if (type == SENSOR_TYPE_TEMPERATURE) { + rc = bno055_get_temp(databuf); + if (rc) { + console_printf("Read failed: %d\n", rc); + goto err; + } + temp = databuf; + + console_printf("Temperature:%u\n", *temp); } else { rc = bno055_get_vector_data(databuf, type); if (rc) { @@ -307,6 +317,50 @@ err: } static int +bno055_shell_units_cmd(int argc, char **argv) +{ + long val; + int rc; + + if (argc > 3) { + return bno055_shell_err_too_many_args(argv[1]); + } + + /* Display the units */ + if (argc == 2) { + val = bno055_get_units(); + console_printf("Acc, linear acc, gravity: %s\n" + "Mag field strength: Micro Tesla\n" + "Ang rate: %s\n" + "Euler ang: %s\n", + (uint8_t)val & BNO055_ACC_UNIT_MG ? "mg":"m/s^2", + (uint8_t)val & BNO055_ANGRATE_UNIT_RPS ? "Rps":"Dps", + (uint8_t)val & BNO055_EULER_UNIT_RAD ? "Rad":"Deg"); + console_printf("Quat: Quat units\n" + "Temp: %s\n" + "Fusion data output: %s\n", + (uint8_t)val & BNO055_TEMP_UNIT_DEGF ? "Deg F":"Deg C", + (uint8_t)val & BNO055_DO_FORMAT_ANDROID ? "Android":"Windows"); + } + + /* Update the units */ + if (argc == 3) { + if (bno055_shell_stol(argv[2], 0, UINT8_MAX, &val)) { + return bno055_shell_err_invalid_arg(argv[2]); + } + + rc = bno055_set_units(val); + if (rc) { + goto err; + } + } + + return 0; +err: + return rc; +} + +static int bno055_shell_cmd_dumpreg(int argc, char **argv) { long addr; @@ -427,6 +481,10 @@ bno055_shell_cmd(int argc, char **argv) if (argc > 1 && strcmp(argv[1], "i2cscan") == 0) { return shell_i2cscan_cmd(argc, argv); } + + if (argc > 1 && strcmp(argv[1], "units") == 0) { + return bno055_shell_units_cmd(argc, argv); + } return bno055_shell_err_unknown_arg(argv[1]); }
