Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 20:35:33 +03:00
Code Issues Wiki Activity

Calculate distance flown with a speed threshold

Browse source
This commit is contained in:
Tony Cabello 2018-11-29 06:11:06 +01:00
parent 8a4ea0785e
commit d87e40725e
1 changed files with 6 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -82,6 +82,8 @@ float dTnav; // Delta Time in milliseconds for navigation computatio
int16_t nav_takeoff_bearing; int16_t nav_takeoff_bearing;
navigationMode_e nav_mode = NAV_MODE_NONE; // Navigation mode navigationMode_e nav_mode = NAV_MODE_NONE; // Navigation mode
#define GPS_DISTANCE_FLOWN_MIN_GROUND_SPEED_THRESHOLD_CM_S 15 // 5.4Km/h 3.35mph
// moving average filter variables // moving average filter variables
#define GPS_FILTERING 1 // add a 5 element moving average filter to GPS coordinates, helps eliminate gps noise but adds latency #define GPS_FILTERING 1 // add a 5 element moving average filter to GPS coordinates, helps eliminate gps noise but adds latency
#ifdef GPS_FILTERING #ifdef GPS_FILTERING
@ -1316,28 +1318,21 @@ void GPS_calculateDistanceAndDirectionToHome(void)
static void GPS_calculateDistanceFlown(bool initialize) static void GPS_calculateDistanceFlown(bool initialize)
{ {
static int32_t lastCoord[2] = { 0, 0 }; static int32_t lastCoord[2] = { 0, 0 };
static int32_t lastMillis = 0;
if (initialize) { if (initialize) {
GPS_distanceFlownInCm = 0; GPS_distanceFlownInCm = 0;
lastMillis = millis();
lastCoord[LON] = gpsSol.llh.lon;
lastCoord[LAT] = gpsSol.llh.lat;
} else { } else {
int32_t currentMillis = millis(); // Only add up movement when speed is faster than minimum threshold
// update the calculation less frequently when accuracy is low, to mitigate adding up error if (gpsSol.groundSpeed > GPS_DISTANCE_FLOWN_MIN_GROUND_SPEED_THRESHOLD_CM_S) {
if ((currentMillis - lastMillis) > (10 * constrain(gpsSol.hdop, 100, 1000))) {
uint32_t dist; uint32_t dist;
int32_t dir; int32_t dir;
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; }
}
lastCoord[LON] = gpsSol.llh.lon; lastCoord[LON] = gpsSol.llh.lon;
lastCoord[LAT] = gpsSol.llh.lat; lastCoord[LAT] = gpsSol.llh.lat;
} }
}
}
void onGpsNewData(void) void onGpsNewData(void)
{ {