Bug fix: absolute altitude before arming is now correct when using GPS and Baro
This commit is contained in:
parent
9c3d4603b7
commit
5209a83e6a
|
@ -146,7 +146,11 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
|
|||
|
||||
|
||||
if (haveGpsAlt && haveBaroAlt && positionConfig()->altSource == DEFAULT) {
|
||||
estimatedAltitudeCm = gpsAlt * gpsTrust + baroAlt * (1 - gpsTrust);
|
||||
if (ARMING_FLAG(ARMED)) {
|
||||
estimatedAltitudeCm = gpsAlt * gpsTrust + baroAlt * (1 - gpsTrust);
|
||||
} else {
|
||||
estimatedAltitudeCm = gpsAlt; //absolute altitude is shown before arming, ignore baro
|
||||
}
|
||||
#ifdef USE_VARIO
|
||||
// baro is a better source for vario, so ignore gpsVertSpeed
|
||||
estimatedVario = calculateEstimatedVario(baroAlt, dTime);
|
||||
|
|
Loading…
Reference in New Issue