1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-20 14:55:21 +03:00
This commit is contained in:
Filipp Bakanov 2022-01-28 15:14:29 +03:00
parent 3670b7d410
commit ca0f25b265
No known key found for this signature in database
GPG key ID: A8B7CC1737CDF1D2
3 changed files with 20 additions and 14 deletions

View file

@ -1718,8 +1718,6 @@ const clivalue_t valueTable[] = {
// PG_POSITION // PG_POSITION
{ "position_alt_source", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_POSITION_ALT_SOURCE }, PG_POSITION, offsetof(positionConfig_t, altSource) }, { "position_alt_source", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_POSITION_ALT_SOURCE }, PG_POSITION, offsetof(positionConfig_t, altSource) },
{ "position_gps_alt_min_sats", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_POSITION, offsetof(positionConfig_t, gpsAltMinSats) },
// PG_MODE_ACTIVATION_CONFIG // PG_MODE_ACTIVATION_CONFIG
#if defined(USE_CUSTOM_BOX_NAMES) #if defined(USE_CUSTOM_BOX_NAMES)
{ "box_user_1_name", VAR_UINT8 | HARDWARE_VALUE | MODE_STRING, .config.string = { 1, MAX_BOX_USER_NAME_LENGTH, STRING_FLAGS_NONE }, PG_MODE_ACTIVATION_CONFIG, offsetof(modeActivationConfig_t, box_user_1_name) }, { "box_user_1_name", VAR_UINT8 | HARDWARE_VALUE | MODE_STRING, .config.string = { 1, MAX_BOX_USER_NAME_LENGTH, STRING_FLAGS_NONE }, PG_MODE_ACTIVATION_CONFIG, offsetof(modeActivationConfig_t, box_user_1_name) },

View file

@ -55,8 +55,7 @@ typedef enum {
PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 1); PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 1);
PG_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_RESET_TEMPLATE(positionConfig_t, positionConfig,
.altSource = DEFAULT, .altSource = DEFAULT,
.gpsAltMinSats = 9,
); );
static int32_t estimatedAltitudeCm = 0; // in cm static int32_t estimatedAltitudeCm = 0; // in cm
@ -87,7 +86,7 @@ int16_t calculateEstimatedVario(int32_t baroAlt, const uint32_t dTime) {
#endif #endif
#if defined(USE_BARO) || defined(USE_GPS) #if defined(USE_BARO) || defined(USE_GPS)
static bool altitudeOffsetSet = false; static bool altitudeOffsetSetBaro = false;
static bool altitudeOffsetSetGPS = false; static bool altitudeOffsetSetGPS = false;
void calculateEstimatedAltitude(timeUs_t currentTimeUs) void calculateEstimatedAltitude(timeUs_t currentTimeUs)
@ -141,20 +140,29 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
} }
#endif #endif
if (ARMING_FLAG(ARMED) && !altitudeOffsetSet) { if (ARMING_FLAG(ARMED) && !altitudeOffsetSetBaro) {
baroAltOffset = baroAlt; baroAltOffset = baroAlt;
gpsAltOffset = gpsAlt; altitudeOffsetSetBaro = true;
altitudeOffsetSet = true; } else if (!ARMING_FLAG(ARMED) && altitudeOffsetSetBaro) {
} else if (!ARMING_FLAG(ARMED) && altitudeOffsetSet) { altitudeOffsetSetBaro = false;
altitudeOffsetSet = false;
} }
baroAlt -= baroAltOffset; baroAlt -= baroAltOffset;
if (ARMING_FLAG(ARMED) && !altitudeOffsetSetGPS) { int goodGpsSats = 999;
if (haveBaroAlt && gpsNumSat >= positionConfig()->gpsAltMinSats && altitudeOffsetSet) { int badGpsSats = 0;
if (haveBaroAlt) {
goodGpsSats = 10;
badGpsSats = 7;
}
if (ARMING_FLAG(ARMED)) {
if (!altitudeOffsetSetGPS && gpsNumSat >= goodGpsSats) {
gpsAltOffset = gpsAlt - baroAlt; gpsAltOffset = gpsAlt - baroAlt;
altitudeOffsetSetGPS = true; altitudeOffsetSetGPS = true;
} else if (gpsNumSat <= badGpsSats) {
altitudeOffsetSetGPS = false;
} }
} else if (!ARMING_FLAG(ARMED) && altitudeOffsetSetGPS) { } else if (!ARMING_FLAG(ARMED) && altitudeOffsetSetGPS) {
altitudeOffsetSetGPS = false; altitudeOffsetSetGPS = false;
@ -164,6 +172,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
if (!altitudeOffsetSetGPS) { if (!altitudeOffsetSetGPS) {
haveGpsAlt = false; haveGpsAlt = false;
gpsTrust = 0.0f;
} }
if (haveGpsAlt && haveBaroAlt && positionConfig()->altSource == DEFAULT) { if (haveGpsAlt && haveBaroAlt && positionConfig()->altSource == DEFAULT) {
@ -200,7 +209,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
bool isAltitudeOffset(void) bool isAltitudeOffset(void)
{ {
return altitudeOffsetSet; return altitudeOffsetSetBaro || altitudeOffsetSetGPS;
} }
#endif #endif

View file

@ -24,7 +24,6 @@
typedef struct positionConfig_s { typedef struct positionConfig_s {
uint8_t altSource; uint8_t altSource;
uint8_t gpsAltMinSats;
} positionConfig_t; } positionConfig_t;
PG_DECLARE(positionConfig_t, positionConfig); PG_DECLARE(positionConfig_t, positionConfig);