1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 17:25:20 +03:00

Refactor sat count checks and GPS trust code

single minimum GPS satellite setting
single required GPS satellite setting
CLI Baro vs GPS trust user interface
GPS trust refactoring
allow arming with GPS_FIX even if not enough sats
required sats must be present to arm
set required sat count to 8
add blackbox headers
This commit is contained in:
ctzsnooze 2022-07-04 22:21:39 +10:00
parent 9dc2f127b8
commit 09ee27cd97
28 changed files with 180 additions and 120 deletions

View file

@ -58,12 +58,11 @@ typedef enum {
GPS_ONLY
} altSource_e;
PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 2);
PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 3);
PG_RESET_TEMPLATE(positionConfig_t, positionConfig,
.altSource = DEFAULT,
.altNumSatsGpsUse = POSITION_DEFAULT_ALT_NUM_SATS_GPS_USE,
.altNumSatsBaroFallback = POSITION_DEFAULT_ALT_NUM_SATS_BARO_FALLBACK,
.altPreferBaro = 100,
);
#ifdef USE_VARIO
@ -82,23 +81,20 @@ static bool altitudeOffsetSetGPS = false;
void calculateEstimatedAltitude()
{
static float baroAltOffset = 0;
static float gpsAltOffset = 0;
float baroAlt = 0;
float baroAltCm = 0;
float gpsAltCm = 0;
float baroAltVelocity = 0;
float gpsAlt = 0;
uint8_t gpsNumSat = 0;
#if defined(USE_GPS) && defined(USE_VARIO)
float gpsVertSpeed = 0;
#endif
float gpsTrust = 0.3; //conservative default
bool haveBaroAlt = false;
bool haveGpsAlt = false;
#if defined(USE_GPS) && defined(USE_VARIO)
float gpsVertSpeed = 0;
#endif
// *** Set baroAlt once its calibration is complete (cannot arm until it is)
#ifdef USE_BARO
if (sensors(SENSOR_BARO)) {
static float lastBaroAlt = 0;
static float lastBaroAltCm = 0;
static bool initBaroFilter = false;
if (!initBaroFilter) {
const float cutoffHz = barometerConfig()->baro_vario_lpf / 100.0f;
@ -107,96 +103,133 @@ void calculateEstimatedAltitude()
pt2FilterInit(&baroDerivativeLpf, gain);
initBaroFilter = true;
}
baroAlt = baroUpsampleAltitude();
const float baroAltVelocityRaw = (baroAlt - lastBaroAlt) * TASK_ALTITUDE_RATE_HZ; // cm/s
baroAltCm = baroUpsampleAltitude();
const float baroAltVelocityRaw = (baroAltCm - lastBaroAltCm) * TASK_ALTITUDE_RATE_HZ; // cm/s
baroAltVelocity = pt2FilterApply(&baroDerivativeLpf, baroAltVelocityRaw);
lastBaroAlt = baroAlt;
lastBaroAltCm = baroAltCm;
if (baroIsCalibrated()) {
haveBaroAlt = true;
}
}
#endif
// *** Check GPS for 3D fix, set haveGpsAlt if we have a 3D fix, and GPS Trust based on hdop, or leave at default of 0.3
#ifdef USE_GPS
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
gpsAlt = gpsSol.llh.altCm;
gpsNumSat = gpsSol.numSat;
// Need a 3D fix, which requires min 4 sats
// if not, gpsAltCm remains zero, haveGpsAlt stays false, and gpsTrust is zero.
gpsAltCm = gpsSol.llh.altCm;
#ifdef USE_VARIO
gpsVertSpeed = GPS_verticalSpeedInCmS;
#endif
haveGpsAlt = true;
haveGpsAlt = true; // remains false if no 3D fix
#ifdef USE_GPS
if (gpsSol.hdop != 0) {
gpsTrust = 100.0 / gpsSol.hdop;
// *** TO DO - investigate if we should use vDOP or vACC with UBlox units; this hDOP value is actually pDOP in UBLox code !!!
}
// always use at least 10% of other sources besides gps if available
#endif
// always use at least 10% of other sources besides gps if available; limit effect of HDOP
gpsTrust = MIN(gpsTrust, 0.9f);
// With a 3D fix, GPS trust starts at 0.3
} else {
gpsTrust = 0.0f; // don't trust GPS if no sensor or 3D fix
}
#endif
if (ARMING_FLAG(ARMED) && !altitudeOffsetSetBaro) {
baroAltOffset = baroAlt;
altitudeOffsetSetBaro = true;
} else if (!ARMING_FLAG(ARMED) && altitudeOffsetSetBaro) {
altitudeOffsetSetBaro = false;
}
baroAlt -= baroAltOffset;
int goodGpsSats = 0;
int badGpsSats = -1;
if (haveBaroAlt) {
goodGpsSats = positionConfig()->altNumSatsGpsUse;
badGpsSats = positionConfig()->altNumSatsBaroFallback;
}
// *** Zero Baro Altitude on arming (every time we re-arm, also)
// note that arming is blocked until baro 'calibration' (baro ground zeroing) is complete, no need to check status of haveBaroAlt
// this code adds a secondary zeroing to whatever baro altitude value exists on arming
// since props spin on arming, we want the last value before arming
// provided that this function is called before significant motor spin-up has occured, this may not be a big problem
#ifdef USE_BARO
static float baroAltOffsetCm = 0;
if (ARMING_FLAG(ARMED)) {
if (!altitudeOffsetSetGPS && gpsNumSat >= goodGpsSats) {
gpsAltOffset = gpsAlt - baroAlt;
baroAltCm -= baroAltOffsetCm; // use the last offset value from the disarm period
altitudeOffsetSetBaro = true; // inevitable, but needed if no GPS
} else {
baroAltOffsetCm = baroAltCm; // while disarmed, keep capturing current altitude to zero any offset once armed
}
#endif
// *** Zero GPS Altitude on every arm or re-arm using most recent disarmed values
// but do not use GPS if there are not the required satellites
// note that if GPS Rescue is enabled, a Home fix is required and reset when the user arms, so the checks below aren't needed
// tryArm() runs code that sets GPS_FIX_HOME, and if the result is no home fix, arming is blocked
// a check like this is essential for GPS Rescue
// if an altitude hold function was enabled, the same test would be good for ensuring a reasonable altitude estimate before takeoff
// If GPS Rescue is not enabled, the user can arm without a Home fix, or indeed without a 3D fix, with GPS active
// While disarmed, we use the current GPS value, regardless of GPS quality
// Also while disarmed, we monitor for a 3D fix and at least the required number of satellites
// If these are achieved before arming, we lock the offset at the gpsAltitude value
// If we don't, we wait until we get a 3D fix, and then correct the offset using the baro value
// This won't be very accurate, but all we need is a 3D fix.
// Note that without the 3D fix, GPS trust will be zero, and on getting a 3D fix, will depend on hDOP
#ifdef USE_GPS
static float gpsAltOffsetCm = 0;
if (ARMING_FLAG(ARMED)) {
gpsAltCm -= gpsAltOffsetCm; // while armed, use the last offset value from the disarm period
if (!altitudeOffsetSetGPS && haveBaroAlt && haveGpsAlt) {
// if we get a 3D fix after not getting a decent altitude offset, zero GPS to the Baro reading
gpsAltOffsetCm = gpsAltCm - baroAltCm; // set GPS to Baro altitude once we get a GPS fix
altitudeOffsetSetGPS = true;
} else if (gpsNumSat <= badGpsSats) {
altitudeOffsetSetGPS = false;
}
} else if (!ARMING_FLAG(ARMED) && altitudeOffsetSetGPS) {
altitudeOffsetSetGPS = false;
} else {
gpsAltOffsetCm = gpsAltCm; // while disarmed, keep capturing current altitude, to zero any offset once armed
altitudeOffsetSetGPS = (haveGpsAlt && gpsConfig()->gpsRequiredSats);
// if no 3D fix and not enough sats, set the GPS offset flag to false, and use baro to correct once we get a fix
}
#endif
gpsAlt -= gpsAltOffset;
if (!altitudeOffsetSetGPS) {
haveGpsAlt = false;
gpsTrust = 0.0f;
// *** adjust gpsTrust, favouring Baro increasingly when there is a discrepancy of more than a meter
// favour GPS if Baro reads negative, this happens due to ground effects
float gpsTrustModifier = gpsTrust;
const float absDifferenceM = fabsf(gpsAltCm - baroAltCm) * positionConfig()->altPreferBaro / 10000.0f;
if (absDifferenceM > 1.0f && baroAltCm > -100.0f) { // significant difference, and baro altitude not negative
gpsTrustModifier /= absDifferenceM;
}
// eg if discrepancy is 3m and GPS trust was 0.9, it would now be 0.3
// *** If we have a GPS with 3D fix and a Baro signal, blend them
if (haveGpsAlt && haveBaroAlt && positionConfig()->altSource == DEFAULT) {
if (ARMING_FLAG(ARMED)) {
estimatedAltitudeCm = gpsAlt * gpsTrust + baroAlt * (1 - gpsTrust);
estimatedAltitudeCm = gpsAltCm * gpsTrustModifier + baroAltCm * (1 - gpsTrustModifier);
} else {
estimatedAltitudeCm = gpsAlt; //absolute altitude is shown before arming, ignore baro
estimatedAltitudeCm = gpsAltCm;
//absolute GPS altitude is shown before arming, ignoring baro (I have no clue why this is)
}
#ifdef USE_VARIO
// baro is a better source for vario, so ignore gpsVertSpeed
estimatedVario = calculateEstimatedVario(baroAltVelocity);
#endif
// *** If we have a GPS but no baro, and are in Default or GPS_ONLY modes, use GPS values
} else if (haveGpsAlt && (positionConfig()->altSource == GPS_ONLY || positionConfig()->altSource == DEFAULT )) {
estimatedAltitudeCm = gpsAlt;
estimatedAltitudeCm = gpsAltCm;
#if defined(USE_VARIO) && defined(USE_GPS)
estimatedVario = gpsVertSpeed;
#endif
// *** If we have a Baro, and can work with it in Default or Baro Only modes
} else if (haveBaroAlt && (positionConfig()->altSource == BARO_ONLY || positionConfig()->altSource == DEFAULT)) {
estimatedAltitudeCm = baroAlt;
estimatedAltitudeCm = baroAltCm;
#ifdef USE_VARIO
estimatedVario = calculateEstimatedVario(baroAltVelocity); // cm/s
#endif
}
// Note that if we have no GPS but user chooses GPS Only, or no Baro but user chooses Baro only, then the reported altitude will be zero
// The latter may cause GPS rescue to fail, but the user should notice an absence of altitude values.
DEBUG_SET(DEBUG_ALTITUDE, 0, (int32_t)(100 * gpsTrust));
DEBUG_SET(DEBUG_ALTITUDE, 1, baroAlt);
DEBUG_SET(DEBUG_ALTITUDE, 2, gpsAlt);
#ifdef USE_VARIO
DEBUG_SET(DEBUG_ALTITUDE, 3, estimatedVario);
#endif
DEBUG_SET(DEBUG_ALTITUDE, 1, baroAltCm);
DEBUG_SET(DEBUG_ALTITUDE, 2, gpsAltCm);
DEBUG_SET(DEBUG_ALTITUDE, 3, estimatedAltitudeCm);
}
bool isAltitudeOffset(void)