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:
parent
ee0ac209c3
commit
fe4c3ac149
5 changed files with 60 additions and 12 deletions
|
@ -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);
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue