From dd1fcd4d6ad397a393a632e9d3c35ee80ea912e7 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 1/9] [ARP] Propagator state log
Now PropagatorState is logged, the user-defined reference costructor and eigen vector3f were removed since logger needs trivially copiable structs
---
.../algorithms/Propagator/Propagator.cpp | 6 +-
src/shared/algorithms/Propagator/Propagator.h | 1 +
.../algorithms/Propagator/PropagatorData.h | 91 ++++++++++---------
3 files changed, 52 insertions(+), 46 deletions(-)
diff --git a/src/shared/algorithms/Propagator/Propagator.cpp b/src/shared/algorithms/Propagator/Propagator.cpp
index 634d1bf26..9076981b9 100644
--- a/src/shared/algorithms/Propagator/Propagator.cpp
+++ b/src/shared/algorithms/Propagator/Propagator.cpp
@@ -52,9 +52,13 @@ 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)
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..6fbd2aeb6 100644
--- a/src/shared/algorithms/Propagator/PropagatorData.h
+++ b/src/shared/algorithms/Propagator/PropagatorData.h
@@ -31,50 +31,22 @@ 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(const PropagatorState& newState)
- : PropagatorState(newState.timestamp, newState.nPropagations,
- newState.x_prop, newState.v_prop, newState.q_prop,
- newState.b_prop)
- {
- }
-
- 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)
- {
- }
+ PropagatorState() : timestamp(0), nPropagations(0), nas() {}
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)
+ : timestamp(timestamp), nPropagations(nPropagations), nas(nasState)
{
}
@@ -85,19 +57,48 @@ struct PropagatorState
void print(std::ostream& os) const
{
- 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";
+ 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";
}
- NASState getNasState() const
+ NASState getNasState() const { return nas; }
+
+ Eigen::Vector3f getXProp() { return Eigen::Vector3f(nas.n, nas.e, nas.d); }
+ void setXProp(Eigen::Vector3f xProp)
+ {
+ nas.n = xProp(0);
+ nas.e = xProp(1);
+ nas.d = xProp(2);
+ }
+ Eigen::Vector3f getVProp()
+ {
+ return Eigen::Vector3f(nas.vn, nas.ve, nas.vd);
+ }
+ void setVProp(Eigen::Vector3f vProp)
+ {
+ nas.vn = vProp(0);
+ nas.ve = vProp(1);
+ nas.vd = vProp(2);
+ }
+ Eigen::Vector4f getQProp()
+ {
+ return Eigen::Vector4f(nas.qx, nas.qy, nas.qz, nas.qw);
+ }
+ void setQProp(Eigen::Vector4f qProp)
+ {
+ nas.qx = qProp(0);
+ nas.qy = qProp(1);
+ nas.qz = qProp(2);
+ nas.qw = qProp(3);
+ }
+ Eigen::Vector3f getBProp() { return Eigen::Vector3f(nas.n, nas.e, nas.d); }
+ 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
From 5b3cb1afb43ab692f5dd25c9ca0cc57e5cd7f8e3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Thu, 28 Nov 2024 13:56:43 +0100
Subject: [PATCH 2/9] [ARP] PropagatorData documentation for vector get/set
NED and quaternion getter and setter now documented
---
.../algorithms/Propagator/PropagatorData.h | 39 +++++++++++++++++++
1 file changed, 39 insertions(+)
diff --git a/src/shared/algorithms/Propagator/PropagatorData.h b/src/shared/algorithms/Propagator/PropagatorData.h
index 6fbd2aeb6..228f306f0 100644
--- a/src/shared/algorithms/Propagator/PropagatorData.h
+++ b/src/shared/algorithms/Propagator/PropagatorData.h
@@ -65,27 +65,56 @@ struct PropagatorState
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);
}
+
+ /**
+ * @brief Getter for the vector of velocities NED
+ *
+ * @return Eigen::Vector3f the NED velocities vector
+ */
Eigen::Vector3f getVProp()
{
return Eigen::Vector3f(nas.vn, nas.ve, nas.vd);
}
+
+ /**
+ * @brief Setter for the vector of velocities NED
+ */
void setVProp(Eigen::Vector3f vProp)
{
nas.vn = vProp(0);
nas.ve = vProp(1);
nas.vd = vProp(2);
}
+
+ /**
+ * @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);
@@ -93,7 +122,17 @@ struct PropagatorState
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)
{
nas.bx = bProp(0);
--
GitLab
From 1f71112057fba1853510d550a0cdcb933222dcb5 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 3/9] [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
From 35a791d0ba2d69939ce1d064a9935110c567c877 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Thu, 5 Dec 2024 19:10:22 +0100
Subject: [PATCH 4/9] [ARP] Minor fix about atomic bools in Follower
---
src/shared/algorithms/Follower/Follower.cpp | 2 +-
src/shared/algorithms/Follower/Follower.h | 4 ++--
2 files changed, 3 insertions(+), 3 deletions(-)
diff --git a/src/shared/algorithms/Follower/Follower.cpp b/src/shared/algorithms/Follower/Follower.cpp
index e652bac3c..18c0575b5 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}), firstAntennaAttitudeSet(false), isInit(false)
+ targetAngles({0, 0, 0})
{
}
diff --git a/src/shared/algorithms/Follower/Follower.h b/src/shared/algorithms/Follower/Follower.h
index 073453a1b..2f84b00e3 100644
--- a/src/shared/algorithms/Follower/Follower.h
+++ b/src/shared/algorithms/Follower/Follower.h
@@ -145,14 +145,14 @@ private:
// actuation update period [s]
float updatePeriod;
// Initialization flag
- std::atomic<bool> isInit;
+ std::atomic<bool> isInit{false};
// max number of retries for GPS data acquisition
const uint8_t maxInitRetries = 120;
bool antennaCoordinatesSet = false;
bool rocketCoordinatesSet = false;
- std::atomic<bool> firstAntennaAttitudeSet;
+ std::atomic<bool> firstAntennaAttitudeSet{false};
VN300Data lastAntennaAttitude;
miosix::FastMutex lastAntennaAttitudeMutex;
--
GitLab
From be446d228fee193bc60660211584a52de8566e5f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Tue, 14 Jan 2025 11:58:52 +0100
Subject: [PATCH 5/9] [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
---
src/shared/algorithms/Follower/Follower.cpp | 119 ++++++++----------
src/shared/algorithms/Follower/Follower.h | 30 ++---
src/shared/algorithms/Follower/FollowerData.h | 6 +-
3 files changed, 70 insertions(+), 85 deletions(-)
diff --git a/src/shared/algorithms/Follower/Follower.cpp b/src/shared/algorithms/Follower/Follower.cpp
index 18c0575b5..254f09ab2 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(rocketNASOriginMutex);
+ Lock<FastMutex> lock(followerMutex);
rocketNASOrigin = {gpsData.latitude, gpsData.longitude, gpsData.height};
Boardcore::Logger::getInstance().log(LogRocketCoordinates(gpsData));
rocketCoordinatesSet = true;
@@ -74,44 +74,45 @@ void Follower::setRocketNASOrigin(const Boardcore::GPSData& gpsData)
void Follower::setLastAntennaAttitude(const VN300Data& attitudeData)
{
- Lock<FastMutex> lock(lastAntennaAttitudeMutex);
+ 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(targetAnglesMutex);
+ miosix::Lock<miosix::FastMutex> lock(followerMutex);
return targetAngles;
}
@@ -124,62 +125,43 @@ bool Follower::init()
LOG_ERR(logger, "Antenna or rocket coordinates not set");
return false;
}
-
- // Antenna Coordinates (w/out altitude)
- Eigen::Vector2f antennaCoord{getAntennaCoordinates().head<2>()};
- // Rocket coordinates (w/out altitude)
- Eigen::Vector2f rocketCoord{getRocketNASOrigin().head<2>()};
-
- initialAntennaRocketDistance =
- Aeroutils::geodetic2NED(rocketCoord, antennaCoord);
-
- 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};
-
AntennaAngles diffAngles;
VN300Data vn300;
+ NASState lastRocketNas;
+ NEDCoords rocketPosition;
+ // Read the data for the step computation
{
- miosix::Lock<miosix::FastMutex> lockAngle(targetAnglesMutex);
-
- // Calculate the antenna target angles from the NED rocket coordinates
- targetAngles = rocketPositionToAntennaAngles(rocketPosition);
+ Lock<FastMutex> lock(followerMutex);
+ if (!firstAntennaAttitudeSet)
{
- 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;
- }
+ 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
- vn300 = getLastAntennaAttitude();
-
- // Calculate the amount to move from the current position
- diffAngles = {targetAngles.timestamp, targetAngles.yaw - vn300.yaw,
- targetAngles.pitch - vn300.pitch};
+ lastRocketNas = lastRocketNasState;
+ vn300 = lastAntennaAttitude;
}
+ // 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 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 = YAW_GAIN * minimizeRotation(diffAngles.yaw);
diffAngles.pitch = PITCH_GAIN * minimizeRotation(diffAngles.pitch);
@@ -197,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 << ", "
@@ -208,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};
}
@@ -224,18 +211,16 @@ AntennaAngles Follower::rocketPositionToAntennaAngles(
const NEDCoords& rocketNed)
{
// Antenna Coordinates
- Eigen::Vector2f antennaCoord = getAntennaCoordinates().head<2>();
+ Eigen::Vector2f antennaCoord = getAntennaCoordinates();
- // Rocket coordinates
+ // Rocket coordinates, w/out altitude
Eigen::Vector2f rocketCoord = getRocketNASOrigin().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 2f84b00e3..3c09112ec 100644
--- a/src/shared/algorithms/Follower/Follower.h
+++ b/src/shared/algorithms/Follower/Follower.h
@@ -133,9 +133,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.
@@ -152,33 +152,33 @@ private:
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;
- miosix::FastMutex targetAnglesMutex;
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 92ddec638..59a1b7c2b 100644
--- a/src/shared/algorithms/Follower/FollowerData.h
+++ b/src/shared/algorithms/Follower/FollowerData.h
@@ -48,10 +48,10 @@ struct AntennaAngles
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) {};
};
/**
@@ -64,7 +64,7 @@ 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,
--
GitLab
From 1f8856eda5d5f3078515d717057dad093dedce2a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Tue, 14 Jan 2025 13:39:38 +0100
Subject: [PATCH 6/9] [ARP] Follower Format
---
src/shared/algorithms/Follower/Follower.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/shared/algorithms/Follower/Follower.cpp b/src/shared/algorithms/Follower/Follower.cpp
index 254f09ab2..b34ce5d0f 100644
--- a/src/shared/algorithms/Follower/Follower.cpp
+++ b/src/shared/algorithms/Follower/Follower.cpp
@@ -76,7 +76,7 @@ void Follower::setLastAntennaAttitude(const VN300Data& attitudeData)
{
Lock<FastMutex> lock(followerMutex);
firstAntennaAttitudeSet = true;
- lastAntennaAttitude = attitudeData;
+ lastAntennaAttitude = attitudeData;
}
VN300Data Follower::getLastAntennaAttitude()
--
GitLab
From 37c71ae3fb1b6164f605c9e684383b95e7e78a2a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Fri, 17 Jan 2025 16:39:14 +0100
Subject: [PATCH 7/9] [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 | 22 +++++++++----------
src/shared/algorithms/Follower/Follower.h | 2 ++
.../algorithms/Propagator/Propagator.cpp | 3 ++-
3 files changed, 15 insertions(+), 12 deletions(-)
diff --git a/src/shared/algorithms/Follower/Follower.cpp b/src/shared/algorithms/Follower/Follower.cpp
index b34ce5d0f..6c9e71d28 100644
--- a/src/shared/algorithms/Follower/Follower.cpp
+++ b/src/shared/algorithms/Follower/Follower.cpp
@@ -149,18 +149,18 @@ void Follower::step()
lastRocketNas = lastRocketNasState;
vn300 = lastAntennaAttitude;
- }
- // Local variable checks and updates
- // Getting the position of the rocket wrt the antennas in NED frame
- rocketPosition = {lastRocketNas.n, lastRocketNas.e, lastRocketNas.d};
+ // 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 antenna target angles from the NED rocket coordinates
- targetAngles = rocketPositionToAntennaAngles(rocketPosition);
+ // 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};
+ // 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 = YAW_GAIN * minimizeRotation(diffAngles.yaw);
@@ -211,10 +211,10 @@ AntennaAngles Follower::rocketPositionToAntennaAngles(
const NEDCoords& rocketNed)
{
// Antenna Coordinates
- Eigen::Vector2f antennaCoord = getAntennaCoordinates();
+ Eigen::Vector2f antennaCoord = antennaCoordinates;
// Rocket coordinates, w/out altitude
- Eigen::Vector2f rocketCoord = getRocketNASOrigin().head<2>();
+ Eigen::Vector2f rocketCoord = rocketNASOrigin.head<2>();
antennaRocketDistance = Aeroutils::geodetic2NED(rocketCoord, antennaCoord);
diff --git a/src/shared/algorithms/Follower/Follower.h b/src/shared/algorithms/Follower/Follower.h
index 3c09112ec..3a6fcda47 100644
--- a/src/shared/algorithms/Follower/Follower.h
+++ b/src/shared/algorithms/Follower/Follower.h
@@ -123,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);
diff --git a/src/shared/algorithms/Propagator/Propagator.cpp b/src/shared/algorithms/Propagator/Propagator.cpp
index 9076981b9..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;
@@ -64,7 +65,7 @@ void Propagator::step()
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;
--
GitLab
From fd97f97db4a4a7eb1ce9e85e3b0289efe4f05214 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Wed, 5 Mar 2025 23:09:56 +0100
Subject: [PATCH 8/9] [VN300] Adding accelerations to reduced VN300 set
Added accelerations to the reduced sampling set
---
src/shared/sensors/Vectornav/VN300/VN300.cpp | 7 +++++++
src/shared/sensors/Vectornav/VN300/VN300Defs.h | 3 +++
2 files changed, 10 insertions(+)
diff --git a/src/shared/sensors/Vectornav/VN300/VN300.cpp b/src/shared/sensors/Vectornav/VN300/VN300.cpp
index d5243bced..e23dc78b6 100644
--- a/src/shared/sensors/Vectornav/VN300/VN300.cpp
+++ b/src/shared/sensors/Vectornav/VN300/VN300.cpp
@@ -314,6 +314,12 @@ void VN300::buildBinaryDataReduced(const VN300Defs::BinaryDataReduced& rawData,
data.quaternionX = rawData.quaternionX;
data.quaternionY = rawData.quaternionY;
data.quaternionZ = rawData.quaternionZ;
+
+ // Accelerometer
+ data.accelerationTimestamp = timestamp;
+ data.accelerationX = rawData.accelX;
+ data.accelerationY = rawData.accelY;
+ data.accelerationZ = rawData.accelZ;
}
bool VN300::setAntennaA(VN300Defs::AntennaPosition antPos)
@@ -399,6 +405,7 @@ bool VN300::setBinaryOutput()
VN300Defs::BINARYGROUP_COMMON | VN300Defs::BINARYGROUP_GPS;
commonGroup = VN300Defs::COMMONGROUP_YAWPITCHROLL |
VN300Defs::COMMONGROUP_QUATERNION |
+ VN300Defs::COMMONGROUP_ACCEL |
VN300Defs::COMMONGROUP_ANGULARRATE;
gnssGroup = VN300Defs::GPSGROUP_FIX | VN300Defs::GPSGROUP_POSLLA;
diff --git a/src/shared/sensors/Vectornav/VN300/VN300Defs.h b/src/shared/sensors/Vectornav/VN300/VN300Defs.h
index 7e2b186d0..9a32dddee 100755
--- a/src/shared/sensors/Vectornav/VN300/VN300Defs.h
+++ b/src/shared/sensors/Vectornav/VN300/VN300Defs.h
@@ -277,6 +277,9 @@ struct __attribute__((packed)) BinaryDataReduced
float angularX;
float angularY;
float angularZ;
+ float accelX;
+ float accelY;
+ float accelZ;
uint8_t gpsFix;
double latitude;
double longitude;
--
GitLab
From 9e4f5cd7f7410ee42c0141172782b83968535dbf Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <nicolo.caruso@skywarder.eu>
Date: Thu, 27 Mar 2025 11:59:49 +0100
Subject: [PATCH 9/9] [ARP] Fix follower stepper velocity
- Follower: the stepper velocity was saturating due to the fact that it was used the time as seconds while treated as in ms. This issue made saturate always the stepper velocities
---
src/shared/algorithms/Follower/Follower.cpp | 13 ++++++++-----
src/shared/algorithms/Follower/Follower.h | 4 ++--
2 files changed, 10 insertions(+), 7 deletions(-)
diff --git a/src/shared/algorithms/Follower/Follower.cpp b/src/shared/algorithms/Follower/Follower.cpp
index 6c9e71d28..bc5b45f8e 100644
--- a/src/shared/algorithms/Follower/Follower.cpp
+++ b/src/shared/algorithms/Follower/Follower.cpp
@@ -50,9 +50,8 @@ float minimizeRotation(float angle)
return angle;
}
-Follower::Follower(std::chrono::milliseconds updatePeriod)
- : updatePeriod(static_cast<float>(updatePeriod.count()) / 1000),
- targetAngles({0, 0, 0})
+Follower::Follower(std::chrono::milliseconds updPeriod)
+ : updatePeriod{updPeriod}, targetAngles({0, 0, 0})
{
}
@@ -168,9 +167,13 @@ void Follower::step()
// Calculate angular velocity for moving the antennas toward position
float horizontalSpeed =
- std::abs((diffAngles.yaw * 1000) / (360 * updatePeriod));
+ std::abs((diffAngles.yaw) /
+ (360 * (static_cast<float>(updatePeriod.count()) / 1000)));
+ TRACE("[Follower] horizontalSpeed is: %f\n", horizontalSpeed);
float verticalSpeed =
- std::abs((diffAngles.pitch * 1000) / (360 * updatePeriod));
+ std::abs((diffAngles.pitch) /
+ (360 * (static_cast<float>(updatePeriod.count()) / 1000)));
+ TRACE("[Follower] Vertical speed is: %f\n", horizontalSpeed);
// Update the state of the follower
FollowerState newState;
diff --git a/src/shared/algorithms/Follower/Follower.h b/src/shared/algorithms/Follower/Follower.h
index 3a6fcda47..850a63f90 100644
--- a/src/shared/algorithms/Follower/Follower.h
+++ b/src/shared/algorithms/Follower/Follower.h
@@ -144,8 +144,8 @@ private:
*/
Eigen::Vector3f getRocketNASOrigin();
- // actuation update period [s]
- float updatePeriod;
+ // actuation update period [ms]
+ std::chrono::milliseconds updatePeriod;
// Initialization flag
std::atomic<bool> isInit{false};
--
GitLab