diff --git a/src/shared/algorithms/Follower/Follower.cpp b/src/shared/algorithms/Follower/Follower.cpp
index b34ce5d0f4d41bd6dc794103f8a9f0d9664e0265..6c9e71d281e415f2a7d637c283c303e12d7561c1 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 3c09112ec19e6716e2112bb521f0010eb31c8b3e..3a6fcda47b1208489d65e8c3429fb69f4a1c828b 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 9076981b930bc85739afef672470ed42626992d6..8fa10602dffb0ab12f8e11f5485cd8a857b98f91 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;