1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 14:25:20 +03:00

add POSITION_ALT_SOURCE lookup table to enum

add cli setting to choose alt source
modify alt calculation
This commit is contained in:
Maciej Bunia 2019-04-24 10:21:46 +02:00 committed by Maciej Bunia
parent ee0ac209c3
commit fe4c3ac149
5 changed files with 60 additions and 12 deletions

View file

@ -41,6 +41,21 @@
#include "sensors/sensors.h"
#include "sensors/barometer.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
typedef enum {
DEFAULT = 0,
BARO_ONLY,
GPS_ONLY
} altSource_e;
PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 1);
PG_RESET_TEMPLATE(positionConfig_t, positionConfig,
.altSource = DEFAULT,
);
static int32_t estimatedAltitudeCm = 0; // in cm
#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
@ -129,23 +144,39 @@ if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
baroAlt -= baroAltOffset;
gpsAlt -= gpsAltOffset;
if (haveGpsAlt && haveBaroAlt) {
estimatedAltitudeCm = gpsAlt * gpsTrust + baroAlt * (1 - gpsTrust);
#ifdef USE_VARIO
// baro is a better source for vario, so ignore gpsVertSpeed
estimatedVario = calculateEstimatedVario(baroAlt, dTime);
#endif
} else if (haveGpsAlt) {
estimatedAltitudeCm = gpsAlt;
#if defined(USE_VARIO) && defined(USE_GPS)
estimatedVario = gpsVertSpeed;
#endif
} else if (haveBaroAlt) {
if(positionConfig()->altSource == BARO_ONLY){
estimatedAltitudeCm = baroAlt;
#ifdef USE_VARIO
estimatedVario = calculateEstimatedVario(baroAlt, dTime);
#endif
}
else if (positionConfig()->altSource == GPS_ONLY){
estimatedAltitudeCm = gpsAlt;
#if defined(USE_VARIO) && defined(USE_GPS)
estimatedVario = gpsVertSpeed;
#endif
}
else {
if (haveGpsAlt && haveBaroAlt) {
estimatedAltitudeCm = gpsAlt * gpsTrust + baroAlt * (1 - gpsTrust);
#ifdef USE_VARIO
// baro is a better source for vario, so ignore gpsVertSpeed
estimatedVario = calculateEstimatedVario(baroAlt, dTime);
#endif
} else if (haveGpsAlt) {
estimatedAltitudeCm = gpsAlt;
#if defined(USE_VARIO) && defined(USE_GPS)
estimatedVario = gpsVertSpeed;
#endif
} else if (haveBaroAlt) {
estimatedAltitudeCm = baroAlt;
#ifdef USE_VARIO
estimatedVario = calculateEstimatedVario(baroAlt, dTime);
#endif
}
}
DEBUG_SET(DEBUG_ALTITUDE, 0, (int32_t)(100 * gpsTrust));
DEBUG_SET(DEBUG_ALTITUDE, 1, baroAlt);