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 index f5e88dd8d..ad8b55389 100644 --- 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 index a7d1e5007..b8a795216 100644 --- 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 index 3a4a08a71..a7aa68e45 100644 --- 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)