Darkages/SpotMicroESP32-Leika
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix calling of IMU/MAG data when no new data available

Browse Source
This commit is contained in:
Niklas Jensen
2025-12-26 22:52:06 +01:00
parent da87d12588
commit d1c2e5f447
3 changed files with 13 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/include/peripherals/imu.h
+3 -1
View File
@@ -142,8 +142,10 @@ class IMU : public SensorBase<IMUAnglesMsg> {
#ifndef ICM20948_GET_AGMT_UPDATED_ONCE_PER_LOOP #ifndef ICM20948_GET_AGMT_UPDATED_ONCE_PER_LOOP
#define ICM20948_GET_AGMT_UPDATED_ONCE_PER_LOOP #define ICM20948_GET_AGMT_UPDATED_ONCE_PER_LOOP
if (_imu->dataReady()) if (_imu->dataReady())
{ {
_imu->getAGMT(); _imu->getAGMT();
} else {
return false;
} }
#endif #endif
_msg.rpy[0] = _imu->accX(); _msg.rpy[0] = _imu->accX();
esp32/include/peripherals/magnetometer.h
+2
View File
@@ -75,6 +75,8 @@ class Magnetometer : public SensorBase<MagnetometerMsg> {
if (_imu->dataReady()) if (_imu->dataReady())
{ {
_imu->getAGMT(); _imu->getAGMT();
} else {
return false;
} }
#endif #endif
_msg.rpy[0] = _mag->magX(); _msg.rpy[0] = _mag->magX();
esp32/src/peripherals/peripherals.cpp
+8 -2
View File
@@ -56,8 +56,14 @@ void Peripherals::begin() {
}; };
void Peripherals::update() { void Peripherals::update() {
CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_imu, readImu()); bool res = true;
CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_mag, readMag()); CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_imu, res = readImu());
#ifdef FT_ENABLED(USE_ICM20948)
// IF ICM_20948 fails to get IMU, it means that mag also does not have new data
CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_mag, if (res) { res = readMag(); } );
#else
CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_mag, res = readMag());
#endif
CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_gesture, EXECUTE_EVERY_N_MS(100, { readGesture(); }) ); CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_gesture, EXECUTE_EVERY_N_MS(100, { readGesture(); }) );
CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_bmp, EXECUTE_EVERY_N_MS(500, { readBMP(); }) ); CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_bmp, EXECUTE_EVERY_N_MS(500, { readBMP(); }) );
CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_sonar, EXECUTE_EVERY_N_MS(500, { readSonar(); }) ); CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_sonar, EXECUTE_EVERY_N_MS(500, { readSonar(); }) );