Merge pull request vedderb#510 from Mitchlol/lsm6ds3_improovments

Set lsm6ds3 odr nearest to configured frequency
jrmout · Jul 25, 2022 · d8b98b6 · d8b98b6
2 parents 0f40933 + 5e61b94
commit d8b98b6
Show file tree

Hide file tree

Showing 3 changed files with 66 additions and 11 deletions.
diff --git a/imu/imu.c b/imu/imu.c
@@ -63,9 +63,10 @@ void imu_init(imu_config *set) {
 	mpu9150_set_rate_hz(MIN(set->sample_rate_hz, 1000));
 	m_icm20948_state.rate_hz = MIN(set->sample_rate_hz, 1000);
 	m_bmi_state.rate_hz = set->sample_rate_hz;
-	lsm6ds3_set_rate_hz(MIN(set->sample_rate_hz, 1000));
+	lsm6ds3_set_rate_hz(set->sample_rate_hz);
 
 	m_bmi_state.filter = set->filter;
+	lsm6ds3_set_filter(set->filter);
 
 	if (set->type == IMU_TYPE_INTERNAL) {
 #ifdef MPU9X50_SDA_GPIO

diff --git a/imu/lsm6ds3.c b/imu/lsm6ds3.c
@@ -30,6 +30,7 @@ static thread_t *lsm6ds3_thread_ref = NULL;
 static i2c_bb_state m_i2c_bb;
 static volatile uint16_t lsm6ds3_addr;
 static int rate_hz = 1000;
+static IMU_FILTER filter;
 
 static void terminal_read_reg(int argc, const char **argv);
 static uint8_t read_single_reg(uint8_t reg);
@@ -43,6 +44,10 @@ void lsm6ds3_set_rate_hz(int hz) {
 	rate_hz = hz;
 }
 
+void lsm6ds3_set_filter(IMU_FILTER f) {
+	filter = f;
+}
+
 void lsm6ds3_init(stm32_gpio_t *sda_gpio, int sda_pin,
 		stm32_gpio_t *scl_gpio, int scl_pin,
 		stkalign_t *work_area, size_t work_area_size) {
@@ -70,29 +75,75 @@ void lsm6ds3_init(stm32_gpio_t *sda_gpio, int sda_pin,
 			commands_printf("LSM6DS3 Address B failed");
 			return;
 		}
+	}
 
+	bool is_trc = false;
+	if(rxb[0] == 0x6A){
+		is_trc = true;
 	}
 
 	// Configure imu
-	// Set all accel speeds to MAX
+	// Set all accel speeds
 	txb[0] = LSM6DS3_ACC_GYRO_CTRL1_XL;
-	txb[1] = LSM6DS3_ACC_GYRO_BW_XL_400Hz | LSM6DS3_ACC_GYRO_FS_XL_16g | LSM6DS3_ACC_GYRO_ODR_XL_6660Hz;
+	txb[1] = LSM6DS3_ACC_GYRO_BW_XL_400Hz | LSM6DS3_ACC_GYRO_FS_XL_16g;
+	if(rate_hz <= 13){
+		txb[1] |= LSM6DS3_ACC_GYRO_ODR_XL_13Hz;
+	}else if(rate_hz <= 26){
+		txb[1] |= LSM6DS3_ACC_GYRO_ODR_XL_26Hz;
+	}else if(rate_hz <= 52){
+		txb[1] |= LSM6DS3_ACC_GYRO_ODR_XL_52Hz;
+	}else if(rate_hz <= 104){
+		txb[1] |= LSM6DS3_ACC_GYRO_ODR_XL_104Hz;
+	}else if(rate_hz <= 208){
+		txb[1] |= LSM6DS3_ACC_GYRO_ODR_XL_208Hz;
+	}else if(rate_hz <= 416){
+		txb[1] |= LSM6DS3_ACC_GYRO_ODR_XL_416Hz;
+	}else if(rate_hz <= 833){
+		txb[1] |= LSM6DS3_ACC_GYRO_ODR_XL_833Hz;
+	}else if(rate_hz <= 1660){
+		txb[1] |= LSM6DS3_ACC_GYRO_ODR_XL_1660Hz;
+	}else if(rate_hz <= 3330){
+		txb[1] |= LSM6DS3_ACC_GYRO_ODR_XL_3330Hz;
+	}else{
+		txb[1] |= LSM6DS3_ACC_GYRO_ODR_XL_6660Hz;
+	}
 	res = i2c_bb_tx_rx(&m_i2c_bb, lsm6ds3_addr, txb, 2, rxb, 1);
 	if(!res){
 		commands_printf("LSM6DS3 Accel Config FAILED");
 		return;
 	}
 
-	// Set all gyro speeds to MAX
+	// Set all gyro speeds
 	txb[0] = LSM6DS3_ACC_GYRO_CTRL2_G;
-	txb[1] = LSM6DS3_ACC_GYRO_FS_G_2000dps | LSM6DS3_ACC_GYRO_ODR_G_1660Hz;
+	txb[1] = LSM6DS3_ACC_GYRO_FS_G_2000dps;
+	if(rate_hz <= 13){
+		txb[1] |= LSM6DS3_ACC_GYRO_ODR_G_13Hz;
+	}else if(rate_hz <= 26){
+		txb[1] |= LSM6DS3_ACC_GYRO_ODR_G_26Hz;
+	}else if(rate_hz <= 52){
+		txb[1] |= LSM6DS3_ACC_GYRO_ODR_G_52Hz;
+	}else if(rate_hz <= 104){
+		txb[1] |= LSM6DS3_ACC_GYRO_ODR_G_104Hz;
+	}else if(rate_hz <= 208){
+		txb[1] |= LSM6DS3_ACC_GYRO_ODR_G_208Hz;
+	}else if(rate_hz <= 416){
+		txb[1] |= LSM6DS3_ACC_GYRO_ODR_G_416Hz;
+	}else if(rate_hz <= 833){
+		txb[1] |= LSM6DS3_ACC_GYRO_ODR_G_833Hz;
+	}else if(rate_hz <= 1660 || is_trc == false){
+		txb[1] |= LSM6DS3_ACC_GYRO_ODR_G_1660Hz;
+	}else if(rate_hz <= 3330){
+		txb[1] |= LSM6DS3TRC_ACC_GYRO_ODR_G_3330Hz;
+	}else{
+		txb[1] |= LSM6DS3TRC_ACC_GYRO_ODR_G_6660Hz;
+	}
 	res = i2c_bb_tx_rx(&m_i2c_bb, lsm6ds3_addr, txb, 2, rxb, 1);
 	if(!res){
 		commands_printf("LSM6DS3 Gyro Config FAILED");
 		return;
 	}
 
-	// Set ODR???
+	// Set XL anti-aliasing filter to be manually configured
 	txb[0] = LSM6DS3_ACC_GYRO_CTRL4_C;
 	txb[1] = LSM6DS3_ACC_GYRO_BW_SCAL_ODR_ENABLED;
 	res = i2c_bb_tx_rx(&m_i2c_bb, lsm6ds3_addr, txb, 1, rxb, 1);

diff --git a/imu/lsm6ds3.h b/imu/lsm6ds3.h
@@ -37,6 +37,7 @@
 #include "hal.h"
 
 void lsm6ds3_set_rate_hz(int hz);
+void lsm6ds3_set_filter(IMU_FILTER f);
 void lsm6ds3_init(stm32_gpio_t *sda_gpio, int sda_pin,
 		stm32_gpio_t *scl_gpio, int scl_pin,
 		stkalign_t *work_area, size_t work_area_size);
@@ -652,10 +653,10 @@ typedef enum {
 * Permission    : RW
 *******************************************************************************/
 typedef enum {
-	LSM6DS3_ACC_GYRO_ODR_XL_POWER_DOWN 		 = 0x00,
-	LSM6DS3_ACC_GYRO_ODR_XL_13Hz 		         = 0x10,
-	LSM6DS3_ACC_GYRO_ODR_XL_26Hz 		         = 0x20,
-	LSM6DS3_ACC_GYRO_ODR_XL_52Hz 		         = 0x30,
+	LSM6DS3_ACC_GYRO_ODR_XL_POWER_DOWN   = 0x00,
+	LSM6DS3_ACC_GYRO_ODR_XL_13Hz 		 = 0x10,
+	LSM6DS3_ACC_GYRO_ODR_XL_26Hz 		 = 0x20,
+	LSM6DS3_ACC_GYRO_ODR_XL_52Hz 		 = 0x30,
 	LSM6DS3_ACC_GYRO_ODR_XL_104Hz 		 = 0x40,
 	LSM6DS3_ACC_GYRO_ODR_XL_208Hz 		 = 0x50,
 	LSM6DS3_ACC_GYRO_ODR_XL_416Hz 		 = 0x60,
@@ -697,7 +698,7 @@ typedef enum {
 * Permission    : RW
 *******************************************************************************/
 typedef enum {
-	LSM6DS3_ACC_GYRO_ODR_G_POWER_DOWN 		 = 0x00,
+	LSM6DS3_ACC_GYRO_ODR_G_POWER_DOWN 	 = 0x00,
 	LSM6DS3_ACC_GYRO_ODR_G_13Hz 		 = 0x10,
 	LSM6DS3_ACC_GYRO_ODR_G_26Hz 		 = 0x20,
 	LSM6DS3_ACC_GYRO_ODR_G_52Hz 		 = 0x30,
@@ -706,6 +707,8 @@ typedef enum {
 	LSM6DS3_ACC_GYRO_ODR_G_416Hz 		 = 0x60,
 	LSM6DS3_ACC_GYRO_ODR_G_833Hz 		 = 0x70,
 	LSM6DS3_ACC_GYRO_ODR_G_1660Hz 		 = 0x80,
+	LSM6DS3TRC_ACC_GYRO_ODR_G_3330Hz 	 = 0x90,
+	LSM6DS3TRC_ACC_GYRO_ODR_G_6660Hz 	 = 0xA0,
 } LSM6DS3_ACC_GYRO_ODR_G_t;
 
 /*******************************************************************************