[Spinal][Imu] add accel correction based on temperaure sensor

jsk-ros-pkg · Jun 21, 2024 · b06b03f · b06b03f
1 parent 09d0628
commit b06b03f
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 6 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
@@ -104,6 +104,7 @@ void ICM20948::updateRawData()
   gyroReadRad(&raw_gyro_adc_);
   accelReadG(&raw_acc_adc_);
   magReadUT(&raw_mag_adc_);
+  tempReadC(&raw_temp_adc_);
 }
 
 void ICM20948::gyroRead(Vector3f* data)
@@ -155,6 +156,15 @@ bool ICM20948::magRead(Vector3f* data)
   return true;
 }
 
+void ICM20948::tempRead(float* data)
+{
+  readSingleIcm20948(ub_0, B0_TEMP_OUT_H);
+  uint8_t temp_high = single_adc_;
+  readSingleIcm20948(ub_0, B0_TEMP_OUT_L);
+  uint8_t temp_low = single_adc_;
+  *data = (float)(temp_high << 8 | temp_low);
+}
+
 void ICM20948::gyroReadRad(Vector3f* data)
 {
   gyroRead(data);
@@ -167,10 +177,11 @@ void ICM20948::gyroReadRad(Vector3f* data)
 void ICM20948::accelReadG(Vector3f* data)
 {
   accelRead(data);
-
-  data->x = data->x / accel_scale_factor_ * GRAVITY_MSS;
-  data->y = data->y / accel_scale_factor_ * GRAVITY_MSS;
-  data->z = data->z / accel_scale_factor_ * GRAVITY_MSS;
+  /*Temperature correction*/
+  float corrected_scale_factor = accel_scale_factor_+ accel_scale_factor_ * (0.026f * raw_temp_adc_ )/100.0f;
+  data->x = data->x / corrected_scale_factor * GRAVITY_MSS;
+  data->y = data->y / corrected_scale_factor * GRAVITY_MSS;
+  data->z = data->z / corrected_scale_factor * GRAVITY_MSS;
 }
 
 bool ICM20948::magReadUT(Vector3f* data)
@@ -186,6 +197,11 @@ bool ICM20948::magReadUT(Vector3f* data)
   return true;
 }	
 
+void ICM20948::tempReadC(float* data)
+{
+  tempRead(data);
+  *data = *data/333.87f + 21.0f;
+}
 
 /* Sub Functions */
 bool ICM20948::getIcm20948WhoAmI()

diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/icm20948/icm_20948.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/drivers/icm20948/icm_20948.h
@@ -235,6 +235,7 @@ class ICM20948 : public IMUOnboard {
   /* raw adc data */
   uint8_t single_adc_;
   uint8_t multi_adc_[6];
+  float raw_temp_adc_;
 
   /* Initialization */
   void gyroInit(void) override;
@@ -247,13 +248,14 @@ class ICM20948 : public IMUOnboard {
   /* Raw 16 bits adc data reading function */
   void gyroRead(Vector3f* data);	
   void accelRead(Vector3f* data);
-  bool magRead(Vector3f* data); 
+  bool magRead(Vector3f* data);
+  void tempRead(float* data);
 
   /* Conversion from raw adc data to proper units */
   void gyroReadRad(Vector3f* data);
   void accelReadG(Vector3f* data);
   bool magReadUT(Vector3f* data);
-
+  void tempReadC(float* data);
 
   /* Sub functions */
   bool getIcm20948WhoAmI();