implement review from PL - thank you!

src/main/common/axis.h

@@ -42,6 +42,4 @@ typedef enum {
     AI_PITCH
 } angle_index_t;
 
-#define ANGLE_INDEX_COUNT 2
-
 #define GET_DIRECTION(isReversed) ((isReversed) ? -1 : 1)

src/main/flight/autopilot.c

@@ -67,9 +67,6 @@ typedef enum {
     NS
 } axisEF_t;
 
-earthFrame_t eastWest;
-earthFrame_t northSouth;
-
 typedef struct {
     float gpsDataIntervalS;
     float gpsDataFreqHz;
@@ -95,7 +92,7 @@ static posHoldState posHold = {
 };
 
 static gpsLocation_t currentTargetLocation = {0, 0, 0};
-float autopilotAngle[ANGLE_INDEX_COUNT];
+float autopilotAngle[2];
 
 void resetPositionControlEFParams(earthFrame_t *efAxis) {
     // at start only
@@ -117,8 +114,6 @@ void resetPositionControl(gpsLocation_t initialTargetLocation) {
 
 void autopilotInit(const autopilotConfig_t *config)
 {
-    eastWest = posHold.efAxis[EW];
-    northSouth = posHold.efAxis[NS];
     posHold.sticksActive = false;
     altitudePidCoeffs.Kp = config->altitude_P * ALTITUDE_P_SCALE;
     altitudePidCoeffs.Ki = config->altitude_I * ALTITUDE_I_SCALE;
@@ -191,7 +186,8 @@ void setSticksActiveStatus(bool areSticksActive)
 void setTargetLocation(gpsLocation_t newTargetLocation)
 {
     currentTargetLocation = newTargetLocation;
-    // note: will cause a large D and A spike if there is an abrupt change in distance
+    posHold.efAxis[EW].previousDistance = 0.0f; // reset to avoid D and A spikess
+    posHold.efAxis[NS].previousDistance = 0.0f;
     // function is intended for only small changes in position
     // for example, where the step distance change reflects an intended velocity, determined by a client function
     // if we had a 'target_ground_speed' value, like in gps_rescue, we can make a function that starts and stops smoothly and targets that velocity
@@ -208,8 +204,8 @@ void resetLocation(earthFrame_t *efAxis, axisEF_t loopAxis)
 }
 
 bool positionControl(void) {
-
-    if (isNewGPSDataAvailable()) {
+    static uint16_t gpsStamp = ~0;
+    if (isNewGPSDataAvailable(&gpsStamp)) {
         posHold.gpsDataIntervalS = getGpsDataIntervalSeconds(); // interval for current GPS data value 0.01s to 1.0s
         posHold.gpsDataFreqHz = 1.0f / posHold.gpsDataIntervalS;
 
@@ -280,13 +276,10 @@ bool positionControl(void) {
                 }
             }
 
-            // limit sum of D and A per axis because otherwise can be too aggressive when starting at speed
-     //       const float maxDAAngle = 35.0f; // limit in degrees; arbitrary.  20 is a bit too low, allows a lot of overshoot
-     //       const float pidDA = constrainf(pidD + pidA, -maxDAAngle, maxDAAngle);
-            // note: an angle of more than 35 degrees can still be achieved as P and I grow, and on diagonal
-            // note: angle mode limit is absolute max, as set in CLI default is 60
+            // limit sum of D and A per axis based on total DA vector length, otherwise can be too aggressive when starting at speed
+            // limit is 35 degrees from D and A alone, arbitrary value.  20 is a bit too low, allows a lot of overshoot
+            // note: an angle of more than 35 degrees can still be achieved as P and I grow
 
-            //total vector length for DA limiting, if needed:
             float pidDA = pidD + pidA;
             const float pidDASquared = pidDA * pidDA;
             float mag = sqrtf(pidDASquared + prevPidDASquared);

src/main/flight/autopilot.h

@@ -21,7 +21,7 @@
 #include "flight/pid.h"
 #include "io/gps.h"
 
-extern float autopilotAngle[ANGLE_INDEX_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES
+extern float autopilotAngle[2]; // NOTE: ANGLES ARE IN CENTIDEGREES
 
 void autopilotInit(const autopilotConfig_t *config);
 void resetAltitudeControl(void);

src/main/flight/gps_rescue.c

@@ -123,7 +123,7 @@ typedef struct {
 static const float taskIntervalSeconds = HZ_TO_INTERVAL(TASK_GPS_RESCUE_RATE_HZ); // i.e. 0.01 s
 static float rescueThrottle;
 static float rescueYaw;
-float gpsRescueAngle[ANGLE_INDEX_COUNT] = { 0, 0 };
+float gpsRescueAngle[2] = { 0, 0 };
 bool magForceDisable = false;
 static bool newGPSData = false;
 static pt1Filter_t velocityDLpf;

src/main/flight/gps_rescue.h

@@ -45,7 +45,7 @@ typedef enum {
     GPS_RESCUE_ALT_MODE_COUNT
 } gpsRescueAltitudeMode_e;
 
-extern float gpsRescueAngle[ANGLE_INDEX_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES
+extern float gpsRescueAngle[2]; // NOTE: ANGLES ARE IN CENTIDEGREES
 
 void gpsRescueInit(void);
 void gpsRescueUpdate(void);

src/main/flight/imu.c

@@ -609,7 +609,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
 #else
 
 #if defined(USE_GPS)
-static void isGpsHeadingUsable(float groundspeedGain, float imuCourseError, float dt)
+static void updateGpsHeadingUsable(float groundspeedGain, float imuCourseError, float dt)
 {
     if (!canUseGPSHeading) {
         static float gpsHeadingTruth = 0;
@@ -689,7 +689,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
             cogErr = imuCourseError * groundspeedGain;
             // cogErr is greater with larger heading errors and greater speed in straight pitch forward flight
 
-            isGpsHeadingUsable(groundspeedGain, imuCourseError, dt);
+            updateGpsHeadingUsable(groundspeedGain, imuCourseError, dt);
 
         } else if (gpsSol.groundSpeed > GPS_COG_MIN_GROUNDSPEED) {
             // Reset the reference and reinitialize quaternion factors when GPS groundspeed > GPS_COG_MIN_GROUNDSPEED

src/main/io/gps.c

@@ -2626,10 +2626,10 @@ void onGpsNewData(void)
 
 }
 
-bool isNewGPSDataAvailable(void) {
+bool isNewGPSDataAvailable(uint16_t* gpsStamp) {
     static uint16_t lastGpsStamp = ~0; // Initialize to an invalid state
-    if (lastGpsStamp != gpsStamp) {
-        lastGpsStamp = gpsStamp; // Update the last known stamp
+    if (lastGpsStamp != *gpsStamp) {
+        lastGpsStamp = *gpsStamp; // Update the last known stamp
         return true; // New GPS data is available
     }
     return false; // No new data

src/main/io/gps.h

@@ -393,6 +393,6 @@ void GPS_distances(const gpsLocation_t *from, const gpsLocation_t *to, float *pE
 
 void gpsSetFixState(bool state);
 float getGpsDataIntervalSeconds(void);      // sends GPS Nav Data interval to GPS Rescue
-bool isNewGPSDataAvailable(void);
+bool isNewGPSDataAvailable(uint16_t* gpsStamp);
 
 baudRate_e getGpsPortActualBaudRateIndex(void);

src/test/unit/althold_unittest.cc

@@ -114,9 +114,13 @@ extern "C" {
     float getAltitudeDerivative(void) {return 0.0f;}
     float getCosTiltAngle(void) { return 0.0f; }
     float getGpsDataIntervalSeconds(void) { return 0.01f; }//    gpsSolutionData_t gpsSol;
-    bool isNewGPSDataAvailable(void){ return true; }
     float rcCommand[4];
 
+    bool isNewGPSDataAvailable(uint16_t* gpsStamp) {
+        UNUSED(*gpsStamp);
+        return true;
+    }
+
     float vector2Norm(const vector2_t *v) {
        UNUSED(*v);
        return 0.0f;

src/test/unit/arming_prevention_unittest.cc

@@ -1196,5 +1196,9 @@ void GPS_distances(const gpsLocation_t *from, const gpsLocation_t *to, float *pE
 
     bool canUseGPSHeading;
     bool compassIsHealthy;
-    bool isNewGPSDataAvailable(void){ return true; }
+
+    bool isNewGPSDataAvailable(uint16_t* gpsStamp) {
+        UNUSED(*gpsStamp);
+        return true;
+    }
 }