[Payload][WingController] General improvements for code reliability and readibility

src/boards/Payload/BoardScheduler.h

@@ -74,6 +74,11 @@ public:
         return Priority::MEDIUM;
     }
 
+    static Priority::PriorityLevel wingControllerPriority()
+    {
+        return Priority::MEDIUM;
+    }
+
     /**
      * @brief Starts all the schedulers
      */

src/boards/Payload/Configs/WingConfig.h

@@ -25,6 +25,8 @@
 #include <units/Frequency.h>
 #include <utils/Constants.h>
 
+#include <chrono>
+
 namespace Payload
 {
 namespace Config
@@ -32,45 +34,57 @@ namespace Config
 namespace Wing
 {
 
-// Algorithm configuration
-#if defined(CLOSED_LOOP)
-constexpr int SELECTED_ALGORITHM = 0;
-#elif EARLY_MANEUVER
-constexpr int SELECTED_ALGORITHM   = 1;
-#elif SEQUENCE
-constexpr int SELECTED_ALGORITHM   = 2;
-#elif ROTATION
-constexpr int SELECTED_ALGORITHM = 3;
-#else
-constexpr int SELECTED_ALGORITHM = 0;
-#endif
+/* linter off */ using namespace std::chrono_literals;
+/* linter off */ using namespace Boardcore::Units::Frequency;
 
+/**
+ * @brief The available algorithms for the wing controller.
+ */
+enum class AlgorithmId : size_t
+{
+    EARLY_MANEUVER = 0,
+    CLOSED_LOOP,
+    SEQUENCE,  ///< A predefined sequence of maneuvers
+    ROTATION,  ///< A sequence of maneuvers to rotate the wing
+    LAST,      ///< Used to count the number of algorithms
+};
+
+namespace Default
+{
+// TODO: Verify the default target coordinates for all sites
 #if defined(EUROC)
-constexpr float DEFAULT_TARGET_LAT = 39.389733;
-constexpr float DEFAULT_TARGET_LON = -8.288992;
+constexpr auto TARGET_LAT = 39.389733f;
+constexpr auto TARGET_LON = -8.288992f;
 #elif defined(ROCCARASO)
-constexpr float DEFAULT_TARGET_LAT = 41.8089005;
-constexpr float DEFAULT_TARGET_LON = 14.0546716;
+constexpr auto TARGET_LAT = 41.809216;
+constexpr auto TARGET_LON = 14.055310;
 #else  // Milan
-constexpr float DEFAULT_TARGET_LAT = 45.5010679;
-constexpr float DEFAULT_TARGET_LON = 9.1563769;
+constexpr auto TARGET_LAT = 45.5010679f;
+constexpr auto TARGET_LON = 9.1563769f;
 #endif
 
-constexpr int WING_STRAIGHT_FLIGHT_TIMEOUT = 15 * 1000;  // [ms]
+constexpr auto ALGORITHM = AlgorithmId::EARLY_MANEUVER;
+}  // namespace Default
 
-constexpr int WING_UPDATE_PERIOD = 1000;  // [ms]
+constexpr auto STRAIGHT_FLIGHT_TIMEOUT = 15s;
+constexpr auto UPDATE_RATE             = 1_hz;
 
-constexpr float PI_CONTROLLER_SATURATION_MAX_LIMIT = Boardcore::Constants::PI;
-constexpr float PI_CONTROLLER_SATURATION_MIN_LIMIT = -Boardcore::Constants::PI;
+namespace PI
+{
+constexpr auto SATURATION_MIN_LIMIT = -Boardcore::Constants::PI;
+constexpr auto SATURATION_MAX_LIMIT = Boardcore::Constants::PI;
 
-constexpr int GUIDANCE_CONFIDENCE                = 15;
-constexpr int GUIDANCE_M1_ALTITUDE_THRESHOLD     = 250;  //[m]
-constexpr int GUIDANCE_M2_ALTITUDE_THRESHOLD     = 150;  //[m]
-constexpr int GUIDANCE_TARGET_ALTITUDE_THRESHOLD = 50;   //[m]
+constexpr auto KP = 0.9f;
+constexpr auto KI = 0.05f;
+}  // namespace PI
 
-// TODO check this parameter preflight
-constexpr float KP = 0.9;
-constexpr float KI = 0.05;
+namespace Guidance
+{
+constexpr auto CONFIDENCE                = 15;   // [samples]
+constexpr auto M1_ALTITUDE_THRESHOLD     = 250;  // [m]
+constexpr auto M2_ALTITUDE_THRESHOLD     = 150;  // [m]
+constexpr auto TARGET_ALTITUDE_THRESHOLD = 50;   // [m]
+}  // namespace Guidance
 
 constexpr float TWIRL_RADIUS = 0.5;  // [%]
 

src/boards/Payload/StateMachines/WingController/WingController.h

 /* Copyright (c) 2024 Skyward Experimental Rocketry
- * Authors: Federico Mandelli, Angelo Prete
+ * Authors: Federico Mandelli, Angelo Prete, Niccolò Betto
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -22,6 +22,7 @@
 
 #pragma once
 
+#include <Payload/Configs/WingConfig.h>
 #include <Payload/Wing/Guidance/ClosedLoopGuidanceAlgorithm.h>
 #include <Payload/Wing/Guidance/EarlyManeuversGuidanceAlgorithm.h>
 #include <Payload/Wing/WingAlgorithm.h>
@@ -83,40 +84,41 @@ public:
      */
     ~WingController();
 
-    Boardcore::State state_idle(const Boardcore::Event& event);
-    Boardcore::State state_flying(const Boardcore::Event& event);
-    Boardcore::State state_calibration(const Boardcore::Event& event);
-    Boardcore::State state_controlled_descent(const Boardcore::Event& event);
-    Boardcore::State state_on_ground(const Boardcore::Event& event);
+    // HSM states
+    Boardcore::State Idle(const Boardcore::Event& event);
+    Boardcore::State Flying(const Boardcore::Event& event);
+    Boardcore::State FlyingCalibration(const Boardcore::Event& event);
+    Boardcore::State FlyingControlledDescent(const Boardcore::Event& event);
+    Boardcore::State OnGround(const Boardcore::Event& event);
 
     /**
-     * @brief Method to set the target position.
+     * @brief Override the inject method to inject dependencies into the
+     * algorithms, which are insantiated later than top-level modules.
      */
-    void setTargetPosition(Eigen::Vector2f target);
+    void inject(Boardcore::DependencyInjector& injector) override;
 
-    /**
-     * @brief Selects the algorithm if present.
-     *
-     * @param index The algorithms vector index
-     *
-     * @return true if the selection was successful
-     */
-    bool selectAlgorithm(unsigned int index);
+    bool start() override;
 
-    WingControllerStatus getStatus();
+    bool isStarted();
 
-    bool start() override;
+    WingControllerState getState();
 
     /**
-     * @brief Override the inject method to inject dependencies into the
-     * algorithms, which are insantiated later than top-level modules.
+     * @brief Sets the target position.
+     * @note The provided position must be using geodetic coordinates.
      */
-    void inject(Boardcore::DependencyInjector& injector) override;
+    bool setTargetPosition(Eigen::Vector2f targetPositionGEO);
 
     /**
-     * @brief This method is a forward method from early maneuvers guidance
-     * algorithm to calculate the yaw angle of the parafoil knowing the current
-     * position and the target position.
+     * @brief Changes the selected algorithm.
+     * @return Whether the provided index selected a valid algorithm.
+     */
+    bool selectAlgorithm(uint8_t index);
+
+    /**
+     * @brief This is a forward method to the early maneuvers guidance algorithm
+     * to calculate the yaw angle of the parafoil knowing the current position
+     * and the target position.
      *
      * @param[in] currentPositionNED the current NED position of the parafoil in
      * m
@@ -124,7 +126,6 @@ public:
      * @param[out] heading Saves the heading vector for logging purposes
      *
      * @returns the yaw angle of the parafoil in rad
-     *
      */
     float calculateTargetAngle(const Eigen::Vector3f& currentPositionNED,
                                Eigen::Vector2f& heading)
@@ -134,58 +135,75 @@ public:
 
 private:
     /**
-     * @brief Instantiates the algorithms and adds them to the algorithms
-     * vector.
+     * @brief Loads all algorithms.
      *
      * @note Algorithms should be instantiated before dependency injection so
      * that they can be injected with dependencies when the WingController is
      * injected.
      */
-    bool addAlgorithms();
+    void loadAlgorithms();
 
     /**
-     * @brief target position getter
+     * @brief Returns the currently selected algorithm.
      */
-    Eigen::Vector2f convertTargetPositionToNED(Eigen::Vector2f targetGEO);
+    WingAlgorithm& getCurrentAlgorithm();
 
     /**
-     * @brief set points needed by the Guidance
+     * @brief Starts the currently selected algorithm. If the algorithm is
+     * already running, it resets the algorithm.
      */
-    void setEarlyManeuverPoints(Eigen::Vector2f targetNED,
-                                Eigen::Vector2f currentPosNED);
-
-    void logStatus(WingControllerState state);
-
-    WingControllerStatus status;
+    void startAlgorithm();
 
-    miosix::FastMutex statusMutex;
+    /**
+     * @brief Stops the currently selected algorithm.
+     */
+    void stopAlgorithm();
 
     /**
-     * @brief Target position
+     * @brief Update early maneuver guidance points (EMC, M1, M2) based on the
+     * current position and the target position.
      */
-    Eigen::Vector2f targetPositionGEO;
+    void updateEarlyManeuverPoints();
 
     /**
-     * @brief List of loaded algorithms (from SD or not)
+     * @brief Periodic update method that steps the currently selected
+     * algorithm.
      */
-    std::vector<WingAlgorithm*> algorithms;
+    void update();
 
     /**
-     * @brief PrintLogger
+     * @brief Flare the wing.
+     * Pulls the two ropes as skydiving people do.
      */
-    Boardcore::PrintLogger logger =
-        Boardcore::Logging::getLogger("WingController");
+    void flareWing();
 
     /**
-     * @brief Internal running state
+     * @brief Twirl the wing to the left.
      */
-    std::atomic<bool> running;
+    void twirlWing();
+
     /**
-     * @brief This attribute is modified by the mavlink radio section.
-     * The user using the Ground Station can select the pre-enumerated algorithm
-     * to execute
+     * @brief Reset the wing to the initial position.
      */
-    std::atomic<size_t> selectedAlgorithm;
+    void resetWing();
+
+    void updateState(WingControllerState newState);
+
+    Eigen::Vector2f targetPositionGEO{Config::Wing::Default::TARGET_LAT,
+                                      Config::Wing::Default::TARGET_LON};
+
+    std::atomic<bool> targetPositionDirty{
+        false};  ///< Whether the target position was changed and EM points need
+                 ///< to be updated
+
+    std::atomic<Config::Wing::AlgorithmId> selectedAlgorithm{
+        Config::Wing::Default::ALGORITHM};
+
+    std::atomic<WingControllerState> state{WingControllerState::UNINIT};
+
+    std::array<std::unique_ptr<WingAlgorithm>,
+               static_cast<size_t>(Config::Wing::AlgorithmId::LAST)>
+        algorithms;  ///< The available algorithms
 
     /**
      * @brief Instance of the Early Maneuver Guidance Algorithm used by
@@ -199,30 +217,11 @@ private:
      */
     ClosedLoopGuidanceAlgorithm clGuidance;
 
-    /**
-     * @brief  starts the selected algorithm
-     */
-    void startAlgorithm();
-
-    /**
-     * @brief Sets the internal state to stop and
-     * stops the selected algorithm
-     */
-    void stopAlgorithm();
-
-    /**
-     * @brief Stops any on going algorithm and flares the wing
-     */
-    void flare();
-
-    void twirl();
+    std::atomic<bool> started{false};
+    std::atomic<bool> running{false};  ///< Whether the algorithm is running
 
-    /**
-     * @brief Resets the servos in their initial position
-     */
-    void reset();
-
-    void update();
+    Boardcore::PrintLogger logger =
+        Boardcore::Logging::getLogger("WingController");
 };
 
 }  // namespace Payload

