[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

@@ -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;
         }

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);
 

src/Groundstation/Automated/Sensors/Sensors.cpp

@@ -63,8 +63,28 @@ 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()
 {
+    if (calibrating)
+    {
+        if (calibrationStart - std::chrono::nanoseconds(miosix::getTime()) >
+            VN300_CAL_TIME)
+            vn300->stopHSIEstimator();
+    }
+    else
         Logger::getInstance().log(vn300->getLastSample());
 }
 

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