Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 13:25:30 +03:00
Code Issues Wiki Activity

GPS Rescue no longer forces a reset home position when disarmed

Browse source 
Home position is deinitialized on disarm at gpsUpdate, so it can't be missed
DistanceFlown is reset inside GPS_calculateDistanceFlown
DistanceFlown was not calculated properly
This commit is contained in:
Tony Cabello 2018-11-19 23:23:40 +01:00
parent e24a4fe4de
commit 3199c48ee9
3 changed files with 9 additions and 9 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/fc/core.c
View file

@ -277,7 +277,7 @@ void updateArmingStatus(void)
#ifdef USE_GPS_RESCUE #ifdef USE_GPS_RESCUE
if (isModeActivationConditionPresent(BOXGPSRESCUE)) { if (isModeActivationConditionPresent(BOXGPSRESCUE)) {
if (!gpsRescueConfig()->minSats || STATE(GPS_FIX_HOME) || ARMING_FLAG(WAS_EVER_ARMED)) { if (!gpsRescueConfig()->minSats || STATE(GPS_FIX) || ARMING_FLAG(WAS_EVER_ARMED)) {
unsetArmingDisabled(ARMING_DISABLED_GPS); unsetArmingDisabled(ARMING_DISABLED_GPS);
} else { } else {
setArmingDisabled(ARMING_DISABLED_GPS); setArmingDisabled(ARMING_DISABLED_GPS);

2
src/main/flight/gps_rescue.c
View file

@ -158,8 +158,6 @@ rescueState_s rescueState;
*/ */
void rescueNewGpsData(void) void rescueNewGpsData(void)
{ {
if (!ARMING_FLAG(ARMED))
GPS_reset_home_position();
newGPSData = true; newGPSData = true;
} }

14
src/main/io/gps.c
View file

@ -544,6 +544,9 @@ void gpsUpdate(timeUs_t currentTimeUs)
if (sensors(SENSOR_GPS)) { if (sensors(SENSOR_GPS)) {
updateGpsIndicator(currentTimeUs); updateGpsIndicator(currentTimeUs);
} }
if (!ARMING_FLAG(ARMED)) {
DISABLE_STATE(GPS_FIX_HOME);
}
#if defined(USE_GPS_RESCUE) #if defined(USE_GPS_RESCUE)
if (gpsRescueIsConfigured()) { if (gpsRescueIsConfigured()) {
updateGPSRescueState(); updateGPSRescueState();
@ -1272,7 +1275,6 @@ void GPS_reset_home_position(void)
GPS_home[LAT] = gpsSol.llh.lat; GPS_home[LAT] = gpsSol.llh.lat;
GPS_home[LON] = gpsSol.llh.lon; GPS_home[LON] = gpsSol.llh.lon;
GPS_calc_longitude_scaling(gpsSol.llh.lat); // need an initial value for distance and bearing calc GPS_calc_longitude_scaling(gpsSol.llh.lat); // need an initial value for distance and bearing calc
GPS_distanceFlownInCm = 0;
// Set ground altitude // Set ground altitude
ENABLE_STATE(GPS_FIX_HOME); ENABLE_STATE(GPS_FIX_HOME);
} }
@ -1317,7 +1319,10 @@ static void GPS_calculateDistanceFlown(bool initialize)
static int32_t lastMillis = 0; static int32_t lastMillis = 0;
if (initialize) { if (initialize) {
GPS_distanceFlownInCm = 0;
lastMillis = millis(); lastMillis = millis();
lastCoord[LON] = gpsSol.llh.lon;
lastCoord[LAT] = gpsSol.llh.lat;
} else { } else {
int32_t currentMillis = millis(); int32_t currentMillis = millis();
// update the calculation less frequently when accuracy is low, to mitigate adding up error // update the calculation less frequently when accuracy is low, to mitigate adding up error
@ -1327,11 +1332,11 @@ static void GPS_calculateDistanceFlown(bool initialize)
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &lastCoord[LAT], &lastCoord[LON], &dist, &dir); GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &lastCoord[LAT], &lastCoord[LON], &dist, &dir);
GPS_distanceFlownInCm += dist; GPS_distanceFlownInCm += dist;
lastMillis = currentMillis; lastMillis = currentMillis;
lastCoord[LON] = gpsSol.llh.lon;
lastCoord[LAT] = gpsSol.llh.lat;
} }
} }
lastCoord[LON] = gpsSol.llh.lon;
lastCoord[LAT] = gpsSol.llh.lat;
} }
void onGpsNewData(void) void onGpsNewData(void)
@ -1340,9 +1345,6 @@ void onGpsNewData(void)
return; return;
} }
if (!ARMING_FLAG(ARMED))
DISABLE_STATE(GPS_FIX_HOME);
if (!STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED)) { if (!STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED)) {
GPS_reset_home_position(); GPS_reset_home_position();
GPS_calculateDistanceFlown(true); GPS_calculateDistanceFlown(true);