src/boards/Payload/StateMachines/WingController/WingControllerData.h

-/* Copyright (c) 2022 Skyward Experimental Rocketry
- * Author: Federico Mandelli
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Federico Mandelli, Niccolò Betto
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -22,8 +22,7 @@
 
 #pragma once
 
-#include <stdint.h>
-
+#include <cstdint>
 #include <iostream>
 #include <string>
 
@@ -34,15 +33,15 @@ enum class WingControllerState : uint8_t
 {
     UNINIT = 0,
     IDLE,
-    CALIBRATION,
-    ALGORITHM_CONTROLLED,
+    FLYING_CALIBRATION,
+    FLYING_CONTROLLED_DESCENT,
     ON_GROUND,
     END
 };
 
 struct WingControllerStatus
 {
-    long long timestamp       = 0;
+    uint64_t timestamp        = 0;
     WingControllerState state = WingControllerState::UNINIT;
 
     static std::string header() { return "timestamp,state\n"; }
@@ -53,4 +52,17 @@ struct WingControllerStatus
     }
 };
 
+struct WingControllerAlgorithmData
+{
+    uint64_t timestamp = 0;
+    uint8_t algorithm  = 0;
+
+    static std::string header() { return "timestamp,algorithm\n"; }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << (int)algorithm << "\n";
+    }
+};
+
 }  // namespace Payload

src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp

@@ -44,12 +44,13 @@ AutomaticWingAlgorithm::AutomaticWingAlgorithm(float Kp, float Ki,
                                                GuidanceAlgorithm& guidance)
     : Super(servo1, servo2), guidance(guidance)
 {
-    controller = new PIController(Kp, Ki, WING_UPDATE_PERIOD / 1000.0f,
-                                  PI_CONTROLLER_SATURATION_MIN_LIMIT,
-                                  PI_CONTROLLER_SATURATION_MAX_LIMIT);
-}
+    // PIController needs the sample period in floating point seconds
+    auto samplePeriod = 1.0f / Hertz{UPDATE_RATE}.value();
 
-AutomaticWingAlgorithm::~AutomaticWingAlgorithm() { delete (controller); }
+    controller = std::make_unique<PIController>(Kp, Ki, samplePeriod,
+                                                PI::SATURATION_MIN_LIMIT,
+                                                PI::SATURATION_MAX_LIMIT);
+}
 
 void AutomaticWingAlgorithm::step()
 {
@@ -72,14 +73,14 @@ void AutomaticWingAlgorithm::step()
         if (result > 0)
         {
             // Activate the servo2 and reset servo1
-            getModule<Actuators>()->setServoAngle(servo1, 0);
+            getModule<Actuators>()->setServoAngle(servo1, 0.0f);
             getModule<Actuators>()->setServoAngle(servo2, result);
         }
         else
         {
             // Activate the servo1 and reset servo2
-            getModule<Actuators>()->setServoAngle(servo1, result * -1);
-            getModule<Actuators>()->setServoAngle(servo2, 0);
+            getModule<Actuators>()->setServoAngle(servo1, result * -1.0f);
+            getModule<Actuators>()->setServoAngle(servo2, 0.0f);
         }
 
         // Log the servo positions
@@ -95,8 +96,8 @@ void AutomaticWingAlgorithm::step()
     else
     {
         // If we loose fix we set both servo at 0
-        getModule<Actuators>()->setServoAngle(servo1, 0);
-        getModule<Actuators>()->setServoAngle(servo2, 0);
+        getModule<Actuators>()->setServoAngle(servo1, 0.0f);
+        getModule<Actuators>()->setServoAngle(servo2, 0.0f);
     }
 }
 

src/boards/Payload/Wing/AutomaticWingAlgorithm.h

@@ -56,17 +56,12 @@ public:
     AutomaticWingAlgorithm(float Kp, float Ki, ServosList servo1,
                            ServosList servo2, GuidanceAlgorithm& guidance);
 
-    /**
-     * @brief Destroy the Automatic Wing Algorithm object and the PI
-     */
-    ~AutomaticWingAlgorithm();
-
 protected:
     // Guidance algorithm that sets the yaw.
     GuidanceAlgorithm& guidance;
 
     // PI controller tuned on the Kp and Ki passed through constructor
