From 98b0966e31b9becbd9afda267e227d4f82b9ad40 Mon Sep 17 00:00:00 2001
From: Davide Basso <davide.basso@skywarder.eu>
Date: Sun, 1 Dec 2024 10:55:21 +0100
Subject: [PATCH] [Parafoil] [WIP] Add NAS controller

---
 src/Parafoil/BoardScheduler.h                 |   1 +
 src/Parafoil/Configs/NASConfig.h              |  81 ++++
 .../NASController/NASController.cpp           | 356 ++++++++++++++++++
 .../NASController/NASController.h             | 100 +++++
 .../NASController/NASControllerData.h         |  54 +++
 src/Parafoil/parafoil-entry.cpp               |   5 +
 6 files changed, 597 insertions(+)
 create mode 100644 src/Parafoil/Configs/NASConfig.h
 create mode 100644 src/Parafoil/StateMachines/NASController/NASController.cpp
 create mode 100644 src/Parafoil/StateMachines/NASController/NASController.h
 create mode 100644 src/Parafoil/StateMachines/NASController/NASControllerData.h

diff --git a/src/Parafoil/BoardScheduler.h b/src/Parafoil/BoardScheduler.h
index 1b9b1d2de..2843c6cc6 100644
--- a/src/Parafoil/BoardScheduler.h
+++ b/src/Parafoil/BoardScheduler.h
@@ -52,6 +52,7 @@ public:
         };
     };
 
+    Boardcore::TaskScheduler& nasController() { return critical; }
     Boardcore::TaskScheduler& sensors() { return high; }
 
     static Priority::PriorityLevel flightModeManagerPriority()
