From 32966a343017468bd0baec8f9cc777225571621e Mon Sep 17 00:00:00 2001 From: Niklas Jensen Date: Fri, 26 Dec 2025 22:33:16 +0100 Subject: [PATCH] Make sure that ICM20948 is only updated once per loop when needed --- esp32/include/peripherals/imu.h | 12 ++++++++---- esp32/include/peripherals/magnetometer.h | 15 ++++++++++----- 2 files changed, 18 insertions(+), 9 deletions(-) diff --git a/esp32/include/peripherals/imu.h b/esp32/include/peripherals/imu.h index baee25a..6f2f813 100644 --- a/esp32/include/peripherals/imu.h +++ b/esp32/include/peripherals/imu.h @@ -82,13 +82,17 @@ class IMU : public SensorBase { _msg.temperature = _imu.getTemperature(); #endif #if FT_ENABLED(USE_ICM20948) + + #ifndef ICM20948_GET_AGMT_UPDATED_ONCE_PER_LOOP + #define ICM20948_GET_AGMT_UPDATED_ONCE_PER_LOOP if (_imu->dataReady()) - { + { _imu->getAGMT(); - _msg.rpy[0] = _imu->gyrX(); - _msg.rpy[1] = _imu->gyrY(); - _msg.rpy[2] = _imu->gyrZ(); } + #endif + _msg.rpy[0] = _imu->accX(); + _msg.rpy[1] = _imu->accY(); + _msg.rpy[2] = _imu->accZ(); #endif #if FT_ENABLED(USE_BNO055) #endif diff --git a/esp32/include/peripherals/magnetometer.h b/esp32/include/peripherals/magnetometer.h index 7cc1260..ad019e2 100644 --- a/esp32/include/peripherals/magnetometer.h +++ b/esp32/include/peripherals/magnetometer.h @@ -43,11 +43,16 @@ class Magnetometer : public SensorBase { bool update() override { if (!_msg.success) return false; #if FT_ENABLED(USE_ICM20948) - _mag->getAGMT(); - if (_mag->status != ICM_20948_Stat_Ok){ return false; } - _msg.rpy[0] = _mag->magX(); - _msg.rpy[1] = _mag->magY(); - _msg.rpy[2] = _mag->magZ(); + #ifndef ICM20948_GET_AGMT_UPDATED_ONCE_PER_LOOP + #define ICM20948_GET_AGMT_UPDATED_ONCE_PER_LOOP + if (_imu->dataReady()) + { + _imu->getAGMT(); + } + #endif + _msg.rpy[0] = _mag->magX(); + _msg.rpy[1] = _mag->magY(); + _msg.rpy[2] = _mag->magZ(); #elif FT_ENABLED(USE_HMC5883) sensors_event_t event;