Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-13 19:40:27 +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
COMPASS_CALIBRATED = (1 << 8),
ACCELEROMETER_CALIBRATED = (1 << 9),
#ifdef USE_GPS_FIX_ESTIMATION
GPS_ESTIMATED_FIX = (1 << 10),
#endif
NAV_CRUISE_BRAKING = (1 << 11),
NAV_CRUISE_BRAKING_BOOST = (1 << 12),
NAV_CRUISE_BRAKING_LOCKED = (1 << 13),

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

@ -834,6 +834,7 @@ groups:
max: 600
- 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."
condition: USE_GPS_FIX_ESTIMATION
default_value: 7
min: -1
max: 600
@ -2195,6 +2196,7 @@ groups:
type: bool
- 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."
condition: USE_GPS_FIX_ESTIMATION
default_value: OFF
field: allow_gps_fix_estimation
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_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)
#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
#endif
);
typedef enum {
@ -355,7 +357,11 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
// Craft is closer than minimum failsafe procedure distance (if set to non-zero)
// GPS must also be working, and home position set
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
uint32_t distance = calculateDistanceToDestination(&original_rth_home);
@ -368,7 +374,7 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
return failsafeConfig()->failsafe_procedure;
}
#ifdef USE_GPS_FIX_ESTIMATION
bool checkGPSFixFailsafe(void)
{
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;
}
#endif
void failsafeUpdateState(void)
@ -418,9 +425,12 @@ void failsafeUpdateState(void)
failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
}
#ifdef USE_GPS_FIX_ESTIMATION
if ( checkGPSFixFailsafe() ) {
reprocessState = true;
} else if (!receivingRxDataAndNotFailsafeMode) {
} else
#endif
if (!receivingRxDataAndNotFailsafeMode) {
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
// 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
failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
reprocessState = true;
} else {
}
#ifdef USE_GPS_FIX_ESTIMATION
else {
if ( checkGPSFixFailsafe() ) {
reprocessState = true;
}
}
#endif
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)
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)
#ifdef USE_GPS_FIX_ESTIMATION
int16_t failsafe_gps_fix_estimation_delay; // Time delay before Failsafe triggered when GPX Fix estimation is applied (s)
#endif
} failsafeConfig_t;
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 receivingRxDataPeriodPreset; // preset for the required period of valid rxData
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
#endif
failsafeProcedure_e activeProcedure;
failsafePhase_e phase;
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)
{
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)

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

@ -52,7 +52,11 @@ static float lastFuselageDirection[XYZ_AXIS_COUNT];
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)
@ -90,8 +94,11 @@ void updateWindEstimator(timeUs_t currentTimeUs)
if (!STATE(FIXED_WING_LEGACY) ||
!isGPSHeadingValid() ||
!gpsSol.flags.validVelNE ||
!gpsSol.flags.validVelD ||
STATE(GPS_ESTIMATED_FIX)) {
!gpsSol.flags.validVelD
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) {
return;
}

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

