From e300a61df50c441f183980a25beb88fca28b7f2b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Mon, 9 Dec 2024 14:30:02 +0100
Subject: [PATCH] [ARP] Fix on calibration time for VN300 soft-hard iron

---
 src/Groundstation/Automated/SMA/SMA.cpp         | 3 +++
 src/Groundstation/Automated/Sensors/Sensors.cpp | 6 +++++-
 2 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/src/Groundstation/Automated/SMA/SMA.cpp b/src/Groundstation/Automated/SMA/SMA.cpp
index 03321da8e..cf800147d 100644
--- a/src/Groundstation/Automated/SMA/SMA.cpp
+++ b/src/Groundstation/Automated/SMA/SMA.cpp
@@ -332,7 +332,10 @@ void SMA::update()
         case SMAState::CALIBRATE:
         {
             if (!sensors->isCalibrating())
+            {
                 EventBroker::getInstance().post(ARP_CAL_DONE, TOPIC_ARP);
+                LOG_DEBUG(logger, "Exit from calibration\n");
+            }
         }
         break;
         default:
diff --git a/src/Groundstation/Automated/Sensors/Sensors.cpp b/src/Groundstation/Automated/Sensors/Sensors.cpp
index 315756771..0fe17cf13 100644
--- a/src/Groundstation/Automated/Sensors/Sensors.cpp
+++ b/src/Groundstation/Automated/Sensors/Sensors.cpp
@@ -80,9 +80,13 @@ void Sensors::vn300Callback()
 {
     if (calibrating)
     {
-        if (calibrationStart - std::chrono::nanoseconds(miosix::getTime()) >
+        LOG_DEBUG(logger, "Sensor Manager VN300 calibrating\n");
+        if (std::chrono::nanoseconds(miosix::getTime()) - calibrationStart >
             VN300_CAL_TIME)
+        {
             vn300->stopHSIEstimator();
+            calibrating = false;
+        }
     }
     else
         Logger::getInstance().log(vn300->getLastSample());
-- 
GitLab