Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-28 01:53:48 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[imu] Fix Invensense V3 driver temeperature and disable AFSR (#3192)

Browse Source
This commit is contained in:
Freek van Tienen
2023-12-04 09:09:23 +01:00
committed by GitHub
parent cfae3bc0d9
commit 8b60965be5
3 changed files with 15 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/imu_cube.xml
+1 -1
View File
@@ -46,7 +46,7 @@
<!-- Configure the heater (ICM20602) --> <!-- Configure the heater (ICM20602) -->
<define name="IMU_HEATER_GYRO_ID" value="IMU_CUBE2_ID"/> <define name="IMU_HEATER_GYRO_ID" value="IMU_CUBE2_ID"/>
<define name="IMU_HEATER_TARGET_TEMP" value="55.0"/> <define name="IMU_HEATER_TARGET_TEMP" value="45.0"/>
<define name="IMU_HEATER_P_GAIN" value="50.0"/> <define name="IMU_HEATER_P_GAIN" value="50.0"/>
<define name="IMU_HEATER_I_GAIN" value="0.07"/> <define name="IMU_HEATER_I_GAIN" value="0.07"/>
sw/airborne/peripherals/invensense3.c
+13 -8
View File
@@ -488,7 +488,7 @@ static void invensense3_parse_fifo_data(struct invensense3_t *inv, uint8_t *data
// Temperature sensor register data TEMP_DATA is updated with new data at max(Accelerometer ODR, Gyroscope ODR) // Temperature sensor register data TEMP_DATA is updated with new data at max(Accelerometer ODR, Gyroscope ODR)
if(gyro_samplerate == faster_odr) if(gyro_samplerate == faster_odr)
temp += data[13]; temp += (int8_t)data[13];
i++; i++;
} }
@@ -500,7 +500,7 @@ static void invensense3_parse_fifo_data(struct invensense3_t *inv, uint8_t *data
accel[j].z = (int16_t)((uint16_t)data[5] | data[6] << 8 ); accel[j].z = (int16_t)((uint16_t)data[5] | data[6] << 8 );
if(accel_samplerate == faster_odr) if(accel_samplerate == faster_odr)
temp += data[13]; temp += (int8_t)data[13];
j++; j++;
} }
@@ -919,27 +919,32 @@ static bool invensense3_config(struct invensense3_t *inv) {
inv->config_idx++; inv->config_idx++;
break; break;
case 12: case 12:
/* Disable AFSR, which changes noise sensitivity with angular rate, causing stuck gyro's for 2ms resulting in DC gyro bias */
if(invensense3_register_write(inv, INV3REG_INTF_CONFIG1, 0x51))
inv->config_idx++;
break;
case 13:
/* FIFO Stop-on-Full mode (enable the FIFO) */ /* FIFO Stop-on-Full mode (enable the FIFO) */
if(invensense3_register_write(inv, INV3REG_FIFO_CONFIG , FIFO_CONFIG_MODE_STOP_ON_FULL << FIFO_CONFIG_MODE_SHIFT)) if(invensense3_register_write(inv, INV3REG_FIFO_CONFIG , FIFO_CONFIG_MODE_STOP_ON_FULL << FIFO_CONFIG_MODE_SHIFT))
inv->config_idx++; inv->config_idx++;
break; break;
case 13: case 14:
/* FIFO content: enable accel, gyro, temperature */ /* FIFO content: enable accel, gyro, temperature */
if(invensense3_register_write(inv, INV3REG_FIFO_CONFIG1 , BIT_FIFO_CONFIG1_ACCEL_EN | if(invensense3_register_write(inv, INV3REG_FIFO_CONFIG1 , BIT_FIFO_CONFIG1_ACCEL_EN |
BIT_FIFO_CONFIG1_GYRO_EN | BIT_FIFO_CONFIG1_TEMP_EN)) BIT_FIFO_CONFIG1_GYRO_EN | BIT_FIFO_CONFIG1_TEMP_EN))
inv->config_idx++; inv->config_idx++;
break; break;
case 14: case 15:
/* Set FIFO watermark to 1 (so that INT is triggered for each packet) */ /* Set FIFO watermark to 1 (so that INT is triggered for each packet) */
if(invensense3_register_write(inv, INV3REG_FIFO_CONFIG2, 20)) if(invensense3_register_write(inv, INV3REG_FIFO_CONFIG2, 20))
inv->config_idx++; inv->config_idx++;
break; break;
case 15: case 16:
/* Set FIFO watermark to 1 (so that INT is triggered for each packet) */ /* Set FIFO watermark to 1 (so that INT is triggered for each packet) */
if(invensense3_register_write(inv, INV3REG_FIFO_CONFIG3, 0x00)) if(invensense3_register_write(inv, INV3REG_FIFO_CONFIG3, 0x00))
inv->config_idx++; inv->config_idx++;
break; break;
case 16: case 17:
/* Enable interrupt pin/status */ /* Enable interrupt pin/status */
switch(inv->parser) { switch(inv->parser) {
case INVENSENSE3_PARSER_REGISTERS: case INVENSENSE3_PARSER_REGISTERS:
@@ -954,13 +959,13 @@ static bool invensense3_config(struct invensense3_t *inv) {
break; break;
} }
break; break;
case 17: case 18:
/* Interrupt pins ASYNC_RESET configuration */ /* Interrupt pins ASYNC_RESET configuration */
// Datasheet: "User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation" // Datasheet: "User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation"
if(invensense3_register_write(inv, INV3REG_INT_CONFIG1, BIT_INT_ASYNC_RESET)) if(invensense3_register_write(inv, INV3REG_INT_CONFIG1, BIT_INT_ASYNC_RESET))
inv->config_idx++; inv->config_idx++;
break; break;
case 18: case 19:
/* FIFO flush */ /* FIFO flush */
if(invensense3_reset_fifo(inv)) if(invensense3_reset_fifo(inv))
inv->config_idx++; inv->config_idx++;
sw/airborne/peripherals/invensense3_regs.h
+1
View File
@@ -66,6 +66,7 @@
# define FIFO_COUNT_BIG_ENDIAN 0x20 # define FIFO_COUNT_BIG_ENDIAN 0x20
# define FIFO_COUNT_REC 0x40 # define FIFO_COUNT_REC 0x40
# define FIFO_HOLD_LAST_DATA_EN 0x80 # define FIFO_HOLD_LAST_DATA_EN 0x80
#define INV3REG_INTF_CONFIG1 INV3REG(INV3_BANK0,0x4DU)
#define INV3REG_PWR_MGMT0 INV3REG(INV3_BANK0,0x4EU) #define INV3REG_PWR_MGMT0 INV3REG(INV3_BANK0,0x4EU)
# define ACCEL_MODE_OFF 0x00 # define ACCEL_MODE_OFF 0x00
# define ACCEL_MODE_LN 0x03 # define ACCEL_MODE_LN 0x03