diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake
index 22c1ee78d2b3fce712f1596061739c9ca0ddb45f..067c01b4ec674882dbf6d7e5eee58a528a112800 100644
--- a/cmake/boardcore.cmake
+++ b/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
diff --git a/src/shared/algorithms/Follower/Follower.cpp b/src/shared/algorithms/Follower/Follower.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a6006fdeea4620a473c9d929c04f631c5b3291f6
--- /dev/null
+++ b/src/shared/algorithms/Follower/Follower.cpp
@@ -0,0 +1,229 @@
+/* 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
diff --git a/src/shared/algorithms/Follower/Follower.h b/src/shared/algorithms/Follower/Follower.h
new file mode 100644
index 0000000000000000000000000000000000000000..3fb6817a137fd06c992a7f7f97d1c69f2d96ac92
--- /dev/null
+++ b/src/shared/algorithms/Follower/Follower.h
@@ -0,0 +1,177 @@
+/* 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
diff --git a/src/shared/algorithms/Follower/FollowerData.h b/src/shared/algorithms/Follower/FollowerData.h
new file mode 100644
index 0000000000000000000000000000000000000000..a3ae02756d777afa84c67ba8b8e2048a4180c974
--- /dev/null
+++ b/src/shared/algorithms/Follower/FollowerData.h
@@ -0,0 +1,116 @@
+/* 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
diff --git a/src/shared/logger/LogTypes.h b/src/shared/logger/LogTypes.h
index 98884a8dc501f617aba545be8ae4763b7e7d444c..dea1b762f93657f0f322b381a86cb435181a2c78 100644
--- a/src/shared/logger/LogTypes.h
+++ b/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