From d065926770637865877b3dbf80108f165cdf7f9b 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] [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