From 599e45c48f2eb8cd91c462712b9eecdfbe05c84b Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Wed, 9 Aug 2023 09:48:46 +0200 Subject: [PATCH] added USE_GPS_FIX_ESTIMATION fixed indentation --- src/main/fc/fc_msp.c | 174 ++++---- src/main/fc/runtime_config.h | 20 +- src/main/fc/settings.yaml | 2 + src/main/flight/failsafe.c | 23 +- src/main/flight/failsafe.h | 4 + src/main/flight/imu.c | 6 +- src/main/flight/wind_estimator.c | 13 +- src/main/io/gps.c | 34 +- src/main/io/gps_ublox.c | 52 +-- src/main/io/osd.c | 408 ++++++++++-------- src/main/io/osd_dji_hd.c | 6 +- src/main/navigation/navigation.c | 8 +- src/main/navigation/navigation.h | 2 + src/main/navigation/navigation_fixedwing.c | 10 +- .../navigation/navigation_pos_estimator.c | 35 +- src/main/programming/logic_condition.c | 13 +- src/main/programming/logic_condition.h | 4 + src/main/sensors/diagnostics.c | 38 +- src/main/sensors/pitotmeter.c | 8 +- src/main/target/common.h | 1 + src/main/telemetry/ghst.c | 6 +- src/main/telemetry/hott.c | 12 +- src/main/telemetry/ibus_shared.c | 96 ++++- src/main/telemetry/jetiexbus.c | 6 +- src/main/telemetry/ltm.c | 6 +- src/main/telemetry/mavlink.c | 12 +- src/main/telemetry/sim.c | 6 +- src/main/telemetry/smartport.c | 6 +- src/main/telemetry/srxl.c | 12 +- 29 files changed, 626 insertions(+), 397 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 7ea2ff20eb..b4e2208745 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3492,8 +3492,8 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu if (!SIMULATOR_HAS_OPTION(HITL_ENABLE)) { - if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once - DISABLE_ARMING_FLAG(SIMULATOR_MODE_HITL); + if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once + DISABLE_ARMING_FLAG(SIMULATOR_MODE_HITL); #ifdef USE_BARO if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) { @@ -3508,15 +3508,15 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu // Review: Many states were affected. Reboot? disarm(DISARM_SWITCH); // Disarm to prevent motor output!!! - } + } } else { - if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once + if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once #ifdef USE_BARO if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) { sensorsSet(SENSOR_BARO); setTaskEnabled(TASK_BARO, true); DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE); - baroStartCalibration(); + baroStartCalibration(); } #endif @@ -3530,80 +3530,80 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu mag.magADC[Z] = 0; } #endif - ENABLE_ARMING_FLAG(SIMULATOR_MODE_HITL); + ENABLE_ARMING_FLAG(SIMULATOR_MODE_HITL); LOG_DEBUG(SYSTEM, "Simulator enabled"); } - if (dataSize >= 14) { + if (dataSize >= 14) { - if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) { - gpsSolDRV.fixType = sbufReadU8(src); - gpsSolDRV.hdop = gpsSolDRV.fixType == GPS_NO_FIX ? 9999 : 100; - gpsSolDRV.numSat = sbufReadU8(src); + if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) { + gpsSolDRV.fixType = sbufReadU8(src); + gpsSolDRV.hdop = gpsSolDRV.fixType == GPS_NO_FIX ? 9999 : 100; + gpsSolDRV.numSat = sbufReadU8(src); - if (gpsSolDRV.fixType != GPS_NO_FIX) { - gpsSolDRV.flags.validVelNE = true; - gpsSolDRV.flags.validVelD = true; - gpsSolDRV.flags.validEPE = true; - gpsSolDRV.flags.validTime = false; + if (gpsSolDRV.fixType != GPS_NO_FIX) { + gpsSolDRV.flags.validVelNE = true; + gpsSolDRV.flags.validVelD = true; + gpsSolDRV.flags.validEPE = true; + gpsSolDRV.flags.validTime = false; - gpsSolDRV.llh.lat = sbufReadU32(src); - gpsSolDRV.llh.lon = sbufReadU32(src); - gpsSolDRV.llh.alt = sbufReadU32(src); - gpsSolDRV.groundSpeed = (int16_t)sbufReadU16(src); - gpsSolDRV.groundCourse = (int16_t)sbufReadU16(src); + gpsSolDRV.llh.lat = sbufReadU32(src); + gpsSolDRV.llh.lon = sbufReadU32(src); + gpsSolDRV.llh.alt = sbufReadU32(src); + gpsSolDRV.groundSpeed = (int16_t)sbufReadU16(src); + gpsSolDRV.groundCourse = (int16_t)sbufReadU16(src); - gpsSolDRV.velNED[X] = (int16_t)sbufReadU16(src); - gpsSolDRV.velNED[Y] = (int16_t)sbufReadU16(src); - gpsSolDRV.velNED[Z] = (int16_t)sbufReadU16(src); + gpsSolDRV.velNED[X] = (int16_t)sbufReadU16(src); + gpsSolDRV.velNED[Y] = (int16_t)sbufReadU16(src); + gpsSolDRV.velNED[Z] = (int16_t)sbufReadU16(src); - gpsSolDRV.eph = 100; - gpsSolDRV.epv = 100; - } else { - sbufAdvance(src, sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); - } + gpsSolDRV.eph = 100; + gpsSolDRV.epv = 100; + } else { + sbufAdvance(src, sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); + } // Feed data to navigation - gpsProcessNewDriverData(); - gpsProcessNewSolutionData(false); - } else { - sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); - } + gpsProcessNewDriverData(); + gpsProcessNewSolutionData(false); + } else { + sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); + } - if (!SIMULATOR_HAS_OPTION(HITL_USE_IMU)) { - attitude.values.roll = (int16_t)sbufReadU16(src); - attitude.values.pitch = (int16_t)sbufReadU16(src); - attitude.values.yaw = (int16_t)sbufReadU16(src); - } else { - sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT); - } + if (!SIMULATOR_HAS_OPTION(HITL_USE_IMU)) { + attitude.values.roll = (int16_t)sbufReadU16(src); + attitude.values.pitch = (int16_t)sbufReadU16(src); + attitude.values.yaw = (int16_t)sbufReadU16(src); + } else { + sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT); + } // Get the acceleration in 1G units - acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f; - acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f; - acc.accADCf[Z] = ((int16_t)sbufReadU16(src)) / 1000.0f; - acc.accVibeSq[X] = 0.0f; - acc.accVibeSq[Y] = 0.0f; - acc.accVibeSq[Z] = 0.0f; + acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f; + acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f; + acc.accADCf[Z] = ((int16_t)sbufReadU16(src)) / 1000.0f; + acc.accVibeSq[X] = 0.0f; + acc.accVibeSq[Y] = 0.0f; + acc.accVibeSq[Z] = 0.0f; // Get the angular velocity in DPS - gyro.gyroADCf[X] = ((int16_t)sbufReadU16(src)) / 16.0f; - gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f; - gyro.gyroADCf[Z] = ((int16_t)sbufReadU16(src)) / 16.0f; + gyro.gyroADCf[X] = ((int16_t)sbufReadU16(src)) / 16.0f; + gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f; + gyro.gyroADCf[Z] = ((int16_t)sbufReadU16(src)) / 16.0f; - if (sensors(SENSOR_BARO)) { - baro.baroPressure = (int32_t)sbufReadU32(src); - baro.baroTemperature = DEGREES_TO_CENTIDEGREES(SIMULATOR_BARO_TEMP); - } else { - sbufAdvance(src, sizeof(uint32_t)); - } + if (sensors(SENSOR_BARO)) { + baro.baroPressure = (int32_t)sbufReadU32(src); + baro.baroTemperature = DEGREES_TO_CENTIDEGREES(SIMULATOR_BARO_TEMP); + } else { + sbufAdvance(src, sizeof(uint32_t)); + } - if (sensors(SENSOR_MAG)) { - mag.magADC[X] = ((int16_t)sbufReadU16(src)) / 20; // 16000 / 20 = 800uT - mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20; //note that mag failure is simulated by setting all readings to zero - mag.magADC[Z] = ((int16_t)sbufReadU16(src)) / 20; - } else { - sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT); - } + if (sensors(SENSOR_MAG)) { + mag.magADC[X] = ((int16_t)sbufReadU16(src)) / 20; // 16000 / 20 = 800uT + mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20; //note that mag failure is simulated by setting all readings to zero + mag.magADC[Z] = ((int16_t)sbufReadU16(src)) / 20; + } else { + sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT); + } #if defined(USE_FAKE_BATT_SENSOR) if (SIMULATOR_HAS_OPTION(HITL_EXT_BATTERY_VOLTAGE)) { @@ -3617,7 +3617,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { simulatorData.airSpeed = sbufReadU16(src); - } else { + } else { if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) { sbufReadU16(src); } @@ -3626,35 +3626,35 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) { simulatorData.flags |= ((uint16_t)sbufReadU8(src)) << 8; } - } else { - DISABLE_STATE(GPS_FIX); - } - } + } else { + DISABLE_STATE(GPS_FIX); + } + } - sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_ROLL]); - sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_PITCH]); - sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_YAW]); - sbufWriteU16(dst, (uint16_t)(ARMING_FLAG(ARMED) ? simulatorData.input[INPUT_STABILIZED_THROTTLE] : -500)); + sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_ROLL]); + sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_PITCH]); + sbufWriteU16(dst, (uint16_t)simulatorData.input[INPUT_STABILIZED_YAW]); + sbufWriteU16(dst, (uint16_t)(ARMING_FLAG(ARMED) ? simulatorData.input[INPUT_STABILIZED_THROTTLE] : -500)); - simulatorData.debugIndex++; - if (simulatorData.debugIndex == 8) { - simulatorData.debugIndex = 0; - } + simulatorData.debugIndex++; + if (simulatorData.debugIndex == 8) { + simulatorData.debugIndex = 0; + } - tmp_u8 = simulatorData.debugIndex | - ((mixerConfig()->platformType == PLATFORM_AIRPLANE) ? 128 : 0) | - (ARMING_FLAG(ARMED) ? 64 : 0) | - (!feature(FEATURE_OSD) ? 32: 0) | - (!isOSDTypeSupportedBySimulator() ? 16 : 0); + tmp_u8 = simulatorData.debugIndex | + ((mixerConfig()->platformType == PLATFORM_AIRPLANE) ? 128 : 0) | + (ARMING_FLAG(ARMED) ? 64 : 0) | + (!feature(FEATURE_OSD) ? 32: 0) | + (!isOSDTypeSupportedBySimulator() ? 16 : 0); - sbufWriteU8(dst, tmp_u8); - sbufWriteU32(dst, debug[simulatorData.debugIndex]); + sbufWriteU8(dst, tmp_u8); + sbufWriteU32(dst, debug[simulatorData.debugIndex]); - sbufWriteU16(dst, attitude.values.roll); - sbufWriteU16(dst, attitude.values.pitch); - sbufWriteU16(dst, attitude.values.yaw); + sbufWriteU16(dst, attitude.values.roll); + sbufWriteU16(dst, attitude.values.pitch); + sbufWriteU16(dst, attitude.values.yaw); - mspWriteSimulatorOSD(dst); + mspWriteSimulatorOSD(dst); *ret = MSP_RESULT_ACK; break; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 3b55a4b281..48cf080ddd 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -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), @@ -176,12 +178,12 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void); typedef enum { HITL_RESET_FLAGS = (0 << 0), - HITL_ENABLE = (1 << 0), - HITL_SIMULATE_BATTERY = (1 << 1), - HITL_MUTE_BEEPER = (1 << 2), - HITL_USE_IMU = (1 << 3), // Use the Acc and Gyro data provided by XPlane to calculate Attitude (i.e. 100% of the calculations made by AHRS from INAV) - HITL_HAS_NEW_GPS_DATA = (1 << 4), - HITL_EXT_BATTERY_VOLTAGE = (1 << 5), // Extend MSP_SIMULATOR format 2 + HITL_ENABLE = (1 << 0), + HITL_SIMULATE_BATTERY = (1 << 1), + HITL_MUTE_BEEPER = (1 << 2), + HITL_USE_IMU = (1 << 3), // Use the Acc and Gyro data provided by XPlane to calculate Attitude (i.e. 100% of the calculations made by AHRS from INAV) + HITL_HAS_NEW_GPS_DATA = (1 << 4), + HITL_EXT_BATTERY_VOLTAGE = (1 << 5), // Extend MSP_SIMULATOR format 2 HITL_AIRSPEED = (1 << 6), HITL_EXTENDED_FLAGS = (1 << 7), // Extend MSP_SIMULATOR format 2 HITL_GPS_TIMEOUT = (1 << 8), @@ -189,10 +191,10 @@ typedef enum { } simulatorFlags_t; typedef struct { - simulatorFlags_t flags; - uint8_t debugIndex; + simulatorFlags_t flags; + uint8_t debugIndex; uint8_t vbat; // 126 -> 12.6V - uint16_t airSpeed; // cm/s + uint16_t airSpeed; // cm/s int16_t input[4]; } simulatorData_t; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index fb25aeee7c..b1e4d3f603 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -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 diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index e2cd71cc9d..d1d1ede628 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -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 @@ -432,7 +442,7 @@ void failsafeUpdateState(void) failsafeState.wpModeDelayedFailsafeStart = 0; } reprocessState = true; - } + } } else { // When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode) if (!receivingRxDataAndNotFailsafeMode) { @@ -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; diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index d80ab650c7..459a201d24 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -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; diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index b46224063f..94223f0449 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -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) diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c index b6174611ad..1cf3b8941e 100644 --- a/src/main/flight/wind_estimator.c +++ b/src/main/flight/wind_estimator.c @@ -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; } diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 965b89531a..0e5d166c7c 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -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) @@ -238,10 +238,11 @@ bool canEstimateGPSFix(void) #else return false; -#endif - +#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); - updateEstimatedGPSFix(); +#ifdef USE_GPS_FIX_ESTIMATION + if ( canEstimateGPSFix() ) { + updateEstimatedGPSFix(); - if (gpsSol.fixType == GPS_FIX_3D) { //estimation kicked in - gpsProcessNewSolutionData(true); + 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 diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 10f1e6d947..59eb68bd93 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -147,7 +147,7 @@ static union { bool gpsUbloxHasGalileo(void) { return (ubx_capabilities.supported & UBX_MON_GNSS_GALILEO_MASK); - } +} bool gpsUbloxHasBeidou(void) { @@ -622,8 +622,8 @@ static bool gpsParseFrameUBLOX(void) gpsState.hwVersion = gpsDecodeHardwareVersion(_buffer.ver.hwVersion, sizeof(_buffer.ver.hwVersion)); if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) { if (_buffer.ver.swVersion[9] > '2' || true) { - // check extensions; - // after hw + sw vers; each is 30 bytes + // check extensions; + // after hw + sw vers; each is 30 bytes bool found = false; for (int j = 40; j < _payload_length && !found; j += 30) { @@ -863,8 +863,8 @@ STATIC_PROTOTHREAD(gpsConfigure) if(_ack_state == UBX_ACK_GOT_NAK) { // Fallback to safe 5Hz in case of error configureRATE(hz2rate(5)); // 5Hz - ptWait(_ack_state == UBX_ACK_GOT_ACK); - } + ptWait(_ack_state == UBX_ACK_GOT_ACK); + } } else { // u-Blox 5/6/7/8 or unknown @@ -901,8 +901,8 @@ STATIC_PROTOTHREAD(gpsConfigure) if(_ack_state == UBX_ACK_GOT_NAK) { // Fallback to safe 5Hz in case of error configureRATE(hz2rate(5)); // 5Hz - ptWait(_ack_state == UBX_ACK_GOT_ACK); - } + ptWait(_ack_state == UBX_ACK_GOT_ACK); + } } // u-Blox 5/6 doesn't support PVT, use legacy config // UNKNOWN also falls here, use as a last resort @@ -944,19 +944,19 @@ STATIC_PROTOTHREAD(gpsConfigure) // Configure GNSS for M8N and later if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) { - gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); - if(gpsState.hwVersion >= UBX_HW_VERSION_UBLOX10 || (gpsState.swVersionMajor>=23 && gpsState.swVersionMinor >= 1)) { + gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); + if(gpsState.hwVersion >= UBX_HW_VERSION_UBLOX10 || (gpsState.swVersionMajor>=23 && gpsState.swVersionMinor >= 1)) { configureGNSS10(); - } else { - configureGNSS(); - } - ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); + } else { + configureGNSS(); + } + ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); - if(_ack_state == UBX_ACK_GOT_NAK) { + if(_ack_state == UBX_ACK_GOT_NAK) { gpsConfigMutable()->ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT; gpsConfigMutable()->ubloxUseBeidou = SETTING_GPS_UBLOX_USE_BEIDOU_DEFAULT; gpsConfigMutable()->ubloxUseGlonass = SETTING_GPS_UBLOX_USE_GLONASS_DEFAULT; - } + } } ptEnd(0); @@ -1019,18 +1019,18 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]); } - // Reset protocol timeout - gpsSetProtocolTimeout(MAX(GPS_TIMEOUT, ((GPS_VERSION_RETRY_TIMES + 3) * GPS_CFG_CMD_TIMEOUT_MS))); + // Reset protocol timeout + gpsSetProtocolTimeout(MAX(GPS_TIMEOUT, ((GPS_VERSION_RETRY_TIMES + 3) * GPS_CFG_CMD_TIMEOUT_MS))); - // Attempt to detect GPS hw version - gpsState.hwVersion = UBX_HW_VERSION_UNKNOWN; - gpsState.autoConfigStep = 0; + // Attempt to detect GPS hw version + gpsState.hwVersion = UBX_HW_VERSION_UNKNOWN; + gpsState.autoConfigStep = 0; - do { - pollVersion(); - gpsState.autoConfigStep++; - ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS); - } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN); + do { + pollVersion(); + gpsState.autoConfigStep++; + ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS); + } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN); gpsState.autoConfigStep = 0; ubx_capabilities.supported = ubx_capabilities.enabledGnss = ubx_capabilities.defaultGnss = 0; @@ -1062,7 +1062,7 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) { pollVersion(); ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); - } + } pollGnssCapabilities(); ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 518cdc61aa..51ca9d03a7 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -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) @@ -1114,7 +1114,7 @@ static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes if (useScaled) { buff[0] = SYM_SCALE; } else { - buff[0] = SYM_BLANK; + buff[0] = SYM_BLANK; } buff[1] = SYM_THR; if (navigationIsControllingThrottle()) { @@ -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; @@ -1729,7 +1737,7 @@ static bool osdDrawSingleElement(uint8_t item) // Shown in Ah buff[mah_digits] = SYM_AH; } else { - // Shown in mAh + // Shown in mAh buff[mah_digits] = SYM_MAH; } buff[mah_digits + 1] = '\0'; @@ -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) { - strcpy(buff + 2, "X!"); + } 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,8 +2983,12 @@ static bool osdDrawSingleElement(uint8_t item) digits = 4U; } #endif - if ((STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && gpsSol.groundSpeed > 0) { - if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { + 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 @@ -4112,13 +4148,13 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) if (isSinglePageStatsCompatible) { displayWrite(osdDisplayPort, statNameX, top++, "--- STATS ---"); } else if (page == 0) { - displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 1/2 ->"); + displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 1/2 ->"); } else if (page == 1) { displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- <- 2/2"); } if (isSinglePageStatsCompatible || page == 0) { - if (feature(FEATURE_GPS)) { + if (feature(FEATURE_GPS)) { if (isSinglePageStatsCompatible) { displayWrite(osdDisplayPort, statNameX, top, "MAX/AVG SPEED :"); osdFormatVelocityStr(buff, stats.max_3D_speed, true, false); @@ -4130,32 +4166,32 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) osdLeftAlignString(buff); displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); } else { - displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED :"); - osdFormatVelocityStr(buff, stats.max_3D_speed, true, false); - osdLeftAlignString(buff); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED :"); + osdFormatVelocityStr(buff, stats.max_3D_speed, true, false); + osdLeftAlignString(buff); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "AVG SPEED :"); - osdGenerateAverageVelocityStr(buff); - osdLeftAlignString(buff); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "AVG SPEED :"); + osdGenerateAverageVelocityStr(buff); + osdLeftAlignString(buff); + displayWrite(osdDisplayPort, statValuesX, top++, buff); } - displayWrite(osdDisplayPort, statNameX, top, "MAX DISTANCE :"); - osdFormatDistanceStr(buff, stats.max_distance*100); + displayWrite(osdDisplayPort, statNameX, top, "MAX DISTANCE :"); + osdFormatDistanceStr(buff, stats.max_distance*100); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + + displayWrite(osdDisplayPort, statNameX, top, "TRAVELED DISTANCE:"); + osdFormatDistanceStr(buff, getTotalTravelDistance()); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + } + + displayWrite(osdDisplayPort, statNameX, top, "MAX ALTITUDE :"); + osdFormatAltitudeStr(buff, stats.max_altitude); displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "TRAVELED DISTANCE:"); - osdFormatDistanceStr(buff, getTotalTravelDistance()); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - } - - displayWrite(osdDisplayPort, statNameX, top, "MAX ALTITUDE :"); - osdFormatAltitudeStr(buff, stats.max_altitude); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - - switch (rxConfig()->serialrx_provider) { - case SERIALRX_CRSF: + switch (rxConfig()->serialrx_provider) { + case SERIALRX_CRSF: if (isSinglePageStatsCompatible) { displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI %/DBM :"); itoa(stats.min_rssi, buff, 10); @@ -4168,172 +4204,172 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) osdLeftAlignString(buff); displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); } else { - displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI % :"); - itoa(stats.min_rssi, buff, 10); - strcat(buff, "%"); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI % :"); + itoa(stats.min_rssi, buff, 10); + strcat(buff, "%"); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI DBM :"); - itoa(stats.min_rssi_dbm, buff, 10); - tfp_sprintf(buff, "%s%c", buff, SYM_DBM); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI DBM :"); + itoa(stats.min_rssi_dbm, buff, 10); + tfp_sprintf(buff, "%s%c", buff, SYM_DBM); + displayWrite(osdDisplayPort, statValuesX, top++, buff); } - displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :"); - itoa(stats.min_lq, buff, 10); - strcat(buff, "%"); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - break; - default: - displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); - itoa(stats.min_rssi, buff, 10); - strcat(buff, "%"); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :"); + itoa(stats.min_lq, buff, 10); + strcat(buff, "%"); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + break; + default: + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); + itoa(stats.min_rssi, buff, 10); + strcat(buff, "%"); + displayWrite(osdDisplayPort, statValuesX, top++, buff); } - displayWrite(osdDisplayPort, statNameX, top, "FLY TIME :"); - uint16_t flySeconds = getFlightTime(); - uint16_t flyMinutes = flySeconds / 60; - flySeconds %= 60; - uint16_t flyHours = flyMinutes / 60; - flyMinutes %= 60; - tfp_sprintf(buff, "%02u:%02u:%02u", flyHours, flyMinutes, flySeconds); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "FLY TIME :"); + uint16_t flySeconds = getFlightTime(); + uint16_t flyMinutes = flySeconds / 60; + flySeconds %= 60; + uint16_t flyHours = flyMinutes / 60; + flyMinutes %= 60; + tfp_sprintf(buff, "%02u:%02u:%02u", flyHours, flyMinutes, flySeconds); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY :"); - displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]); - } + displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY :"); + displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]); + } if (isSinglePageStatsCompatible || page == 1) { - if (osdConfig()->stats_min_voltage_unit == OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY) { - displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :"); - osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2); - } else { - displayWrite(osdDisplayPort, statNameX, top, "MIN CELL VOLTAGE :"); - osdFormatCentiNumber(buff, stats.min_voltage/getBatteryCellCount(), 0, 2, 0, 3); - } - tfp_sprintf(buff, "%s%c", buff, SYM_VOLT); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - - if (feature(FEATURE_CURRENT_METER)) { - displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT :"); - osdFormatCentiNumber(buff, stats.max_current, 0, 2, 0, 3); - tfp_sprintf(buff, "%s%c", buff, SYM_AMP); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - - displayWrite(osdDisplayPort, statNameX, top, "MAX POWER :"); - bool kiloWatt = osdFormatCentiNumber(buff, stats.max_power, 1000, 2, 2, 3); - buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; - buff[4] = '\0'; - displayWrite(osdDisplayPort, statValuesX, top++, buff); - - displayWrite(osdDisplayPort, statNameX, top, "USED CAPACITY :"); - if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - tfp_sprintf(buff, "%d%c", (int)getMAhDrawn(), SYM_MAH); + if (osdConfig()->stats_min_voltage_unit == OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY) { + displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :"); + osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2); } else { - osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3); - tfp_sprintf(buff, "%s%c", buff, SYM_WH); + displayWrite(osdDisplayPort, statNameX, top, "MIN CELL VOLTAGE :"); + osdFormatCentiNumber(buff, stats.min_voltage/getBatteryCellCount(), 0, 2, 0, 3); } + tfp_sprintf(buff, "%s%c", buff, SYM_VOLT); displayWrite(osdDisplayPort, statValuesX, top++, buff); - int32_t totalDistance = getTotalTravelDistance(); - bool moreThanAh = false; - bool efficiencyValid = totalDistance >= 10000; - if (feature(FEATURE_GPS)) { - displayWrite(osdDisplayPort, statNameX, top, "AVG EFFICIENCY :"); - uint8_t digits = 3U; // Total number of digits (including decimal point) - #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values - if (isBfCompatibleVideoSystem(osdConfig())) { - // Add one digit so no switch to scaled decimal occurs above 99 - digits = 4U; - } - #endif - switch (osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, digits); - if (!moreThanAh) { - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_1); - } else { - tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI); - } - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; - buff[3] = SYM_MAH_MI_0; - buff[4] = SYM_MAH_MI_1; - buff[5] = '\0'; - } - } else { - osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits); - tfp_sprintf(buff, "%s%c", buff, SYM_WH_MI); - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; - } - } - break; - case OSD_UNIT_GA: - if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, digits); - if (!moreThanAh) { - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_NM_0, SYM_MAH_NM_1); - } else { - tfp_sprintf(buff, "%s%c", buff, SYM_AH_NM); - } - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; - buff[3] = SYM_MAH_NM_0; - buff[4] = SYM_MAH_NM_1; - buff[5] = '\0'; - } - } else { - osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_NAUTICALMILE / totalDistance), 0, 2, 0, digits); - tfp_sprintf(buff, "%s%c", buff, SYM_WH_NM); - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; - } - } - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, digits); - if (!moreThanAh) { - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_1); - } else { - tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM); - } - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; - buff[3] = SYM_MAH_KM_0; - buff[4] = SYM_MAH_KM_1; - buff[5] = '\0'; - } - } else { - osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10000.0f / totalDistance), 0, 2, 0, digits); - tfp_sprintf(buff, "%s%c", buff, SYM_WH_KM); - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; - } - } - break; - } - osdLeftAlignString(buff); + if (feature(FEATURE_CURRENT_METER)) { + displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT :"); + osdFormatCentiNumber(buff, stats.max_current, 0, 2, 0, 3); + tfp_sprintf(buff, "%s%c", buff, SYM_AMP); displayWrite(osdDisplayPort, statValuesX, top++, buff); + + displayWrite(osdDisplayPort, statNameX, top, "MAX POWER :"); + bool kiloWatt = osdFormatCentiNumber(buff, stats.max_power, 1000, 2, 2, 3); + buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; + buff[4] = '\0'; + displayWrite(osdDisplayPort, statValuesX, top++, buff); + + displayWrite(osdDisplayPort, statNameX, top, "USED CAPACITY :"); + if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { + tfp_sprintf(buff, "%d%c", (int)getMAhDrawn(), SYM_MAH); + } else { + osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3); + tfp_sprintf(buff, "%s%c", buff, SYM_WH); + } + displayWrite(osdDisplayPort, statValuesX, top++, buff); + + int32_t totalDistance = getTotalTravelDistance(); + bool moreThanAh = false; + bool efficiencyValid = totalDistance >= 10000; + if (feature(FEATURE_GPS)) { + displayWrite(osdDisplayPort, statNameX, top, "AVG EFFICIENCY :"); + uint8_t digits = 3U; // Total number of digits (including decimal point) + #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values + if (isBfCompatibleVideoSystem(osdConfig())) { + // Add one digit so no switch to scaled decimal occurs above 99 + digits = 4U; + } + #endif + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, digits); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI); + } + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + buff[3] = SYM_MAH_MI_0; + buff[4] = SYM_MAH_MI_1; + buff[5] = '\0'; + } + } else { + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits); + tfp_sprintf(buff, "%s%c", buff, SYM_WH_MI); + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + } + } + break; + case OSD_UNIT_GA: + if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, digits); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_NM_0, SYM_MAH_NM_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_NM); + } + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + buff[3] = SYM_MAH_NM_0; + buff[4] = SYM_MAH_NM_1; + buff[5] = '\0'; + } + } else { + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_NAUTICALMILE / totalDistance), 0, 2, 0, digits); + tfp_sprintf(buff, "%s%c", buff, SYM_WH_NM); + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + } + } + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, digits); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM); + } + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + buff[3] = SYM_MAH_KM_0; + buff[4] = SYM_MAH_KM_1; + buff[5] = '\0'; + } + } else { + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10000.0f / totalDistance), 0, 2, 0, digits); + tfp_sprintf(buff, "%s%c", buff, SYM_WH_KM); + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + } + } + break; + } + osdLeftAlignString(buff); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + } } - } - const float max_gforce = accGetMeasuredMaxG(); - displayWrite(osdDisplayPort, statNameX, top, "MAX G-FORCE :"); - osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + const float max_gforce = accGetMeasuredMaxG(); + displayWrite(osdDisplayPort, statNameX, top, "MAX G-FORCE :"); + osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - const acc_extremes_t *acc_extremes = accGetMeasuredExtremes(); + const acc_extremes_t *acc_extremes = accGetMeasuredExtremes(); const float acc_extremes_min = acc_extremes[Z].min; const float acc_extremes_max = acc_extremes[Z].max; - displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:"); + displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:"); osdFormatCentiNumber(buff, acc_extremes_min * 100, 0, 2, 0, 4); osdLeftAlignString(buff); strcat(osdFormatTrimWhiteSpace(buff),"/"); diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index b8ddd058cb..7b837e8cee 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -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)); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 47d644b70b..f289a4ab27 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -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) { diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 5b1abf0f88..717f0df0e7 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -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); diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 51aa03b5d4..405a06b4a5 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -313,13 +313,13 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) loiterCenterPos.x = posControl.activeWaypoint.pos.x + navLoiterRadius * cos_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing)); loiterCenterPos.y = posControl.activeWaypoint.pos.y + navLoiterRadius * sin_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing)); - posErrorX = loiterCenterPos.x - navGetCurrentActualPositionAndVelocity()->pos.x; - posErrorY = loiterCenterPos.y - navGetCurrentActualPositionAndVelocity()->pos.y; + posErrorX = loiterCenterPos.x - navGetCurrentActualPositionAndVelocity()->pos.x; + posErrorY = loiterCenterPos.y - navGetCurrentActualPositionAndVelocity()->pos.y; - // turn direction to next waypoint - loiterTurnDirection = posControl.activeWaypoint.nextTurnAngle > 0 ? 1 : -1; // 1 = right + // turn direction to next waypoint + loiterTurnDirection = posControl.activeWaypoint.nextTurnAngle > 0 ? 1 : -1; // 1 = right - needToCalculateCircularLoiter = true; + needToCalculateCircularLoiter = true; } posControl.flags.wpTurnSmoothingActive = true; } diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 9fed0f6978..50f961f1d6 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -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 diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index f4600b7351..19ef76e515 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -471,8 +471,9 @@ static int logicConditionCompute( } else { return false; } - break; + 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); @@ -480,8 +481,9 @@ static int logicConditionCompute( LOGIC_CONDITION_GLOBAL_FLAG_DISABLE(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX); } return true; - break; - + break; +#endif + default: return false; break; @@ -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; diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 6956539391..1dd197be9f 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -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 { diff --git a/src/main/sensors/diagnostics.c b/src/main/sensors/diagnostics.c index cc874b6bb8..56c4d12417 100644 --- a/src/main/sensors/diagnostics.c +++ b/src/main/sensors/diagnostics.c @@ -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; @@ -64,13 +62,13 @@ hardwareSensorStatus_e getHwCompassStatus(void) { #if defined(USE_MAG) #ifdef USE_SIMULATOR - if ((ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) && sensors(SENSOR_MAG)) { + if ((ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) && sensors(SENSOR_MAG)) { if (compassIsHealthy()) { return HW_SENSOR_OK; } else { return HW_SENSOR_UNHEALTHY; } - } + } #endif if (detectedSensors[SENSOR_INDEX_MAG] != MAG_NONE) { if (compassIsHealthy()) { @@ -96,7 +94,7 @@ hardwareSensorStatus_e getHwBarometerStatus(void) { #if defined(USE_BARO) #ifdef USE_SIMULATOR - if (ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) { + if (ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) { if (requestedSensors[SENSOR_INDEX_BARO] == BARO_NONE) { return HW_SENSOR_NONE; } else if (baroIsHealthy()) { @@ -104,7 +102,7 @@ hardwareSensorStatus_e getHwBarometerStatus(void) } else { return HW_SENSOR_UNHEALTHY; } - } + } #endif if (detectedSensors[SENSOR_INDEX_BARO] != BARO_NONE) { if (baroIsHealthy()) { @@ -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; } diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index 2fae0a1692..e24898cdd1 100644 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -215,14 +215,14 @@ STATIC_PROTOTHREAD(pitotThread) pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, NULL); #ifdef USE_SIMULATOR - if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { + if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE; } #endif #if defined(USE_PITOT_FAKE) if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { pitotPressureTmp = sq(fakePitotGetAirspeed()) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE; - } + } #endif ptYield(); @@ -248,14 +248,14 @@ STATIC_PROTOTHREAD(pitotThread) pitot.airSpeed = 0.0f; } #ifdef USE_SIMULATOR - if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { + if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { pitot.airSpeed = simulatorData.airSpeed; } #endif #if defined(USE_PITOT_FAKE) if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { pitot.airSpeed = fakePitotGetAirspeed(); - } + } #endif } diff --git a/src/main/target/common.h b/src/main/target/common.h index d18bdfb1a2..242e03fc62 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -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 diff --git a/src/main/telemetry/ghst.c b/src/main/telemetry/ghst.c index a9630efa31..c468d2cfac 100644 --- a/src/main/telemetry/ghst.c +++ b/src/main/telemetry/ghst.c @@ -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)) { diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index a9810eb8f6..984b03caa6 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -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; diff --git a/src/main/telemetry/ibus_shared.c b/src/main/telemetry/ibus_shared.c index 1bab900776..3c7dfd2875 100644 --- a/src/main/telemetry/ibus_shared.c +++ b/src/main/telemetry/ibus_shared.c @@ -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); } diff --git a/src/main/telemetry/jetiexbus.c b/src/main/telemetry/jetiexbus.c index 53de96cc5d..b0ffad8434 100644 --- a/src/main/telemetry/jetiexbus.c +++ b/src/main/telemetry/jetiexbus.c @@ -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; } diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index e194cfed93..84dccddb86 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -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) diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index c075926580..3090ed68dc 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -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 diff --git a/src/main/telemetry/sim.c b/src/main/telemetry/sim.c index a0537bfcd5..9b1f7cc640 100644 --- a/src/main/telemetry/sim.c +++ b/src/main/telemetry/sim.c @@ -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]; diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 64a6559f1d..9d804be52d 100755 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -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) diff --git a/src/main/telemetry/srxl.c b/src/main/telemetry/srxl.c index d8a8c15c33..c341363f16 100644 --- a/src/main/telemetry/srxl.c +++ b/src/main/telemetry/srxl.c @@ -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; }