diff --git a/src/shared/algorithms/Follower/Follower.cpp b/src/shared/algorithms/Follower/Follower.cpp
index 46beec180682ff59bc351efa995101c257ab698b..e652bac3cd9545dfa8ede7a3e9c0f012fcd7c3d5 100644
--- a/src/shared/algorithms/Follower/Follower.cpp
+++ b/src/shared/algorithms/Follower/Follower.cpp
@@ -52,7 +52,7 @@ float minimizeRotation(float angle)
Follower::Follower(std::chrono::milliseconds updatePeriod)
: updatePeriod(static_cast<float>(updatePeriod.count()) / 1000),
- targetAngles({0, 0, 0})
+ targetAngles({0, 0, 0}), firstAntennaAttitudeSet(false), isInit(false)
{
}
@@ -66,7 +66,7 @@ void Follower::setAntennaCoordinates(const Boardcore::GPSData& gpsData)
void Follower::setRocketNASOrigin(const Boardcore::GPSData& gpsData)
{
- Lock<FastMutex> lock(lastAntennaAttitudeMutex);
+ Lock<FastMutex> lock(rocketNASOriginMutex);
rocketNASOrigin = {gpsData.latitude, gpsData.longitude, gpsData.height};
Boardcore::Logger::getInstance().log(LogRocketCoordinates(gpsData));
rocketCoordinatesSet = true;
@@ -109,17 +109,25 @@ void Follower::setState(const FollowerState& newState)
state = newState;
}
+AntennaAngles Follower::getTargetAngles()
+{
+ miosix::Lock<miosix::FastMutex> lock(targetAnglesMutex);
+ return targetAngles;
+}
+
bool Follower::init()
{
+ if (isInit)
+ return true;
if (!antennaCoordinatesSet || !rocketCoordinatesSet)
{
LOG_ERR(logger, "Antenna or rocket coordinates not set");
return false;
}
- // Antenna Coordinates
+ // Antenna Coordinates (w/out altitude)
Eigen::Vector2f antennaCoord{getAntennaCoordinates().head<2>()};
- // Rocket coordinates
+ // Rocket coordinates (w/out altitude)
Eigen::Vector2f rocketCoord{getRocketNASOrigin().head<2>()};
initialAntennaRocketDistance =
@@ -128,37 +136,53 @@ bool Follower::init()
LOG_INFO(logger, "Initial antenna - rocket distance: [{}, {}] [m]\n",
initialAntennaRocketDistance[0], initialAntennaRocketDistance[1]);
+ isInit = true;
return true;
}
void Follower::step()
{
+ if (!isInit)
+ {
+ LOG_ERR(logger, "Not initialized Follower\n");
+ return;
+ }
+
NASState lastRocketNas = getLastRocketNasState();
// Getting the position of the rocket wrt the antennas in NED frame
NEDCoords rocketPosition = {lastRocketNas.n, lastRocketNas.e,
lastRocketNas.d};
- // Calculate the antenna target angles from the NED rocket coordinates
- targetAngles = rocketPositionToAntennaAngles(rocketPosition);
+ AntennaAngles diffAngles;
+ VN300Data vn300;
- // If attitude data has never been set, do not actuate the steppers
- if (!firstAntennaAttitudeSet)
{
- LOG_ERR(logger, "Antenna attitude not set");
- return;
+ miosix::Lock<miosix::FastMutex> lockAngle(targetAnglesMutex);
+
+ // Calculate the antenna target angles from the NED rocket coordinates
+ targetAngles = rocketPositionToAntennaAngles(rocketPosition);
+
+ {
+ Lock<FastMutex> lockLastRocketNasState(lastRocketNasStateMutex);
+ // If attitude data has never been set, do not actuate the steppers
+ if (!firstAntennaAttitudeSet)
+ {
+ LOG_ERR(logger, "Antenna attitude not set\n");
+ return;
+ }
+ }
+
+ vn300 = getLastAntennaAttitude();
+
+ // Calculate the amount to move from the current position
+ diffAngles = {targetAngles.timestamp, targetAngles.yaw - vn300.yaw,
+ targetAngles.pitch - vn300.pitch};
}
- 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);
+ diffAngles.yaw = YAW_GAIN * minimizeRotation(diffAngles.yaw);
+ diffAngles.pitch = PITCH_GAIN * minimizeRotation(diffAngles.pitch);
// Calculate angular velocity for moving the antennas toward position
float horizontalSpeed =
diff --git a/src/shared/algorithms/Follower/Follower.h b/src/shared/algorithms/Follower/Follower.h
index 527144188bca05a2ae808cec7cf538026a252e92..073453a1b93d65bf9b936c62c0e08510bd8d8e0b 100644
--- a/src/shared/algorithms/Follower/Follower.h
+++ b/src/shared/algorithms/Follower/Follower.h
@@ -38,6 +38,15 @@
namespace Boardcore
{
+static constexpr float YAW_GAIN = 0.1;
+static constexpr float PITCH_GAIN = 1.0;
+
+/**
+ * @brief Follower class to output the yaw ad pitch necessary to track from the
+ * GPS origin the rocket. Computes the angle to follow the rocket using its NAS
+ * origin, NED position and velocity
+ *
+ */
class Follower : public Algorithm
{
public:
@@ -57,12 +66,14 @@ public:
/**
* @brief Setter for the GPS coordinates of the antenna.
+ * @note No checks for the GPS fix are done
*/
void setAntennaCoordinates(const GPSData& gpsData);
/**
* @brief Setter for the GPS coordinates of the rocket's NAS origin
* reference.
+ * @note No checks for the GPS fix are done
*/
void setRocketNASOrigin(const GPSData& gpsData);
@@ -88,7 +99,7 @@ public:
* @brief Getter for the target antenna position computed by the algorithm.
* @returns The target antenna positions.
*/
- AntennaAngles getTargetAngles() { return targetAngles; }
+ AntennaAngles getTargetAngles();
private:
/**
@@ -133,13 +144,15 @@ private:
// actuation update period [s]
float updatePeriod;
+ // Initialization flag
+ std::atomic<bool> isInit;
// max number of retries for GPS data acquisition
const uint8_t maxInitRetries = 120;
- bool antennaCoordinatesSet = false;
- bool rocketCoordinatesSet = false;
- bool firstAntennaAttitudeSet = false;
+ bool antennaCoordinatesSet = false;
+ bool rocketCoordinatesSet = false;
+ std::atomic<bool> firstAntennaAttitudeSet;
VN300Data lastAntennaAttitude;
miosix::FastMutex lastAntennaAttitudeMutex;
@@ -160,6 +173,7 @@ private:
// Target yaw and pitch of the system [deg, deg]
AntennaAngles targetAngles;
+ miosix::FastMutex targetAnglesMutex;
FollowerState state;
miosix::FastMutex stateMutex;
diff --git a/src/shared/algorithms/Follower/FollowerData.h b/src/shared/algorithms/Follower/FollowerData.h
index d8e2c9df072b131ac13424c0b9117c087a813f4d..92ddec6388c5eb592ca6df670c372cc3787a8005 100644
--- a/src/shared/algorithms/Follower/FollowerData.h
+++ b/src/shared/algorithms/Follower/FollowerData.h
@@ -42,27 +42,29 @@ 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]
+ //!< on the XY plane (NE plane), positive clockwise [deg]
float pitch; //!< Angle between the XY plane (NE plane) and the target
- //!< position [deg]
+ //!< position, positive UP [deg]
AntennaAngles() : timestamp{0}, yaw{0}, pitch{0} {};
AntennaAngles(uint64_t timestamp, float yaw, float pitch)
- : timestamp{timestamp}, yaw(yaw), pitch(pitch) {};
+ : timestamp{timestamp}, yaw(yaw), pitch(pitch){};
AntennaAngles(uint64_t timestamp, float yaw, float pitch,
uint32_t nrPropagations)
- : timestamp{timestamp}, yaw(yaw), pitch(pitch) {};
+ : timestamp{timestamp}, yaw(yaw), pitch(pitch){};
};
/**
- * @brief A structure for storing angles relative to the NED frame.
+ * @brief A structure for storing angles relative to the NED frame and the
+ * number of propagations that produce such angle, 0 if no propagation step has
+ * been used. Used for logging.
*/
struct AntennaAnglesLog : public AntennaAngles
{
uint32_t nrPropagations =
0; //!< Nr of propagations by the propagator (0 if no propagation)
- AntennaAnglesLog() : AntennaAngles(), nrPropagations(0) {};
+ AntennaAnglesLog() : AntennaAngles(), nrPropagations(0){};
AntennaAnglesLog(uint64_t timestamp, float yaw, float pitch)
: AntennaAngles(timestamp, yaw, pitch), nrPropagations{0} {};
AntennaAnglesLog(uint64_t timestamp, float yaw, float pitch,
@@ -84,6 +86,10 @@ struct AntennaAnglesLog : public AntennaAngles
}
};
+/**
+ * @brief State of the Follower algorithm, with the angles and speeds
+ *
+ */
struct FollowerState
{
uint64_t timestamp;