diff --git a/src/Parafoil/Configs/NASConfig.h b/src/Parafoil/Configs/NASConfig.h
new file mode 100644
index 000000000..d5b49789f
--- /dev/null
+++ b/src/Parafoil/Configs/NASConfig.h
@@ -0,0 +1,81 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Davide Mor, Niccolò Betto, Davide Basso
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <algorithms/NAS/NASConfig.h>
+#include <common/ReferenceConfig.h>
+#include <units/Frequency.h>
+
+namespace Parafoil
+{
+namespace Config
+{
+namespace NAS
+{
+
+/* linter off */ using namespace Boardcore::Units::Frequency;
+
+constexpr Hertz UPDATE_RATE         = 50_hz;
+constexpr float UPDATE_RATE_SECONDS = 0.02;  // [s]
+
+constexpr int CALIBRATION_SAMPLES_COUNT       = 20;
+constexpr unsigned int CALIBRATION_SLEEP_TIME = 100;  // [ms]
+
+static const Boardcore::NASConfig CONFIG = {
+    .T              = UPDATE_RATE_SECONDS,
+    .SIGMA_BETA     = 0.0001,
+    .SIGMA_W        = 0.0019,
+    .SIGMA_ACC      = 0.202,
+    .SIGMA_MAG      = 0.0047,
+    .SIGMA_GPS      = {0.0447f, 0.0447f, 1.0f / 30.0f, 1.0f / 30.0f},
+    .SIGMA_BAR      = 400.0f,
+    .SIGMA_POS      = 2.0,
+    .SIGMA_VEL      = 1.0,
+    .SIGMA_PITOT    = 1e-3,
+    .P_POS          = 0.0,
+    .P_POS_VERTICAL = 0.0,
+    .P_VEL          = 0.0,
+    .P_VEL_VERTICAL = 0.0,
+    .P_ATT          = 0.1,
+    .P_BIAS         = 0.01,
+    .SATS_NUM       = 6.0,
+    .NED_MAG        = Common::ReferenceConfig::nedMag};
+
+// TODO: make sure this is correct!
+
+// Only use one out of every 50 samples (1 Hz)
+constexpr int MAGNETOMETER_DECIMATE = 50;
+
+// Maximum allowed acceleration to correct with GPS
+constexpr float DISABLE_GPS_ACCELERATION = 34.0f;  // [m/s^2]
+
+// How much confidence (in m/s^2) to apply to the accelerometer to check if it
+// is 1g
+constexpr float ACCELERATION_1G_CONFIDENCE = 0.5;
+// How many samples will determine that we are in fact measuring gravity
+// acceleration
+constexpr int ACCELERATION_1G_SAMPLES = 20;
+
+}  // namespace NAS
+}  // namespace Config
+}  // namespace Parafoil
diff --git a/src/Parafoil/StateMachines/NASController/NASController.cpp b/src/Parafoil/StateMachines/NASController/NASController.cpp
new file mode 100644
index 000000000..872f0c5b6
--- /dev/null
+++ b/src/Parafoil/StateMachines/NASController/NASController.cpp
@@ -0,0 +1,356 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Niccolò Betto, Davide Mor, Davide Basso
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <Parafoil/BoardScheduler.h>
+#include <Parafoil/Configs/NASConfig.h>
+#include <Parafoil/Sensors/Sensors.h>
+#include <Parafoil/StateMachines/NASController/NASController.h>
+#include <algorithms/NAS/StateInitializer.h>
+#include <common/Events.h>
+#include <common/ReferenceConfig.h>
+#include <events/EventBroker.h>
+#include <utils/SkyQuaternion/SkyQuaternion.h>
+
+using namespace Boardcore;
+using namespace Eigen;
+using namespace Common;
+namespace config = Parafoil::Config::NAS;
+
+namespace Parafoil
+{
+
+NASController::NASController()
+    : FSM(&NASController::Init, miosix::STACK_DEFAULT_FOR_PTHREAD,
+          BoardScheduler::nasControllerPriority()),
+      nas(config::CONFIG)
+{
+    EventBroker::getInstance().subscribe(this, TOPIC_NAS);
+    EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
+}
+
+bool NASController::start()
+{
+    // Setup the NAS
+    Matrix<float, 13, 1> x = Matrix<float, 13, 1>::Zero();
+    // Create the initial quaternion
+    Vector4f q = SkyQuaternion::eul2quat({0, 0, 0});
+
+    // Set the initial quaternion inside the matrix
+    x(NAS::IDX_QUAT + 0) = q(0);
+    x(NAS::IDX_QUAT + 1) = q(1);
+    x(NAS::IDX_QUAT + 2) = q(2);
+    x(NAS::IDX_QUAT + 3) = q(3);
+
+    // Set the NAS x matrix
+    nas.setX(x);
+    // Set the initial reference values from the default ones
+    nas.setReferenceValues(ReferenceConfig::defaultReferenceValues);
+
+    auto& scheduler = getModule<BoardScheduler>()->nasController();
+    // Add the task to the scheduler
+    auto task = scheduler.addTask([this] { update(); }, config::UPDATE_RATE,
+                                  TaskScheduler::Policy::RECOVER);
+
+    if (task == 0)
+    {
+        LOG_ERR(logger, "Failed to add NAS update task");
+        return false;
+    }
+
+    if (!FSM::start())
+    {
+        LOG_ERR(logger, "Failed to start NASController FSM active object");
+        return false;
+    }
+
+    started = true;
+    return true;
+}
+
+NASState NASController::getNasState()
+{
+    miosix::Lock<miosix::FastMutex> l(nasMutex);
+    return nas.getState();
+}
+
+ReferenceValues NASController::getReferenceValues()
+{
+    miosix::Lock<miosix::FastMutex> l(nasMutex);
+    return nas.getReferenceValues();
+}
+
+NASControllerState NASController::getState() { return state; }
+
+void NASController::setOrientation(const Eigen::Quaternionf& quat)
+{
+    Lock<FastMutex> lock{nasMutex};
+
+    auto x               = nas.getX();
+    x(NAS::IDX_QUAT + 0) = quat.x();
+    x(NAS::IDX_QUAT + 1) = quat.y();
+    x(NAS::IDX_QUAT + 2) = quat.z();
+    x(NAS::IDX_QUAT + 3) = quat.w();
+    nas.setX(x);
+}
+
+void NASController::Init(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            updateState(NASControllerState::INIT);
+            break;
+        }
+
+        case NAS_CALIBRATE:
+        {
+            transition(&NASController::Calibrating);
+            break;
+        }
+    }
+}
+
+void NASController::Calibrating(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            updateState(NASControllerState::CALIBRATING);
+            calibrate();
+            break;
+        }
+
+        case NAS_READY:
+        {
+            transition(&NASController::Ready);
+            break;
+        }
+    }
+}
+
+void NASController::Ready(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            updateState(NASControllerState::READY);
+            break;
+        }
+
+        case NAS_CALIBRATE:
+        {
+            transition(&NASController::Calibrating);
+            break;
+        }
+
+        case NAS_FORCE_START:
+        case FLIGHT_ARMED:
+        {
+            transition(&NASController::Active);
+            break;
+        }
+    }
+}
+
+void NASController::Active(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            updateState(NASControllerState::ACTIVE);
+            break;
+        }
+
+        case FLIGHT_LANDING_DETECTED:
+        {
+            transition(&NASController::End);
+            break;
+        }
+
+        case NAS_FORCE_STOP:
+        case FLIGHT_DISARMED:
+        {
+            transition(&NASController::Ready);
+            break;
+        }
+    }
+}
+
+void NASController::End(const Event& event)
+{
+    switch (event)
+    {
+        case EV_ENTRY:
+        {
+            updateState(NASControllerState::END);
+            break;
+        }
+    }
+}
+
+void NASController::calibrate()
+{
+    Sensors* sensors = getModule<Sensors>();
+
+    Vector3f accSum = Vector3f::Zero();
+    Vector3f magSum = Vector3f::Zero();
+    float baroSum   = 0.0f;
+
+    for (int i = 0; i < config::CALIBRATION_SAMPLES_COUNT; i++)
+    {
+        BMX160Data imu    = sensors->getBMX160WithCorrectionLastSample();
+        PressureData baro = sensors->getLPS22DFLastSample();
+
+        accSum +=
+            Vector3f{imu.accelerationX, imu.accelerationY, imu.accelerationZ};
+        magSum += Vector3f{imu.magneticFieldX, imu.magneticFieldY,
+                           imu.magneticFieldZ};
+
+        baroSum += baro.pressure;
+
+        Thread::sleep(config::CALIBRATION_SLEEP_TIME);
+    }
+
+    Vector3f meanAcc = accSum / config::CALIBRATION_SAMPLES_COUNT;
+    meanAcc.normalize();
+    Vector3f meanMag = magSum / config::CALIBRATION_SAMPLES_COUNT;
+    meanMag.normalize();
+    float meanBaro = baroSum / config::CALIBRATION_SAMPLES_COUNT;
+
+    // Use the triad to compute initial state
+    StateInitializer init;
+    init.triad(meanAcc, meanMag, ReferenceConfig::nedMag);
+
+    miosix::Lock<miosix::FastMutex> l(nasMutex);
+
+    // Compute reference values
+    ReferenceValues reference = nas.getReferenceValues();
+    reference.refPressure     = meanBaro;
+    reference.refAltitude     = Aeroutils::relAltitude(
+        reference.refPressure, reference.mslPressure, reference.mslTemperature);
+
+    // Also update the reference with the GPS if we have fix
+    UBXGPSData gps = sensors->getUBXGPSLastSample();
+    if (gps.fix == 3)
+    {
+        // Don't use the GPS altitude because it is not reliable
+        reference.refLatitude  = gps.latitude;
+        reference.refLongitude = gps.longitude;
+    }
+
+    // Update the algorithm reference values
+    nas.setX(init.getInitX());
+    nas.setReferenceValues(reference);
+
+    EventBroker::getInstance().post(NAS_READY, TOPIC_NAS);
+}
+
+void NASController::update()
+{
+    // Update the NAS state only if the FSM is active
+    if (state != NASControllerState::ACTIVE)
+    {
+        return;
+    }
+
+    auto* sensors = getModule<Sensors>();
+
+    auto imu  = sensors->getBMX160LastSample();
+    auto gps  = sensors->getUBXGPSLastSample();
+    auto baro = sensors->getLPS22DFLastSample();
+
+    // Calculate acceleration
+    Vector3f acc    = static_cast<AccelerometerData>(imu);
+    float accLength = acc.norm();
+
+    miosix::Lock<miosix::FastMutex> l(nasMutex);
+
+    // Perform initial NAS prediction
+    nas.predictGyro(imu);
+    nas.predictAcc(imu);
+
+    if (lastGpsTimestamp < gps.gpsTimestamp && gps.fix == 3 &&
+        accLength < Config::NAS::DISABLE_GPS_ACCELERATION)
+    {
+        nas.correctGPS(gps);
+    }
+
+    if (lastBaroTimestamp < baro.pressureTimestamp)
+    {
+        nas.correctBaro(baro.pressure);
+    }
+
+    // Correct with accelerometer if the acceleration is in specs
+    if (lastAccTimestamp < imu.accelerationTimestamp && acc1g)
+    {
+        nas.correctAcc(imu);
+    }
+
+    // Check if the accelerometer is measuring 1g
+    if (accLength <
+            (Constants::g + Config::NAS::ACCELERATION_1G_CONFIDENCE / 2) &&
+        accLength >
+            (Constants::g - Config::NAS::ACCELERATION_1G_CONFIDENCE / 2))
+    {
+        if (acc1gSamplesCount < Config::NAS::ACCELERATION_1G_SAMPLES)
+        {
+            acc1gSamplesCount++;
+        }
+        else
+        {
+            acc1g = true;
+        }
+    }
+    else
+    {
+        acc1gSamplesCount = 0;
+        acc1g             = false;
+    }
+
+    lastGyroTimestamp = imu.angularSpeedTimestamp;
+    lastAccTimestamp  = imu.accelerationTimestamp;
+    lastMagTimestamp  = imu.magneticFieldTimestamp;
+    lastGpsTimestamp  = gps.gpsTimestamp;
+    lastBaroTimestamp = baro.pressureTimestamp;
+
+    auto state = nas.getState();
+    auto ref   = nas.getReferenceValues();
+
+    Logger::getInstance().log(state);
+}
+
+void NASController::updateState(NASControllerState newState)
+{
+    state = newState;
+
+    auto status = NASControllerStatus{
+        .timestamp = TimestampTimer::getTimestamp(),
+        .state     = newState,
+    };
+    Logger::getInstance().log(status);
+}
+
+}  // namespace Parafoil
diff --git a/src/Parafoil/StateMachines/NASController/NASController.h b/src/Parafoil/StateMachines/NASController/NASController.h
new file mode 100644
index 000000000..6c75c5830
--- /dev/null
+++ b/src/Parafoil/StateMachines/NASController/NASController.h
@@ -0,0 +1,100 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Niccolò Betto, Davide Basso
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <algorithms/NAS/NAS.h>
+#include <diagnostic/PrintLogger.h>
+#include <events/FSM.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+#include "NASControllerData.h"
+
+namespace Parafoil
+{
+class BoardScheduler;
+class Sensors;
+
+class NASController
+    : public Boardcore::FSM<NASController>,
+      public Boardcore::InjectableWithDeps<BoardScheduler, Sensors>
+{
+public:
+    /**
+     * @brief Initializes the NAS controller.
+     *
+     * Sets the initial FSM state to idle, subscribes to NAS and flight events
+     * and initializes the NAS algorithm.
+     */
+    NASController();
+
+    /**
+     * @brief Adds the NAS update function into the scheduler and starts the FSM
+     * thread
+     */
+    bool start() override;
+
+    Boardcore::NASState getNasState();
+    Boardcore::ReferenceValues getReferenceValues();
+
+    NASControllerState getState();
+
+    void setOrientation(const Eigen::Quaternionf& orientation);
+
+private:
+    void calibrate();
+
+    /**
+     * @brief Update the NAS estimation
+     */
+    void update();
+
+    // FSM states
+    void Init(const Boardcore::Event& event);
+    void Calibrating(const Boardcore::Event& event);
+    void Ready(const Boardcore::Event& event);
+    void Active(const Boardcore::Event& event);
+    void End(const Boardcore::Event& event);
+
+    void updateState(NASControllerState newState);
+
+    std::atomic<NASControllerState> state{NASControllerState::INIT};
+
+    Boardcore::NAS nas;  ///< The NAS algorithm instance
+    miosix::FastMutex nasMutex;
+
+    int magDecimateCount  = 0;
+    int acc1gSamplesCount = 0;
+    bool acc1g            = false;
+
+    uint64_t lastGyroTimestamp = 0;
+    uint64_t lastAccTimestamp  = 0;
+    uint64_t lastMagTimestamp  = 0;
+    uint64_t lastGpsTimestamp  = 0;
+    uint64_t lastBaroTimestamp = 0;
+
+    std::atomic<bool> started{false};
+
+    Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("NAS");
+};
+
+}  // namespace Parafoil
diff --git a/src/Parafoil/StateMachines/NASController/NASControllerData.h b/src/Parafoil/StateMachines/NASController/NASControllerData.h
new file mode 100644
index 000000000..020f69541
--- /dev/null
+++ b/src/Parafoil/StateMachines/NASController/NASControllerData.h
@@ -0,0 +1,54 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Niccolò Betto, Davide Basso
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <cstdint>
+#include <ostream>
+#include <string>
+
+namespace Parafoil
+{
+
+enum class NASControllerState : uint8_t
+{
+    INIT = 0,
+    CALIBRATING,
+    READY,
+    ACTIVE,
+    END
+};
+
+struct NASControllerStatus
+{
+    uint64_t timestamp       = 0;
+    NASControllerState state = NASControllerState::INIT;
+
+    static std::string header() { return "timestamp,state\n"; }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << (int)state << "\n";
+    }
+};
+
+}  // namespace Parafoil
diff --git a/src/Parafoil/parafoil-entry.cpp b/src/Parafoil/parafoil-entry.cpp
index a38d1413b..acbf3e24b 100644
--- a/src/Parafoil/parafoil-entry.cpp
+++ b/src/Parafoil/parafoil-entry.cpp
@@ -22,6 +22,7 @@
 
 #include <Parafoil/BoardScheduler.h>
 #include <Parafoil/Buses.h>
+#include <Parafoil/Sensors/Sensors.h>
 #include <diagnostic/PrintLogger.h>
 #include <utils/DependencyManager/DependencyManager.h>
 
@@ -54,5 +55,9 @@ int main()
     auto scheduler = new BoardScheduler();
     initResult &= depman.insert(scheduler);
 
+    // Sensors
+    auto sensors = new Sensors();
+    initResult &= depman.insert(sensors);
+
     return 0;
 }
\ No newline at end of file
-- 
GitLab