[SPinal][ICM20948] change imu update interval

sugihara-16 · Mar 31, 2024 · 46ffb8c · 46ffb8c
1 parent 371e375
commit 46ffb8c
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 7 deletions.
diff --git a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Src/main.c b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Src/main.c
@@ -1060,6 +1060,9 @@ void coreTaskFunc(void const * argument)
   nh_.initNode(dst_addr, 12345,12345);
 #endif
 
+  /* IMU timer */
+  static uint32_t pre_imu_update = 0;
+
   imu_.gyroCalib(true, IMU::GYRO_DEFAULT_CALIB_DURATION); // re-calibrate gyroscope because of the HAL_Delay in spine init
 
   osSemaphoreWait(coreTaskSemHandle, osWaitForever);
@@ -1071,8 +1074,11 @@ void coreTaskFunc(void const * argument)
 #if NERVE_COMM
       Spine::send();
 #endif
-
-      imu_.update();
+      if(HAL_GetTick() - pre_imu_update > IMU_UPDATE_INTERVAL)
+        {
+          imu_.update();
+          pre_imu_update = HAL_GetTick();
+        } 
       baro_.update();
       gps_.update();
       estimator_.update();

diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/config.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/config.h
@@ -47,6 +47,7 @@
 //2.2 State Estimate
 //2.2.1 Attitude Estimate
 #define ATTITUDE_ESTIMATE_FLAG 1
+#define IMU_UPDATE_INTERVAL 100
 //* Do not change following code!!!
 ///////////////////////////////////
 #if !IMU_FLAG

diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/icm_20948.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/icm_20948.cpp
@@ -102,11 +102,10 @@ void ICM20948::magInit(void)
 void ICM20948::updateRawData()
 {
   // if(getIcm20948WhoAmI()) nh_->logerror("ok icm");
-  if(getAk09916WhoAmI()) nh_->logerror("ok mag");
-  gyroRead(&raw_gyro_adc_);
-  // gyroReadDps(&raw_gyro_adc_);
-  // accelReadG(&raw_acc_adc_);
-  // magReadUT(&raw_mag_adc_);
+  // if(getAk09916WhoAmI()) nh_->logerror("ok mag");
+  gyroReadDps(&raw_gyro_adc_);
+  accelReadG(&raw_acc_adc_);
+  magReadUT(&raw_mag_adc_);
 }
 
 void ICM20948::gyroRead(Vector3f* data)