From 81f70dd9a7f0d564011824c703844590586c20dd Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Mon, 25 Nov 2024 15:35:09 +0100
Subject: [PATCH] [ARP] Propagator state log and Follower gain set
Now PropagatorState is logged, the user-defined reference costructor and eigen vector3f were removed since logger needs trivially copiable structs
[ARP] PropagatorData documentation for vector get/set
NED and quaternion getter and setter now documented
[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.
[ARP] Minor fix about atomic bools in Follower
[ARP] Follower removing mutex and changing some GPS data
Removed mutex hell and substituted with just one for the follower.
Some GPSData now removed and used instead structures just for the components needed
[ARP] Follower Format
[ARP] Fixed deadlock in Follower
Follower was having deadlocks by using mutex-protected functions to get the state in functions already owning the mutex.
---
src/shared/algorithms/Follower/Follower.cpp | 125 +++++++++--------
src/shared/algorithms/Follower/Follower.h | 52 +++++---
src/shared/algorithms/Follower/FollowerData.h | 12 +-
.../algorithms/Propagator/Propagator.cpp | 9 +-
src/shared/algorithms/Propagator/Propagator.h | 1 +
.../algorithms/Propagator/PropagatorData.h | 126 ++++++++++++------
6 files changed, 201 insertions(+), 124 deletions(-)
diff --git a/src/shared/algorithms/Follower/Follower.cpp b/src/shared/algorithms/Follower/Follower.cpp
index 46beec180..6c9e71d28 100644
--- a/src/shared/algorithms/Follower/Follower.cpp
+++ b/src/shared/algorithms/Follower/Follower.cpp
@@ -56,17 +56,17 @@ Follower::Follower(std::chrono::milliseconds updatePeriod)
{
}
-void Follower::setAntennaCoordinates(const Boardcore::GPSData& gpsData)
+void Follower::setAntennaCoordinates(const GPSData& gpsData)
{
- Lock<FastMutex> lock(lastAntennaAttitudeMutex);
- antennaCoordinates = {gpsData.latitude, gpsData.longitude, gpsData.height};
+ Lock<FastMutex> lock(followerMutex);
+ antennaCoordinates = {gpsData.latitude, gpsData.longitude};
Boardcore::Logger::getInstance().log(LogAntennasCoordinates(gpsData));
antennaCoordinatesSet = true;
}
-void Follower::setRocketNASOrigin(const Boardcore::GPSData& gpsData)
+void Follower::setRocketNASOrigin(const GPSData& gpsData)
{
- Lock<FastMutex> lock(lastAntennaAttitudeMutex);
+ Lock<FastMutex> lock(followerMutex);
rocketNASOrigin = {gpsData.latitude, gpsData.longitude, gpsData.height};
Boardcore::Logger::getInstance().log(LogRocketCoordinates(gpsData));
rocketCoordinatesSet = true;
@@ -74,91 +74,97 @@ void Follower::setRocketNASOrigin(const Boardcore::GPSData& gpsData)
void Follower::setLastAntennaAttitude(const VN300Data& attitudeData)
{
- Lock<FastMutex> lock(lastAntennaAttitudeMutex);
- lastAntennaAttitude = attitudeData;
+ Lock<FastMutex> lock(followerMutex);
+ firstAntennaAttitudeSet = true;
+ lastAntennaAttitude = attitudeData;
}
VN300Data Follower::getLastAntennaAttitude()
{
- Lock<FastMutex> lock(lastAntennaAttitudeMutex);
+ Lock<FastMutex> lock(followerMutex);
return lastAntennaAttitude;
}
void Follower::setLastRocketNasState(const NASState& nasState)
{
- Lock<FastMutex> lock(lastRocketNasStateMutex);
- lastRocketNasState = nasState;
- firstAntennaAttitudeSet = true;
+ Lock<FastMutex> lock(followerMutex);
+ lastRocketNasState = nasState;
+ lastRocketNasStateSet = true;
}
NASState Follower::getLastRocketNasState()
{
- Lock<FastMutex> lock(lastRocketNasStateMutex);
+ Lock<FastMutex> lock(followerMutex);
return lastRocketNasState;
}
FollowerState Follower::getState()
{
- miosix::Lock<miosix::FastMutex> lock(stateMutex);
+ miosix::Lock<miosix::FastMutex> lock(followerMutex);
return state;
}
void Follower::setState(const FollowerState& newState)
{
- miosix::Lock<miosix::FastMutex> lock(stateMutex);
+ miosix::Lock<miosix::FastMutex> lock(followerMutex);
state = newState;
}
+AntennaAngles Follower::getTargetAngles()
+{
+ miosix::Lock<miosix::FastMutex> lock(followerMutex);
+ 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
- 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;
}
void Follower::step()
{
- NASState lastRocketNas = getLastRocketNasState();
+ AntennaAngles diffAngles;
+ VN300Data vn300;
+ NASState lastRocketNas;
+ NEDCoords rocketPosition;
- // Getting the position of the rocket wrt the antennas in NED frame
- NEDCoords rocketPosition = {lastRocketNas.n, lastRocketNas.e,
- lastRocketNas.d};
+ // Read the data for the step computation
+ {
+ Lock<FastMutex> lock(followerMutex);
- // Calculate the antenna target angles from the NED rocket coordinates
- targetAngles = rocketPositionToAntennaAngles(rocketPosition);
+ if (!firstAntennaAttitudeSet)
+ {
+ LOG_ERR(logger, "Antenna attitude not set\n");
+ return;
+ }
+ // TODO: See if needed to check the NAS or rather point to the NAS
+ // origin if missing
- // If attitude data has never been set, do not actuate the steppers
- if (!firstAntennaAttitudeSet)
- {
- LOG_ERR(logger, "Antenna attitude not set");
- return;
- }
+ lastRocketNas = lastRocketNasState;
+ vn300 = lastAntennaAttitude;
- VN300Data vn300 = getLastAntennaAttitude();
+ // Local variable checks and updates
+ // Getting the position of the rocket wrt the antennas in NED frame
+ rocketPosition = {lastRocketNas.n, lastRocketNas.e, lastRocketNas.d};
- // Calculate the amount to move from the current position
- AntennaAngles diffAngles{targetAngles.timestamp,
- targetAngles.yaw - vn300.yaw,
- targetAngles.pitch - vn300.pitch};
+ // Calculate the antenna target angles from the NED rocket coordinates
+ targetAngles = rocketPositionToAntennaAngles(rocketPosition);
+
+ // Calculate the amount to move from the current position
+ 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 =
@@ -173,7 +179,12 @@ void Follower::step()
newState.pitch = diffAngles.pitch;
newState.horizontalSpeed = horizontalSpeed;
newState.verticalSpeed = verticalSpeed;
- setState(newState);
+
+ // Write the new state for the follower
+ {
+ Lock<FastMutex> lockWrite(followerMutex);
+ state = newState;
+ }
#ifndef NDEBUG
std::cout << "[FOLLOWER] STEPPER " << "Angles: [" << newState.yaw << ", "
@@ -184,15 +195,15 @@ void Follower::step()
#endif
}
-Eigen::Vector3f Follower::getAntennaCoordinates()
+Eigen::Vector2f Follower::getAntennaCoordinates()
{
- Lock<FastMutex> lock(antennaCoordinatesMutex);
- return Eigen::Vector3f{antennaCoordinates};
+ Lock<FastMutex> lock(followerMutex);
+ return Eigen::Vector2f{antennaCoordinates};
}
Eigen::Vector3f Follower::getRocketNASOrigin()
{
- Lock<FastMutex> lock(rocketNASOriginMutex);
+ Lock<FastMutex> lock(followerMutex);
return Eigen::Vector3f{rocketNASOrigin};
}
@@ -200,18 +211,16 @@ AntennaAngles Follower::rocketPositionToAntennaAngles(
const NEDCoords& rocketNed)
{
// Antenna Coordinates
- Eigen::Vector2f antennaCoord = getAntennaCoordinates().head<2>();
+ Eigen::Vector2f antennaCoord = antennaCoordinates;
- // Rocket coordinates
- Eigen::Vector2f rocketCoord = getRocketNASOrigin().head<2>();
+ // Rocket coordinates, w/out altitude
+ Eigen::Vector2f rocketCoord = rocketNASOrigin.head<2>();
- initialAntennaRocketDistance =
- Aeroutils::geodetic2NED(rocketCoord, antennaCoord);
+ antennaRocketDistance = 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};
+ NEDCoords ned = {rocketNed.n + antennaRocketDistance.x(),
+ rocketNed.e + antennaRocketDistance.y(), rocketNed.d};
AntennaAngles angles;
angles.timestamp = TimestampTimer::getTimestamp();
diff --git a/src/shared/algorithms/Follower/Follower.h b/src/shared/algorithms/Follower/Follower.h
index 527144188..3a6fcda47 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:
/**
@@ -112,6 +123,8 @@ private:
/**
* @brief Calculates the target angles from the given NED coordinates that
* the antenna should point to.
+ *
+ * @note Called by a mutex-protected function
*/
AntennaAngles rocketPositionToAntennaAngles(const NEDCoords& ned);
@@ -122,9 +135,9 @@ private:
void setState(const FollowerState& newState);
/**
- * @brief Get for the GPS coordinates of the antenna.
+ * @brief Get for the [lat, lon] coordinates of the antenna.
*/
- Eigen::Vector3f getAntennaCoordinates();
+ Eigen::Vector2f getAntennaCoordinates();
/**
* @brief Get for the GPS coordinates of the rocket's NAS origin reference.
@@ -133,38 +146,41 @@ private:
// actuation update period [s]
float updatePeriod;
+ // Initialization flag
+ std::atomic<bool> isInit{false};
// 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;
+ bool lastRocketNasStateSet = false;
+ std::atomic<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]
+ // TODO: See if assumption has sense...
+ /* GPS coordinates of the antenna [lat, lon] [deg, deg],
+ altitude is considered same as NAS Origin */
+ Eigen::Vector2f antennaCoordinates;
+ /* 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;
+ /* Distance between the antenna and the rocket [lat,
+ lon, alt] [deg, deg, m] */
+ Eigen::Vector2f antennaRocketDistance;
// Target yaw and pitch of the system [deg, deg]
AntennaAngles targetAngles;
FollowerState state;
- miosix::FastMutex stateMutex;
PrintLogger logger = Logging::getLogger("Follower");
+
+ // General mutex for the follower
+ miosix::FastMutex followerMutex;
};
} // namespace Boardcore
diff --git a/src/shared/algorithms/Follower/FollowerData.h b/src/shared/algorithms/Follower/FollowerData.h
index d8e2c9df0..59a1b7c2b 100644
--- a/src/shared/algorithms/Follower/FollowerData.h
+++ b/src/shared/algorithms/Follower/FollowerData.h
@@ -42,9 +42,9 @@ 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)
@@ -55,7 +55,9 @@ struct AntennaAngles
};
/**
- * @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
{
@@ -84,6 +86,10 @@ struct AntennaAnglesLog : public AntennaAngles
}
};
+/**
+ * @brief State of the Follower algorithm, with the angles and speeds
+ *
+ */
struct FollowerState
{
uint64_t timestamp;
diff --git a/src/shared/algorithms/Propagator/Propagator.cpp b/src/shared/algorithms/Propagator/Propagator.cpp
index 634d1bf26..8fa10602d 100644
--- a/src/shared/algorithms/Propagator/Propagator.cpp
+++ b/src/shared/algorithms/Propagator/Propagator.cpp
@@ -24,6 +24,7 @@
#include <drivers/timer/TimestampTimer.h>
#include <utils/AeroUtils/AeroUtils.h>
+#include <utils/Debug.h>
using namespace Eigen;
@@ -52,15 +53,19 @@ void Propagator::step()
: oldState);
// Update Position propagating it with velocity
- state.x_prop = state.x_prop + state.v_prop * updatePeriod;
+ state.setXProp(state.getXProp() + state.getVProp() * updatePeriod);
state.nPropagations++;
state.timestamp = TimestampTimer::getTimestamp();
+
+ // Log propagator state
+ PropagatorState logState(state);
+ Boardcore::Logger::getInstance().log(logState);
}
void Propagator::setRocketNasState(const NASState& newRocketNasState)
{
miosix::Lock<miosix::FastMutex> lockState(stateMutex);
- miosix::Lock<miosix::FastMutex> lockNAS(nasStateMutex);
+ miosix::Lock<miosix::FastMutex> lock(nasStateMutex);
// Reset nPropagations to notify another received "real" packet
state.nPropagations = 0;
diff --git a/src/shared/algorithms/Propagator/Propagator.h b/src/shared/algorithms/Propagator/Propagator.h
index a8d43d4c0..142fc41c2 100644
--- a/src/shared/algorithms/Propagator/Propagator.h
+++ b/src/shared/algorithms/Propagator/Propagator.h
@@ -23,6 +23,7 @@
#pragma once
#include <algorithms/Algorithm.h>
+#include <logger/Logger.h>
#include <miosix.h>
#include <chrono>
diff --git a/src/shared/algorithms/Propagator/PropagatorData.h b/src/shared/algorithms/Propagator/PropagatorData.h
index cab262da5..228f306f0 100644
--- a/src/shared/algorithms/Propagator/PropagatorData.h
+++ b/src/shared/algorithms/Propagator/PropagatorData.h
@@ -31,73 +31,113 @@ namespace Boardcore
{
/**
- * @brief Struct containing the state of the propagator. Stores the timestamp of
- * the propagation, the number of propagations since the last NAS rocket packet
- * and the propagated NAS state already divided in significant vectors so that
- * computations with the NAS state are easier.
- * It also exposes a method to retrieve the propagated NAS state as a NASState
- * structure.
+ * @brief State of the propagator, taking into account the prediction steps (0
+ * if true NAS state) and the propagated NAS
+ * @note Can be logged.
+ *
*/
struct PropagatorState
{
uint64_t timestamp; ///< Prediction timestamp [ms]
uint32_t nPropagations; ///< Predictions from last received NAS state
- Eigen::Vector3f x_prop; ///< Position propagation state NED [m]
- Eigen::Vector3f v_prop; ///< Speed propagation state NED [m]
- Eigen::Vector4f q_prop; ///< Quaternion propagation (scalar last)
- Eigen::Vector3f b_prop; ///< Gyroscope bias propagation
-
- PropagatorState()
- : timestamp(0), nPropagations(0), x_prop(0, 0, 0), v_prop(0, 0, 0),
- q_prop(0, 0, 0, 1), b_prop(0, 0, 0)
+ NASState nas;
+
+ PropagatorState() : timestamp(0), nPropagations(0), nas() {}
+
+ PropagatorState(uint64_t timestamp, uint32_t nPropagations,
+ NASState nasState)
+ : timestamp(timestamp), nPropagations(nPropagations), nas(nasState)
{
}
- PropagatorState(const PropagatorState& newState)
- : PropagatorState(newState.timestamp, newState.nPropagations,
- newState.x_prop, newState.v_prop, newState.q_prop,
- newState.b_prop)
+ static std::string header()
{
+ return "timestamp,nPropagations,n,e,d,vn,ve,vd,qx,qy,qz,qw,bx,by,bz\n";
}
- PropagatorState(uint64_t timestamp, uint32_t nPropagations,
- Eigen::Vector3f x_prop, Eigen::Vector3f v_prop,
- Eigen::Vector4f q_prop, Eigen::Vector3f b_prop)
- : timestamp(timestamp), nPropagations(nPropagations), x_prop(x_prop),
- v_prop(v_prop), q_prop(q_prop), b_prop(b_prop)
+ void print(std::ostream& os) const
{
+ os << timestamp << "," << nPropagations << "," << nas.n << "," << nas.e
+ << "," << nas.d << "," << nas.vn << "," << nas.ve << "," << nas.vd
+ << "," << nas.qx << "," << nas.qy << "," << nas.qz << "," << nas.qw
+ << "," << nas.bx << "," << nas.by << "," << nas.bz << "\n";
}
- PropagatorState(uint64_t timestamp, uint32_t nPropagations,
- NASState nasState)
- : timestamp(timestamp), nPropagations(nPropagations),
- x_prop(nasState.n, nasState.e, nasState.d),
- v_prop(nasState.vn, nasState.ve, nasState.vd),
- q_prop(nasState.qx, nasState.qy, nasState.qz, nasState.qw),
- b_prop(nasState.bx, nasState.by, nasState.bz)
+ NASState getNasState() const { return nas; }
+
+ /**
+ * @brief Getter for the vector of positions NED
+ *
+ * @return Eigen::Vector3f the NED position vector
+ */
+ Eigen::Vector3f getXProp() { return Eigen::Vector3f(nas.n, nas.e, nas.d); }
+
+ /**
+ * @brief Setter for the vector of positions NED
+ */
+ void setXProp(Eigen::Vector3f xProp)
{
+ nas.n = xProp(0);
+ nas.e = xProp(1);
+ nas.d = xProp(2);
}
- static std::string header()
+ /**
+ * @brief Getter for the vector of velocities NED
+ *
+ * @return Eigen::Vector3f the NED velocities vector
+ */
+ Eigen::Vector3f getVProp()
{
- return "timestamp,nPropagations,n,e,d,vn,ve,vd,qx,qy,qz,qw,bx,by,bz\n";
+ return Eigen::Vector3f(nas.vn, nas.ve, nas.vd);
}
- void print(std::ostream& os) const
+ /**
+ * @brief Setter for the vector of velocities NED
+ */
+ void setVProp(Eigen::Vector3f vProp)
{
- os << timestamp << "," << nPropagations << "," << x_prop(0) << ","
- << x_prop(1) << "," << x_prop(2) << "," << v_prop(0) << ","
- << v_prop(1) << "," << v_prop(2) << "," << q_prop(0) << ","
- << q_prop(1) << "," << q_prop(2) << "," << q_prop(3) << ","
- << b_prop(0) << "," << b_prop(1) << "," << b_prop(2) << "\n";
+ nas.vn = vProp(0);
+ nas.ve = vProp(1);
+ nas.vd = vProp(2);
}
- NASState getNasState() const
+ /**
+ * @brief Getter for the vector of quaternions
+ *
+ * @return Eigen::Vector3f the quaternions vector
+ */
+ Eigen::Vector4f getQProp()
+ {
+ return Eigen::Vector4f(nas.qx, nas.qy, nas.qz, nas.qw);
+ }
+
+ /**
+ * @brief Setter for the vector of quaternions
+ */
+ void setQProp(Eigen::Vector4f qProp)
+ {
+ nas.qx = qProp(0);
+ nas.qy = qProp(1);
+ nas.qz = qProp(2);
+ nas.qw = qProp(3);
+ }
+
+ /**
+ * @brief Getter for the vector of quaternions' bias
+ *
+ * @return Eigen::Vector3f the quaternions' bias vector
+ */
+ Eigen::Vector3f getBProp() { return Eigen::Vector3f(nas.n, nas.e, nas.d); }
+
+ /**
+ * @brief Setter for the vector of quaternions' bias
+ */
+ void setBProp(Eigen::Vector3f bProp)
{
- Eigen::Matrix<float, 13, 1> nasState;
- // cppcheck-suppress constStatement
- nasState << x_prop, v_prop, q_prop, b_prop;
- return NASState(timestamp, nasState);
+ nas.bx = bProp(0);
+ nas.by = bProp(1);
+ nas.bz = bProp(2);
}
};
--
GitLab