mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-26 01:35:41 +03:00
Merge pull request #7113 from TonyBlit/gps_total_distance_hdop
Total Distance calculation fixes & cleanup
This commit is contained in:
commit
e24a4fe4de
2 changed files with 24 additions and 28 deletions
|
@ -79,7 +79,6 @@ uint16_t GPS_distanceToHome; // distance to home point in meters
|
||||||
int16_t GPS_directionToHome; // direction to home or hol point in degrees
|
int16_t GPS_directionToHome; // direction to home or hol point in degrees
|
||||||
uint32_t GPS_distanceFlownInCm; // distance flown since armed in centimeters
|
uint32_t GPS_distanceFlownInCm; // distance flown since armed in centimeters
|
||||||
float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read
|
float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read
|
||||||
int16_t actual_speed[2] = { 0, 0 };
|
|
||||||
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
|
||||||
|
|
||||||
|
@ -1310,34 +1309,29 @@ void GPS_calculateDistanceAndDirectionToHome(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
////////////////////////////////////////////////////////////////////////////////////
|
////////////////////////////////////////////////////////////////////////////////////
|
||||||
// Calculate our current speed vector and the distance flown from gps position data
|
// Calculate the distance flown from gps position data
|
||||||
//
|
//
|
||||||
static void GPS_calculateVelocityAndDistanceFlown(void)
|
static void GPS_calculateDistanceFlown(bool initialize)
|
||||||
{
|
{
|
||||||
static int16_t speed_old[2] = { 0, 0 };
|
static int32_t lastCoord[2] = { 0, 0 };
|
||||||
static int32_t last_coord[2] = { 0, 0 };
|
static int32_t lastMillis = 0;
|
||||||
static uint8_t init = 0;
|
|
||||||
|
|
||||||
if (init) {
|
if (initialize) {
|
||||||
float tmp = 1.0f / dTnav;
|
lastMillis = millis();
|
||||||
actual_speed[GPS_X] = (float)(gpsSol.llh.lon - last_coord[LON]) * GPS_scaleLonDown * tmp;
|
} else {
|
||||||
actual_speed[GPS_Y] = (float)(gpsSol.llh.lat - last_coord[LAT]) * tmp;
|
int32_t currentMillis = millis();
|
||||||
|
// update the calculation less frequently when accuracy is low, to mitigate adding up error
|
||||||
actual_speed[GPS_X] = (actual_speed[GPS_X] + speed_old[GPS_X]) / 2;
|
if ((currentMillis - lastMillis) > (10 * constrain(gpsSol.hdop, 100, 1000))) {
|
||||||
actual_speed[GPS_Y] = (actual_speed[GPS_Y] + speed_old[GPS_Y]) / 2;
|
uint32_t dist;
|
||||||
|
int32_t dir;
|
||||||
speed_old[GPS_X] = actual_speed[GPS_X];
|
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &lastCoord[LAT], &lastCoord[LON], &dist, &dir);
|
||||||
speed_old[GPS_Y] = actual_speed[GPS_Y];
|
GPS_distanceFlownInCm += dist;
|
||||||
|
lastMillis = currentMillis;
|
||||||
uint32_t dist;
|
}
|
||||||
int32_t dir;
|
|
||||||
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &last_coord[LAT], &last_coord[LON], &dist, &dir);
|
|
||||||
GPS_distanceFlownInCm += dist;
|
|
||||||
}
|
}
|
||||||
init = 1;
|
|
||||||
|
|
||||||
last_coord[LON] = gpsSol.llh.lon;
|
lastCoord[LON] = gpsSol.llh.lon;
|
||||||
last_coord[LAT] = gpsSol.llh.lat;
|
lastCoord[LAT] = gpsSol.llh.lat;
|
||||||
}
|
}
|
||||||
|
|
||||||
void onGpsNewData(void)
|
void onGpsNewData(void)
|
||||||
|
@ -1349,8 +1343,10 @@ void onGpsNewData(void)
|
||||||
if (!ARMING_FLAG(ARMED))
|
if (!ARMING_FLAG(ARMED))
|
||||||
DISABLE_STATE(GPS_FIX_HOME);
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
// Apply moving average filter to GPS data
|
// Apply moving average filter to GPS data
|
||||||
#if defined(GPS_FILTERING)
|
#if defined(GPS_FILTERING)
|
||||||
|
@ -1390,8 +1386,9 @@ void onGpsNewData(void)
|
||||||
dTnav = MIN(dTnav, 1.0f);
|
dTnav = MIN(dTnav, 1.0f);
|
||||||
|
|
||||||
GPS_calculateDistanceAndDirectionToHome();
|
GPS_calculateDistanceAndDirectionToHome();
|
||||||
// calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating
|
if (ARMING_FLAG(ARMED)) {
|
||||||
GPS_calculateVelocityAndDistanceFlown();
|
GPS_calculateDistanceFlown(false);
|
||||||
|
}
|
||||||
|
|
||||||
#ifdef USE_GPS_RESCUE
|
#ifdef USE_GPS_RESCUE
|
||||||
rescueNewGpsData();
|
rescueNewGpsData();
|
||||||
|
|
|
@ -129,7 +129,6 @@ extern uint32_t GPS_distanceFlownInCm; // distance flown since armed in cent
|
||||||
extern int16_t GPS_angle[ANGLE_INDEX_COUNT]; // it's the angles that must be applied for GPS correction
|
extern int16_t GPS_angle[ANGLE_INDEX_COUNT]; // it's the angles that must be applied for GPS correction
|
||||||
extern float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read
|
extern float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read
|
||||||
extern float GPS_scaleLonDown; // this is used to offset the shrinking longitude as we go towards the poles
|
extern float GPS_scaleLonDown; // this is used to offset the shrinking longitude as we go towards the poles
|
||||||
extern int16_t actual_speed[2];
|
|
||||||
extern int16_t nav_takeoff_bearing;
|
extern int16_t nav_takeoff_bearing;
|
||||||
// navigation mode
|
// navigation mode
|
||||||
typedef enum {
|
typedef enum {
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue