Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-14 20:10:15 +03:00
Code Issues Wiki Activity

added USE_GPS_FIX_ESTIMATION

Browse source 
fixed indentation
This commit is contained in:
Roman Lut 2023-08-09 09:48:46 +02:00
parent 2b9a5a6a8d
commit 599e45c48f
29 changed files with 626 additions and 397 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/fc/runtime_config.h
View file

@ -123,7 +123,9 @@ typedef enum {
NAV_MOTOR_STOP_OR_IDLE = (1 << 7), // navigation requests MOTOR_STOP or motor idle regardless of throttle stick, will only activate if MOTOR_STOP feature is available NAV_MOTOR_STOP_OR_IDLE = (1 << 7), // navigation requests MOTOR_STOP or motor idle regardless of throttle stick, will only activate if MOTOR_STOP feature is available
COMPASS_CALIBRATED = (1 << 8), COMPASS_CALIBRATED = (1 << 8),
ACCELEROMETER_CALIBRATED = (1 << 9), ACCELEROMETER_CALIBRATED = (1 << 9),
#ifdef USE_GPS_FIX_ESTIMATION
GPS_ESTIMATED_FIX = (1 << 10), GPS_ESTIMATED_FIX = (1 << 10),
#endif
NAV_CRUISE_BRAKING = (1 << 11), NAV_CRUISE_BRAKING = (1 << 11),
NAV_CRUISE_BRAKING_BOOST = (1 << 12), NAV_CRUISE_BRAKING_BOOST = (1 << 12),
NAV_CRUISE_BRAKING_LOCKED = (1 << 13), NAV_CRUISE_BRAKING_LOCKED = (1 << 13),

2
src/main/fc/settings.yaml
View file

@ -834,6 +834,7 @@ groups:
max: 600 max: 600
- name: failsafe_gps_fix_estimation_delay - name: failsafe_gps_fix_estimation_delay
description: "Controls whether waypoint mission is allowed to proceed with gps fix estimation. Sets the time delay in seconds between gps fix lost event and RTH activation. Minimum delay is 7 seconds. If set to -1 the mission will continue until the end. With default setting(7), waypoint mission is aborted and switched to RTH with 7 seconds delay. RTH is done with GPS Fix estimation." description: "Controls whether waypoint mission is allowed to proceed with gps fix estimation. Sets the time delay in seconds between gps fix lost event and RTH activation. Minimum delay is 7 seconds. If set to -1 the mission will continue until the end. With default setting(7), waypoint mission is aborted and switched to RTH with 7 seconds delay. RTH is done with GPS Fix estimation."
condition: USE_GPS_FIX_ESTIMATION
default_value: 7 default_value: 7
min: -1 min: -1
max: 600 max: 600
@ -2195,6 +2196,7 @@ groups:
type: bool type: bool
- name: inav_allow_gps_fix_estimation - name: inav_allow_gps_fix_estimation
description: "Defines if inav will estimate GPS fix with magnetometer and barometer on GPS outages. Enables navigation and RTH without GPS fix on fixed wing. Also see failsafe_gps_fix_estimation_delay." description: "Defines if inav will estimate GPS fix with magnetometer and barometer on GPS outages. Enables navigation and RTH without GPS fix on fixed wing. Also see failsafe_gps_fix_estimation_delay."
condition: USE_GPS_FIX_ESTIMATION
default_value: OFF default_value: OFF
field: allow_gps_fix_estimation field: allow_gps_fix_estimation
type: bool type: bool

21
src/main/flight/failsafe.c
View file

@ -82,7 +82,9 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
.failsafe_min_distance = SETTING_FAILSAFE_MIN_DISTANCE_DEFAULT, // No minimum distance for failsafe by default .failsafe_min_distance = SETTING_FAILSAFE_MIN_DISTANCE_DEFAULT, // No minimum distance for failsafe by default
.failsafe_min_distance_procedure = SETTING_FAILSAFE_MIN_DISTANCE_PROCEDURE_DEFAULT, // default minimum distance failsafe procedure .failsafe_min_distance_procedure = SETTING_FAILSAFE_MIN_DISTANCE_PROCEDURE_DEFAULT, // default minimum distance failsafe procedure
.failsafe_mission_delay = SETTING_FAILSAFE_MISSION_DELAY_DEFAULT, // Time delay before Failsafe activated during WP mission (s) .failsafe_mission_delay = SETTING_FAILSAFE_MISSION_DELAY_DEFAULT, // Time delay before Failsafe activated during WP mission (s)
#ifdef USE_GPS_FIX_ESTIMATION
.failsafe_gps_fix_estimation_delay = SETTING_FAILSAFE_GPS_FIX_ESTIMATION_DELAY_DEFAULT, // Time delay before Failsafe activated when GPS Fix estimation is allied .failsafe_gps_fix_estimation_delay = SETTING_FAILSAFE_GPS_FIX_ESTIMATION_DELAY_DEFAULT, // Time delay before Failsafe activated when GPS Fix estimation is allied
#endif
); );
typedef enum { typedef enum {
@ -355,7 +357,11 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
// Craft is closer than minimum failsafe procedure distance (if set to non-zero) // Craft is closer than minimum failsafe procedure distance (if set to non-zero)
// GPS must also be working, and home position set // GPS must also be working, and home position set
if (failsafeConfig()->failsafe_min_distance > 0 && if (failsafeConfig()->failsafe_min_distance > 0 &&
((sensors(SENSOR_GPS) && STATE(GPS_FIX)) || STATE(GPS_ESTIMATED_FIX)) && STATE(GPS_FIX_HOME)) { ((sensors(SENSOR_GPS) && STATE(GPS_FIX))
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && STATE(GPS_FIX_HOME)) {
// get the distance to the original arming point // get the distance to the original arming point
uint32_t distance = calculateDistanceToDestination(&original_rth_home); uint32_t distance = calculateDistanceToDestination(&original_rth_home);
@ -368,7 +374,7 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
return failsafeConfig()->failsafe_procedure; return failsafeConfig()->failsafe_procedure;
} }
#ifdef USE_GPS_FIX_ESTIMATION
bool checkGPSFixFailsafe(void) bool checkGPSFixFailsafe(void)
{ {
if (STATE(GPS_ESTIMATED_FIX) && (FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive()) && (failsafeConfig()->failsafe_gps_fix_estimation_delay >= 0)) { if (STATE(GPS_ESTIMATED_FIX) && (FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive()) && (failsafeConfig()->failsafe_gps_fix_estimation_delay >= 0)) {
@ -387,6 +393,7 @@ bool checkGPSFixFailsafe(void)
} }
return false; return false;
} }
#endif
void failsafeUpdateState(void) void failsafeUpdateState(void)
@ -418,9 +425,12 @@ void failsafeUpdateState(void)
failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
} }
#ifdef USE_GPS_FIX_ESTIMATION
if ( checkGPSFixFailsafe() ) { if ( checkGPSFixFailsafe() ) {
reprocessState = true; reprocessState = true;
} else if (!receivingRxDataAndNotFailsafeMode) { } else
#endif
if (!receivingRxDataAndNotFailsafeMode) {
if ((failsafeConfig()->failsafe_throttle_low_delay && (millis() > failsafeState.throttleLowPeriod)) || STATE(NAV_MOTOR_STOP_OR_IDLE)) { if ((failsafeConfig()->failsafe_throttle_low_delay && (millis() > failsafeState.throttleLowPeriod)) || STATE(NAV_MOTOR_STOP_OR_IDLE)) {
// JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds or waiting for launch // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds or waiting for launch
// Don't disarm at all if `failsafe_throttle_low_delay` is set to zero // Don't disarm at all if `failsafe_throttle_low_delay` is set to zero
@ -488,11 +498,14 @@ void failsafeUpdateState(void)
} else if (failsafeChooseFailsafeProcedure() != FAILSAFE_PROCEDURE_NONE) { // trigger new failsafe procedure if changed } else if (failsafeChooseFailsafeProcedure() != FAILSAFE_PROCEDURE_NONE) { // trigger new failsafe procedure if changed
failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED; failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
reprocessState = true; reprocessState = true;
} else { }
#ifdef USE_GPS_FIX_ESTIMATION
else {
if ( checkGPSFixFailsafe() ) { if ( checkGPSFixFailsafe() ) {
reprocessState = true; reprocessState = true;
} }
} }
#endif
break; break;

4
src/main/flight/failsafe.h
View file

@ -43,7 +43,9 @@ typedef struct failsafeConfig_s {
uint16_t failsafe_min_distance; // Minimum distance required for failsafe procedure to be taken. 1 step = 1 centimeter. 0 = Regular failsafe_procedure always active (default) uint16_t failsafe_min_distance; // Minimum distance required for failsafe procedure to be taken. 1 step = 1 centimeter. 0 = Regular failsafe_procedure always active (default)
uint8_t failsafe_min_distance_procedure; // selected minimum distance failsafe procedure is 0: auto-landing, 1: Drop it, 2: Return To Home (RTH) uint8_t failsafe_min_distance_procedure; // selected minimum distance failsafe procedure is 0: auto-landing, 1: Drop it, 2: Return To Home (RTH)
int16_t failsafe_mission_delay; // Time delay before Failsafe triggered when WP mission in progress (s) int16_t failsafe_mission_delay; // Time delay before Failsafe triggered when WP mission in progress (s)
#ifdef USE_GPS_FIX_ESTIMATION
int16_t failsafe_gps_fix_estimation_delay; // Time delay before Failsafe triggered when GPX Fix estimation is applied (s) int16_t failsafe_gps_fix_estimation_delay; // Time delay before Failsafe triggered when GPX Fix estimation is applied (s)
#endif
} failsafeConfig_t; } failsafeConfig_t;
PG_DECLARE(failsafeConfig_t, failsafeConfig); PG_DECLARE(failsafeConfig_t, failsafeConfig);
@ -150,7 +152,9 @@ typedef struct failsafeState_s {
timeMs_t receivingRxDataPeriod; // period for the required period of valid rxData timeMs_t receivingRxDataPeriod; // period for the required period of valid rxData
timeMs_t receivingRxDataPeriodPreset; // preset for the required period of valid rxData timeMs_t receivingRxDataPeriodPreset; // preset for the required period of valid rxData
timeMs_t wpModeDelayedFailsafeStart; // waypoint mission delayed failsafe timer start time timeMs_t wpModeDelayedFailsafeStart; // waypoint mission delayed failsafe timer start time
#ifdef USE_GPS_FIX_ESTIMATION
timeMs_t wpModeGPSFixEstimationDelayedFailsafeStart; // waypoint mission delayed failsafe timer start time on GPS fix estimation timeMs_t wpModeGPSFixEstimationDelayedFailsafeStart; // waypoint mission delayed failsafe timer start time on GPS fix estimation
#endif
failsafeProcedure_e activeProcedure; failsafeProcedure_e activeProcedure;
failsafePhase_e phase; failsafePhase_e phase;
failsafeRxLinkState_e rxLinkState; failsafeRxLinkState_e rxLinkState;

6
src/main/flight/imu.c
View file

@ -323,7 +323,11 @@ static void imuCheckAndResetOrientationQuaternion(const fpQuaternion_t * quat, c
bool isGPSTrustworthy(void) bool isGPSTrustworthy(void)
{ {
return (sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6) || STATE(GPS_ESTIMATED_FIX); return (sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
;
} }
static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, bool useCOG, float courseOverGround, float accWScaler, float magWScaler) static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, bool useCOG, float courseOverGround, float accWScaler, float magWScaler)

13
src/main/flight/wind_estimator.c
View file

@ -52,7 +52,11 @@ static float lastFuselageDirection[XYZ_AXIS_COUNT];
bool isEstimatedWindSpeedValid(void) bool isEstimatedWindSpeedValid(void)
{ {
return hasValidWindEstimate || STATE(GPS_ESTIMATED_FIX); //use any wind estimate with GPS fix estimation. return hasValidWindEstimate
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX) //use any wind estimate with GPS fix estimation.
#endif
;
} }
float getEstimatedWindSpeed(int axis) float getEstimatedWindSpeed(int axis)
@ -90,8 +94,11 @@ void updateWindEstimator(timeUs_t currentTimeUs)
if (!STATE(FIXED_WING_LEGACY) || if (!STATE(FIXED_WING_LEGACY) ||
!isGPSHeadingValid() || !isGPSHeadingValid() ||
!gpsSol.flags.validVelNE || !gpsSol.flags.validVelNE ||
!gpsSol.flags.validVelD || !gpsSol.flags.validVelD
STATE(GPS_ESTIMATED_FIX)) { #ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
return; return;
} }

26
src/main/io/gps.c
View file

@ -223,7 +223,7 @@ void gpsSetProtocolTimeout(timeMs_t timeoutMs)
gpsState.timeoutMs = timeoutMs; gpsState.timeoutMs = timeoutMs;
} }
#ifdef USE_GPS_FIX_ESTIMATION
bool canEstimateGPSFix(void) bool canEstimateGPSFix(void)
{ {
#if defined(USE_GPS) && defined(USE_MAG) && defined(USE_BARO) #if defined(USE_GPS) && defined(USE_MAG) && defined(USE_BARO)
@ -239,9 +239,10 @@ bool canEstimateGPSFix(void)
#else #else
return false; return false;
#endif #endif
} }
#endif
#ifdef USE_GPS_FIX_ESTIMATION
void processDisableGPSFix(void) void processDisableGPSFix(void)
{ {
static int32_t last_lat = 0; static int32_t last_lat = 0;
@ -268,7 +269,9 @@ void processDisableGPSFix(void)
last_alt = gpsSol.llh.alt; last_alt = gpsSol.llh.alt;
} }
} }
#endif
#ifdef USE_GPS_FIX_ESTIMATION
//called after gpsSolDRV is copied to gpsSol and processed by "Disable GPS Fix logical condition" //called after gpsSolDRV is copied to gpsSol and processed by "Disable GPS Fix logical condition"
void updateEstimatedGPSFix(void) void updateEstimatedGPSFix(void)
{ {
@ -345,14 +348,17 @@ void updateEstimatedGPSFix(void)
gpsSol.velNED[Y] = (int16_t)(velY); gpsSol.velNED[Y] = (int16_t)(velY);
gpsSol.velNED[Z] = 0; gpsSol.velNED[Z] = 0;
} }
#endif
void gpsProcessNewDriverData(void) void gpsProcessNewDriverData(void)
{ {
gpsSol = gpsSolDRV; gpsSol = gpsSolDRV;
#ifdef USE_GPS_FIX_ESTIMATION
processDisableGPSFix(); processDisableGPSFix();
updateEstimatedGPSFix(); updateEstimatedGPSFix();
#endif
} }
//called after: //called after:
@ -362,11 +368,13 @@ void gpsProcessNewDriverData(void)
//On GPS sensor timeout - called after updateEstimatedGPSFix() //On GPS sensor timeout - called after updateEstimatedGPSFix()
void gpsProcessNewSolutionData(bool timeout) void gpsProcessNewSolutionData(bool timeout)
{ {
#ifdef USE_GPS_FIX_ESTIMATION
if ( gpsSol.numSat == 99 ) { if ( gpsSol.numSat == 99 ) {
ENABLE_STATE(GPS_ESTIMATED_FIX); ENABLE_STATE(GPS_ESTIMATED_FIX);
DISABLE_STATE(GPS_FIX); DISABLE_STATE(GPS_FIX);
} else { } else {
DISABLE_STATE(GPS_ESTIMATED_FIX); DISABLE_STATE(GPS_ESTIMATED_FIX);
#endif
// Set GPS fix flag only if we have 3D fix // Set GPS fix flag only if we have 3D fix
if (gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats) { if (gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats) {
@ -379,7 +387,9 @@ void gpsProcessNewSolutionData(bool timeout)
gpsSol.flags.validEPE = false; gpsSol.flags.validEPE = false;
DISABLE_STATE(GPS_FIX); DISABLE_STATE(GPS_FIX);
} }
#ifdef USE_GPS_FIX_ESTIMATION
} }
#endif
if (!timeout) { if (!timeout) {
// Data came from GPS sensor - set sensor as ready and available (it may still not have GPS fix) // Data came from GPS sensor - set sensor as ready and available (it may still not have GPS fix)
@ -423,11 +433,15 @@ void gpsTryEstimateOnTimeout(void)
gpsResetSolution(&gpsSol); gpsResetSolution(&gpsSol);
DISABLE_STATE(GPS_FIX); DISABLE_STATE(GPS_FIX);
#ifdef USE_GPS_FIX_ESTIMATION
if ( canEstimateGPSFix() ) {
updateEstimatedGPSFix(); updateEstimatedGPSFix();
if (gpsSol.fixType == GPS_FIX_3D) { //estimation kicked in if (gpsSol.fixType == GPS_FIX_3D) { //estimation kicked in
gpsProcessNewSolutionData(true); gpsProcessNewSolutionData(true);
} }
}
#endif
} }
void gpsPreInit(void) void gpsPreInit(void)
@ -577,7 +591,7 @@ bool gpsUpdate(void)
break; break;
} }
if ( !sensors(SENSOR_GPS) && canEstimateGPSFix() ) { if ( !sensors(SENSOR_GPS) ) {
gpsTryEstimateOnTimeout(); gpsTryEstimateOnTimeout();
} }
@ -630,7 +644,11 @@ bool isGPSHealthy(void)
bool isGPSHeadingValid(void) bool isGPSHeadingValid(void)
{ {
return ((STATE(GPS_FIX) && gpsSol.numSat >= 6) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed >= 300; return ((STATE(GPS_FIX) && gpsSol.numSat >= 6)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && gpsSol.groundSpeed >= 300;
} }
#endif #endif

2
src/main/io/gps_ublox.c
View file

@ -147,7 +147,7 @@ static union {
bool gpsUbloxHasGalileo(void) bool gpsUbloxHasGalileo(void)
{ {
return (ubx_capabilities.supported & UBX_MON_GNSS_GALILEO_MASK); return (ubx_capabilities.supported & UBX_MON_GNSS_GALILEO_MASK);
} }
bool gpsUbloxHasBeidou(void) bool gpsUbloxHasBeidou(void)
{ {

58
src/main/io/osd.c
View file

@ -1080,7 +1080,7 @@ static void osdUpdateBatteryCapacityOrVoltageTextAttributes(textAttributes_t *at
if (batteryState == BATTERY_WARNING || batteryState == BATTERY_CRITICAL) { if (batteryState == BATTERY_WARNING || batteryState == BATTERY_CRITICAL) {
TEXT_ATTRIBUTES_ADD_BLINK(*attr); TEXT_ATTRIBUTES_ADD_BLINK(*attr);
} }
} }
void osdCrosshairPosition(uint8_t *x, uint8_t *y) void osdCrosshairPosition(uint8_t *x, uint8_t *y)
@ -1315,7 +1315,11 @@ static void osdDrawMap(int referenceHeading, uint16_t referenceSym, uint16_t cen
} }
} }
if (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) { if (STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
int directionToPoi = osdGetHeadingAngle(poiDirection - referenceHeading); int directionToPoi = osdGetHeadingAngle(poiDirection - referenceHeading);
float poiAngle = DEGREES_TO_RADIANS(directionToPoi); float poiAngle = DEGREES_TO_RADIANS(directionToPoi);
@ -1429,7 +1433,11 @@ static void osdDisplayTelemetry(void)
static uint16_t trk_bearing = 0; static uint16_t trk_bearing = 0;
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {
if (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)){ if (STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
){
if (GPS_distanceToHome > 5) { if (GPS_distanceToHome > 5) {
trk_bearing = GPS_directionToHome; trk_bearing = GPS_directionToHome;
trk_bearing += 360 + 180; trk_bearing += 360 + 180;
@ -1786,12 +1794,16 @@ static bool osdDrawSingleElement(uint8_t item)
buff[0] = SYM_SAT_L; buff[0] = SYM_SAT_L;
buff[1] = SYM_SAT_R; buff[1] = SYM_SAT_R;
tfp_sprintf(buff + 2, "%2d", gpsSol.numSat); tfp_sprintf(buff + 2, "%2d", gpsSol.numSat);
#ifdef USE_GPS_FIX_ESTIMATION
if (STATE(GPS_ESTIMATED_FIX)) { if (STATE(GPS_ESTIMATED_FIX)) {
strcpy(buff + 2, "ES"); strcpy(buff + 2, "ES");
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
} } else
else if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { #endif
if (!STATE(GPS_FIX)) {
if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) {
strcpy(buff + 2, "X!"); strcpy(buff + 2, "X!");
}
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
} }
break; break;
@ -1843,7 +1855,11 @@ static bool osdDrawSingleElement(uint8_t item)
case OSD_HOME_DIR: case OSD_HOME_DIR:
{ {
if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { if ((STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) {
if (GPS_distanceToHome < (navConfig()->general.min_rth_distance / 100) ) { if (GPS_distanceToHome < (navConfig()->general.min_rth_distance / 100) ) {
displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_HOME_NEAR); displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_HOME_NEAR);
} }
@ -2331,11 +2347,19 @@ static bool osdDrawSingleElement(uint8_t item)
osdCrosshairPosition(&elemPosX, &elemPosY); osdCrosshairPosition(&elemPosX, &elemPosY);
osdHudDrawCrosshair(osdGetDisplayPortCanvas(), elemPosX, elemPosY); osdHudDrawCrosshair(osdGetDisplayPortCanvas(), elemPosX, elemPosY);
if (osdConfig()->hud_homing && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { if (osdConfig()->hud_homing && (STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) {
osdHudDrawHoming(elemPosX, elemPosY); osdHudDrawHoming(elemPosX, elemPosY);
} }
if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && isImuHeadingValid()) { if ((STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && isImuHeadingValid()) {
if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0 || osdConfig()->hud_wp_disp > 0) { if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0 || osdConfig()->hud_wp_disp > 0) {
osdHudClear(); osdHudClear();
@ -2959,7 +2983,11 @@ static bool osdDrawSingleElement(uint8_t item)
digits = 4U; digits = 4U;
} }
#endif #endif
if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed > 0) { if ((STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && gpsSol.groundSpeed > 0) {
if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
1, US2S(efficiencyTimeDelta)); 1, US2S(efficiencyTimeDelta));
@ -3030,7 +3058,11 @@ static bool osdDrawSingleElement(uint8_t item)
int32_t value = 0; int32_t value = 0;
timeUs_t currentTimeUs = micros(); timeUs_t currentTimeUs = micros();
timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated);
if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))&& gpsSol.groundSpeed > 0) { if ((STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && gpsSol.groundSpeed > 0) {
if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f, value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f,
1, US2S(efficiencyTimeDelta)); 1, US2S(efficiencyTimeDelta));
@ -3182,7 +3214,11 @@ static bool osdDrawSingleElement(uint8_t item)
STATIC_ASSERT(GPS_DEGREES_DIVIDER == OLC_DEG_MULTIPLIER, invalid_olc_deg_multiplier); STATIC_ASSERT(GPS_DEGREES_DIVIDER == OLC_DEG_MULTIPLIER, invalid_olc_deg_multiplier);
int digits = osdConfig()->plus_code_digits; int digits = osdConfig()->plus_code_digits;
int digitsRemoved = osdConfig()->plus_code_short * 2; int digitsRemoved = osdConfig()->plus_code_short * 2;
if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { if ((STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
)) {
olc_encode(gpsSol.llh.lat, gpsSol.llh.lon, digits, buff, sizeof(buff)); olc_encode(gpsSol.llh.lat, gpsSol.llh.lon, digits, buff, sizeof(buff));
} else { } else {
// +codes with > 8 digits have a + at the 9th digit // +codes with > 8 digits have a + at the 9th digit

6
src/main/io/osd_dji_hd.c
View file

@ -786,7 +786,11 @@ static void osdDJIEfficiencyMahPerKM(char *buff)
timeUs_t currentTimeUs = micros(); timeUs_t currentTimeUs = micros();
timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated);
if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed > 0) { if ((STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && gpsSol.groundSpeed > 0) {
if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
1, US2S(efficiencyTimeDelta)); 1, US2S(efficiencyTimeDelta));

8
src/main/navigation/navigation.c
View file

@ -2378,6 +2378,7 @@ bool validateRTHSanityChecker(void)
return true; return true;
} }
#ifdef USE_GPS_FIX_ESTIMATION
if (STATE(GPS_ESTIMATED_FIX)) { if (STATE(GPS_ESTIMATED_FIX)) {
//disable sanity checks in GPS estimation mode //disable sanity checks in GPS estimation mode
//when estimated GPS fix is replaced with real fix, coordinates may jump //when estimated GPS fix is replaced with real fix, coordinates may jump
@ -2386,6 +2387,7 @@ bool validateRTHSanityChecker(void)
posControl.rthSanityChecker.lastCheckTime = currentTimeMs + 5000; posControl.rthSanityChecker.lastCheckTime = currentTimeMs + 5000;
return true; return true;
} }
#endif
// Check at 10Hz rate // Check at 10Hz rate
if ( ((int32_t)(currentTimeMs - posControl.rthSanityChecker.lastCheckTime)) > 100) { if ( ((int32_t)(currentTimeMs - posControl.rthSanityChecker.lastCheckTime)) > 100) {
@ -3993,7 +3995,11 @@ void updateWpMissionPlanner(void)
{ {
static timeMs_t resetTimerStart = 0; static timeMs_t resetTimerStart = 0;
if (IS_RC_MODE_ACTIVE(BOXPLANWPMISSION) && !(FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive())) { if (IS_RC_MODE_ACTIVE(BOXPLANWPMISSION) && !(FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive())) {
const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)); const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && (STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
);
posControl.flags.wpMissionPlannerActive = true; posControl.flags.wpMissionPlannerActive = true;
if (millis() - resetTimerStart < 1000 && navConfig()->general.flags.mission_planner_reset) { if (millis() - resetTimerStart < 1000 && navConfig()->general.flags.mission_planner_reset) {

2
src/main/navigation/navigation.h
View file

@ -210,7 +210,9 @@ typedef struct positionEstimationConfig_s {
uint8_t use_gps_no_baro; uint8_t use_gps_no_baro;
#ifdef USE_GPS_FIX_ESTIMATION
uint8_t allow_gps_fix_estimation; uint8_t allow_gps_fix_estimation;
#endif
} positionEstimationConfig_t; } positionEstimationConfig_t;
PG_DECLARE(positionEstimationConfig_t, positionEstimationConfig); PG_DECLARE(positionEstimationConfig_t, positionEstimationConfig);

35
src/main/navigation/navigation_pos_estimator.c
View file

@ -92,8 +92,9 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
.max_eph_epv = SETTING_INAV_MAX_EPH_EPV_DEFAULT, .max_eph_epv = SETTING_INAV_MAX_EPH_EPV_DEFAULT,
.baro_epv = SETTING_INAV_BARO_EPV_DEFAULT, .baro_epv = SETTING_INAV_BARO_EPV_DEFAULT,
#ifdef USE_GPS_FIX_ESTIMATION
.allow_gps_fix_estimation = SETTING_INAV_ALLOW_GPS_FIX_ESTIMATION_DEFAULT .allow_gps_fix_estimation = SETTING_INAV_ALLOW_GPS_FIX_ESTIMATION_DEFAULT
#endif
); );
#define resetTimer(tim, currentTimeUs) { (tim)->deltaTime = 0; (tim)->lastTriggeredTime = currentTimeUs; } #define resetTimer(tim, currentTimeUs) { (tim)->deltaTime = 0; (tim)->lastTriggeredTime = currentTimeUs; }
@ -152,12 +153,14 @@ static bool detectGPSGlitch(timeUs_t currentTimeUs)
bool isGlitching = false; bool isGlitching = false;
#ifdef USE_GPS_FIX_ESTIMATION
if (STATE(GPS_ESTIMATED_FIX)) { if (STATE(GPS_ESTIMATED_FIX)) {
//disable sanity checks in GPS estimation mode //disable sanity checks in GPS estimation mode
//when estimated GPS fix is replaced with real fix, coordinates may jump //when estimated GPS fix is replaced with real fix, coordinates may jump
previousTime = 0; previousTime = 0;
return true; return true;
} }
#endif
if (previousTime == 0) { if (previousTime == 0) {
isGlitching = false; isGlitching = false;
@ -210,8 +213,16 @@ void onNewGPSData(void)
newLLH.lon = gpsSol.llh.lon; newLLH.lon = gpsSol.llh.lon;
newLLH.alt = gpsSol.llh.alt; newLLH.alt = gpsSol.llh.alt;
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { if (sensors(SENSOR_GPS)
if (!(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { #ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
if (!(STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
)) {
isFirstGPSUpdate = true; isFirstGPSUpdate = true;
return; return;
} }
@ -492,7 +503,11 @@ static bool navIsAccelerationUsable(void)
static bool navIsHeadingUsable(void) static bool navIsHeadingUsable(void)
{ {
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
// If we have GPS - we need true IMU north (valid heading) // If we have GPS - we need true IMU north (valid heading)
return isImuHeadingValid(); return isImuHeadingValid();
} }
@ -507,7 +522,11 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
/* Figure out if we have valid position data from our data sources */ /* Figure out if we have valid position data from our data sources */
uint32_t newFlags = 0; uint32_t newFlags = 0;
if ((sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) && posControl.gpsOrigin.valid && if ((sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && posControl.gpsOrigin.valid &&
((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) && ((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) &&
(posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv)) { (posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv)) {
if (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv) { if (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv) {
@ -704,7 +723,11 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
static void estimationCalculateGroundCourse(timeUs_t currentTimeUs) static void estimationCalculateGroundCourse(timeUs_t currentTimeUs)
{ {
if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && navIsHeadingUsable()) { if ((STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && navIsHeadingUsable()) {
static timeUs_t lastUpdateTimeUs = 0; static timeUs_t lastUpdateTimeUs = 0;
if (currentTimeUs - lastUpdateTimeUs >= HZ2US(INAV_COG_UPDATE_RATE_HZ)) { // limit update rate if (currentTimeUs - lastUpdateTimeUs >= HZ2US(INAV_COG_UPDATE_RATE_HZ)) { // limit update rate

7
src/main/programming/logic_condition.c
View file

@ -473,6 +473,7 @@ static int logicConditionCompute(
} }
break; break;
#ifdef USE_GPS_FIX_ESTIMATION
case LOGIC_CONDITION_DISABLE_GPS_FIX: case LOGIC_CONDITION_DISABLE_GPS_FIX:
if (operandA > 0) { if (operandA > 0) {
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX); LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX);
@ -481,6 +482,7 @@ static int logicConditionCompute(
} }
return true; return true;
break; break;
#endif
default: default:
return false; return false;
@ -663,9 +665,12 @@ static int logicConditionGetFlightOperandValue(int operand) {
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS: case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS:
#ifdef USE_GPS_FIX_ESTIMATION
if ( STATE(GPS_ESTIMATED_FIX) ){ if ( STATE(GPS_ESTIMATED_FIX) ){
return gpsSol.numSat; //99 return gpsSol.numSat; //99
} else if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { } else
#endif
if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) {
return 0; return 0;
} else { } else {
return gpsSol.numSat; return gpsSol.numSat;

4
src/main/programming/logic_condition.h
View file

@ -81,7 +81,9 @@ typedef enum {
LOGIC_CONDITION_TIMER = 49, LOGIC_CONDITION_TIMER = 49,
LOGIC_CONDITION_DELTA = 50, LOGIC_CONDITION_DELTA = 50,
LOGIC_CONDITION_APPROX_EQUAL = 51, LOGIC_CONDITION_APPROX_EQUAL = 51,
#ifdef USE_GPS_FIX_ESTIMATION
LOGIC_CONDITION_DISABLE_GPS_FIX = 52, LOGIC_CONDITION_DISABLE_GPS_FIX = 52,
#endif
LOGIC_CONDITION_LAST = 53, LOGIC_CONDITION_LAST = 53,
} logicOperation_e; } logicOperation_e;
@ -186,7 +188,9 @@ typedef enum {
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL = (1 << 8), LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL = (1 << 8),
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS = (1 << 9), LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS = (1 << 9),
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_FLIGHT_AXIS = (1 << 10), LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_FLIGHT_AXIS = (1 << 10),
#ifdef USE_GPS_FIX_ESTIMATION
LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX = (1 << 11), LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX = (1 << 11),
#endif
} logicConditionsGlobalFlags_t; } logicConditionsGlobalFlags_t;
typedef enum { typedef enum {

30
src/main/sensors/diagnostics.c
View file

@ -43,12 +43,10 @@ hardwareSensorStatus_e getHwAccelerometerStatus(void)
if (detectedSensors[SENSOR_INDEX_ACC] != ACC_NONE) { if (detectedSensors[SENSOR_INDEX_ACC] != ACC_NONE) {
if (accIsHealthy()) { if (accIsHealthy()) {
return HW_SENSOR_OK; return HW_SENSOR_OK;
} } else {
else {
return HW_SENSOR_UNHEALTHY; return HW_SENSOR_UNHEALTHY;
} }
} } else {
else {
if (requestedSensors[SENSOR_INDEX_ACC] != ACC_NONE) { if (requestedSensors[SENSOR_INDEX_ACC] != ACC_NONE) {
// Selected but not detected // Selected but not detected
return HW_SENSOR_UNAVAILABLE; return HW_SENSOR_UNAVAILABLE;
@ -135,8 +133,7 @@ hardwareSensorStatus_e getHwRangefinderStatus(void)
if (detectedSensors[SENSOR_INDEX_RANGEFINDER] != RANGEFINDER_NONE) { if (detectedSensors[SENSOR_INDEX_RANGEFINDER] != RANGEFINDER_NONE) {
if (rangefinderIsHealthy()) { if (rangefinderIsHealthy()) {
return HW_SENSOR_OK; return HW_SENSOR_OK;
} } else {
else {
return HW_SENSOR_UNHEALTHY; return HW_SENSOR_UNHEALTHY;
} }
} }
@ -144,8 +141,7 @@ hardwareSensorStatus_e getHwRangefinderStatus(void)
if (requestedSensors[SENSOR_INDEX_RANGEFINDER] != RANGEFINDER_NONE) { if (requestedSensors[SENSOR_INDEX_RANGEFINDER] != RANGEFINDER_NONE) {
// Selected but not detected // Selected but not detected
return HW_SENSOR_UNAVAILABLE; return HW_SENSOR_UNAVAILABLE;
} } else {
else {
// Not selected and not detected // Not selected and not detected
return HW_SENSOR_NONE; return HW_SENSOR_NONE;
} }
@ -161,8 +157,7 @@ hardwareSensorStatus_e getHwPitotmeterStatus(void)
if (detectedSensors[SENSOR_INDEX_PITOT] != PITOT_NONE) { if (detectedSensors[SENSOR_INDEX_PITOT] != PITOT_NONE) {
if (pitotIsHealthy()) { if (pitotIsHealthy()) {
return HW_SENSOR_OK; return HW_SENSOR_OK;
} } else {
else {
return HW_SENSOR_UNHEALTHY; return HW_SENSOR_UNHEALTHY;
} }
} }
@ -170,8 +165,7 @@ hardwareSensorStatus_e getHwPitotmeterStatus(void)
if (requestedSensors[SENSOR_INDEX_PITOT] != PITOT_NONE) { if (requestedSensors[SENSOR_INDEX_PITOT] != PITOT_NONE) {
// Selected but not detected // Selected but not detected
return HW_SENSOR_UNAVAILABLE; return HW_SENSOR_UNAVAILABLE;
} } else {
else {
// Not selected and not detected // Not selected and not detected
return HW_SENSOR_NONE; return HW_SENSOR_NONE;
} }
@ -187,8 +181,7 @@ hardwareSensorStatus_e getHwGPSStatus(void)
if (sensors(SENSOR_GPS)) { if (sensors(SENSOR_GPS)) {
if (isGPSHealthy()) { if (isGPSHealthy()) {
return HW_SENSOR_OK; return HW_SENSOR_OK;
} } else {
else {
return HW_SENSOR_UNHEALTHY; return HW_SENSOR_UNHEALTHY;
} }
} }
@ -196,8 +189,7 @@ hardwareSensorStatus_e getHwGPSStatus(void)
if (feature(FEATURE_GPS) && gpsStats.timeouts > 4) { if (feature(FEATURE_GPS) && gpsStats.timeouts > 4) {
// Selected but not detected // Selected but not detected
return HW_SENSOR_UNAVAILABLE; return HW_SENSOR_UNAVAILABLE;
} } else {
else {
// Not selected and not detected // Not selected and not detected
return HW_SENSOR_NONE; return HW_SENSOR_NONE;
} }
@ -213,8 +205,7 @@ hardwareSensorStatus_e getHwOpticalFlowStatus(void)
if (detectedSensors[SENSOR_INDEX_OPFLOW] != OPFLOW_NONE) { if (detectedSensors[SENSOR_INDEX_OPFLOW] != OPFLOW_NONE) {
if (opflowIsHealthy()) { if (opflowIsHealthy()) {
return HW_SENSOR_OK; return HW_SENSOR_OK;
} } else {
else {
return HW_SENSOR_UNHEALTHY; return HW_SENSOR_UNHEALTHY;
} }
} }
@ -222,8 +213,7 @@ hardwareSensorStatus_e getHwOpticalFlowStatus(void)
if (requestedSensors[SENSOR_INDEX_OPFLOW] != OPFLOW_NONE) { if (requestedSensors[SENSOR_INDEX_OPFLOW] != OPFLOW_NONE) {
// Selected but not detected // Selected but not detected
return HW_SENSOR_UNAVAILABLE; return HW_SENSOR_UNAVAILABLE;
} } else {
else {
// Not selected and not detected // Not selected and not detected
return HW_SENSOR_NONE; return HW_SENSOR_NONE;
} }

1
src/main/target/common.h
View file

@ -56,6 +56,7 @@
#define USE_GPS_PROTO_MSP #define USE_GPS_PROTO_MSP
#define USE_TELEMETRY #define USE_TELEMETRY
#define USE_TELEMETRY_LTM #define USE_TELEMETRY_LTM
#define USE_GPS_FIX_ESTIMATION
// This is the shortest period in microseconds that the scheduler will allow // This is the shortest period in microseconds that the scheduler will allow
#define SCHEDULER_DELAY_LIMIT 10 #define SCHEDULER_DELAY_LIMIT 10

6
src/main/telemetry/ghst.c
View file

@ -147,7 +147,11 @@ void ghstFrameGpsSecondaryTelemetry(sbuf_t *dst)
sbufWriteU16(dst, GPS_directionToHome); sbufWriteU16(dst, GPS_directionToHome);
uint8_t gpsFlags = 0; uint8_t gpsFlags = 0;
if (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) { if (STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
gpsFlags |= GPS_FLAGS_FIX; gpsFlags |= GPS_FLAGS_FIX;
} }
if (STATE(GPS_FIX_HOME)) { if (STATE(GPS_FIX_HOME)) {

12
src/main/telemetry/hott.c
View file

@ -234,7 +234,11 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage)
const int32_t climbrate3s = MAX(0, 3.0f * getEstimatedActualVelocity(Z) / 100 + 120); const int32_t climbrate3s = MAX(0, 3.0f * getEstimatedActualVelocity(Z) / 100 + 120);
hottGPSMessage->climbrate3s = climbrate3s & 0xFF; hottGPSMessage->climbrate3s = climbrate3s & 0xFF;
if (!(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) { if (!(STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX))
#endif
) {
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE; hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE;
return; return;
} }
@ -476,7 +480,11 @@ static bool processBinaryModeRequest(uint8_t address)
switch (address) { switch (address) {
#ifdef USE_GPS #ifdef USE_GPS
case 0x8A: case 0x8A:
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
hottPrepareGPSResponse(&hottGPSMessage); hottPrepareGPSResponse(&hottGPSMessage);
hottQueueSendResponse((uint8_t *)&hottGPSMessage, sizeof(hottGPSMessage)); hottQueueSendResponse((uint8_t *)&hottGPSMessage, sizeof(hottGPSMessage));
return true; return true;

96
src/main/telemetry/ibus_shared.c
View file

@ -136,7 +136,11 @@ static uint8_t flightModeToIBusTelemetryMode2[FLM_COUNT] = { 5, 1, 1, 0, 7, 2, 8
static uint8_t dispatchMeasurementRequest(ibusAddress_t address) { static uint8_t dispatchMeasurementRequest(ibusAddress_t address) {
#if defined(USE_GPS) #if defined(USE_GPS)
uint8_t fix = 0; uint8_t fix = 0;
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
if (gpsSol.fixType == GPS_NO_FIX) fix = 1; if (gpsSol.fixType == GPS_NO_FIX) fix = 1;
else if (gpsSol.fixType == GPS_FIX_2D) fix = 2; else if (gpsSol.fixType == GPS_FIX_2D) fix = 2;
else if (gpsSol.fixType == GPS_FIX_3D) fix = 3; else if (gpsSol.fixType == GPS_FIX_3D) fix = 3;
@ -196,7 +200,11 @@ static uint8_t dispatchMeasurementRequest(ibusAddress_t address) {
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_STATUS) { //STATUS sat num AS #0, FIX AS 0, HDOP AS 0, Mode AS 0 } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_STATUS) { //STATUS sat num AS #0, FIX AS 0, HDOP AS 0, Mode AS 0
uint16_t status = flightModeToIBusTelemetryMode1[getFlightModeForTelemetry()]; uint16_t status = flightModeToIBusTelemetryMode1[getFlightModeForTelemetry()];
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
status += gpsSol.numSat * 1000; status += gpsSol.numSat * 1000;
status += fix * 100; status += fix * 100;
if (STATE(GPS_FIX_HOME)) status += 500; if (STATE(GPS_FIX_HOME)) status += 500;
@ -206,72 +214,128 @@ static uint8_t dispatchMeasurementRequest(ibusAddress_t address) {
return sendIbusMeasurement2(address, status); return sendIbusMeasurement2(address, status);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_HEADING) { //HOME_DIR 0-360deg } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_HEADING) { //HOME_DIR 0-360deg
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) GPS_directionToHome); else //int16_t if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement2(address, (uint16_t) GPS_directionToHome); else //int16_t
#endif #endif
return sendIbusMeasurement2(address, 0); return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_DIST) { //HOME_DIST in m } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_DIST) { //HOME_DIST in m
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) GPS_distanceToHome); else //uint16_t if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement2(address, (uint16_t) GPS_distanceToHome); else //uint16_t
#endif #endif
return sendIbusMeasurement2(address, 0); return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_SPE) { //GPS_SPEED in cm/s => km/h, 1cm/s = 0.036 km/h } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_SPE) { //GPS_SPEED in cm/s => km/h, 1cm/s = 0.036 km/h
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed * 36 / 100); else //int16_t if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed * 36 / 100); else //int16_t
#endif #endif
return sendIbusMeasurement2(address, 0); return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_SPEED) {//SPEED in cm/s } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_SPEED) {//SPEED in cm/s
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed); //int16_t if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement2(address, (uint16_t) gpsSol.groundSpeed); //int16_t
#endif #endif
return sendIbusMeasurement2(address, 0); return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_COG) { //GPS_COURSE (0-360deg, 0=north) } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_COG) { //GPS_COURSE (0-360deg, 0=north)
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.groundCourse / 10)); else //int16_t if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.groundCourse / 10)); else //int16_t
#endif #endif
return sendIbusMeasurement2(address, 0); return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_STATUS) { //GPS_STATUS fix sat } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_STATUS) { //GPS_STATUS fix sat
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (((uint16_t)fix)<<8) + gpsSol.numSat); else //uint8_t, uint8_t if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement2(address, (((uint16_t)fix)<<8) + gpsSol.numSat); else //uint8_t, uint8_t
#endif #endif
return sendIbusMeasurement2(address, 0); return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT) { //4byte } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT) { //4byte
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lat); else //int32_t if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lat); else //int32_t
#endif #endif
return sendIbusMeasurement4(address, 0); return sendIbusMeasurement4(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON) { //4byte } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON) { //4byte
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lon); else //int32_t if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement4(address, (int32_t)gpsSol.llh.lon); else //int32_t
#endif #endif
return sendIbusMeasurement4(address, 0); return sendIbusMeasurement4(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT1) { //GPS_LAT1 //Lattitude * 1e+7 } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT1) { //GPS_LAT1 //Lattitude * 1e+7
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lat / 100000)); else if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lat / 100000)); else
#endif #endif
return sendIbusMeasurement2(address, 0); return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON1) { //GPS_LON1 //Longitude * 1e+7 } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON1) { //GPS_LON1 //Longitude * 1e+7
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lon / 100000)); else if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.lon / 100000)); else
#endif #endif
return sendIbusMeasurement2(address, 0); return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT2) { //GPS_LAT2 //Lattitude * 1e+7 } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT2) { //GPS_LAT2 //Lattitude * 1e+7
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lat % 100000)/10)); if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lat % 100000)/10));
#endif #endif
return sendIbusMeasurement2(address, 0); return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON2) { //GPS_LON2 //Longitude * 1e+7 } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON2) { //GPS_LON2 //Longitude * 1e+7
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lon % 100000)/10)); else if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement2(address, (uint16_t) ((gpsSol.llh.lon % 100000)/10)); else
#endif #endif
return sendIbusMeasurement2(address, 0); return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GALT4) { //GPS_ALT //In cm => m } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GALT4) { //GPS_ALT //In cm => m
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement4(address, (int32_t) (gpsSol.llh.alt)); else //int32_t if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement4(address, (int32_t) (gpsSol.llh.alt)); else //int32_t
#endif #endif
return sendIbusMeasurement4(address, 0); return sendIbusMeasurement4(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GALT) { //GPS_ALT //In cm => m } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GALT) { //GPS_ALT //In cm => m
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.alt / 100)); else //int32_t if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) return sendIbusMeasurement2(address, (uint16_t) (gpsSol.llh.alt / 100)); else //int32_t
#endif #endif
return sendIbusMeasurement2(address, 0); return sendIbusMeasurement2(address, 0);
} }

6
src/main/telemetry/jetiexbus.c
View file

@ -575,7 +575,11 @@ uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item)
createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID); createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID);
if (!allSensorsActive) { if (!allSensorsActive) {
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
enableGpsTelemetry(true); enableGpsTelemetry(true);
allSensorsActive = true; allSensorsActive = true;
} }

6
src/main/telemetry/ltm.c
View file

@ -117,7 +117,11 @@ void ltm_gframe(sbuf_t *dst)
uint8_t gps_fix_type = 0; uint8_t gps_fix_type = 0;
int32_t ltm_lat = 0, ltm_lon = 0, ltm_alt = 0, ltm_gs = 0; int32_t ltm_lat = 0, ltm_lon = 0, ltm_alt = 0, ltm_gs = 0;
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
if (gpsSol.fixType == GPS_NO_FIX) if (gpsSol.fixType == GPS_NO_FIX)
gps_fix_type = 1; gps_fix_type = 1;
else if (gpsSol.fixType == GPS_FIX_2D) else if (gpsSol.fixType == GPS_FIX_2D)

12
src/main/telemetry/mavlink.c
View file

@ -523,7 +523,11 @@ void mavlinkSendPosition(timeUs_t currentTimeUs)
{ {
uint8_t gpsFixType = 0; uint8_t gpsFixType = 0;
if (!(sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX))) if (!(sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
))
return; return;
if (gpsSol.fixType == GPS_NO_FIX) if (gpsSol.fixType == GPS_NO_FIX)
@ -638,7 +642,11 @@ void mavlinkSendHUDAndHeartbeat(void)
#if defined(USE_GPS) #if defined(USE_GPS)
// use ground speed if source available // use ground speed if source available
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) { if (sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
mavGroundSpeed = gpsSol.groundSpeed / 100.0f; mavGroundSpeed = gpsSol.groundSpeed / 100.0f;
} }
#endif #endif

6
src/main/telemetry/sim.c
View file

@ -348,7 +348,11 @@ static void sendSMS(void)
ZERO_FARRAY(pluscode_url); ZERO_FARRAY(pluscode_url);
if ((sensors(SENSOR_GPS) && STATE(GPS_FIX)) || STATE(GPS_ESTIMATED_FIX)) { if ((sensors(SENSOR_GPS) && STATE(GPS_FIX))
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
groundSpeed = gpsSol.groundSpeed / 100; groundSpeed = gpsSol.groundSpeed / 100;
char buf[20]; char buf[20];

6
src/main/telemetry/smartport.c
View file

@ -414,7 +414,11 @@ static bool smartPortShouldSendGPSData(void)
// or the craft has never been armed yet. This way if GPS stops working // or the craft has never been armed yet. This way if GPS stops working
// while in flight, the user will easily notice because the sensor will stop // while in flight, the user will easily notice because the sensor will stop
// updating. // updating.
return feature(FEATURE_GPS) && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX) || !ARMING_FLAG(WAS_EVER_ARMED)); return feature(FEATURE_GPS) && (STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
|| !ARMING_FLAG(WAS_EVER_ARMED));
} }
void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clearToSend, const uint32_t *requestTimeout) void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clearToSend, const uint32_t *requestTimeout)

12
src/main/telemetry/srxl.c
View file

@ -303,7 +303,11 @@ bool srxlFrameGpsLoc(sbuf_t *dst, timeUs_t currentTimeUs)
uint16_t altitudeLoBcd, groundCourseBcd, hdop; uint16_t altitudeLoBcd, groundCourseBcd, hdop;
uint8_t hdopBcd, gpsFlags; uint8_t hdopBcd, gpsFlags;
if (!feature(FEATURE_GPS) || !(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) || gpsSol.numSat < 6) { if (!feature(FEATURE_GPS) || !(STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) || gpsSol.numSat < 6) {
return false; return false;
} }
@ -371,7 +375,11 @@ bool srxlFrameGpsStat(sbuf_t *dst, timeUs_t currentTimeUs)
uint8_t numSatBcd, altitudeHighBcd; uint8_t numSatBcd, altitudeHighBcd;
bool timeProvided = false; bool timeProvided = false;
if (!feature(FEATURE_GPS) || !(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))|| gpsSol.numSat < 6) { if (!feature(FEATURE_GPS) || !(STATE(GPS_FIX)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
)|| gpsSol.numSat < 6) {
return false; return false;
} }