From 90f561a62b4514ef48d670a200fa45796fd34e33 Mon Sep 17 00:00:00 2001
From: Niklas Jensen <niklaspj2004@gmail.com>
Date: Fri, 26 Dec 2025 22:52:06 +0100
Subject: [PATCH] Fix calling of IMU/MAG data when no new data available

---
 esp32/include/peripherals/imu.h          | 4 +++-
 esp32/include/peripherals/magnetometer.h | 2 ++
 2 files changed, 5 insertions(+), 1 deletion(-)

diff --git a/esp32/include/peripherals/imu.h b/esp32/include/peripherals/imu.h
index 6f2f813..6b095ff 100644
--- a/esp32/include/peripherals/imu.h
+++ b/esp32/include/peripherals/imu.h
@@ -86,8 +86,10 @@ class IMU : public SensorBase<IMUAnglesMsg> {
         #ifndef ICM20948_GET_AGMT_UPDATED_ONCE_PER_LOOP
         #define ICM20948_GET_AGMT_UPDATED_ONCE_PER_LOOP
         if (_imu->dataReady())
-        { 
+        {
             _imu->getAGMT();
+        } else {
+            return false;
         }
         #endif
         _msg.rpy[0] = _imu->accX();
diff --git a/esp32/include/peripherals/magnetometer.h b/esp32/include/peripherals/magnetometer.h
index ad019e2..9dd05ae 100644
--- a/esp32/include/peripherals/magnetometer.h
+++ b/esp32/include/peripherals/magnetometer.h
@@ -48,6 +48,8 @@ class Magnetometer : public SensorBase<MagnetometerMsg> {
         if (_imu->dataReady())
         { 
             _imu->getAGMT();
+        } else {
+            return false;
         }
         #endif
         _msg.rpy[0] = _mag->magX();