From 2cd8d657cf6c8302489863d8f086d6e7ea370b0d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Thu, 5 Dec 2024 19:12:14 +0100
Subject: [PATCH] [ARP] SMA calibration with vn300 soft-hard iron calibration

Now the CALIBRATION state performs a soft-hard iron calibration of the VN300.
---
 src/Groundstation/Automated/SMA/SMA.cpp       |  9 ++++++++
 src/Groundstation/Automated/SMA/SMA.h         |  1 +
 .../Automated/Sensors/Sensors.cpp             | 22 ++++++++++++++++++-
 src/Groundstation/Automated/Sensors/Sensors.h | 22 +++++++++++++++++++
 4 files changed, 53 insertions(+), 1 deletion(-)

diff --git a/src/Groundstation/Automated/SMA/SMA.cpp b/src/Groundstation/Automated/SMA/SMA.cpp
index 57b170743..03321da8e 100644
--- a/src/Groundstation/Automated/SMA/SMA.cpp
+++ b/src/Groundstation/Automated/SMA/SMA.cpp
@@ -329,6 +329,12 @@ void SMA::update()
 
             break;
         }
+        case SMAState::CALIBRATE:
+        {
+            if (!sensors->isCalibrating())
+                EventBroker::getInstance().post(ARP_CAL_DONE, TOPIC_ARP);
+        }
+        break;
         default:
         {
             break;
@@ -702,6 +708,9 @@ State SMA::state_calibrate(const Event& event)
     {
         case EV_ENTRY:
         {
+            auto* sensors = getModule<Sensors>();
+            if (!sensors->calibrate() && sensors->isCalibrating())
+                transition(&SMA::state_test);
             logStatus(SMAState::CALIBRATE);
             return HANDLED;
         }
diff --git a/src/Groundstation/Automated/SMA/SMA.h b/src/Groundstation/Automated/SMA/SMA.h
index 2bc290d9a..d86934ac0 100644
--- a/src/Groundstation/Automated/SMA/SMA.h
+++ b/src/Groundstation/Automated/SMA/SMA.h
@@ -97,6 +97,7 @@ public:
 
     /**
      * @brief move the stepper `stepperId` of `angle` degrees
+     *
      */
     ActuationStatus moveStepperDeg(StepperList stepperId, float angle);
 
diff --git a/src/Groundstation/Automated/Sensors/Sensors.cpp b/src/Groundstation/Automated/Sensors/Sensors.cpp
index 866231a74..315756771 100644
--- a/src/Groundstation/Automated/Sensors/Sensors.cpp
+++ b/src/Groundstation/Automated/Sensors/Sensors.cpp
@@ -63,9 +63,29 @@ bool Sensors::vn300Init()
     return true;
 }
 
+bool Sensors::calibrate()
+{
+    // Already in calibration mode.
+    if (calibrating)
+        return false;
+    calibrationStart = std::chrono::nanoseconds(miosix::getTime());
+    calibrating      = true;
+    vn300->startHSIEstimator(VN300_CAL_CONVERGENCE);
+    return true;
+}
+
+bool Sensors::isCalibrating() { return calibrating; }
+
 void Sensors::vn300Callback()
 {
-    Logger::getInstance().log(vn300->getLastSample());
+    if (calibrating)
+    {
+        if (calibrationStart - std::chrono::nanoseconds(miosix::getTime()) >
+            VN300_CAL_TIME)
+            vn300->stopHSIEstimator();
+    }
+    else
+        Logger::getInstance().log(vn300->getLastSample());
 }
 
 VN300Data Sensors::getVN300LastSample() { return vn300->getLastSample(); }
diff --git a/src/Groundstation/Automated/Sensors/Sensors.h b/src/Groundstation/Automated/Sensors/Sensors.h
index 87ce9a237..43d06d76b 100644
--- a/src/Groundstation/Automated/Sensors/Sensors.h
+++ b/src/Groundstation/Automated/Sensors/Sensors.h
@@ -21,14 +21,23 @@
  */
 #pragma once
 
+#include <drivers/timer/TimestampTimer.h>
 #include <utils/DependencyManager/DependencyManager.h>
 
+#include <chrono>
+
 #include "Groundstation/LyraGS/Buses.h"
 #include "sensors/SensorManager.h"
 #include "sensors/Vectornav/VN300/VN300.h"
 
 namespace Antennas
 {
+
+static constexpr uint8_t VN300_CAL_CONVERGENCE =
+    4;  ///< Calibration convergence parameter for VN300 soft and hard iron
+        ///< calibration. 5: converge in 60-90sec, 1: converge in 15-20sec
+static constexpr std::chrono::seconds VN300_CAL_TIME = std::chrono::seconds(30);
+
 class Sensors : public Boardcore::InjectableWithDeps<LyraGS::Buses>
 {
 public:
@@ -44,14 +53,27 @@ public:
      */
     Boardcore::VN300Data getVN300LastSample();
 
+    /**
+     * @brief Trigger the calibration process for soft-hard iron in the VN300
+     */
+    bool calibrate();
+
+    /**
+     * @brief Returns the status of the calibration
+     */
+    bool isCalibrating();
+
 private:
     bool vn300Init();
     void vn300Callback();
 
+    std::atomic<bool> calibrating{false};
+
     Boardcore::VN300* vn300 = nullptr;
 
     Boardcore::SensorManager* sm = nullptr;
     Boardcore::SensorManager::SensorMap_t sensorsMap;
     Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("sensors");
+    std::chrono::nanoseconds calibrationStart;
 };
 }  // namespace Antennas
-- 
GitLab