[Follower] Add follower algorithm from obsw

[Follower] Add minimize rotationa as suggested in previous obsw pull req.

See [this](avn/swd/obsw!34)
for more context

[logger] Updated logger

[ARP Follower] Yaw gain now 0.1

The yaw gain gain now is no more 1 but fixed to 0.1

[ARP] Added timestamp to AntennaAngles for log

Added timestamp to the AntennaAngles logged. Changed accordingly the constructors.

[Fix] Follower algorithm limitation issue due to proportional on yaw

The 0.1* product to have a proportional on the yaw was creating issues with the angle limiting function.

[ARP] Added nrPropagations for log in AntennaAngle now logged by SMA

Added nrPropagations to AntennaAngles
Moved the log of it from the Follower to SMA

[ARP] Follower initRocketCoordinates now is NASOrigin

Changed semantic of initialRocketCoordinates to NASOrigin since now it is set always as we have a NASOrigin.

[ARP] WIP fix for online initial coordinates support

Add support for initial coordinates online setting

[ARP] Follower Mutexes for antenna and NAS origin's coordinates

Now getter states to protect such variables with mutexes

cmake/boardcore.cmake

@@ -43,7 +43,7 @@ set(BOARDCORE_SRC
     ${BOARDCORE_PATH}/src/shared/algorithms/NAS/NAS.cpp
     ${BOARDCORE_PATH}/src/shared/algorithms/NAS/StateInitializer.cpp
     ${SBS_BASE}/src/shared/algorithms/Propagator/Propagator.cpp
-
+    ${SBS_BASE}/src/shared/algorithms/Follower/Follower.cpp
 
     # Debug
     ${BOARDCORE_PATH}/src/shared/utils/Debug.cpp

src/shared/algorithms/Follower/Follower.cpp

+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Emilio Corigliano, Niccolò Betto, Federico Lolli
+ *
+ * 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 "Follower.h"
+
+#include <drivers/timer/TimestampTimer.h>
+#include <utils/AeroUtils/AeroUtils.h>
+
+#include <Eigen/Dense>
+#include <cmath>
+#include <iostream>
+
+using namespace miosix;
+
+namespace Boardcore
+{
+
+Follower::Follower(float updatePeriod)
+    : updatePeriod(updatePeriod), targetAngles({0, 0, 0})
+{
+}
+
+void Follower::setAntennaCoordinates(const Boardcore::GPSData& gpsData)
+{
+    Lock<FastMutex> lock(lastAntennaAttitudeMutex);
+    antennaCoordinates = {gpsData.latitude, gpsData.longitude, gpsData.height};
+    Boardcore::Logger::getInstance().log(
+        static_cast<LogAntennasCoordinates>(gpsData));
+    antennaCoordinatesSet = true;
+}
+
+void Follower::setRocketNASOrigin(const Boardcore::GPSData& gpsData)
+{
+    Lock<FastMutex> lock(lastAntennaAttitudeMutex);
+    rocketNASOrigin = {gpsData.latitude, gpsData.longitude, gpsData.height};
+    Boardcore::Logger::getInstance().log(
+        static_cast<LogRocketCoordinates>(gpsData));
+    rocketCoordinatesSet = true;
+}
+
+void Follower::setLastAntennaAttitude(const VN300Data& attitudeData)
+{
+    Lock<FastMutex> lock(lastAntennaAttitudeMutex);
+    lastAntennaAttitude = attitudeData;
+}
+
+VN300Data Follower::getLastAntennaAttitude()
+{
+    Lock<FastMutex> lock(lastAntennaAttitudeMutex);
+    return lastAntennaAttitude;
+}
+
+void Follower::setLastRocketNasState(const NASState nasState)
+{
+    Lock<FastMutex> lock(lastRocketNasStateMutex);
+    lastRocketNasState      = nasState;
+    firstAntennaAttitudeSet = true;
+}
+
+NASState Follower::getLastRocketNasState()
+{
+    Lock<FastMutex> lock(lastRocketNasStateMutex);
+    return lastRocketNasState;
+}
+
+FollowerState Follower::getState()
+{
+    miosix::Lock<miosix::FastMutex> lock(stateMutex);
+    return state;
+}
+
+void Follower::setState(const FollowerState& newState)
+{
+    miosix::Lock<miosix::FastMutex> lock(stateMutex);
+    state = newState;
+}
+
+bool Follower::init()
+{
+    if (!antennaCoordinatesSet || !rocketCoordinatesSet)
+    {
+        LOG_ERR(logger, "Antenna or rocket coordinates not set");
+        return false;
+    }
+
+    // Antenna Coordinates
+    Eigen::Vector2f antennaCoord{getAntennaCoordinates().head<2>()};
+    // Rocket coordinates
+    Eigen::Vector2f rocketCoord{getRocketNASOrigin().head<2>()};
+
+    initialAntennaRocketDistance =
+        Aeroutils::geodetic2NED(rocketCoord, antennaCoord);
+
+    LOG_INFO(logger, "Initial antenna - rocket distance: [{}, {}] [m]\n",
+             initialAntennaRocketDistance[0], initialAntennaRocketDistance[1]);
+
+    return true;
+}
+
+float Follower::minimizeRotation(float angle)
+{
+    if (angle > 180)
+    {
+        angle -= 360;
+    }
+    else if (angle < -180)
+    {
+        angle += 360;
+    }
+
+    return angle;
+}
+
+void Follower::step()
+{
+    NASState lastRocketNasState = getLastRocketNasState();
+
+    // Getting the position of the rocket wrt the antennas in NED frame
+    NEDCoords rocketPosition = {lastRocketNasState.n, lastRocketNasState.e,
+                                lastRocketNasState.d};
+
+    // Calculate the antenna target angles from the NED rocket coordinates
+    targetAngles = rocketPositionToAntennaAngles(rocketPosition);
+
+    // If attitude data has never been set, do not actuate the steppers
+    if (!firstAntennaAttitudeSet)
+    {
+        LOG_ERR(logger, "Antenna attitude not set");
+        return;
+    }
+
+    VN300Data vn300 = getLastAntennaAttitude();
+
+    // Calculate the amount to move from the current position
+    AntennaAngles diffAngles{targetAngles.timestamp,
+                             targetAngles.yaw - vn300.yaw,
+                             targetAngles.pitch - vn300.pitch};
+
+    // Rotate in the shortest direction
+    diffAngles.yaw   = 0.1 * minimizeRotation(diffAngles.yaw);
+    diffAngles.pitch = minimizeRotation(diffAngles.pitch);
+
+    // Calculate angular velocity for moving the antennas toward position
+    float horizontalSpeed =
+        std::abs((diffAngles.yaw * 1000) / (360 * updatePeriod));
+    float verticalSpeed =
+        std::abs((diffAngles.pitch * 1000) / (360 * updatePeriod));
+
+    // Update the state of the follower
+    FollowerState state;
+    state.timestamp       = TimestampTimer::getTimestamp();
+    state.yaw             = diffAngles.yaw;
+    state.pitch           = diffAngles.pitch;
+    state.horizontalSpeed = horizontalSpeed;
+    state.verticalSpeed   = verticalSpeed;
+    setState(state);
+
+#ifndef NDEBUG
+    std::cout << "[FOLLOWER] STEPPER "
+              << "Angles: [" << state.yaw << ", " << state.pitch << "] "
+              << "Speed: [" << state.horizontalSpeed << ", "
+              << state.verticalSpeed << "]   VN300 measure: [" << vn300.yaw
+              << ", " << vn300.pitch << "]\n";
+#endif
+}
+
+Eigen::Vector3f Follower::getAntennaCoordinates()
+{
+    Lock<FastMutex> lock(antennaCoordinatesMutex);
+    return Eigen::Vector3f{antennaCoordinates};
+}
+
+Eigen::Vector3f Follower::getRocketNASOrigin()
+{
+    Lock<FastMutex> lock(rocketNASOriginMutex);
+    return Eigen::Vector3f{rocketNASOrigin};
+}
+
+AntennaAngles Follower::rocketPositionToAntennaAngles(
+    const NEDCoords& rocketNed)
+{
+    // Antenna Coordinates
+    Eigen::Vector2f antennaCoord = getAntennaCoordinates().head<2>();
+
+    // Rocket coordinates
+    Eigen::Vector2f rocketCoord = getRocketNASOrigin().head<2>();
+
+    initialAntennaRocketDistance =
+        Aeroutils::geodetic2NED(rocketCoord, antennaCoord);
+
+    // NED coordinates of the rocket in the NED antenna frame
+    NEDCoords ned = {rocketNed.n + initialAntennaRocketDistance.x(),
+                     rocketNed.e + initialAntennaRocketDistance.y(),
+                     rocketNed.d};
+
+    AntennaAngles angles;
+    angles.timestamp = TimestampTimer::getTimestamp();
+
+    // Calculate the horizontal angle relative to the NED frame
+    // std::atan2 outputs angles in radians, convert to degrees
+    angles.yaw = std::atan2(ned.e, ned.n) / EIGEN_PI * 180;
+
+    float distance = std::sqrt(ned.n * ned.n + ned.e * ned.e);
+    // Calculate the vertical angle relative to the NED frame
+    angles.pitch = std::atan2(-ned.d, distance) / EIGEN_PI * 180;
+
+    return angles;
+}
+
+}  // namespace Boardcore

src/shared/algorithms/Follower/Follower.h

+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Authors: Emilio Corigliano, Niccolò Betto, Federico Lolli
+ *
+ * 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/Algorithm.h>
+#include <algorithms/NAS/NASState.h>
+#include <diagnostic/PrintLogger.h>
+#include <logger/Logger.h>
+#include <sensors/SensorData.h>
+#include <sensors/Vectornav/VN300/VN300Data.h>
+
+#include <atomic>
+#include <utils/ModuleManager/ModuleManager.hpp>
+
+#include "FollowerData.h"
+
+namespace Boardcore
+{
+
+class Follower : public Algorithm
+{
+public:
+    /**
+     * @brief Constructor of the follower class.
+     *
+     * @param updatePeriod The period of update of the follower algorithm [s].
+     */
+    explicit Follower(float updatePeriod);
+
+    /**
+     * @brief Check that both the antenna and rocket coordinates have been set.
+     * @return `true` if initial rocket-antenna distance has been set,
+     * `false` otherwise.
+     */
+    bool init() override;
+
+    /**
+     * @brief Setter for the GPS coordinates of the antenna.
+     */
+    void setAntennaCoordinates(const GPSData& gpsData);
+
+    /**
+     * @brief Setter for the GPS coordinates of the rocket's NAS origin
+     * reference.
+     */
+    void setRocketNASOrigin(const GPSData& gpsData);
+
+    /**
+     * @brief Setter for the NAS state of the rocket.
+     */
+    void setLastRocketNasState(const NASState nasState);
+
+    /**
+     * @brief Setter for the attitude of the antenna.
+     * @param attitudeData The VN300 data containing the attitude of the antenna
+     * (yaw, pitch)
+     */
+    void setLastAntennaAttitude(const VN300Data& attitudeData);
+
+    /**
+     * @brief Synchronized getter for the State of the follower algorithm.
+     * @returns The state of the follower algorithm.
+     */
+    FollowerState getState();
+
+    /**
+     * @brief Getter for the target antenna position computed by the algorithm.
+     * @returns The target antenna positions.
+     */
+    AntennaAngles getTargetAngles() { return targetAngles; }
+
+private:
+    /**
+     * @brief Use the last rocket NAS state and initial rocket-antenna distance
+     * to compute the target antenna angles, retrievable with `getTargetAngles`.
+     */
+    void step() override;
+
+    /**
+     * @brief Synchronized getter that returns a copy of the last antenna
+     * attitude of the antenna.
+     */
+    VN300Data getLastAntennaAttitude();
+
+    /**
+     * @brief Synchronized getter that returns a copy of the last NAS state
+     * of the rocket
+     */
+    NASState getLastRocketNasState();
+
+    /**
+     * @brief Calculates the target angles from the given NED coordinates that
+     * the antenna should point to.
+     */
+    AntennaAngles rocketPositionToAntennaAngles(const NEDCoords& ned);
+
+    /**
+     * @brief Minimize rotation angle.
+     *
+     * @param angle Angle of movement [deg]
+     * @return The minimized rotation angle [deg]
+     */
+    float minimizeRotation(float angle);
+
+    /**
+     * @brief Synchronized setter for the state of the follower algorithm.
+     * @warning Should NOT be called if not in a test.
+     */
+    void setState(const FollowerState& newState);
+
+    /**
+     * @brief Get for the GPS coordinates of the antenna.
+     */
+    Eigen::Vector3f getAntennaCoordinates();
+
+    /**
+     * @brief Get for the GPS coordinates of the rocket's NAS origin reference.
+     */
+    Eigen::Vector3f getRocketNASOrigin();
+
+    // actuation update period [s]
+    float updatePeriod;
+
+    // max number of retries for GPS data acquisition
+    const uint8_t maxInitRetries = 120;
+
+    bool antennaCoordinatesSet   = false;
+    bool rocketCoordinatesSet    = false;
+    bool firstAntennaAttitudeSet = false;
+
+    VN300Data lastAntennaAttitude;
+    miosix::FastMutex lastAntennaAttitudeMutex;
+
+    NASState lastRocketNasState;
+    miosix::FastMutex lastRocketNasStateMutex;
+
+    // GPS coordinates of the antenna [lat, lon, alt] [deg, deg, m]
+    Eigen::Vector3f antennaCoordinates;
+    miosix::FastMutex antennaCoordinatesMutex;
+    // GPS coordinates of the NAS origin taken from reference origin [lat, lon,
+    // alt] [deg, deg, m]
+    Eigen::Vector3f rocketNASOrigin;
+    miosix::FastMutex rocketNASOriginMutex;
+    // Initial distance between the antenna and the rocket while in ramp [lat,
+    // lon, alt] [deg, deg, m]
+    Eigen::Vector2f initialAntennaRocketDistance;
+
+    // Target yaw and pitch of the system [deg, deg]
+    AntennaAngles targetAngles;
+
+    FollowerState state;
+    miosix::FastMutex stateMutex;
+
+    PrintLogger logger = Logging::getLogger("Follower");
+};
+
+}  // namespace Boardcore

src/shared/algorithms/Follower/FollowerData.h

+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Emilio Corigliano
+ *
+ * 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 <sensors/SensorData.h>
+
+namespace Boardcore
+{
+
+struct NEDCoords
+{
+    float n = 0;
+    float e = 0;
+    float d = 0;
+};
+
+/**
+ * @brief A structure for storing angles relative to the NED frame.
+ */
+struct AntennaAngles
+{
+    uint64_t timestamp = 0;
+
+    float yaw;    //!< Angle between the X axis (N axis) and the target position
+                  //!< on the XY plane (NE plane), positive anti-clockwise [deg]
+    float pitch;  //!< Angle between the XY plane (NE plane) and the target
+    //!< position [deg]
+    uint32_t nrPropagations =
+        0;  //!< Nr of propagations by the propagator (0 if no propagation)
+
+    AntennaAngles() : timestamp{0}, yaw(0), pitch(0), nrPropagations{0} {};
+    AntennaAngles(uint64_t timestamp, float yaw, float pitch)
+        : timestamp{timestamp}, yaw(yaw), pitch(pitch), nrPropagations{0} {};
+    AntennaAngles(uint64_t timestamp, float yaw, float pitch,
+                  uint32_t nrPropagations)
+        : timestamp{timestamp}, yaw(yaw),
+          pitch(pitch), nrPropagations{nrPropagations} {};
+
+    static std::string header()
+    {
+        return "timestamp,yaw,pitch,nrPropagations\n";
+    }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << yaw << "," << pitch << "," << nrPropagations
+           << "\n";
+    }
+};
+
+struct FollowerState
+{
+    uint64_t timestamp;
+    float yaw;    // [deg]
+    float pitch;  // [deg]
+    float horizontalSpeed;
+    float verticalSpeed;
+
+    FollowerState()
+        : timestamp(0), yaw(0), pitch(0), horizontalSpeed(0), verticalSpeed(0)
+    {
+    }
+
+    static std::string header()
+    {
+        return "timestamp,yaw,pitch,horizontalSpeed,verticalSpeed\n";
+    }
+
+    void print(std::ostream& os) const
+    {
+        os << timestamp << "," << yaw << "," << pitch << "," << horizontalSpeed
+           << "," << verticalSpeed << "\n";
+    }
+};
+
+/**
+ * @brief A structure for logging the ARP system coordinates set in the
+ * Follower.
+ */
+struct LogAntennasCoordinates : public GPSData
+{
+    explicit LogAntennasCoordinates(const GPSData& data) : GPSData(data) {}
+
+    LogAntennasCoordinates() {}
+};
+
+/**
+ * @brief A structure for logging the Rocket coordinates set in the Follower.
+ */
+struct LogRocketCoordinates : public GPSData
+{
+    explicit LogRocketCoordinates(const GPSData& data) : GPSData(data) {}
+
+    LogRocketCoordinates() {}
+};
+}  // namespace Boardcore

src/shared/logger/LogTypes.h

@@ -25,6 +25,7 @@
 #include <actuators/Servo/ServoData.h>
 #include <actuators/stepper/StepperData.h>
 #include <algorithms/ADA/ADAData.h>
+#include <algorithms/Follower/FollowerData.h>
 #include <algorithms/NAS/NASState.h>
 #include <algorithms/ReferenceValues.h>
 #include <diagnostic/CpuMeter/CpuMeter.h>
@@ -177,6 +178,9 @@ void registerTypes(Deserializer& ds)
     ds.registerType<VN300Data>();
     ds.registerType<VoltageData>();
     ds.registerType<Xbee::XbeeStatus>();
+    ds.registerType<FollowerState>();
+    ds.registerType<LogAntennasCoordinates>();
+    ds.registerType<LogRocketCoordinates>();
 }
 
 }  // namespace LogTypes