-    Boardcore::PIController* controller;
+    std::unique_ptr<Boardcore::PIController> controller;
 
     /**
      * @brief Actual algorithm implementation, all parameters should be in NED

src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp

@@ -70,18 +70,18 @@ float EarlyManeuversGuidanceAlgorithm::calculateTargetAngle(
 void EarlyManeuversGuidanceAlgorithm::computeActiveTarget(float altitude)
 {
     if (altitude <=
-        GUIDANCE_TARGET_ALTITUDE_THRESHOLD)  // Altitude is low, head directly
+        Guidance::TARGET_ALTITUDE_THRESHOLD)  // Altitude is low, head directly
                                               // to target
     {
         targetAltitudeConfidence++;
     }
-    else if (altitude <= GUIDANCE_M2_ALTITUDE_THRESHOLD)  // Altitude is almost
+    else if (altitude <= Guidance::M2_ALTITUDE_THRESHOLD)  // Altitude is almost
                                                            // okay, go to M2
     {
         m2AltitudeConfidence++;
     }
     else if (altitude <=
-             GUIDANCE_M1_ALTITUDE_THRESHOLD)  // Altitude is high, go to M1
+             Guidance::M1_ALTITUDE_THRESHOLD)  // Altitude is high, go to M1
     {
         m1AltitudeConfidence++;
     }
@@ -93,41 +93,44 @@ void EarlyManeuversGuidanceAlgorithm::computeActiveTarget(float altitude)
     switch (activeTarget)
     {
         case Target::EMC:
-            if (targetAltitudeConfidence >= GUIDANCE_CONFIDENCE)
+            if (targetAltitudeConfidence >= Guidance::CONFIDENCE)
             {
                 activeTarget          = Target::FINAL;
                 emcAltitudeConfidence = 0;
             }
-            else if (m2AltitudeConfidence >= GUIDANCE_CONFIDENCE)
+            else if (m2AltitudeConfidence >= Guidance::CONFIDENCE)
             {
                 activeTarget          = Target::M2;
                 emcAltitudeConfidence = 0;
             }
-            else if (m1AltitudeConfidence >= GUIDANCE_CONFIDENCE)
+            else if (m1AltitudeConfidence >= Guidance::CONFIDENCE)
             {
                 activeTarget          = Target::M1;
                 emcAltitudeConfidence = 0;
             }
             break;
+
         case Target::M1:
-            if (targetAltitudeConfidence >= GUIDANCE_CONFIDENCE)
+            if (targetAltitudeConfidence >= Guidance::CONFIDENCE)
             {
                 activeTarget         = Target::FINAL;
                 m1AltitudeConfidence = 0;
             }
-            else if (m2AltitudeConfidence >= GUIDANCE_CONFIDENCE)
+            else if (m2AltitudeConfidence >= Guidance::CONFIDENCE)
             {
                 activeTarget         = Target::M2;
                 m1AltitudeConfidence = 0;
             }
             break;
+
         case Target::M2:
-            if (targetAltitudeConfidence >= GUIDANCE_CONFIDENCE)
+            if (targetAltitudeConfidence >= Guidance::CONFIDENCE)
             {
                 activeTarget         = Target::FINAL;
                 m2AltitudeConfidence = 0;
             }
             break;
+
         case Target::FINAL:
             break;
     }

src/boards/Payload/Wing/WingAlgorithmData.h

@@ -31,15 +31,15 @@ namespace Payload
  */
 struct WingAlgorithmData
 {
-    uint64_t timestamp;   // First timestamp is 0 (in microseconds)
-    float servo1Angle;    // degrees
-    float servo2Angle;    // degrees
-    float targetAngle;    // radians (automatic only)
-    float velocityAngle;  // radians
-    float targetX;        // NED (only automatic algorithm)
-    float targetY;        // NED (only automatic algorithm)
-    float error;
-    float pidOutput;
+    uint64_t timestamp  = 0;  // First timestamp is 0 (in microseconds)
+    float servo1Angle   = 0;  // degrees
+    float servo2Angle   = 0;  // degrees
+    float targetAngle   = 0;  // radians (automatic only)
+    float velocityAngle = 0;  // radians
+    float targetX       = 0;  // NED (only automatic algorithm)
+    float targetY       = 0;  // NED (only automatic algorithm)
+    float error         = 0;
+    float pidOutput     = 0;
 
     static std::string header()
     {

src/boards/Payload/Wing/WingTargetPositionData.h

@@ -30,28 +30,28 @@ namespace Payload
 
 struct WingTargetPositionData
 {
-    float receivedLat;
-    float receivedLon;
-    float targetN;
-    float targetE;
-    float emcN;
-    float emcE;
-    float m1N;
-    float m1E;
-    float m2N;
-    float m2E;
+    float targetLat = 0;
+    float targetLon = 0;
+    float targetN   = 0;
+    float targetE   = 0;
+    float emcN      = 0;
+    float emcE      = 0;
+    float m1N       = 0;
+    float m1E       = 0;
+    float m2N       = 0;
+    float m2E       = 0;
 
     static std::string header()
     {
-        return "receivedLat, receivedLon, "
-               "targetN,targetE,EMCN,EMCE,M1N,M1E,M2N,M2E\n";
+        return "targetLat, "
+               "targetLon,targetN,targetE,EMCN,EMCE,M1N,M1E,M2N,M2E\n";
     }
 
     void print(std::ostream& os) const
     {
-        os << receivedLat << "," << receivedLon << "," << targetN << ","
-           << targetE << "," << emcN << "," << emcE << "," << m1N << "," << m1E
-           << "," << m2N << "," << m2E << "\n";
+        os << targetLat << "," << targetLon << "," << targetN << "," << targetE
+           << "," << emcN << "," << emcE << "," << m1N << "," << m1E << ","
+           << m2N << "," << m2E << "\n";
     }
 };
 

src/scripts/logdecoder/Payload/logdecoder.cpp

@@ -68,6 +68,7 @@ void registerTypes(Deserializer& ds)
     ds.registerType<DynamicPressureData>();
     ds.registerType<SensorsCalibrationParameter>();
     ds.registerType<PinChangeData>();
+    ds.registerType<WingControllerAlgorithmData>();
     ds.registerType<WingAlgorithmData>();
     ds.registerType<WingTargetPositionData>();
     ds.registerType<WindLogging>();