mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-20 14:55:21 +03:00
Refactor
This commit is contained in:
parent
3670b7d410
commit
ca0f25b265
3 changed files with 20 additions and 14 deletions
|
@ -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) },
|
||||||
|
|
|
@ -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
|
||||||
|
|
||||||
|
|
|
@ -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);
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue