From 7f11f533555b66660ddf4de78272699be9a33f8e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Thu, 28 Nov 2024 14:10:59 +0100
Subject: [PATCH] [ARP] Follower race condition fixes, YAW_GAIN and PITCH_GAIN
parameters, clockwise positive yaw
Added isInit variable to avoid step to run in case of non-initialized Follower.
targetAngle now protected with an apposite mutex to avoid race condition between the step and get functions.
Added pitch and yaw gain constexpr parameter
Fix yaw AntennaAngles old documentation assumption of counter-clockwise positive.
---
src/shared/algorithms/Follower/Follower.cpp | 62 +++++++++++++------
src/shared/algorithms/Follower/Follower.h | 22 +++++--
src/shared/algorithms/Follower/FollowerData.h | 18 ++++--
3 files changed, 73 insertions(+), 29 deletions(-)
diff --git a/src/shared/algorithms/Follower/Follower.cpp b/src/shared/algorithms/Follower/Follower.cpp
index 46beec180..e652bac3c 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 527144188..073453a1b 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 d8e2c9df0..92ddec638 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;
--
GitLab