Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 17:25:20 +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

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,
.altSource = DEFAULT,
.gpsAltMinSats = 9,
);
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)
static bool altitudeOffsetSet = false;
static bool altitudeOffsetSetGPS = false;
void calculateEstimatedAltitude(timeUs_t currentTimeUs)
{
@ -103,6 +105,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
int32_t baroAlt = 0;
int32_t gpsAlt = 0;
uint16_t gpsNumSat = 0;
#if defined(USE_GPS) && defined(USE_VARIO)
int16_t gpsVertSpeed = 0;
@ -124,6 +127,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
#ifdef USE_GPS
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
gpsAlt = gpsSol.llh.altCm;
gpsNumSat = gpsSol.numSat;
#ifdef USE_VARIO
gpsVertSpeed = GPS_verticalSpeedInCmS;
#endif
@ -144,9 +148,23 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
} else if (!ARMING_FLAG(ARMED) && altitudeOffsetSet) {
altitudeOffsetSet = false;
}
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;
if (!altitudeOffsetSetGPS) {
haveGpsAlt = false;
}
if (haveGpsAlt && haveBaroAlt && positionConfig()->altSource == DEFAULT) {
if (ARMING_FLAG(ARMED)) {