improved gpsStamp thanks PL

Also cleanup names and notes

src/main/flight/autopilot.c

@@ -222,9 +222,8 @@ void resetLocation(earthFrame_t *efAxis, axisEF_t loopAxis)
 
 bool positionControl(void) 
 {
-    static uint16_t previousGpsStamp = 0;
-    if (getGpsStamp() != previousGpsStamp) {
-        previousGpsStamp = getGpsStamp();
+    static uint16_t gpsStamp = 0;
+    if (gpsHasNewData(&gpsStamp)) {
         posHold.gpsDataIntervalS = getGpsDataIntervalSeconds(); // interval for current GPS data value 0.01s to 1.0s
         posHold.gpsDataFreqHz = 1.0f / posHold.gpsDataIntervalS;
 

src/main/flight/gps_rescue.c

@@ -125,10 +125,11 @@ static float rescueThrottle;
 static float rescueYaw;
 float gpsRescueAngle[RP_AXIS_COUNT] = { 0, 0 };
 bool magForceDisable = false;
-static bool newGPSData = false;
+static bool newGpsData = false;
 static pt1Filter_t velocityDLpf;
 static pt3Filter_t velocityUpsampleLpf;
 
+
 rescueState_s rescueState;
 
 void gpsRescueInit(void)
@@ -145,14 +146,6 @@ void gpsRescueInit(void)
     pt3FilterInit(&velocityUpsampleLpf, gain);
 }
 
-/*
- If we have new GPS data, update home heading if possible and applicable.
-*/
-void gpsRescueNewGpsData(void)
-{
-    newGPSData = true;
-}
-
 static void rescueStart(void)
 {
     rescueState.phase = RESCUE_INITIALIZE;
@@ -175,7 +168,7 @@ static void setReturnAltitude(void)
     // While armed, but not during the rescue, update the max altitude value
     rescueState.intent.maxAltitudeCm = fmaxf(getAltitudeCm(), rescueState.intent.maxAltitudeCm);
 
-    if (newGPSData) {
+    if (newGpsData) {
         // set the target altitude to the current altitude.
         rescueState.intent.targetAltitudeCm = getAltitudeCm();
 
@@ -277,7 +270,7 @@ static void rescueAttainPosition(void)
         Pitch / velocity controller
     */
     static float pitchAdjustment = 0.0f;
-    if (newGPSData) {
+    if (newGpsData) {
 
         const float sampleIntervalNormaliseFactor = rescueState.sensor.gpsDataIntervalSeconds * 10.0f;
 
@@ -513,7 +506,7 @@ static void sensorUpdate(void)
     DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 4, lrintf(attitude.values.yaw));                 // estimated heading of the quad (direction nose is pointing in)
     DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 5, lrintf(rescueState.sensor.directionToHome));  // angle to home derived from GPS location and home position
 
-    if (!newGPSData) {
+    if (!newGpsData) {
         return;
         // GPS ground speed, velocity and distance to home will be held at last good values if no new packets
     }
@@ -641,7 +634,7 @@ void disarmOnImpact(void)
 
 void descend(void)
 {
-    if (newGPSData) {
+    if (newGpsData) {
         // consider landing area to be a circle half landing height around home, to avoid overshooting home point
         const float distanceToLandingAreaM = rescueState.sensor.distanceToHomeM - (0.5f * autopilotConfig()->landing_altitude_m);
         const float proximityToLandingArea = constrainf(distanceToLandingAreaM / rescueState.intent.descentDistanceM, 0.0f, 1.0f);
@@ -720,9 +713,11 @@ void gpsRescueUpdate(void)
         rescueAttainPosition(); // Initialise basic parameters when a Rescue starts (can't initialise sensor data reliably)
         performSanityChecks(); // Initialises sanity check values when a Rescue starts
     }
-
     // Will now be in RESCUE_INITIALIZE mode, if just entered Rescue while IDLE, otherwise stays IDLE
 
+    static uint16_t gpsStamp = 0;
+    newGpsData = gpsHasNewData(&gpsStamp);
+
     sensorUpdate(); // always get latest GPS and Altitude data, update ascend and descend rates
 
     static bool returnAltitudeLow = true;
@@ -824,7 +819,7 @@ void gpsRescueUpdate(void)
         rescueState.intent.velocityPidCutoffModifier = 2.0f - rescueState.intent.velocityItermRelax;
         // higher velocity filter cutoff for initial few seconds to improve accuracy; can be smoother later
 
-        if (newGPSData) {
+        if (newGpsData) {
             // cut back on allowed angle if there is a high groundspeed error
             rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->maxRescueAngle;
             // introduce roll slowly and limit to half the max pitch angle; earth referenced yaw may add more roll via angle code
@@ -874,18 +869,16 @@ void gpsRescueUpdate(void)
 
     performSanityChecks();
     rescueAttainPosition();
-
-    newGPSData = false;
 }
 
 float gpsRescueGetYawRate(void)
 {
-    return rescueYaw;
+    return rescueYaw; // the control yaw value for rc.c to be used while flightMode gps_rescue is active.
 }
 
 float gpsRescueGetImuYawCogGain(void)
 {
-    return rescueState.sensor.imuYawCogGain;
+    return rescueState.sensor.imuYawCogGain; // to speed up the IMU orientation to COG when needed
 }
 
 bool gpsRescueIsConfigured(void)
@@ -895,11 +888,11 @@ bool gpsRescueIsConfigured(void)
 
 bool gpsRescueIsAvailable(void)
 {
-    return rescueState.isAvailable;
+    return rescueState.isAvailable; // flashes the warning when not available (low sats, etc)
 }
 
 bool gpsRescueIsDisabled(void)
-// used for OSD warning
+// used for OSD warning, needs review
 {
     return (!STATE(GPS_FIX_HOME));
 }

src/main/flight/gps_rescue.h

@@ -49,7 +49,6 @@ extern float gpsRescueAngle[RP_AXIS_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES
 
 void gpsRescueInit(void);
 void gpsRescueUpdate(void);
-void gpsRescueNewGpsData(void);
 float gpsRescueGetYawRate(void);
 bool gpsRescueIsConfigured(void);
 bool gpsRescueIsAvailable(void);

src/main/io/gps.c

@@ -93,7 +93,7 @@ GPS_svinfo_t GPS_svinfo[GPS_SV_MAXSATS_M8N];
 
 static serialPort_t *gpsPort;
 static float gpsDataIntervalSeconds;
-static uint16_t gpsStamp = ~0; // Initialize to an invalid state
+static uint16_t currentGpsStamp = 1; // logical timer for received position update
 
 typedef struct gpsInitData_s {
     uint8_t index;
@@ -2554,7 +2554,6 @@ void GPS_reset_home_position(void)
 }
 
 ////////////////////////////////////////////////////////////////////////////////////
-#define EARTH_ANGLE_TO_CM (111.3195f * 1000 * 100 / GPS_DEGREES_DIVIDER)  // latitude unit to cm at equator (111km/deg)
 // Get distance between two points in cm using spherical to Cartesian transform
 // One one latitude unit, or one longitude unit at the equator, equals 1.113195 cm.
 // Get bearing from pos1 to pos2, returns values with 0.01 degree precision
@@ -2608,7 +2607,7 @@ void onGpsNewData(void)
         return;
     }
 
-    gpsStamp++; // increment the stamp
+    currentGpsStamp++; // new GPS data available
 
     gpsDataIntervalSeconds = gpsSol.navIntervalMs / 1000.0f;
 
@@ -2617,18 +2616,21 @@ void onGpsNewData(void)
         GPS_calculateDistanceFlown(false);
     }
 
-#ifdef USE_GPS_RESCUE
-    gpsRescueNewGpsData();
-#endif
-
 #ifdef USE_GPS_LAP_TIMER
     gpsLapTimerNewGpsData();
 #endif // USE_GPS_LAP_TIMER
 
 }
 
-uint16_t getGpsStamp(void) {
-    return gpsStamp;
+// check if new data has been received since last check
+// client stamp should be initialized to 0, then gpsHasNewData will return true on first call
+bool gpsHasNewData(uint16_t* stamp) {
+    if (*stamp != currentGpsStamp) {
+        *stamp = currentGpsStamp;
+        return true;
+    } else {
+        return false;
+    }
 }
 
 void gpsSetFixState(bool state)

src/main/io/gps.h

@@ -31,9 +31,10 @@
 #include "pg/gps.h"
 
 #define GPS_DEGREES_DIVIDER 10000000L
+#define EARTH_ANGLE_TO_CM (111.3195f * 1000 * 100 / GPS_DEGREES_DIVIDER) // 1.113195 cm per latitude unit at the equator (111.3195km/deg)
 #define GPS_X 1
 #define GPS_Y 0
-#define GPS_MIN_SAT_COUNT 4      // number of sats to trigger low sat count sanity check
+#define GPS_MIN_SAT_COUNT 4     // number of sats to trigger low sat count sanity check
 
 #ifdef USE_GPS_UBLOX
 typedef enum {
@@ -282,8 +283,8 @@ typedef struct gpsData_s {
     uint32_t state_position;        // incremental variable for loops
     uint32_t state_ts;              // timestamp for last state_position increment
     uint8_t state;                  // GPS thread state. Used for detecting cable disconnects and configuring attached devices
-    uint8_t userBaudRateIndex;          // index into auto-detecting or current baudrate
-    uint8_t tempBaudRateIndex;          // index into auto-detecting or current baudrate
+    uint8_t userBaudRateIndex;      // index into auto-detecting or current baudrate
+    uint8_t tempBaudRateIndex;      // index into auto-detecting or current baudrate
 
     uint8_t ackWaitingMsgId;        // Message id when waiting for ACK
     ubloxAckState_e ackState;       // Ack State
@@ -301,7 +302,7 @@ typedef struct gpsData_s {
 extern gpsLocation_t GPS_home_llh;
 extern uint16_t GPS_distanceToHome;        // distance to home point in meters
 extern uint32_t GPS_distanceToHomeCm;      // distance to home point in cm
-extern int16_t GPS_directionToHome;        // direction to home or hol point in degrees
+extern int16_t GPS_directionToHome;        // direction to home point in degrees * 10
 extern uint32_t GPS_distanceFlownInCm;     // distance flown since armed in centimeters
 
 typedef enum {
@@ -390,9 +391,9 @@ void GPS_reset_home_position(void);
 void GPS_calc_longitude_scaling(int32_t lat);
 void GPS_distance_cm_bearing(const gpsLocation_t *from, const gpsLocation_t *to, bool dist3d, uint32_t *dist, int32_t *bearing);
 void GPS_distances(const gpsLocation_t *from, const gpsLocation_t *to, float *pEWDist, float *pNSDist);
-
 void gpsSetFixState(bool state);
+
+bool gpsHasNewData(uint16_t *stamp);
 float getGpsDataIntervalSeconds(void);      // sends GPS Nav Data interval to GPS Rescue
-uint16_t getGpsStamp(void);
 
 baudRate_e getGpsPortActualBaudRateIndex(void);

src/test/unit/althold_unittest.cc

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

src/test/unit/arming_prevention_unittest.cc

@@ -1173,7 +1173,7 @@ extern "C" {
     void getRcDeflectionAbs(void) {}
     uint32_t getCpuPercentageLate(void) { return 0; }
     bool crashFlipSuccessful(void) { return false; }
-    
+
     void GPS_distance_cm_bearing(const gpsLocation_t *from, const gpsLocation_t *to, bool dist3d, uint32_t *dist, int32_t *bearing)
     {
        UNUSED(from);
@@ -1182,7 +1182,8 @@ extern "C" {
        UNUSED(dist);
        UNUSED(bearing);
     }
-void GPS_distances(const gpsLocation_t *from, const gpsLocation_t *to, float *pEWDist, float *pNSDist)
+
+    void GPS_distances(const gpsLocation_t *from, const gpsLocation_t *to, float *pEWDist, float *pNSDist)
     {
        UNUSED(from);
        UNUSED(to);
@@ -1196,5 +1197,9 @@ void GPS_distances(const gpsLocation_t *from, const gpsLocation_t *to, float *pE
 
     bool canUseGPSHeading;
     bool compassIsHealthy;
-    uint16_t getGpsStamp(void){ return 0; }
+
+    bool gpsHasNewData(uint16_t* gpsStamp) {
+         UNUSED(*gpsStamp);
+         return true;
+     }
 }