[Spinal][ICM20948] decreaced mag read freqency to balance coretask up…

…date frequency (bucause mag read include some HAL_Delay)
jsk-ros-pkg · Apr 3, 2024 · 7e9bb80 · 7e9bb80
1 parent 2304837
commit 7e9bb80
Show file tree

Hide file tree

Showing 4 changed files with 4 additions and 16 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,9 +1060,6 @@ 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);
@@ -1074,11 +1071,7 @@ void coreTaskFunc(void const * argument)
 #if NERVE_COMM
       Spine::send();
 #endif
-      if(HAL_GetTick() - pre_imu_update > IMU_UPDATE_INTERVAL)
-        {
-          imu_.update();
-          pre_imu_update = HAL_GetTick();
-        } 
+      imu_.update();
       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,7 +47,6 @@
 //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
@@ -42,7 +42,6 @@ void ICM20948::init(SPI_HandleTypeDef* hspi, I2C_HandleTypeDef* hi2c, ros::NodeH
 void ICM20948::gyroInit(void)
 {
   HAL_Delay(100);
-  uint32_t search_start_time = HAL_GetTick();
 
   /* Waiting for finding Imu */
 
@@ -51,15 +50,12 @@ void ICM20948::gyroInit(void)
   /* 1.Clear all bits in ub0-ub3 register */
   deviceReset();
 
-  /* 2.Wakeup icm20948 */
-  // wakeUp();
-
-  /* 3.Set fundamental properties */
+  /* 2.Wakeup icm20948 and set fundamental properties */
   setClockSource(1); // Clock source is automatically selected.
   odrAlignEnable(); // Synchronize odr with sampling rates.
   spiModeEnable(); // Use only SPI mode
 
-  /* 4.Gyro initialization (Do not change following order) */
+  /* 3.Gyro initialization (Do not change following order) */
   setGyroLpf(0);
   setGyroSampleRate(0);
   setGyroFullScale(_2000dps);  

diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/icm_20948.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/sensors/imu/icm_20948.h
@@ -21,7 +21,7 @@
 #define IMU_FIND_TIMEOUT 10000
 #define MAG_FIND_TIMEOUT 10000
 
-#define MAG_READ_INTERVAL 0
+#define MAG_READ_INTERVAL 10
 
 /* ICM-20948 Registers */
 #define ICM20948_ID						0xEA