diff --git a/src/main/flight/autopilot.c b/src/main/flight/autopilot.c index 75da523b67..7cf49aed35 100644 --- a/src/main/flight/autopilot.c +++ b/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; diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index 5a4e81e764..6139d315d6 100644 --- a/src/main/flight/gps_rescue.c +++ b/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)); } diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h index 0128ec887c..c2e60e34b8 100644 --- a/src/main/flight/gps_rescue.h +++ b/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); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 9691d2edda..2248ad98e5 100644 --- a/src/main/io/gps.c +++ b/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) diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 077c8f7e66..1fe7c4c096 100644 --- a/src/main/io/gps.h +++ b/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); diff --git a/src/test/unit/althold_unittest.cc b/src/test/unit/althold_unittest.cc index 03b62b7bed..56df1c9468 100644 --- a/src/test/unit/althold_unittest.cc +++ b/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; } diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index 2011166ca5..be76c6d9d2 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/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; + } }