From 24e1993ed021122d439cc0b23d7478aaa54d72e5 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] [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