@ -223,7 +223,7 @@ void gpsSetProtocolTimeout(timeMs_t timeoutMs)
gpsState.timeoutMs = timeoutMs;
}
#ifdef USE_GPS_FIX_ESTIMATION
bool canEstimateGPSFix(void)
{
#if defined(USE_GPS) && defined(USE_MAG) && defined(USE_BARO)
@ -239,9 +239,10 @@ bool canEstimateGPSFix(void)
#else
return false;
#endif
}
#endif
#ifdef USE_GPS_FIX_ESTIMATION
void processDisableGPSFix(void)
{
static int32_t last_lat = 0;
@ -268,7 +269,9 @@ void processDisableGPSFix(void)
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"
void updateEstimatedGPSFix(void)
{
@ -345,14 +348,17 @@ void updateEstimatedGPSFix(void)
gpsSol.velNED[Y] = (int16_t)(velY);
gpsSol.velNED[Z] = 0;
}
#endif
void gpsProcessNewDriverData(void)
{
gpsSol = gpsSolDRV;
#ifdef USE_GPS_FIX_ESTIMATION
processDisableGPSFix();
updateEstimatedGPSFix();
#endif
}
//called after:
@ -362,11 +368,13 @@ void gpsProcessNewDriverData(void)
//On GPS sensor timeout - called after updateEstimatedGPSFix()
void gpsProcessNewSolutionData(bool timeout)
{
#ifdef USE_GPS_FIX_ESTIMATION
if ( gpsSol.numSat == 99 ) {
ENABLE_STATE(GPS_ESTIMATED_FIX);
DISABLE_STATE(GPS_FIX);
} else {
DISABLE_STATE(GPS_ESTIMATED_FIX);
#endif
// Set GPS fix flag only if we have 3D fix
if (gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats) {
@ -379,7 +387,9 @@ void gpsProcessNewSolutionData(bool timeout)
gpsSol.flags.validEPE = false;
DISABLE_STATE(GPS_FIX);
}
#ifdef USE_GPS_FIX_ESTIMATION
}
#endif
if (!timeout) {
// 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);
DISABLE_STATE(GPS_FIX);
#ifdef USE_GPS_FIX_ESTIMATION
if ( canEstimateGPSFix() ) {
updateEstimatedGPSFix();
if (gpsSol.fixType == GPS_FIX_3D) { //estimation kicked in
gpsProcessNewSolutionData(true);
}
}
#endif
}
void gpsPreInit(void)
@ -577,7 +591,7 @@ bool gpsUpdate(void)
break;
}
if ( !sensors(SENSOR_GPS) && canEstimateGPSFix() ) {
if ( !sensors(SENSOR_GPS) ) {
gpsTryEstimateOnTimeout();
}
@ -630,7 +644,11 @@ bool isGPSHealthy(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

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

@ -147,7 +147,7 @@ static union {
bool gpsUbloxHasGalileo(void)
{
return (ubx_capabilities.supported & UBX_MON_GNSS_GALILEO_MASK);
}
}
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) {
TEXT_ATTRIBUTES_ADD_BLINK(*attr);
}
}
}
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);
float poiAngle = DEGREES_TO_RADIANS(directionToPoi);
@ -1429,7 +1433,11 @@ static void osdDisplayTelemetry(void)
static uint16_t trk_bearing = 0;
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) {
trk_bearing = GPS_directionToHome;
trk_bearing += 360 + 180;
@ -1786,12 +1794,16 @@ static bool osdDrawSingleElement(uint8_t item)
buff[0] = SYM_SAT_L;
buff[1] = SYM_SAT_R;
tfp_sprintf(buff + 2, "%2d", gpsSol.numSat);
#ifdef USE_GPS_FIX_ESTIMATION
if (STATE(GPS_ESTIMATED_FIX)) {
strcpy(buff + 2, "ES");
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
}
else if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) {
} else
#endif
if (!STATE(GPS_FIX)) {
if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) {
strcpy(buff + 2, "X!");
}
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
}
break;
@ -1843,7 +1855,11 @@ static bool osdDrawSingleElement(uint8_t item)
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) ) {
displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_HOME_NEAR);
}
@ -2331,11 +2347,19 @@ static bool osdDrawSingleElement(uint8_t item)
osdCrosshairPosition(&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);
}
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) {
osdHudClear();
@ -2959,7 +2983,11 @@ static bool osdDrawSingleElement(uint8_t item)
digits = 4U;
}
#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) {
value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
1, US2S(efficiencyTimeDelta));
@ -3030,7 +3058,11 @@ static bool osdDrawSingleElement(uint8_t item)
int32_t value = 0;
timeUs_t currentTimeUs = micros();
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) {
value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f,
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);
int digits = osdConfig()->plus_code_digits;
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));
} else {
// +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();
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) {
value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
1, US2S(efficiencyTimeDelta));

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

