From 349361ba36745764645bec3563062b1d0fc71068 Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Fri, 14 Aug 2020 20:13:32 -0700 Subject: [PATCH 1/3] Fix noisy IMU signal LSM6DS3 was writing to the output registers at the same time as reads were happening, causing scrambled data. --- imu/lsm6ds3.c | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/imu/lsm6ds3.c b/imu/lsm6ds3.c index 5a1cd67d..038150de 100644 --- a/imu/lsm6ds3.c +++ b/imu/lsm6ds3.c @@ -162,12 +162,19 @@ static THD_FUNCTION(lsm6ds3_thread, arg) { while (!chThdShouldTerminateX()) { - - uint8_t txb[1]; + uint8_t txb[2]; uint8_t rxb[12]; + // Disable IMU writing to output registers + txb[0] = LSM6DS3_ACC_GYRO_CTRL3_C; + txb[1] = LSM6DS3_ACC_GYRO_BDU_BLOCK_UPDATE | LSM6DS3_ACC_GYRO_IF_INC_ENABLED; + i2c_bb_tx_rx(&m_i2c_bb, lsm6ds3_addr, txb, 2, rxb, 1); + + // Read IMU output registers txb[0] = LSM6DS3_ACC_GYRO_OUTX_L_G; bool res = i2c_bb_tx_rx(&m_i2c_bb, lsm6ds3_addr, txb, 1, rxb, 12); + + // Parse 6 axis values float gx = (float)((int16_t)((uint16_t)rxb[1] << 8) + rxb[0]) * 4.375 * (2000 / 125) / 1000; float gy = (float)((int16_t)((uint16_t)rxb[3] << 8) + rxb[2]) * 4.375 * (2000 / 125) / 1000; float gz = (float)((int16_t)((uint16_t)rxb[5] << 8) + rxb[4]) * 4.375 * (2000 / 125) / 1000; @@ -180,7 +187,6 @@ static THD_FUNCTION(lsm6ds3_thread, arg) { read_callback(tmp_accel, tmp_gyro, tmp_mag); } - // Delay between loops chThdSleepMilliseconds((int)((1000.0 / rate_hz))); } From 0073c57d5955104287bf970d56d1cd0868f73ab4 Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Sun, 23 Aug 2020 21:34:20 -0700 Subject: [PATCH 2/3] Increase BMI160 output data rate VESC tool allows users to set IMU update frequency up to 1khz, but if the BMI160 output data rate is only 200hz, then this option is useless (and performance at 1khz is much better than 200hz). --- imu/bmi160_wrapper.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/imu/bmi160_wrapper.c b/imu/bmi160_wrapper.c index a1c9c2ee..0b7a4451 100644 --- a/imu/bmi160_wrapper.c +++ b/imu/bmi160_wrapper.c @@ -54,12 +54,12 @@ static bool reset_init_bmi(BMI_STATE *s) { bmi160_init(&(s->sensor)); - s->sensor.accel_cfg.odr = BMI160_ACCEL_ODR_200HZ; + s->sensor.accel_cfg.odr = BMI160_ACCEL_ODR_1600HZ; s->sensor.accel_cfg.range = BMI160_ACCEL_RANGE_16G; s->sensor.accel_cfg.bw = BMI160_ACCEL_BW_NORMAL_AVG4; s->sensor.accel_cfg.power = BMI160_ACCEL_NORMAL_MODE; - s->sensor.gyro_cfg.odr = BMI160_GYRO_ODR_200HZ; + s->sensor.gyro_cfg.odr = BMI160_GYRO_ODR_1600HZ; s->sensor.gyro_cfg.range = BMI160_GYRO_RANGE_2000_DPS; s->sensor.gyro_cfg.bw = BMI160_GYRO_BW_NORMAL_MODE; s->sensor.gyro_cfg.power = BMI160_GYRO_NORMAL_MODE; From 1edad098a324cdb530263eb992344f4389adc828 Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Tue, 25 Aug 2020 18:02:19 -0700 Subject: [PATCH 3/3] Match ODR to imu hz --- imu/bmi160_wrapper.c | 25 +++++++++++++++++++++++-- 1 file changed, 23 insertions(+), 2 deletions(-) diff --git a/imu/bmi160_wrapper.c b/imu/bmi160_wrapper.c index 0b7a4451..009b92e9 100644 --- a/imu/bmi160_wrapper.c +++ b/imu/bmi160_wrapper.c @@ -54,16 +54,37 @@ static bool reset_init_bmi(BMI_STATE *s) { bmi160_init(&(s->sensor)); - s->sensor.accel_cfg.odr = BMI160_ACCEL_ODR_1600HZ; s->sensor.accel_cfg.range = BMI160_ACCEL_RANGE_16G; s->sensor.accel_cfg.bw = BMI160_ACCEL_BW_NORMAL_AVG4; s->sensor.accel_cfg.power = BMI160_ACCEL_NORMAL_MODE; - s->sensor.gyro_cfg.odr = BMI160_GYRO_ODR_1600HZ; s->sensor.gyro_cfg.range = BMI160_GYRO_RANGE_2000_DPS; s->sensor.gyro_cfg.bw = BMI160_GYRO_BW_NORMAL_MODE; s->sensor.gyro_cfg.power = BMI160_GYRO_NORMAL_MODE; + if(s->rate_hz <= 25){ + s->sensor.accel_cfg.odr = BMI160_ACCEL_ODR_25HZ; + s->sensor.gyro_cfg.odr = BMI160_GYRO_ODR_25HZ; + }else if(s->rate_hz <= 50){ + s->sensor.accel_cfg.odr = BMI160_ACCEL_ODR_50HZ; + s->sensor.gyro_cfg.odr = BMI160_GYRO_ODR_50HZ; + }else if(s->rate_hz <= 100){ + s->sensor.accel_cfg.odr = BMI160_ACCEL_ODR_100HZ; + s->sensor.gyro_cfg.odr = BMI160_GYRO_ODR_100HZ; + }else if(s->rate_hz <= 200){ + s->sensor.accel_cfg.odr = BMI160_ACCEL_ODR_200HZ; + s->sensor.gyro_cfg.odr = BMI160_GYRO_ODR_200HZ; + }else if(s->rate_hz <= 400){ + s->sensor.accel_cfg.odr = BMI160_ACCEL_ODR_400HZ; + s->sensor.gyro_cfg.odr = BMI160_GYRO_ODR_400HZ; + }else if(s->rate_hz <= 800){ + s->sensor.accel_cfg.odr = BMI160_ACCEL_ODR_800HZ; + s->sensor.gyro_cfg.odr = BMI160_GYRO_ODR_800HZ; + }else{ + s->sensor.accel_cfg.odr = BMI160_ACCEL_ODR_1600HZ; + s->sensor.gyro_cfg.odr = BMI160_GYRO_ODR_1600HZ; + } + int8_t res = bmi160_set_sens_conf(&(s->sensor)); return res == BMI160_OK;