rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Fix BARO altitude display.

This commit is contained in:
mikeller 2018-05-06 20:01:41 +12:00
parent 851ed5f597
commit eb8098e933
1 changed files with 15 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/main/flight/position.c
View File

@ -47,6 +47,10 @@ static int32_t estimatedAltitude = 0; // in cm
void calculateEstimatedAltitude(timeUs_t currentTimeUs)
{
static timeUs_t previousTimeUs = 0;
static int32_t baroAltOffset = 0;
static int32_t gpsAltOffset = 0;
static bool altitudeOffsetSet = false;
const uint32_t dTime = currentTimeUs - previousTimeUs;
if (dTime < BARO_UPDATE_FREQUENCY_40HZ) {
return;
@ -54,13 +58,11 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
previousTimeUs = currentTimeUs;
int32_t baroAlt = 0;
static int32_t baroAltOffset = 0;
static int32_t gpsAltOffset = 0;
int32_t gpsAlt = 0;
float gpsTrust = 0.3; //conservative default
bool haveBaroAlt = false;
bool haveGPSAlt = false;
bool haveGpsAlt = false;
#ifdef USE_BARO
if (sensors(SENSOR_BARO)) {
if (!isBaroCalibrationComplete()) {
@ -75,7 +77,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
#ifdef USE_GPS
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
gpsAlt = gpsSol.llh.alt;
haveGPSAlt = true;
haveGpsAlt = true;
if (gpsSol.hdop != 0) {
gpsTrust = 100.0 / gpsSol.hdop;
@ -85,17 +87,22 @@ if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
}
#endif
if (!ARMING_FLAG(ARMED)) {
if (ARMING_FLAG(ARMED) && !altitudeOffsetSet) {
baroAltOffset = baroAlt;
gpsAltOffset = gpsAlt;
altitudeOffsetSet = true;
} else if (!ARMING_FLAG(ARMED) && altitudeOffsetSet) {
altitudeOffsetSet = false;
}
baroAlt -= baroAltOffset;
gpsAlt -= gpsAltOffset;
if (haveGPSAlt && haveBaroAlt) {
estimatedAltitude = gpsAlt * gpsTrust + baroAlt * (1-gpsTrust);
} else if (haveGPSAlt && !haveBaroAlt) {
if (haveGpsAlt && haveBaroAlt) {
estimatedAltitude = gpsAlt * gpsTrust + baroAlt * (1 - gpsTrust);
} else if (haveGpsAlt) {
estimatedAltitude = gpsAlt;
} else if (haveBaroAlt) {
estimatedAltitude = baroAlt;
}
DEBUG_SET(DEBUG_ALTITUDE, 0, (int32_t)(100 * gpsTrust));