diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index ec5aaa11a2..0c3e236dc0 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -455,6 +455,10 @@ static const char * const lookupTableGyroFilterDebug[] = { "ROLL", "PITCH", "YAW" }; +static const char * const lookupTablePositionAltSource[] = { + "DEFAULT", "BARO_ONLY", "GPS_ONLY" +}; + #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) } const lookupTableEntry_t lookupTables[] = { @@ -566,6 +570,8 @@ const lookupTableEntry_t lookupTables[] = { #endif LOOKUP_TABLE_ENTRY(lookupTableGyroFilterDebug), + + LOOKUP_TABLE_ENTRY(lookupTablePositionAltSource) }; #undef LOOKUP_TABLE_ENTRY @@ -1445,6 +1451,9 @@ const clivalue_t valueTable[] = { #ifdef USE_OSD { "display_name", VAR_UINT8 | MASTER_VALUE | MODE_STRING, .config.string = { 1, MAX_NAME_LENGTH, STRING_FLAGS_NONE }, PG_PILOT_CONFIG, offsetof(pilotConfig_t, displayName) }, #endif + +// PG_POSITION + { "position_alt_source", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_POSITION_ALT_SOURCE }, PG_POSITION, offsetof(positionConfig_t, altSource) }, }; const uint16_t valueTableEntryCount = ARRAYLEN(valueTable); diff --git a/src/main/cli/settings.h b/src/main/cli/settings.h index 9cb290d1db..b6e51671c4 100644 --- a/src/main/cli/settings.h +++ b/src/main/cli/settings.h @@ -133,6 +133,7 @@ typedef enum { TABLE_LEDSTRIP_COLOR, #endif TABLE_GYRO_FILTER_DEBUG, + TABLE_POSITION_ALT_SOURCE, LOOKUP_TABLE_COUNT } lookupTableIndex_e; diff --git a/src/main/flight/position.c b/src/main/flight/position.c index 9c3bca5851..3ba151abac 100644 --- a/src/main/flight/position.c +++ b/src/main/flight/position.c @@ -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,24 +144,27 @@ if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { baroAlt -= baroAltOffset; gpsAlt -= gpsAltOffset; - if (haveGpsAlt && haveBaroAlt) { + + if (haveGpsAlt && haveBaroAlt && positionConfig()->altSource == DEFAULT) { 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) { + } else if (haveGpsAlt && (positionConfig()->altSource == GPS_ONLY || positionConfig()->altSource == DEFAULT )) { estimatedAltitudeCm = gpsAlt; #if defined(USE_VARIO) && defined(USE_GPS) estimatedVario = gpsVertSpeed; #endif - } else if (haveBaroAlt) { + } else if (haveBaroAlt && (positionConfig()->altSource == BARO_ONLY || positionConfig()->altSource == DEFAULT)) { 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); DEBUG_SET(DEBUG_ALTITUDE, 2, gpsAlt); diff --git a/src/main/flight/position.h b/src/main/flight/position.h index bf821ca6a7..ac528848aa 100644 --- a/src/main/flight/position.h +++ b/src/main/flight/position.h @@ -22,6 +22,12 @@ #include "common/time.h" +typedef struct positionConfig_s { + uint8_t altSource; +} positionConfig_t; + +PG_DECLARE(positionConfig_t, positionConfig); + bool isAltitudeOffset(void); void calculateEstimatedAltitude(timeUs_t currentTimeUs); int32_t getEstimatedAltitudeCm(void); diff --git a/src/main/pg/pg_ids.h b/src/main/pg/pg_ids.h index 9abc9712aa..21c349a0b2 100644 --- a/src/main/pg/pg_ids.h +++ b/src/main/pg/pg_ids.h @@ -79,6 +79,7 @@ #define PG_IBUS_TELEMETRY_CONFIG 53 // CF 1.x //#define PG_VTX_CONFIG 54 // CF 1.x #define PG_GPS_RESCUE 55 // struct OK +#define PG_POSITION 56 // Driver configuration #define PG_DRIVER_PWM_RX_CONFIG 100 // does not exist in betaflight