src/shared/algorithms/Follower/Follower.cpp

@@ -149,7 +149,6 @@ void Follower::step()
 
         lastRocketNas = lastRocketNasState;
         vn300         = lastAntennaAttitude;
-    }
 
         // Local variable checks and updates
         // Getting the position of the rocket wrt the antennas in NED frame
@@ -161,6 +160,7 @@ void Follower::step()
         // 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);
 

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);
 

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;