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

Fixed wrong altitude in DEFAULT (GPS + BARO) mode

Browse source
This commit is contained in:
Filipp Bakanov 2022-01-17 17:31:06 +03:00
parent a6b9560b48
commit dc409c29fe
No known key found for this signature in database
GPG key ID: A8B7CC1737CDF1D2
3 changed files with 20 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/cli/settings.c
View file

@ -1714,6 +1714,7 @@ const clivalue_t valueTable[] = {
// PG_POSITION // PG_POSITION
{ "position_alt_source", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_POSITION_ALT_SOURCE }, PG_POSITION, offsetof(positionConfig_t, altSource) }, { "position_alt_source", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_POSITION_ALT_SOURCE }, PG_POSITION, offsetof(positionConfig_t, altSource) },
{ "position_gps_alt_min_sats", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_POSITION, offsetof(positionConfig_t, gpsAltMinSats) },
// PG_MODE_ACTIVATION_CONFIG // PG_MODE_ACTIVATION_CONFIG
#if defined(USE_CUSTOM_BOX_NAMES) #if defined(USE_CUSTOM_BOX_NAMES)

18
src/main/flight/position.c
View file

@ -56,6 +56,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 1
PG_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_RESET_TEMPLATE(positionConfig_t, positionConfig,
.altSource = DEFAULT, .altSource = DEFAULT,
.gpsAltMinSats = 9,
); );
static int32_t estimatedAltitudeCm = 0; // in cm static int32_t estimatedAltitudeCm = 0; // in cm
@ -87,6 +88,7 @@ int16_t calculateEstimatedVario(int32_t baroAlt, const uint32_t dTime) {
#if defined(USE_BARO) || defined(USE_GPS) #if defined(USE_BARO) || defined(USE_GPS)
static bool altitudeOffsetSet = false; static bool altitudeOffsetSet = false;
static bool altitudeOffsetSetGPS = false;
void calculateEstimatedAltitude(timeUs_t currentTimeUs) void calculateEstimatedAltitude(timeUs_t currentTimeUs)
{ {
@ -103,6 +105,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
int32_t baroAlt = 0; int32_t baroAlt = 0;
int32_t gpsAlt = 0; int32_t gpsAlt = 0;
uint16_t gpsNumSat = 0;
#if defined(USE_GPS) && defined(USE_VARIO) #if defined(USE_GPS) && defined(USE_VARIO)
int16_t gpsVertSpeed = 0; int16_t gpsVertSpeed = 0;
@ -124,6 +127,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
#ifdef USE_GPS #ifdef USE_GPS
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
gpsAlt = gpsSol.llh.altCm; gpsAlt = gpsSol.llh.altCm;
gpsNumSat = gpsSol.numSat;
#ifdef USE_VARIO #ifdef USE_VARIO
gpsVertSpeed = GPS_verticalSpeedInCmS; gpsVertSpeed = GPS_verticalSpeedInCmS;
#endif #endif
@ -144,9 +148,23 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
} else if (!ARMING_FLAG(ARMED) && altitudeOffsetSet) { } else if (!ARMING_FLAG(ARMED) && altitudeOffsetSet) {
altitudeOffsetSet = false; altitudeOffsetSet = false;
} }
baroAlt -= baroAltOffset; baroAlt -= baroAltOffset;
if (ARMING_FLAG(ARMED) && !altitudeOffsetSetGPS) {
if (haveBaroAlt && gpsNumSat >= positionConfig()->gpsAltMinSats && altitudeOffsetSet) {
gpsAltOffset = gpsAlt - baroAlt;
altitudeOffsetSetGPS = true;
}
} else if (!ARMING_FLAG(ARMED) && altitudeOffsetSetGPS) {
altitudeOffsetSetGPS = false;
}
gpsAlt -= gpsAltOffset; gpsAlt -= gpsAltOffset;
if (!altitudeOffsetSetGPS) {
haveGpsAlt = false;
}
if (haveGpsAlt && haveBaroAlt && positionConfig()->altSource == DEFAULT) { if (haveGpsAlt && haveBaroAlt && positionConfig()->altSource == DEFAULT) {
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {

1
src/main/flight/position.h
View file

@ -24,6 +24,7 @@
typedef struct positionConfig_s { typedef struct positionConfig_s {
uint8_t altSource; uint8_t altSource;
uint8_t gpsAltMinSats;
} positionConfig_t; } positionConfig_t;
PG_DECLARE(positionConfig_t, positionConfig); PG_DECLARE(positionConfig_t, positionConfig);