Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-20 06:45:16 +03:00
Code Issues Wiki Activity

Fix BARO altitude display.

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