diff --git a/src/shared/algorithms/Follower/Follower.cpp b/src/shared/algorithms/Follower/Follower.cpp
index 18c0575b5c3510a4e02e9173ba5686f38d7dd9e1..254f09ab23baa0ae7967484e9b5b2e33cb16616d 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 2f84b00e31af71808150ade8e01fe5d2d82326a3..3c09112ec19e6716e2112bb521f0010eb31c8b3e 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 92ddec6388c5eb592ca6df670c372cc3787a8005..59a1b7c2ba9869ed94963fb831720df07852eb9b 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,