@ -2378,6 +2378,7 @@ bool validateRTHSanityChecker(void)
return true;
}
#ifdef USE_GPS_FIX_ESTIMATION
if (STATE(GPS_ESTIMATED_FIX)) {
//disable sanity checks in GPS estimation mode
//when estimated GPS fix is replaced with real fix, coordinates may jump
@ -2386,6 +2387,7 @@ bool validateRTHSanityChecker(void)
posControl.rthSanityChecker.lastCheckTime = currentTimeMs + 5000;
return true;
}
#endif
// Check at 10Hz rate
if ( ((int32_t)(currentTimeMs - posControl.rthSanityChecker.lastCheckTime)) > 100) {
@ -3993,7 +3995,11 @@ void updateWpMissionPlanner(void)
{
static timeMs_t resetTimerStart = 0;
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;
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;
#ifdef USE_GPS_FIX_ESTIMATION
uint8_t allow_gps_fix_estimation;
#endif
} positionEstimationConfig_t;
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,
.baro_epv = SETTING_INAV_BARO_EPV_DEFAULT,
#ifdef USE_GPS_FIX_ESTIMATION
.allow_gps_fix_estimation = SETTING_INAV_ALLOW_GPS_FIX_ESTIMATION_DEFAULT
#endif
);
#define resetTimer(tim, currentTimeUs) { (tim)->deltaTime = 0; (tim)->lastTriggeredTime = currentTimeUs; }
@ -152,12 +153,14 @@ static bool detectGPSGlitch(timeUs_t currentTimeUs)
bool isGlitching = false;
#ifdef USE_GPS_FIX_ESTIMATION
if (STATE(GPS_ESTIMATED_FIX)) {
//disable sanity checks in GPS estimation mode
//when estimated GPS fix is replaced with real fix, coordinates may jump
previousTime = 0;
return true;
}
#endif
if (previousTime == 0) {
isGlitching = false;
@ -210,8 +213,16 @@ void onNewGPSData(void)
newLLH.lon = gpsSol.llh.lon;
newLLH.alt = gpsSol.llh.alt;
if (sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)) {
if (!(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) {
if (sensors(SENSOR_GPS)
#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;
return;
}
@ -492,7 +503,11 @@ static bool navIsAccelerationUsable(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)
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 */
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)) &&
(posEstimator.gps.eph < 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)
{
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;
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;
#ifdef USE_GPS_FIX_ESTIMATION
case LOGIC_CONDITION_DISABLE_GPS_FIX:
if (operandA > 0) {
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX);
@ -481,6 +482,7 @@ static int logicConditionCompute(
}
return true;
break;
#endif
default:
return false;
@ -663,9 +665,12 @@ static int logicConditionGetFlightOperandValue(int operand) {
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS:
#ifdef USE_GPS_FIX_ESTIMATION
if ( STATE(GPS_ESTIMATED_FIX) ){
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;
} else {
return gpsSol.numSat;

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

@ -81,7 +81,9 @@ typedef enum {
LOGIC_CONDITION_TIMER = 49,
LOGIC_CONDITION_DELTA = 50,
LOGIC_CONDITION_APPROX_EQUAL = 51,
#ifdef USE_GPS_FIX_ESTIMATION
LOGIC_CONDITION_DISABLE_GPS_FIX = 52,
#endif
LOGIC_CONDITION_LAST = 53,
} logicOperation_e;
@ -186,7 +188,9 @@ typedef enum {
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL = (1 << 8),
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS = (1 << 9),
LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_FLIGHT_AXIS = (1 << 10),
#ifdef USE_GPS_FIX_ESTIMATION
LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX = (1 << 11),
#endif
} logicConditionsGlobalFlags_t;
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 (accIsHealthy()) {
return HW_SENSOR_OK;
}
else {
} else {
return HW_SENSOR_UNHEALTHY;
}
}
else {
} else {
if (requestedSensors[SENSOR_INDEX_ACC] != ACC_NONE) {
// Selected but not detected
return HW_SENSOR_UNAVAILABLE;
@ -135,8 +133,7 @@ hardwareSensorStatus_e getHwRangefinderStatus(void)
if (detectedSensors[SENSOR_INDEX_RANGEFINDER] != RANGEFINDER_NONE) {
if (rangefinderIsHealthy()) {
return HW_SENSOR_OK;
}
else {
} else {
return HW_SENSOR_UNHEALTHY;
}
}
@ -144,8 +141,7 @@ hardwareSensorStatus_e getHwRangefinderStatus(void)
if (requestedSensors[SENSOR_INDEX_RANGEFINDER] != RANGEFINDER_NONE) {
// Selected but not detected
return HW_SENSOR_UNAVAILABLE;
}
else {
} else {
// Not selected and not detected
return HW_SENSOR_NONE;
}
@ -161,8 +157,7 @@ hardwareSensorStatus_e getHwPitotmeterStatus(void)
if (detectedSensors[SENSOR_INDEX_PITOT] != PITOT_NONE) {
if (pitotIsHealthy()) {
return HW_SENSOR_OK;
}
else {
} else {
return HW_SENSOR_UNHEALTHY;
}
}
@ -170,8 +165,7 @@ hardwareSensorStatus_e getHwPitotmeterStatus(void)
if (requestedSensors[SENSOR_INDEX_PITOT] != PITOT_NONE) {
// Selected but not detected
return HW_SENSOR_UNAVAILABLE;
}
else {
} else {
// Not selected and not detected
return HW_SENSOR_NONE;
}
@ -187,8 +181,7 @@ hardwareSensorStatus_e getHwGPSStatus(void)
if (sensors(SENSOR_GPS)) {
if (isGPSHealthy()) {
return HW_SENSOR_OK;
}
else {
} else {
return HW_SENSOR_UNHEALTHY;
}
}
@ -196,8 +189,7 @@ hardwareSensorStatus_e getHwGPSStatus(void)
if (feature(FEATURE_GPS) && gpsStats.timeouts > 4) {
// Selected but not detected
return HW_SENSOR_UNAVAILABLE;
}
else {
} else {
// Not selected and not detected
return HW_SENSOR_NONE;
}
@ -213,8 +205,7 @@ hardwareSensorStatus_e getHwOpticalFlowStatus(void)
if (detectedSensors[SENSOR_INDEX_OPFLOW] != OPFLOW_NONE) {
if (opflowIsHealthy()) {
return HW_SENSOR_OK;
}
else {
} else {
return HW_SENSOR_UNHEALTHY;
}
}
@ -222,8 +213,7 @@ hardwareSensorStatus_e getHwOpticalFlowStatus(void)
if (requestedSensors[SENSOR_INDEX_OPFLOW] != OPFLOW_NONE) {
// Selected but not detected
return HW_SENSOR_UNAVAILABLE;
}
else {
} else {
// Not selected and not detected
return HW_SENSOR_NONE;
}

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

@ -56,6 +56,7 @@
#define USE_GPS_PROTO_MSP
#define USE_TELEMETRY
#define USE_TELEMETRY_LTM
#define USE_GPS_FIX_ESTIMATION
// This is the shortest period in microseconds that the scheduler will allow
#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);
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;
}
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);
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;
return;
}
@ -476,7 +480,11 @@ static bool processBinaryModeRequest(uint8_t address)
switch (address) {
#ifdef USE_GPS
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);
hottQueueSendResponse((uint8_t *)&hottGPSMessage, sizeof(hottGPSMessage));
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) {
#if defined(USE_GPS)
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;
else if (gpsSol.fixType == GPS_FIX_2D) fix = 2;
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
uint16_t status = flightModeToIBusTelemetryMode1[getFlightModeForTelemetry()];
#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 += fix * 100;
if (STATE(GPS_FIX_HOME)) status += 500;
@ -206,72 +214,128 @@ static uint8_t dispatchMeasurementRequest(ibusAddress_t address) {
return sendIbusMeasurement2(address, status);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_HEADING) { //HOME_DIR 0-360deg
#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
return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_DIST) { //HOME_DIST in m
#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
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
#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
return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_SPEED) {//SPEED in cm/s
#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
return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_COG) { //GPS_COURSE (0-360deg, 0=north)
#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
return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_STATUS) { //GPS_STATUS fix sat
#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
return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT) { //4byte
#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
return sendIbusMeasurement4(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON) { //4byte
#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
return sendIbusMeasurement4(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT1) { //GPS_LAT1 //Lattitude * 1e+7
#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
return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON1) { //GPS_LON1 //Longitude * 1e+7
#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
return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LAT2) { //GPS_LAT2 //Lattitude * 1e+7
#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
return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GPS_LON2) { //GPS_LON2 //Longitude * 1e+7
#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
return sendIbusMeasurement2(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GALT4) { //GPS_ALT //In cm => m
#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
return sendIbusMeasurement4(address, 0);
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_GALT) { //GPS_ALT //In cm => m
#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
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);
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);
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;
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)
gps_fix_type = 1;
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;
if (!(sensors(SENSOR_GPS) || STATE(GPS_ESTIMATED_FIX)))
if (!(sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
))
return;
if (gpsSol.fixType == GPS_NO_FIX)
@ -638,7 +642,11 @@ void mavlinkSendHUDAndHeartbeat(void)
#if defined(USE_GPS)
// 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;
}
#endif

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

@ -348,7 +348,11 @@ static void sendSMS(void)
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;
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
// while in flight, the user will easily notice because the sensor will stop
// 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)

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;
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;
}
@ -371,7 +375,11 @@ bool srxlFrameGpsStat(sbuf_t *dst, timeUs_t currentTimeUs)
uint8_t numSatBcd, altitudeHighBcd;
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;
}