From 013fa535d3c7f3391883cc873692da11c260ca08 Mon Sep 17 00:00:00 2001 From: Junichiro Sugihara Date: Wed, 15 May 2024 17:28:23 +0900 Subject: [PATCH] [Spinal][IMU] workaround to use internal DLPF of ICM20948. --- .../Jsk_Lib/sensors/imu/drivers/icm20948/icm_20948.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/icm20948/icm_20948.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/icm20948/icm_20948.cpp index 3092ea202..bca9850c2 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/icm20948/icm_20948.cpp +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/icm20948/icm_20948.cpp @@ -56,7 +56,7 @@ void ICM20948::gyroInit(void) spiModeEnable(); // Use only SPI mode /* 3.Gyro initialization (Do not change following order) */ - setGyroLpf(0); + setGyroLpf(3); setGyroSampleRate(0); setGyroFullScale(_2000dps); } @@ -65,7 +65,7 @@ void ICM20948::accInit (void) { HAL_Delay(100); /* 5.Acc initialization (Do not change following order) */ - setAccelLpf(0); + setAccelLpf(6); setAccelSampleRate(0); setAccelFullScale(_8g); } @@ -313,14 +313,16 @@ void ICM20948::odrAlignEnable() void ICM20948::setGyroLpf(uint8_t config) { - uint8_t new_val = config << 3; + readSingleIcm20948(ub_2, B2_GYRO_CONFIG_1); + uint8_t new_val = single_adc_ | (config << 3); writeSingleIcm20948(ub_2, B2_GYRO_CONFIG_1, new_val); } void ICM20948::setAccelLpf(uint8_t config) { - uint8_t new_val = config << 3; + readSingleIcm20948(ub_2, B2_ACCEL_CONFIG); + uint8_t new_val = single_adc_ | (config << 3); writeSingleIcm20948(ub_2, B2_ACCEL_CONFIG, new_val); }