From 2c99245c9b5aed1dbb0fba32be1fa3fd456a60cd Mon Sep 17 00:00:00 2001
From: Davide Basso <davide.basso@skywarder.eu>
Date: Thu, 28 Nov 2024 10:55:35 +0100
Subject: [PATCH] [Parafoil] [WIP] Add Sensors.h

---
 src/Parafoil/Sensors/Sensors.h                | 143 ++++++++++++++++++
 .../FlightModeManager/FlightModeManager.h     |   9 +-
 2 files changed, 144 insertions(+), 8 deletions(-)
 create mode 100644 src/Parafoil/Sensors/Sensors.h

diff --git a/src/Parafoil/Sensors/Sensors.h b/src/Parafoil/Sensors/Sensors.h
new file mode 100644
index 000000000..39c70a6d8
--- /dev/null
+++ b/src/Parafoil/Sensors/Sensors.h
@@ -0,0 +1,143 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: 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 <drivers/adc/InternalADC.h>
+#include <sensors/ADS131M08/ADS131M08.h>
+#include <sensors/BMX160/BMX160.h>
+#include <sensors/BMX160/BMX160WithCorrection.h>
+#include <sensors/H3LIS331DL/H3LIS331DL.h>
+#include <sensors/LIS3MDL/LIS3MDL.h>
+#include <sensors/LPS22DF/LPS22DF.h>
+#include <sensors/SensorManager.h>
+#include <sensors/UBXGPS/UBXGPSSpi.h>
+#include <sensors/analog/BatteryVoltageSensor.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+namespace Parafoil
+{
+class BoardScheduler;
+class Buses;
+
+/**
+ * @brief Manages all the sensors of the parafoil board.
+ */
+class Sensors : public Boardcore::InjectableWithDeps<BoardScheduler, Buses>
+{
+public:
+    [[nodiscard]] bool start();
+
+    /**
+     * @brief Returns whether all enabled sensors are started and running.
+     */
+    bool isStarted();
+
+    /**
+     * @brief Calibrates the sensors that need calibration.
+     *
+     * @note This function is blocking.
+     */
+    void calibrate();
+
+    /**
+     * @brief Takes the result of the live magnetometer calibration and applies
+     * it to the current calibration + writes it in the csv file
+     *
+     * @return true if the write was successful
+     */
+    bool writeMagCalibration();
+
+    Boardcore::BMX160Data getBMX160LastSample();
+    Boardcore::BMX160WithCorrectionData getBMX160WithCorrectionLastSample();
+    Boardcore::H3LIS331DLData getH3LISLastSample();
+    Boardcore::LIS3MDLData getLIS3MDLLastSample();
+    Boardcore::LPS22DFData getLPS22LastSample();
+    Boardcore::UBXGPSData getUbxGpsLastSample();
+    Boardcore::ADS131M08Data getADS131LastSample();
+    Boardcore::InternalADCData getInternalADCLastSample();
+    Boardcore::BatteryVoltageSensorData getBatteryVoltageLastSample();
+
+    /**
+     * @brief Returns information about all sensors managed by this class
+     */
+    std::vector<Boardcore::SensorInfo> getSensorInfo();
+
+protected:
+    /**
+     * @brief A function that is called after all sensors have been created but
+     * before they are inserted into the sensor manager.
+     *
+     * It can be overridden by subclasses to perform additional setup on the
+     * sensors.
+     *
+     * @return Whether the additional setup was successful. If false is
+     * returned, initialization will stop immediately after returning from this
+     * function and the sensors will not be started.
+     */
+    virtual bool postSensorCreationHook() { return true; }
+
+    std::unique_ptr<Boardcore::BMX160> bmx160;
+    std::unique_ptr<Boardcore::BMX160WithCorrection> bmx160WithCorrection;
+    std::unique_ptr<Boardcore::H3LIS331DL> h3lis331dl;
+    std::unique_ptr<Boardcore::LIS3MDL> lis3mdl;
+    std::unique_ptr<Boardcore::LPS22DF> lps22df;
+    std::unique_ptr<Boardcore::UBXGPSSpi> ubxgps;
+    std::unique_ptr<Boardcore::ADS131M08> ads131m08;
+    std::unique_ptr<Boardcore::InternalADC> internalAdc;
+
+private:
+    /**
+     * Sensors initialization and callback functions.
+     */
+
+    void bmx160Init();
+    void bmx160Callback();
+
+    void bmx160WithCorrectionInit();
+    void bmx160WithCorrectionCallback();
+
+    void h3lisInit();
+    void h3lisCallback();
+
+    void lis3mdlInit();
+    void lis3mdlCallback();
+
+    void lps22Init();
+    void lps22Callback();
+
+    void ubxGpsInit();
+    void ubxGpsCallback();
+
+    void ads131Init();
+    void ads131Callback();
+
+    void internalADCInit();
+    void internalADCCallback();
+
+    void batteryVoltageInit();
+    void batteryVoltageCallback();
+
+    bool sensorManagerInit();
+};
+
+}  // namespace Parafoil
\ No newline at end of file
diff --git a/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.h b/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.h
index 28b98b68c..2abdf7544 100644
--- a/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.h
+++ b/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.h
@@ -30,11 +30,7 @@
 namespace Parafoil
 {
 class Sensors;
-class CanHandler;
 class Actuators;
-class AltitudeTrigger;
-class FlightStatsRecorder;
-class NASController;
 
 /**
  * State machine that manages the flight modes of the Parafoil.
@@ -52,16 +48,13 @@ class NASController;
  * └── ReadyTestMode
  *
  * Flying
- * ├── FlyingDrogueDescent
  * └── FlyingWingDescent
  *
  * Landed
  */
 class FlightModeManager
     : public Boardcore::HSM<FlightModeManager>,
-      public Boardcore::InjectableWithDeps<Sensors, CanHandler, Actuators,
-                                           AltitudeTrigger, FlightStatsRecorder,
-                                           NASController>
+      public Boardcore::InjectableWithDeps<Sensors, Actuators>
 {
     FlightModeManager();
     ~FlightModeManager();
-- 
GitLab