diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index cedd8368b1..20802d616f 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 @@ -1435,6 +1441,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 + +// + { "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..dfbf83a53f 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,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); 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