From da87d1258804f2905484f7c7b619c92da7600ebd 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 5421aa2..265e6b8 100644 --- a/esp32/include/peripherals/imu.h +++ b/esp32/include/peripherals/imu.h @@ -138,13 +138,17 @@ class IMU : public SensorBase { return false; #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) sensors_event_t event; diff --git a/esp32/include/peripherals/magnetometer.h b/esp32/include/peripherals/magnetometer.h index d9841fe..3b7a189 100644 --- a/esp32/include/peripherals/magnetometer.h +++ b/esp32/include/peripherals/magnetometer.h @@ -70,11 +70,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;