From 828d331a292c735de25507759800e8559a78e230 Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Wed, 14 Aug 2024 23:19:30 +0200
Subject: [PATCH] [Main] Added initial implementation of StatsRecorder

---
 cmake/dependencies.cmake                      |   5 +-
 src/boards/Main/Sensors/Sensors.cpp           |  12 +-
 src/boards/Main/Sensors/Sensors.h             |   4 +-
 .../ADAController/ADAController.cpp           |  12 +-
 .../ADAController/ADAController.h             |   4 +-
 .../FlightModeManager/FlightModeManager.cpp   |   9 ++
 .../FlightModeManager/FlightModeManager.h     |   4 +-
 .../NASController/NASController.cpp           |   5 +-
 .../NASController/NASController.h             |   4 +-
 .../Main/StatsRecorder/StatsRecorder.cpp      | 145 ++++++++++++++++++
 src/boards/Main/StatsRecorder/StatsRecorder.h |  99 ++++++++++++
 src/entrypoints/Main/main-entry.cpp           |  39 ++---
 12 files changed, 311 insertions(+), 31 deletions(-)
 create mode 100644 src/boards/Main/StatsRecorder/StatsRecorder.cpp
 create mode 100644 src/boards/Main/StatsRecorder/StatsRecorder.h

diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 1fdb17226..f4007eecd 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -30,15 +30,12 @@ set(MAIN_COMPUTER
     src/boards/Main/CanHandler/CanHandler.cpp
     src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
     src/boards/Main/Actuators/Actuators.cpp
-    # src/boards/Main/Sensors/RotatedIMU/RotatedIMU.cpp
     src/boards/Main/StateMachines/NASController/NASController.cpp
     src/boards/Main/StateMachines/ADAController/ADAController.cpp
     src/boards/Main/PinHandler/PinHandler.cpp
-    # src/boards/Main/AltitudeTrigger/AltitudeTrigger.cpp
     # src/boards/Main/StateMachines/ABKController/ABKController.cpp
     # src/boards/Main/StateMachines/MEAController/MEAController.cpp
-    # src/boards/Main/StateMachines/Deployment/Deployment.cpp
-    # src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp
+    src/boards/Main/StatsRecorder/StatsRecorder.cpp
 )
 
 set(MOTOR_SOURCES
diff --git a/src/boards/Main/Sensors/Sensors.cpp b/src/boards/Main/Sensors/Sensors.cpp
index b91f582bf..0d6e99e85 100644
--- a/src/boards/Main/Sensors/Sensors.cpp
+++ b/src/boards/Main/Sensors/Sensors.cpp
@@ -412,7 +412,11 @@ void Sensors::h3lis331dlInit()
 
 void Sensors::h3lis331dlCallback()
 {
-    Logger::getInstance().log(getH3LIS331DLLastSample());
+    auto sample = getH3LIS331DLLastSample();
+
+    // Also update StatsRecorder
+    getModule<StatsRecorder>()->updateAcc(sample);
+    Logger::getInstance().log(sample);
 }
 
 void Sensors::lis2mdlInit()
@@ -598,8 +602,10 @@ void Sensors::dplBayPressureInit()
 
 void Sensors::dplBayPressureCallback()
 {
-    Logger::getInstance().log(
-        DplBayPressureData{getDplBayPressureLastSample()});
+    auto sample = getDplBayPressureLastSample();
+
+    getModule<StatsRecorder>()->updateDplPressure(sample);
+    Logger::getInstance().log(DplBayPressureData{sample});
 }
 
 void Sensors::rotatedImuInit()
diff --git a/src/boards/Main/Sensors/Sensors.h b/src/boards/Main/Sensors/Sensors.h
index 11476a8da..046b9bcea 100644
--- a/src/boards/Main/Sensors/Sensors.h
+++ b/src/boards/Main/Sensors/Sensors.h
@@ -24,6 +24,7 @@
 
 #include <Main/BoardScheduler.h>
 #include <Main/Buses.h>
+#include <Main/StatsRecorder/StatsRecorder.h>
 #include <diagnostic/PrintLogger.h>
 #include <drivers/adc/InternalADC.h>
 #include <scheduler/TaskScheduler.h>
@@ -46,7 +47,8 @@
 namespace Main
 {
 
-class Sensors : public Boardcore::InjectableWithDeps<Buses, BoardScheduler>
+class Sensors
+    : public Boardcore::InjectableWithDeps<Buses, BoardScheduler, StatsRecorder>
 {
 public:
     Sensors() {}
diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.cpp b/src/boards/Main/StateMachines/ADAController/ADAController.cpp
index bbb5a7be7..9cdcc2f36 100644
--- a/src/boards/Main/StateMachines/ADAController/ADAController.cpp
+++ b/src/boards/Main/StateMachines/ADAController/ADAController.cpp
@@ -177,6 +177,13 @@ void ADAController::update()
             // DO NOT THROW EVENTS IN SHADOW_MODE!
             if (detectedApogees > Config::ADA::APOGEE_N_SAMPLES)
             {
+                auto gps = getModule<Sensors>()->getUBXGPSLastSample();
+
+                // Notify stats recorder
+                getModule<StatsRecorder>()->apogeeDetected(
+                    TimestampTimer::getTimestamp(), gps.latitude, gps.longitude,
+                    ada.getState().mslAltitude);
+
                 EventBroker::getInstance().post(ADA_APOGEE_DETECTED, TOPIC_ADA);
             }
         }
@@ -195,7 +202,10 @@ void ADAController::update()
 
         if (detectedDeployments > Config::ADA::DEPLOYMENT_N_SAMPLES)
         {
-            // TODO(davide.mor): Rename this event
+            // Notify stats recorder
+            getModule<StatsRecorder>()->deploymentDetected(
+                TimestampTimer::getTimestamp(), ada.getState().mslAltitude);
+
             EventBroker::getInstance().post(ADA_DEPLOY_ALTITUDE_DETECTED,
                                             TOPIC_ADA);
         }
diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.h b/src/boards/Main/StateMachines/ADAController/ADAController.h
index e26c7da7d..5b5e12334 100644
--- a/src/boards/Main/StateMachines/ADAController/ADAController.h
+++ b/src/boards/Main/StateMachines/ADAController/ADAController.h
@@ -25,6 +25,7 @@
 #include <Main/BoardScheduler.h>
 #include <Main/Sensors/Sensors.h>
 #include <Main/StateMachines/ADAController/ADAControllerData.h>
+#include <Main/StatsRecorder/StatsRecorder.h>
 #include <algorithms/ADA/ADA.h>
 #include <events/FSM.h>
 #include <utils/DependencyManager/DependencyManager.h>
@@ -33,7 +34,8 @@ namespace Main
 {
 
 class ADAController
-    : public Boardcore::InjectableWithDeps<BoardScheduler, Sensors>,
+    : public Boardcore::InjectableWithDeps<BoardScheduler, Sensors,
+                                           StatsRecorder>,
       public Boardcore::FSM<ADAController>
 {
 public:
diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
index ac2909fad..15c4d775b 100644
--- a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
+++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
@@ -350,6 +350,10 @@ State FlightModeManager::state_test_mode(const Event& event)
         case EV_ENTRY:
         {
             updateAndLogStatus(FlightModeManagerState::TEST_MODE);
+
+            // Reset all stats
+            getModule<StatsRecorder>()->reset();
+
             EventBroker::getInstance().post(ADA_FORCE_START, TOPIC_ADA);
             EventBroker::getInstance().post(NAS_FORCE_START, TOPIC_NAS);
             getModule<Sensors>()->resetMagCalibrator();
@@ -406,6 +410,9 @@ State FlightModeManager::state_armed(const Event& event)
         {
             updateAndLogStatus(FlightModeManagerState::ARMED);
 
+            // Reset all stats
+            getModule<StatsRecorder>()->reset();
+
             Logger::getInstance().stop();
             Logger::getInstance().start();
             Logger::getInstance().resetStats();
@@ -441,6 +448,8 @@ State FlightModeManager::state_armed(const Event& event)
         case TMTC_FORCE_LAUNCH:
         case FLIGHT_LAUNCH_PIN_DETACHED:
         {
+            getModule<StatsRecorder>()->liftoffDetected(
+                TimestampTimer::getTimestamp());
             getModule<CanHandler>()->sendEvent(CanConfig::EventId::LIFTOFF);
             return transition(&FlightModeManager::state_flying);
         }
diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
index 524fa9151..ffe151608 100644
--- a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
+++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
@@ -25,6 +25,7 @@
 #include <Main/Actuators/Actuators.h>
 #include <Main/CanHandler/CanHandler.h>
 #include <Main/Sensors/Sensors.h>
+#include <Main/StatsRecorder/StatsRecorder.h>
 #include <events/EventBroker.h>
 #include <events/HSM.h>
 #include <utils/DependencyManager/DependencyManager.h>
@@ -35,7 +36,8 @@ namespace Main
 {
 
 class FlightModeManager
-    : public Boardcore::InjectableWithDeps<Actuators, Sensors, CanHandler>,
+    : public Boardcore::InjectableWithDeps<Actuators, Sensors, CanHandler,
+                                           StatsRecorder>,
       public Boardcore::HSM<FlightModeManager>
 {
 public:
diff --git a/src/boards/Main/StateMachines/NASController/NASController.cpp b/src/boards/Main/StateMachines/NASController/NASController.cpp
index e377dd3de..5aba8e194 100644
--- a/src/boards/Main/StateMachines/NASController/NASController.cpp
+++ b/src/boards/Main/StateMachines/NASController/NASController.cpp
@@ -175,7 +175,10 @@ void NASController::update()
         lastGpsTimestamp  = gps.gpsTimestamp;
         lastBaroTimestamp = baro.pressureTimestamp;
 
-        sdLogger.log(nas.getState());
+        auto state = nas.getState();
+
+        getModule<StatsRecorder>()->updateNas(state);
+        sdLogger.log(state);
     }
 }
 
diff --git a/src/boards/Main/StateMachines/NASController/NASController.h b/src/boards/Main/StateMachines/NASController/NASController.h
index de145f7d9..6b2f32fe3 100644
--- a/src/boards/Main/StateMachines/NASController/NASController.h
+++ b/src/boards/Main/StateMachines/NASController/NASController.h
@@ -25,6 +25,7 @@
 #include <Main/BoardScheduler.h>
 #include <Main/Sensors/Sensors.h>
 #include <Main/StateMachines/NASController/NASControllerData.h>
+#include <Main/StatsRecorder/StatsRecorder.h>
 #include <algorithms/NAS/NAS.h>
 #include <diagnostic/PrintLogger.h>
 #include <events/FSM.h>
@@ -35,7 +36,8 @@ namespace Main
 
 class NASController
     : public Boardcore::FSM<NASController>,
-      public Boardcore::InjectableWithDeps<BoardScheduler, Sensors>
+      public Boardcore::InjectableWithDeps<BoardScheduler, Sensors,
+                                           StatsRecorder>
 {
 public:
     NASController();
diff --git a/src/boards/Main/StatsRecorder/StatsRecorder.cpp b/src/boards/Main/StatsRecorder/StatsRecorder.cpp
new file mode 100644
index 000000000..424aa6004
--- /dev/null
+++ b/src/boards/Main/StatsRecorder/StatsRecorder.cpp
@@ -0,0 +1,145 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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 "StatsRecorder.h"
+
+#include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
+
+using namespace Boardcore;
+using namespace Main;
+using namespace miosix;
+using namespace Eigen;
+
+StatsRecorder::StatsRecorder() {}
+
+void StatsRecorder::reset()
+{
+    Lock<FastMutex> lock{statsMutex};
+    stats = Stats{};
+}
+
+StatsRecorder::Stats StatsRecorder::getStats()
+{
+    Lock<FastMutex> lock{statsMutex};
+    return stats;
+}
+
+void StatsRecorder::liftoffDetected(uint64_t ts)
+{
+    Lock<FastMutex> lock{statsMutex};
+    stats.liftoffTs = ts;
+}
+
+void StatsRecorder::apogeeDetected(uint64_t ts, float lat, float lon, float alt)
+{
+    Lock<FastMutex> lock{statsMutex};
+    stats.apogeeTs  = ts;
+    stats.apogeeLat = lat;
+    stats.apogeeLon = lon;
+    stats.apogeeAlt = alt;
+}
+
+void StatsRecorder::deploymentDetected(uint64_t ts, float alt)
+{
+    Lock<FastMutex> lock{statsMutex};
+    stats.dplTs  = ts;
+    stats.dplAlt = alt;
+}
+
+void StatsRecorder::updateAcc(const AccelerometerData &data)
+{
+    auto state = getModule<FlightModeManager>()->getState();
+
+    float length = static_cast<Vector3f>(data).norm();
+    Lock<FastMutex> lock{statsMutex};
+
+    if (state == FlightModeManagerState::POWERED_ASCENT ||
+        state == FlightModeManagerState::ARMED)
+    {
+        // Record this event only during liftoff
+        if (length > stats.liftoffMaxAcc)
+        {
+            stats.liftoffMaxAcc   = length;
+            stats.liftoffMaxAccTs = data.accelerationTimestamp;
+        }
+    }
+    else if (state == FlightModeManagerState::DROGUE_DESCENT)
+    {
+        // Record this event only during drogue deployment
+        if (length > stats.apogeeMaxAcc)
+        {
+            stats.apogeeMaxAcc   = length;
+            stats.apogeeMaxAccTs = data.accelerationTimestamp;
+        }
+    }
+    else if (state == FlightModeManagerState::TERMINAL_DESCENT)
+    {
+        // Record this event only during main deployment
+        if (length > stats.dplMaxAcc)
+        {
+            stats.dplMaxAcc   = length;
+            stats.dplMaxAccTs = data.accelerationTimestamp;
+        }
+    }
+}
+
+void StatsRecorder::updateNas(const NASState &data)
+{
+    auto state = getModule<FlightModeManager>()->getState();
+
+    Lock<FastMutex> lock{statsMutex};
+    if (state == FlightModeManagerState::POWERED_ASCENT ||
+        state == FlightModeManagerState::UNPOWERED_ASCENT ||
+        state == FlightModeManagerState::DROGUE_DESCENT ||
+        state == FlightModeManagerState::TERMINAL_DESCENT)
+    {
+        // Record this event only during flight
+        float speed = Vector3f{data.vn, data.vd, data.ve}.norm();
+        float alt   = -data.d;
+
+        if (speed > stats.maxSpeed)
+        {
+            stats.maxSpeed    = speed;
+            stats.maxSpeedAlt = alt;
+            stats.maxSpeedTs  = data.timestamp;
+        }
+
+        // TODO: Update mach
+    }
+}
+
+void StatsRecorder::updateDplPressure(const PressureData &data)
+{
+    auto state = getModule<FlightModeManager>()->getState();
+
+    Lock<FastMutex> lock{statsMutex};
+    if (state == FlightModeManagerState::DROGUE_DESCENT ||
+        state == FlightModeManagerState::TERMINAL_DESCENT)
+    {
+        // Record this event only in drogue or terminal descent
+        if (data.pressure > stats.maxDplPressure)
+        {
+            stats.maxDplPressure   = data.pressure;
+            stats.maxDplPressureTs = data.pressureTimestamp;
+        }
+    }
+}
\ No newline at end of file
diff --git a/src/boards/Main/StatsRecorder/StatsRecorder.h b/src/boards/Main/StatsRecorder/StatsRecorder.h
new file mode 100644
index 000000000..8c960b361
--- /dev/null
+++ b/src/boards/Main/StatsRecorder/StatsRecorder.h
@@ -0,0 +1,99 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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/ADA/ADAData.h>
+#include <algorithms/NAS/NASState.h>
+#include <miosix.h>
+#include <sensors/SensorData.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+namespace Main
+{
+
+class FlightModeManager;
+
+class StatsRecorder : public Boardcore::InjectableWithDeps<FlightModeManager>
+{
+public:
+    struct Stats
+    {
+        // Liftoff
+        uint64_t liftoffTs = 0;
+
+        // Maximum acceleration during liftoff
+        uint64_t liftoffMaxAccTs = 0;
+        float liftoffMaxAcc      = 0.0f;
+
+        // Maximum vertical speed
+        uint64_t maxSpeedTs = 0;
+        float maxSpeed      = 0.0f;
+        float maxSpeedAlt   = 0.0f;
+
+        // Max mach
+        uint64_t maxMachTs = 0;
+        float maxMach      = 0.0f;
+
+        // Apogee
+        uint64_t apogeeTs = 0;
+        float apogeeLat   = 0.0f;
+        float apogeeLon   = 0.0f;
+        float apogeeAlt   = 0.0f;
+
+        // Maximum acceleration after apogee
+        uint64_t apogeeMaxAccTs = 0;
+        float apogeeMaxAcc      = 0.0f;
+
+        // Deployment
+        uint64_t dplTs = 0;
+        float dplAlt   = 0.0f;
+
+        // Maximum acceleration after deployment
+        uint64_t dplMaxAccTs = 0;
+        float dplMaxAcc      = 0.0f;
+
+        // Maximum deployment pressure
+        uint64_t maxDplPressureTs = 0;
+        float maxDplPressure      = 0.0f;
+    };
+
+    StatsRecorder();
+
+    void reset();
+
+    Stats getStats();
+
+    void liftoffDetected(uint64_t ts);
+    void apogeeDetected(uint64_t ts, float lat, float lon, float alt);
+    void deploymentDetected(uint64_t ts, float alt);
+
+    void updateAcc(const Boardcore::AccelerometerData &data);
+    void updateNas(const Boardcore::NASState &data);
+    void updateDplPressure(const Boardcore::PressureData &data);
+
+private:
+    miosix::FastMutex statsMutex;
+    Stats stats;
+};
+
+}  // namespace Main
\ No newline at end of file
diff --git a/src/entrypoints/Main/main-entry.cpp b/src/entrypoints/Main/main-entry.cpp
index 58bf5cd44..8b2356328 100644
--- a/src/entrypoints/Main/main-entry.cpp
+++ b/src/entrypoints/Main/main-entry.cpp
@@ -30,6 +30,7 @@
 #include <Main/StateMachines/ADAController/ADAController.h>
 #include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
 #include <Main/StateMachines/NASController/NASController.h>
+#include <Main/StatsRecorder/StatsRecorder.h>
 #include <actuators/Servo/Servo.h>
 #include <drivers/timer/PWM.h>
 #include <events/EventBroker.h>
@@ -52,26 +53,28 @@ int main()
     Buses *buses              = new Buses();
     BoardScheduler *scheduler = new BoardScheduler();
 
-    Actuators *actuators   = new Actuators();
-    Sensors *sensors       = new Sensors();
-    Radio *radio           = new Radio();
-    CanHandler *canHandler = new CanHandler();
-    PinHandler *pinHandler = new PinHandler();
-    FlightModeManager *fmm = new FlightModeManager();
-    ADAController *ada     = new ADAController();
-    NASController *nas     = new NASController();
+    Actuators *actuators    = new Actuators();
+    Sensors *sensors        = new Sensors();
+    Radio *radio            = new Radio();
+    CanHandler *canHandler  = new CanHandler();
+    PinHandler *pinHandler  = new PinHandler();
+    FlightModeManager *fmm  = new FlightModeManager();
+    ADAController *ada      = new ADAController();
+    NASController *nas      = new NASController();
+    StatsRecorder *recorder = new StatsRecorder();
 
     // Insert modules
-    bool initResult = manager.insert<Buses>(buses) &&
-                      manager.insert<BoardScheduler>(scheduler) &&
-                      manager.insert<Sensors>(sensors) &&
-                      manager.insert<Radio>(radio) &&
-                      manager.insert<Actuators>(actuators) &&
-                      manager.insert<CanHandler>(canHandler) &&
-                      manager.insert<PinHandler>(pinHandler) &&
-                      manager.insert<FlightModeManager>(fmm) &&
-                      manager.insert<ADAController>(ada) &&
-                      manager.insert<NASController>(nas) && manager.inject();
+    bool initResult =
+        manager.insert<Buses>(buses) &&
+        manager.insert<BoardScheduler>(scheduler) &&
+        manager.insert<Sensors>(sensors) && manager.insert<Radio>(radio) &&
+        manager.insert<Actuators>(actuators) &&
+        manager.insert<CanHandler>(canHandler) &&
+        manager.insert<PinHandler>(pinHandler) &&
+        manager.insert<FlightModeManager>(fmm) &&
+        manager.insert<ADAController>(ada) &&
+        manager.insert<NASController>(nas) &&
+        manager.insert<StatsRecorder>(recorder) && manager.inject();
 
     manager.graphviz(std::cout);
 
-- 
GitLab