diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md new file mode 100644 index 00000000000..d8175549d35 --- /dev/null +++ b/docs/GPS_fix_estimation.md @@ -0,0 +1,132 @@ +# GPS Fix estimation (dead reconing, RTH without GPS) for fixed wing + +Video demonstration + +[![IMAGE ALT TEXT HERE](https://img.youtube.com/vi/wzvgRpXCS4U/0.jpg)](https://www.youtube.com/watch?v=wzvgRpXCS4U) + +There is possibility to allow plane to estimate it's position when GPS fix is lost. +The main purpose is RTH without GPS. +It works for fixed wing only. + +Plane should have the following sensors: +- acceleromenter, gyroscope +- barometer +- GPS +- magnethometer (optional, highly recommended) +- pitot (optional) + +By befault, all navigation modes are disabled when GPS fix is lost. If RC signal is lost also, plane will not be able to enable RTH. Plane will switch to LANDING instead. When flying above unreachable spaces, plane will be lost. + +GPS fix estimation allows to recover plane using magnetometer and baromener only. + +GPS Fix is also estimated on GPS Sensor timeouts (hardware failures). + +Note, that GPS fix estimation is not a solution for navigation without GPS. Without GPS fix, position error accumulates quickly. But it is acceptable for RTH. This is not a solution for flying under spoofing also. GPS is the most trusted sensor in Inav. It's output is not validated. + +# How it works ? + +In normal situation, plane is receiving it's position from GPS sensor. This way it is able to hold course, RTH or navigate by waypoints. + +Without GPS fix, plane has nose heading from magnetometer and height from barometer only. + +To navigate without GPS fix, we make the following assumptions: +- plane is flying in the direction where nose is pointing +- (if pitot tube is not installed) plane is flying with constant airspeed, specified in settings + +It is possible to roughly estimate position using theese assumptions. To increase accuracy, plane will use information about wind direction and speed, estimated before GPS fix was lost. To increase groundspeed estimation accuracy, plane will use pitot tube data(if available). + +From estimated heading direction and speed, plane is able to **roughty** estimate it's position. + +It is assumed, that plane will fly in roughly estimated direction to home position untill either GPS fix or RC signal is recovered. + +*Plane has to aquire GPS fix and store home position before takeoff. Estimation completely without GPS fix will not work*. + +# Estimation without magnethometer + +Without magnethometer, navigation accuracy is very poor. The problem is heading drift. + +The longer plane flies without magnethometer or GPS, the bigger is course estimation error. + +After few minutes and few turns, "North" direction estimation can be completely broken. +In general, accuracy is enough to perform RTH U-turn when both RC controls and GPS are lost, and roughtly keep RTH direction in areas with occasional GPS outages. + +![image](https://github.com/RomanLut/inav/assets/11955117/3d5c5d10-f43a-45f9-a647-af3cca87c4da) + +(purple line - estimated position, black line - real position). + +It is recommened to use GPS fix estimation without magnethometer as last resort only. For example, if plane is flying above lake, landing means loss of plane. With GPS Fix estimation, plane will try to do RTH in very rought direction, instead of landing. + +It is up to user to estimate the risk of fly-away. + + +# Settings + +GPS Fix estimation is enabled with CLI command: + +```set inav_allow_gps_fix_estimation=ON``` + +Also you have to specify cruise airspeed of the plane. + +To find out cruise airspeed, make a test flight. Enable ground speed display on OSD. Flight in CRUISE mode in two opposite directions. Take average speed. + +Cruise airspeed is specified in cm/s. + +To convert km/h to m/s, multiply by 27.77. + + +Example: 100 km/h = 100 * 27.77 = 2777 cm/s + +```set fw_reference_airspeed=2777``` + +*It is important, that plane fly with specified speed in CRUISE mode. If you have set option "Increase cruise speed with throttle" - do not use it without GPS Fix.* + +*If pitot is available, pitot sensor data will be used instead of constant. It is not necessary to specify fw_reference_airspeed. However, it is still adviced to specify for the case of pitot failure.* + +*Note related command: to continue mission without RC signal, see command ```set failsafe_mission_delay=-1```.* + +**After entering CLI command, make sure that settings are saved:** + +```save``` + +# Disabling GPS sensor from RC controller + +![](Screenshots/programming_disable_gps_sensor_fix.png) + +For testing purposes, it is possible to disable GPS sensor fix from RC controller in programming tab: + +*GPS can be disabled only after: 1) initial GPS fix is acquired 2) in ARMED mode.* + +# Allowing wp missions with GPS Fix estimation + +```failsafe_gps_fix_estimation_delay``` + +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. RTH is trigerred regradless of failsafe procedure selected in configurator. + +# Expected error (mag + baro) + +Realistic expected error is up to 200m per 1km of flight path. In tests, 500m drift per 5km path was seen. + +To dicrease drift: +- fly one large circle with GPS available to get good wind estimation +- use airspeed sensor. If airspeed sensor is not installed, fly in cruise mode without throttle override. +- do smooth, large turns +- make sure compass is pointing in nose direction precicely +- calibrate compass correctly + +This video shows real world test where GPS was disabled occasionally. Wind is 10km/h south-west: + + +https://github.com/RomanLut/inav/assets/11955117/0599a3c3-df06-4d40-a32a-4d8f96140592 + + +Purple line shows estimated position. Black line shows real position. "EST ERR" sensor shows estimation error in metters. Estimation is running when satellite icon displays "ES". Estimated position snaps to real position when GPS fix is reaquired. + + +# Is it possible to implement this for multirotor ? + +There are some ideas, but there is no solution now. We can not make assumptions with multirotor which we can make with a fixed wing. + + +# Links + +INAV HITL https://github.com/RomanLut/INAV-X-Plane-HITL diff --git a/docs/Screenshots/programming_disable_gps_sensor_fix.png b/docs/Screenshots/programming_disable_gps_sensor_fix.png new file mode 100644 index 00000000000..ba4ccd2be11 Binary files /dev/null and b/docs/Screenshots/programming_disable_gps_sensor_fix.png differ diff --git a/docs/Settings.md b/docs/Settings.md index 17384f92732..af94ba0aec2 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1002,6 +1002,16 @@ Requested yaw rate to execute when `LAND` (or old `SET-THR`) failsafe is active --- +### failsafe_gps_fix_estimation_delay + +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. + +| Default | Min | Max | +| --- | --- | --- | +| 7 | -1 | 600 | + +--- + ### failsafe_lights Enable or disable the lights when the `FAILSAFE` flight mode is enabled. The target needs to be compiled with `USE_LIGHTS` [ON/OFF]. @@ -1762,6 +1772,16 @@ Defines if INAV will dead-reckon over short GPS outages. May also be useful for --- +### inav_allow_gps_fix_estimation + +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. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### inav_auto_mag_decl Automatic setting of magnetic declination based on GPS position. When used manual magnetic declination is ignored. diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c index 55e15ff0435..6e5d8f4eac4 100644 --- a/src/main/drivers/display.c +++ b/src/main/drivers/display.c @@ -180,13 +180,13 @@ int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint16_t c) #ifdef USE_SIMULATOR if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { - //some FCs do not power max7456 from USB power - //driver can not read font metadata - //chip assumed to not support second bank of font - //artifical horizon, variometer and home direction are not drawn ( display.c: displayUpdateMaxChar()) - //return dummy metadata to let all OSD elements to work in simulator mode - instance->maxChar = 512; - } + //some FCs do not power max7456 from USB power + //driver can not read font metadata + //chip assumed to not support second bank of font + //artifical horizon, variometer and home direction are not drawn ( display.c: displayUpdateMaxChar()) + //return dummy metadata to let all OSD elements to work in simulator mode + instance->maxChar = 512; + } #endif if (c > instance->maxChar) { diff --git a/src/main/drivers/sound_beeper.c b/src/main/drivers/sound_beeper.c index 6577b26d5a3..bc76117d180 100644 --- a/src/main/drivers/sound_beeper.c +++ b/src/main/drivers/sound_beeper.c @@ -48,10 +48,10 @@ void systemBeep(bool onoff) #ifdef USE_SIMULATOR if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { - if (SIMULATOR_HAS_OPTION(HITL_MUTE_BEEPER)) { - onoff = false; - } - } + if (SIMULATOR_HAS_OPTION(HITL_MUTE_BEEPER)) { + onoff = false; + } + } #endif if (beeperConfig()->pwmMode) { diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index f504ffb0598..02981f1942f 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1083,7 +1083,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF legacyLedConfig |= ledConfig->led_function << shiftCount; shiftCount += 4; legacyLedConfig |= (ledConfig->led_overlay & 0x3F) << (shiftCount); - shiftCount += 6; + shiftCount += 6; legacyLedConfig |= (ledConfig->led_color) << (shiftCount); shiftCount += 4; legacyLedConfig |= (ledConfig->led_direction) << (shiftCount); @@ -2827,7 +2827,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) ledConfig->led_position = legacyConfig & 0xFF; ledConfig->led_function = (legacyConfig >> 8) & 0xF; ledConfig->led_overlay = (legacyConfig >> 12) & 0x3F; - ledConfig->led_color = (legacyConfig >> 18) & 0xF; + ledConfig->led_color = (legacyConfig >> 18) & 0xF; ledConfig->led_direction = (legacyConfig >> 22) & 0x3F; ledConfig->led_params = (legacyConfig >> 28) & 0xF; @@ -3396,7 +3396,7 @@ static bool mspParameterGroupsCommand(sbuf_t *dst, sbuf_t *src) bool isOSDTypeSupportedBySimulator(void) { #ifdef USE_OSD - displayPort_t *osdDisplayPort = osdGetDisplayPort(); + displayPort_t *osdDisplayPort = osdGetDisplayPort(); return (!!osdDisplayPort && !!osdDisplayPort->vTable->readChar); #else return false; @@ -3619,132 +3619,130 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu #ifdef USE_SIMULATOR case MSP_SIMULATOR: - tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version - + tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version + // Check the MSP version of simulator - if (tmp_u8 != SIMULATOR_MSP_VERSION) { + if (tmp_u8 != SIMULATOR_MSP_VERSION) { break; } - simulatorData.flags = sbufReadU8(src); + simulatorData.flags = sbufReadU8(src); 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 ) { - baroStartCalibration(); + baroStartCalibration(); } #endif #ifdef USE_MAG - DISABLE_STATE(COMPASS_CALIBRATED); - compassInit(); + DISABLE_STATE(COMPASS_CALIBRATED); + compassInit(); #endif - simulatorData.flags = HITL_RESET_FLAGS; + simulatorData.flags = HITL_RESET_FLAGS; // Review: Many states were affected. Reboot? - disarm(DISARM_SWITCH); // Disarm to prevent motor output!!! + 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 #ifdef USE_MAG - if (compassConfig()->mag_hardware != MAG_NONE) { - sensorsSet(SENSOR_MAG); - ENABLE_STATE(COMPASS_CALIBRATED); - DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE); - mag.magADC[X] = 800; - mag.magADC[Y] = 0; - mag.magADC[Z] = 0; - } + if (compassConfig()->mag_hardware != MAG_NONE) { + sensorsSet(SENSOR_MAG); + ENABLE_STATE(COMPASS_CALIBRATED); + DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE); + mag.magADC[X] = 800; + mag.magADC[Y] = 0; + mag.magADC[Z] = 0; + } #endif - ENABLE_ARMING_FLAG(SIMULATOR_MODE_HITL); + ENABLE_ARMING_FLAG(SIMULATOR_MODE_HITL); ENABLE_STATE(ACCELEROMETER_CALIBRATED); - LOG_DEBUG(SYSTEM, "Simulator enabled"); - } - - if (dataSize >= 14) { + LOG_DEBUG(SYSTEM, "Simulator enabled"); + } - if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) { - gpsSol.fixType = sbufReadU8(src); - gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100; - gpsSol.flags.hasNewData = true; - gpsSol.numSat = sbufReadU8(src); + if (dataSize >= 14) { - if (gpsSol.fixType != GPS_NO_FIX) { - gpsSol.flags.validVelNE = true; - gpsSol.flags.validVelD = true; - gpsSol.flags.validEPE = true; - gpsSol.flags.validTime = false; + 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); - gpsSol.llh.lat = sbufReadU32(src); - gpsSol.llh.lon = sbufReadU32(src); - gpsSol.llh.alt = sbufReadU32(src); - gpsSol.groundSpeed = (int16_t)sbufReadU16(src); - gpsSol.groundCourse = (int16_t)sbufReadU16(src); + if (gpsSolDRV.fixType != GPS_NO_FIX) { + gpsSolDRV.flags.validVelNE = true; + gpsSolDRV.flags.validVelD = true; + gpsSolDRV.flags.validEPE = true; + gpsSolDRV.flags.validTime = false; - gpsSol.velNED[X] = (int16_t)sbufReadU16(src); - gpsSol.velNED[Y] = (int16_t)sbufReadU16(src); - gpsSol.velNED[Z] = (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); - gpsSol.eph = 100; - gpsSol.epv = 100; + gpsSolDRV.velNED[X] = (int16_t)sbufReadU16(src); + gpsSolDRV.velNED[Y] = (int16_t)sbufReadU16(src); + gpsSolDRV.velNED[Z] = (int16_t)sbufReadU16(src); - ENABLE_STATE(GPS_FIX); - } 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 - gpsProcessNewSolutionData(); - } 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); - } + 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); + } + // 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; - - if (sensors(SENSOR_BARO)) { - baro.baroPressure = (int32_t)sbufReadU32(src); - baro.baroTemperature = DEGREES_TO_CENTIDEGREES(SIMULATOR_BARO_TEMP); - } else { - sbufAdvance(src, sizeof(uint32_t)); - } + 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_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_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 (SIMULATOR_HAS_OPTION(HITL_EXT_BATTERY_VOLTAGE)) { simulatorData.vbat = sbufReadU8(src); @@ -3754,44 +3752,44 @@ 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); + sbufReadU16(src); } } 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 c532b77eee5..8d4f8ae824e 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -124,6 +124,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), diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 4f870fa8f7a..4f739ce728a 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -838,6 +838,12 @@ groups: default_value: 0 min: -1 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 - name: PG_LIGHTS_CONFIG type: lightsConfig_t @@ -2285,6 +2291,12 @@ groups: default_value: OFF field: allow_dead_reckoning 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 - name: inav_reset_altitude description: "Defines when relative estimated altitude is reset to zero. Variants - `NEVER` (once reference is acquired it's used regardless); `FIRST_ARM` (keep altitude at zero until firstly armed), `EACH_ARM` (altitude is reset to zero on each arming)" default_value: "FIRST_ARM" diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 36f4d86f8e2..780de21b0a6 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -82,6 +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 { @@ -350,7 +353,13 @@ 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_FIX_HOME)) { + if (failsafeConfig()->failsafe_min_distance > 0 && + ((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(&posControl.rthState.originalHomePosition); if (distance < failsafeConfig()->failsafe_min_distance) { @@ -362,6 +371,28 @@ 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)) { + if (!failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart) { + failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = millis(); + } else if ((millis() - failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart) > (MILLIS_PER_SECOND * (uint16_t)MAX(failsafeConfig()->failsafe_gps_fix_estimation_delay,7))) { + if ( !posControl.flags.forcedRTHActivated ) { + failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_RTH); + failsafeActivate(FAILSAFE_RETURN_TO_HOME); + activateForcedRTH(); + return true; + } + } + } else { + failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = 0; + } + return false; +} +#endif + + void failsafeUpdateState(void) { if (!failsafeIsMonitoring() || failsafeIsSuspended()) { @@ -390,6 +421,12 @@ void failsafeUpdateState(void) if (!throttleStickIsLow()) { failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; } + +#ifdef USE_GPS_FIX_ESTIMATION + if ( checkGPSFixFailsafe() ) { + reprocessState = true; + } 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 @@ -458,7 +495,15 @@ void failsafeUpdateState(void) } else if (failsafeChooseFailsafeProcedure() != FAILSAFE_PROCEDURE_NONE) { // trigger new failsafe procedure if changed failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED; reprocessState = true; + } +#ifdef USE_GPS_FIX_ESTIMATION + else { + if ( checkGPSFixFailsafe() ) { + reprocessState = true; + } } +#endif + break; case FAILSAFE_RETURN_TO_HOME: diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index c3bc2953a1e..b99ce4e10c5 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -43,6 +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); @@ -149,6 +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 ed5cbcbfaa4..81719b780a4 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; + 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 e0b13a5310d..1cf3b8941ee 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; + return hasValidWindEstimate +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) //use any wind estimate with GPS fix estimation. +#endif + ; } float getEstimatedWindSpeed(int axis) @@ -83,15 +87,18 @@ void updateWindEstimator(timeUs_t currentTimeUs) static float lastValidEstimateAltitude = 0.0f; float currentAltitude = gpsSol.llh.alt / 100.0f; // altitude in m - if ((US2S(currentTimeUs - lastValidWindEstimate) + WINDESTIMATOR_ALTITUDE_SCALE * fabsf(currentAltitude - lastValidEstimateAltitude)) > WINDESTIMATOR_TIMEOUT) - { + if ((US2S(currentTimeUs - lastValidWindEstimate) + WINDESTIMATOR_ALTITUDE_SCALE * fabsf(currentAltitude - lastValidEstimateAltitude)) > WINDESTIMATOR_TIMEOUT) { hasValidWindEstimate = false; } if (!STATE(FIXED_WING_LEGACY) || !isGPSHeadingValid() || !gpsSol.flags.validVelNE || - !gpsSol.flags.validVelD) { + !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 0fae3a7f318..294b9414297 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -45,8 +45,12 @@ #include "fc/runtime_config.h" #endif +#include "fc/rc_modes.h" + #include "sensors/sensors.h" #include "sensors/compass.h" +#include "sensors/barometer.h" +#include "sensors/pitotmeter.h" #include "io/serial.h" #include "io/gps.h" @@ -54,6 +58,7 @@ #include "io/gps_ublox.h" #include "navigation/navigation.h" +#include "navigation/navigation_private.h" #include "config/feature.h" @@ -61,6 +66,13 @@ #include "fc/runtime_config.h" #include "fc/settings.h" +#include "flight/imu.h" +#include "flight/wind_estimator.h" +#include "flight/pid.h" +#include "flight/mixer.h" + +#include "programming/logic_condition.h" + typedef struct { bool isDriverBased; portMode_t portMode; // Port mode RX/TX (only for serial based) @@ -71,7 +83,13 @@ typedef struct { // GPS public data gpsReceiverData_t gpsState; gpsStatistics_t gpsStats; -gpsSolutionData_t gpsSol; + +//it is not safe to access gpsSolDRV which is filled in gps thread by driver. +//gpsSolDRV can be accessed only after driver signalled that data is ready +//we copy gpsSolDRV to gpsSol, process by "Disable GPS logic condition" and "GPS Fix estimation" features +//and use it in the rest of code. +gpsSolutionData_t gpsSolDRV; //filled by driver +gpsSolutionData_t gpsSol; //used in the rest of the code // Map gpsBaudRate_e index to baudRate_e baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT] = { BAUD_115200, BAUD_57600, BAUD_38400, BAUD_19200, BAUD_9600, BAUD_230400, BAUD_460800, BAUD_921600 }; @@ -203,22 +221,177 @@ void gpsSetProtocolTimeout(timeMs_t timeoutMs) gpsState.timeoutMs = timeoutMs; } -void gpsProcessNewSolutionData(void) +#ifdef USE_GPS_FIX_ESTIMATION +bool canEstimateGPSFix(void) { - // Set GPS fix flag only if we have 3D fix - if (gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats) { - ENABLE_STATE(GPS_FIX); - } - else { - /* When no fix available - reset flags as well */ +#if defined(USE_GPS) && defined(USE_BARO) + + //we do not check neither sensors(SENSOR_GPS) nor FEATURE(FEATURE_GPS) because: + //1) checking STATE(GPS_FIX_HOME) is enough to ensure that GPS sensor was initialized once + //2) sensors(SENSOR_GPS) is false on GPS timeout. We also want to support GPS timeouts, not just lost fix + return positionEstimationConfig()->allow_gps_fix_estimation && STATE(AIRPLANE) && + sensors(SENSOR_BARO) && baroIsHealthy() && + ARMING_FLAG(WAS_EVER_ARMED) && STATE(GPS_FIX_HOME); + +#else + return false; +#endif +} +#endif + +#ifdef USE_GPS_FIX_ESTIMATION +void processDisableGPSFix(void) +{ + static int32_t last_lat = 0; + static int32_t last_lon = 0; + static int32_t last_alt = 0; + + if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX)) { + gpsSol.fixType = GPS_NO_FIX; + gpsSol.hdop = 9999; + gpsSol.numSat = 0; + gpsSol.flags.validVelNE = false; - gpsSol.flags.validVelD = false; + gpsSol.flags.validVelD = false; gpsSol.flags.validEPE = false; + gpsSol.flags.validTime = false; + + //freeze coordinates + gpsSol.llh.lat = last_lat; + gpsSol.llh.lon = last_lon; + gpsSol.llh.alt = last_alt; + } else { + last_lat = gpsSol.llh.lat; + last_lon = gpsSol.llh.lon; + 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) +{ + static uint32_t lastUpdateMs = 0; + static int32_t estimated_lat = 0; + static int32_t estimated_lon = 0; + static int32_t estimated_alt = 0; + + uint32_t t = millis(); + int32_t dt = t - lastUpdateMs; + lastUpdateMs = t; + + bool sensorHasFix = gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats; + + if (sensorHasFix || !canEstimateGPSFix()) { + estimated_lat = gpsSol.llh.lat; + estimated_lon = gpsSol.llh.lon; + estimated_alt = posControl.gpsOrigin.alt + baro.BaroAlt; + return; + } + + gpsSol.fixType = GPS_FIX_3D; + gpsSol.hdop = 99; + gpsSol.numSat = 99; + + gpsSol.eph = 100; + gpsSol.epv = 100; + + gpsSol.flags.validVelNE = true; + gpsSol.flags.validVelD = false; //do not provide velocity.z + gpsSol.flags.validEPE = true; + gpsSol.flags.validTime = false; + + float speed = pidProfile()->fixedWingReferenceAirspeed; + +#ifdef USE_PITOT + if (sensors(SENSOR_PITOT) && pitotIsHealthy()) { + speed = getAirspeedEstimate(); + } +#endif + + float velX = rMat[0][0] * speed; + float velY = -rMat[1][0] * speed; + // here (velX, velY) is estimated horizontal speed without wind influence = airspeed, cm/sec in NEU frame + + if (isEstimatedWindSpeedValid()) { + velX += getEstimatedWindSpeed(X); + velY += getEstimatedWindSpeed(Y); + } + // here (velX, velY) is estimated horizontal speed with wind influence = ground speed + + if (STATE(LANDING_DETECTED) || ((posControl.navState == NAV_STATE_RTH_LANDING) && (getThrottlePercent(false) == 0))) { + velX = 0; + velY = 0; + } + + estimated_lat += (int32_t)( velX * dt / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * 1000 ) ); + estimated_lon += (int32_t)( velY * dt / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * 1000 * posControl.gpsOrigin.scale) ); + estimated_alt = posControl.gpsOrigin.alt + baro.BaroAlt; + + gpsSol.llh.lat = estimated_lat; + gpsSol.llh.lon = estimated_lon; + gpsSol.llh.alt = estimated_alt; + + gpsSol.groundSpeed = (int16_t)fast_fsqrtf(velX * velX + velY * velY); + + float groundCourse = atan2_approx(velY, velX); // atan2 returns [-M_PI, M_PI], with 0 indicating the vector points in the X direction + if (groundCourse < 0) { + groundCourse += 2 * M_PIf; + } + gpsSol.groundCourse = RADIANS_TO_DECIDEGREES(groundCourse); + + gpsSol.velNED[X] = (int16_t)(velX); + 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: +//1)driver copies gpsSolDRV to gpsSol +//2)gpsSol is processed by "Disable GPS logical switch" +//3)gpsSol is processed by GPS Fix estimator - updateEstimatedGPSFix() +//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) { + ENABLE_STATE(GPS_FIX); + } + else { + /* When no fix available - reset flags as well */ + gpsSol.flags.validVelNE = false; + gpsSol.flags.validVelD = false; + gpsSol.flags.validEPE = false; + DISABLE_STATE(GPS_FIX); + } +#ifdef USE_GPS_FIX_ESTIMATION } +#endif - // Set sensor as ready and available - sensorsSet(SENSOR_GPS); + if (!timeout) { + // Data came from GPS sensor - set sensor as ready and available (it may still not have GPS fix) + sensorsSet(SENSOR_GPS); + } // Pass on GPS update to NAV and IMU onNewGPSData(); @@ -237,20 +410,35 @@ void gpsProcessNewSolutionData(void) gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat; } -static void gpsResetSolution(void) +static void gpsResetSolution(gpsSolutionData_t* gpsSol) { - gpsSol.eph = 9999; - gpsSol.epv = 9999; - gpsSol.numSat = 0; - gpsSol.hdop = 9999; + gpsSol->eph = 9999; + gpsSol->epv = 9999; + gpsSol->numSat = 0; + gpsSol->hdop = 9999; - gpsSol.fixType = GPS_NO_FIX; + gpsSol->fixType = GPS_NO_FIX; - gpsSol.flags.validVelNE = false; - gpsSol.flags.validVelD = false; - gpsSol.flags.validMag = false; - gpsSol.flags.validEPE = false; - gpsSol.flags.validTime = false; + gpsSol->flags.validVelNE = false; + gpsSol->flags.validVelD = false; + gpsSol->flags.validEPE = false; + gpsSol->flags.validTime = false; +} + +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) @@ -269,7 +457,8 @@ void gpsInit(void) gpsStats.timeouts = 0; // Reset solution, timeout and prepare to start - gpsResetSolution(); + gpsResetSolution(&gpsSolDRV); + gpsResetSolution(&gpsSol); gpsSetProtocolTimeout(gpsState.baseTimeoutMs); gpsSetState(GPS_UNKNOWN); @@ -345,27 +534,21 @@ bool gpsUpdate(void) #ifdef USE_SIMULATOR if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { - if ( SIMULATOR_HAS_OPTION(HITL_GPS_TIMEOUT)) - { + if ( SIMULATOR_HAS_OPTION(HITL_GPS_TIMEOUT)) { gpsSetState(GPS_LOST_COMMUNICATION); sensorsClear(SENSOR_GPS); gpsStats.timeouts = 5; - return false; - } - else - { + gpsTryEstimateOnTimeout(); + } else { gpsSetState(GPS_RUNNING); sensorsSet(SENSOR_GPS); - bool res = gpsSol.flags.hasNewData; - gpsSol.flags.hasNewData = false; - return res; } + bool res = gpsSol.flags.hasNewData; + gpsSol.flags.hasNewData = false; + return res; } #endif - // Assume that we don't have new data this run - gpsSol.flags.hasNewData = false; - switch (gpsState.state) { default: case GPS_INITIALIZING: @@ -373,10 +556,9 @@ bool gpsUpdate(void) if ((millis() - gpsState.lastStateSwitchMs) >= GPS_INIT_DELAY) { // Reset internals DISABLE_STATE(GPS_FIX); - gpsSol.fixType = GPS_NO_FIX; // Reset solution - gpsResetSolution(); + gpsResetSolution(&gpsSolDRV); // Call GPS protocol reset handler gpsProviders[gpsState.gpsConfig->provider].restart(); @@ -406,7 +588,13 @@ bool gpsUpdate(void) break; } - return gpsSol.flags.hasNewData; + if ( !sensors(SENSOR_GPS) ) { + gpsTryEstimateOnTimeout(); + } + + bool res = gpsSol.flags.hasNewData; + gpsSol.flags.hasNewData = false; + return res; } void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort) @@ -453,7 +641,11 @@ bool isGPSHealthy(void) bool isGPSHeadingValid(void) { - return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6 && 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.h b/src/main/io/gps.h index 199652a3a24..80a62354502 100755 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -124,7 +124,6 @@ typedef struct gpsSolutionData_s { bool gpsHeartbeat; // Toggle each update bool validVelNE; bool validVelD; - bool validMag; bool validEPE; // EPH/EPV values are valid - actual accuracy bool validTime; } flags; @@ -133,7 +132,6 @@ typedef struct gpsSolutionData_s { uint8_t numSat; gpsLocation_t llh; - int16_t magData[3]; int16_t velNED[3]; int16_t groundSpeed; diff --git a/src/main/io/gps_fake.c b/src/main/io/gps_fake.c index 6d574893436..8471800a0e4 100644 --- a/src/main/io/gps_fake.c +++ b/src/main/io/gps_fake.c @@ -46,7 +46,7 @@ void gpsFakeRestart(void) void gpsFakeHandle(void) { - gpsProcessNewSolutionData(); + gpsProcessNewSolutionData(false); } void gpsFakeSet( @@ -62,37 +62,38 @@ void gpsFakeSet( int16_t velNED_Z, time_t time) { - gpsSol.fixType = fixType; - gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100; - gpsSol.numSat = numSat; + gpsSolDRV.fixType = fixType; + gpsSolDRV.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100; + gpsSolDRV.numSat = numSat; - gpsSol.llh.lat = lat; - gpsSol.llh.lon = lon; - gpsSol.llh.alt = alt; - gpsSol.groundSpeed = groundSpeed; - gpsSol.groundCourse = groundCourse; - gpsSol.velNED[X] = velNED_X; - gpsSol.velNED[Y] = velNED_Y; - gpsSol.velNED[Z] = velNED_Z; - gpsSol.eph = 100; - gpsSol.epv = 100; - gpsSol.flags.validVelNE = true; - gpsSol.flags.validVelD = true; - gpsSol.flags.validEPE = true; - gpsSol.flags.hasNewData = true; + gpsSolDRV.llh.lat = lat; + gpsSolDRV.llh.lon = lon; + gpsSolDRV.llh.alt = alt; + gpsSolDRV.groundSpeed = groundSpeed; + gpsSolDRV.groundCourse = groundCourse; + gpsSolDRV.velNED[X] = velNED_X; + gpsSolDRV.velNED[Y] = velNED_Y; + gpsSolDRV.velNED[Z] = velNED_Z; + gpsSolDRV.eph = 100; + gpsSolDRV.epv = 100; + gpsSolDRV.flags.validVelNE = true; + gpsSolDRV.flags.validVelD = true; + gpsSolDRV.flags.validEPE = true; if (time) { struct tm* gTime = gmtime(&time); - gpsSol.time.year = (uint16_t)(gTime->tm_year + 1900); - gpsSol.time.month = (uint16_t)(gTime->tm_mon + 1); - gpsSol.time.day = (uint8_t)gTime->tm_mday; - gpsSol.time.hours = (uint8_t)gTime->tm_hour; - gpsSol.time.minutes = (uint8_t)gTime->tm_min; - gpsSol.time.seconds = (uint8_t)gTime->tm_sec; - gpsSol.time.millis = 0; - gpsSol.flags.validTime = gpsSol.fixType >= 3; + gpsSolDRV.time.year = (uint16_t)(gTime->tm_year + 1900); + gpsSolDRV.time.month = (uint16_t)(gTime->tm_mon + 1); + gpsSolDRV.time.day = (uint8_t)gTime->tm_mday; + gpsSolDRV.time.hours = (uint8_t)gTime->tm_hour; + gpsSolDRV.time.minutes = (uint8_t)gTime->tm_min; + gpsSolDRV.time.seconds = (uint8_t)gTime->tm_sec; + gpsSolDRV.time.millis = 0; + gpsSolDRV.flags.validTime = gpsSol.fixType >= 3; } + + gpsProcessNewDriverData(); } #endif diff --git a/src/main/io/gps_msp.c b/src/main/io/gps_msp.c index 8eafb2dff19..1fd4b1cf15e 100644 --- a/src/main/io/gps_msp.c +++ b/src/main/io/gps_msp.c @@ -63,7 +63,7 @@ void gpsRestartMSP(void) void gpsHandleMSP(void) { if (newDataReady) { - gpsProcessNewSolutionData(); + gpsProcessNewSolutionData(false); newDataReady = false; } } @@ -81,33 +81,34 @@ void mspGPSReceiveNewData(const uint8_t * bufferPtr) { const mspSensorGpsDataMessage_t * pkt = (const mspSensorGpsDataMessage_t *)bufferPtr; - gpsSol.fixType = gpsMapFixType(pkt->fixType); - gpsSol.numSat = pkt->satellitesInView; - gpsSol.llh.lon = pkt->longitude; - gpsSol.llh.lat = pkt->latitude; - gpsSol.llh.alt = pkt->mslAltitude; - gpsSol.velNED[X] = pkt->nedVelNorth; - gpsSol.velNED[Y] = pkt->nedVelEast; - gpsSol.velNED[Z] = pkt->nedVelDown; - gpsSol.groundSpeed = calc_length_pythagorean_2D((float)pkt->nedVelNorth, (float)pkt->nedVelEast); - gpsSol.groundCourse = pkt->groundCourse / 10; // in deg * 10 - gpsSol.eph = gpsConstrainEPE(pkt->horizontalPosAccuracy / 10); - gpsSol.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10); - gpsSol.hdop = gpsConstrainHDOP(pkt->hdop); - gpsSol.flags.validVelNE = true; - gpsSol.flags.validVelD = true; - gpsSol.flags.validEPE = true; - - gpsSol.time.year = pkt->year; - gpsSol.time.month = pkt->month; - gpsSol.time.day = pkt->day; - gpsSol.time.hours = pkt->hour; - gpsSol.time.minutes = pkt->min; - gpsSol.time.seconds = pkt->sec; - gpsSol.time.millis = 0; - - gpsSol.flags.validTime = (pkt->fixType >= 3); - + gpsSolDRV.fixType = gpsMapFixType(pkt->fixType); + gpsSolDRV.numSat = pkt->satellitesInView; + gpsSolDRV.llh.lon = pkt->longitude; + gpsSolDRV.llh.lat = pkt->latitude; + gpsSolDRV.llh.alt = pkt->mslAltitude; + gpsSolDRV.velNED[X] = pkt->nedVelNorth; + gpsSolDRV.velNED[Y] = pkt->nedVelEast; + gpsSolDRV.velNED[Z] = pkt->nedVelDown; + gpsSolDRV.groundSpeed = calc_length_pythagorean_2D((float)pkt->nedVelNorth, (float)pkt->nedVelEast); + gpsSolDRV.groundCourse = pkt->groundCourse / 10; // in deg * 10 + gpsSolDRV.eph = gpsConstrainEPE(pkt->horizontalPosAccuracy / 10); + gpsSolDRV.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10); + gpsSolDRV.hdop = gpsConstrainHDOP(pkt->hdop); + gpsSolDRV.flags.validVelNE = true; + gpsSolDRV.flags.validVelD = true; + gpsSolDRV.flags.validEPE = true; + + gpsSolDRV.time.year = pkt->year; + gpsSolDRV.time.month = pkt->month; + gpsSolDRV.time.day = pkt->day; + gpsSolDRV.time.hours = pkt->hour; + gpsSolDRV.time.minutes = pkt->min; + gpsSolDRV.time.seconds = pkt->sec; + gpsSolDRV.time.millis = 0; + + gpsSolDRV.flags.validTime = (pkt->fixType >= 3); + + gpsProcessNewDriverData(); newDataReady = true; } #endif diff --git a/src/main/io/gps_private.h b/src/main/io/gps_private.h index 9136fdcf837..ee2a0b1e215 100755 --- a/src/main/io/gps_private.h +++ b/src/main/io/gps_private.h @@ -61,6 +61,7 @@ typedef struct { } gpsReceiverData_t; extern gpsReceiverData_t gpsState; +extern gpsSolutionData_t gpsSolDRV; extern baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT]; @@ -70,7 +71,8 @@ extern void gpsFinalizeChangeBaud(void); extern uint16_t gpsConstrainEPE(uint32_t epe); extern uint16_t gpsConstrainHDOP(uint32_t hdop); -void gpsProcessNewSolutionData(void); +void gpsProcessNewDriverData(void); +void gpsProcessNewSolutionData(bool); void gpsSetProtocolTimeout(timeMs_t timeoutMs); extern void gpsRestartUBLOX(void); diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index d03c3390776..7f8ed55826b 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -251,8 +251,8 @@ static const uint8_t default_payload[] = { 0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; -#define GNSSID_SBAS 1 -#define GNSSID_GALILEO 2 +#define GNSSID_SBAS 1 +#define GNSSID_GALILEO 2 #define GNSSID_BEIDOU 3 #define GNSSID_GZSS 5 #define GNSSID_GLONASS 6 @@ -410,18 +410,18 @@ static void configureGNSS10(void) static void configureGNSS(void) { - int blocksUsed = 0; - send_buffer.message.header.msg_class = CLASS_CFG; + int blocksUsed = 0; + send_buffer.message.header.msg_class = CLASS_CFG; send_buffer.message.header.msg_id = MSG_CFG_GNSS; // message deprecated in protocol > 23.01, should use UBX-CFG-VALSET/UBX-CFG-VALGET - send_buffer.message.payload.gnss.msgVer = 0; - send_buffer.message.payload.gnss.numTrkChHw = 0; // read only, so unset + send_buffer.message.payload.gnss.msgVer = 0; + send_buffer.message.payload.gnss.numTrkChHw = 0; // read only, so unset send_buffer.message.payload.gnss.numTrkChUse = 0xFF; // If set to 0xFF will use hardware max - /* SBAS, always generated */ - blocksUsed += configureGNSS_SBAS(&send_buffer.message.payload.gnss.config[blocksUsed]); + /* SBAS, always generated */ + blocksUsed += configureGNSS_SBAS(&send_buffer.message.payload.gnss.config[blocksUsed]); - /* Galileo */ - blocksUsed += configureGNSS_GALILEO(&send_buffer.message.payload.gnss.config[blocksUsed]); + /* Galileo */ + blocksUsed += configureGNSS_GALILEO(&send_buffer.message.payload.gnss.config[blocksUsed]); /* BeiDou */ blocksUsed += configureGNSS_BEIDOU(&send_buffer.message.payload.gnss.config[blocksUsed]); @@ -429,9 +429,9 @@ static void configureGNSS(void) /* GLONASS */ blocksUsed += configureGNSS_GLONASS(&send_buffer.message.payload.gnss.config[blocksUsed]); - send_buffer.message.payload.gnss.numConfigBlocks = blocksUsed; - send_buffer.message.header.length = (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t) * blocksUsed); - sendConfigMessageUBLOX(); + send_buffer.message.payload.gnss.numConfigBlocks = blocksUsed; + send_buffer.message.header.length = (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)* blocksUsed); + sendConfigMessageUBLOX(); } static void configureNAV5(uint8_t dynModel, uint8_t fixMode) @@ -538,84 +538,84 @@ static bool gpsParseFrameUBLOX(void) { switch (_msg_id) { case MSG_POSLLH: - gpsSol.llh.lon = _buffer.posllh.longitude; - gpsSol.llh.lat = _buffer.posllh.latitude; - gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10; //alt in cm - gpsSol.eph = gpsConstrainEPE(_buffer.posllh.horizontal_accuracy / 10); - gpsSol.epv = gpsConstrainEPE(_buffer.posllh.vertical_accuracy / 10); - gpsSol.flags.validEPE = true; + gpsSolDRV.llh.lon = _buffer.posllh.longitude; + gpsSolDRV.llh.lat = _buffer.posllh.latitude; + gpsSolDRV.llh.alt = _buffer.posllh.altitude_msl / 10; //alt in cm + gpsSolDRV.eph = gpsConstrainEPE(_buffer.posllh.horizontal_accuracy / 10); + gpsSolDRV.epv = gpsConstrainEPE(_buffer.posllh.vertical_accuracy / 10); + gpsSolDRV.flags.validEPE = true; if (next_fix_type != GPS_NO_FIX) - gpsSol.fixType = next_fix_type; + gpsSolDRV.fixType = next_fix_type; _new_position = true; break; case MSG_STATUS: next_fix_type = gpsMapFixType(_buffer.status.fix_status & NAV_STATUS_FIX_VALID, _buffer.status.fix_type); if (next_fix_type == GPS_NO_FIX) - gpsSol.fixType = GPS_NO_FIX; + gpsSolDRV.fixType = GPS_NO_FIX; break; case MSG_SOL: next_fix_type = gpsMapFixType(_buffer.solution.fix_status & NAV_STATUS_FIX_VALID, _buffer.solution.fix_type); if (next_fix_type == GPS_NO_FIX) - gpsSol.fixType = GPS_NO_FIX; - gpsSol.numSat = _buffer.solution.satellites; - gpsSol.hdop = gpsConstrainHDOP(_buffer.solution.position_DOP); + gpsSolDRV.fixType = GPS_NO_FIX; + gpsSolDRV.numSat = _buffer.solution.satellites; + gpsSolDRV.hdop = gpsConstrainHDOP(_buffer.solution.position_DOP); break; case MSG_VELNED: - gpsSol.groundSpeed = _buffer.velned.speed_2d; // cm/s - gpsSol.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10 - gpsSol.velNED[X] = _buffer.velned.ned_north; - gpsSol.velNED[Y] = _buffer.velned.ned_east; - gpsSol.velNED[Z] = _buffer.velned.ned_down; - gpsSol.flags.validVelNE = true; - gpsSol.flags.validVelD = true; + gpsSolDRV.groundSpeed = _buffer.velned.speed_2d; // cm/s + gpsSolDRV.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10 + gpsSolDRV.velNED[X] = _buffer.velned.ned_north; + gpsSolDRV.velNED[Y] = _buffer.velned.ned_east; + gpsSolDRV.velNED[Z] = _buffer.velned.ned_down; + gpsSolDRV.flags.validVelNE = true; + gpsSolDRV.flags.validVelD = true; _new_speed = true; break; case MSG_TIMEUTC: if (UBX_VALID_GPS_DATE_TIME(_buffer.timeutc.valid)) { - gpsSol.time.year = _buffer.timeutc.year; - gpsSol.time.month = _buffer.timeutc.month; - gpsSol.time.day = _buffer.timeutc.day; - gpsSol.time.hours = _buffer.timeutc.hour; - gpsSol.time.minutes = _buffer.timeutc.min; - gpsSol.time.seconds = _buffer.timeutc.sec; - gpsSol.time.millis = _buffer.timeutc.nano / (1000*1000); - - gpsSol.flags.validTime = true; + gpsSolDRV.time.year = _buffer.timeutc.year; + gpsSolDRV.time.month = _buffer.timeutc.month; + gpsSolDRV.time.day = _buffer.timeutc.day; + gpsSolDRV.time.hours = _buffer.timeutc.hour; + gpsSolDRV.time.minutes = _buffer.timeutc.min; + gpsSolDRV.time.seconds = _buffer.timeutc.sec; + gpsSolDRV.time.millis = _buffer.timeutc.nano / (1000*1000); + + gpsSolDRV.flags.validTime = true; } else { - gpsSol.flags.validTime = false; + gpsSolDRV.flags.validTime = false; } break; case MSG_PVT: next_fix_type = gpsMapFixType(_buffer.pvt.fix_status & NAV_STATUS_FIX_VALID, _buffer.pvt.fix_type); - gpsSol.fixType = next_fix_type; - gpsSol.llh.lon = _buffer.pvt.longitude; - gpsSol.llh.lat = _buffer.pvt.latitude; - gpsSol.llh.alt = _buffer.pvt.altitude_msl / 10; //alt in cm - gpsSol.velNED[X]=_buffer.pvt.ned_north / 10; // to cm/s - gpsSol.velNED[Y]=_buffer.pvt.ned_east / 10; // to cm/s - gpsSol.velNED[Z]=_buffer.pvt.ned_down / 10; // to cm/s - gpsSol.groundSpeed = _buffer.pvt.speed_2d / 10; // to cm/s - gpsSol.groundCourse = (uint16_t) (_buffer.pvt.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10 - gpsSol.numSat = _buffer.pvt.satellites; - gpsSol.eph = gpsConstrainEPE(_buffer.pvt.horizontal_accuracy / 10); - gpsSol.epv = gpsConstrainEPE(_buffer.pvt.vertical_accuracy / 10); - gpsSol.hdop = gpsConstrainHDOP(_buffer.pvt.position_DOP); - gpsSol.flags.validVelNE = true; - gpsSol.flags.validVelD = true; - gpsSol.flags.validEPE = true; + gpsSolDRV.fixType = next_fix_type; + gpsSolDRV.llh.lon = _buffer.pvt.longitude; + gpsSolDRV.llh.lat = _buffer.pvt.latitude; + gpsSolDRV.llh.alt = _buffer.pvt.altitude_msl / 10; //alt in cm + gpsSolDRV.velNED[X]=_buffer.pvt.ned_north / 10; // to cm/s + gpsSolDRV.velNED[Y]=_buffer.pvt.ned_east / 10; // to cm/s + gpsSolDRV.velNED[Z]=_buffer.pvt.ned_down / 10; // to cm/s + gpsSolDRV.groundSpeed = _buffer.pvt.speed_2d / 10; // to cm/s + gpsSolDRV.groundCourse = (uint16_t) (_buffer.pvt.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10 + gpsSolDRV.numSat = _buffer.pvt.satellites; + gpsSolDRV.eph = gpsConstrainEPE(_buffer.pvt.horizontal_accuracy / 10); + gpsSolDRV.epv = gpsConstrainEPE(_buffer.pvt.vertical_accuracy / 10); + gpsSolDRV.hdop = gpsConstrainHDOP(_buffer.pvt.position_DOP); + gpsSolDRV.flags.validVelNE = true; + gpsSolDRV.flags.validVelD = true; + gpsSolDRV.flags.validEPE = true; if (UBX_VALID_GPS_DATE_TIME(_buffer.pvt.valid)) { - gpsSol.time.year = _buffer.pvt.year; - gpsSol.time.month = _buffer.pvt.month; - gpsSol.time.day = _buffer.pvt.day; - gpsSol.time.hours = _buffer.pvt.hour; - gpsSol.time.minutes = _buffer.pvt.min; - gpsSol.time.seconds = _buffer.pvt.sec; - gpsSol.time.millis = _buffer.pvt.nano / (1000*1000); - - gpsSol.flags.validTime = true; + gpsSolDRV.time.year = _buffer.pvt.year; + gpsSolDRV.time.month = _buffer.pvt.month; + gpsSolDRV.time.day = _buffer.pvt.day; + gpsSolDRV.time.hours = _buffer.pvt.hour; + gpsSolDRV.time.minutes = _buffer.pvt.min; + gpsSolDRV.time.seconds = _buffer.pvt.sec; + gpsSolDRV.time.millis = _buffer.pvt.nano / (1000*1000); + + gpsSolDRV.flags.validTime = true; } else { - gpsSol.flags.validTime = false; + gpsSolDRV.flags.validTime = false; } _new_position = true; @@ -649,7 +649,7 @@ static bool gpsParseFrameUBLOX(void) } } } - for (int j = 40; j < _payload_length; j += 30) { + for(int j = 40; j < _payload_length; j += 30) { if (strnstr((const char *)(_buffer.bytes + j), "PROTVER", 30)) { gpsDecodeProtocolVersion((const char *)(_buffer.bytes + j), 30); break; @@ -948,19 +948,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 { + } else { configureGNSS(); - } - ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); + } + 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); @@ -980,6 +980,7 @@ STATIC_PROTOTHREAD(gpsProtocolReceiverThread) while (serialRxBytesWaiting(gpsState.gpsPort)) { uint8_t newChar = serialRead(gpsState.gpsPort); if (gpsNewFrameUBLOX(newChar)) { + gpsProcessNewDriverData(); ptSemaphoreSignal(semNewDataReady); break; } @@ -1038,7 +1039,7 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) 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); + } 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; @@ -1060,7 +1061,7 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) // GPS is ready - execute the gpsProcessNewSolutionData() based on gpsProtocolReceiverThread semaphore while (1) { ptSemaphoreWait(semNewDataReady); - gpsProcessNewSolutionData(); + gpsProcessNewSolutionData(false); if ((gpsState.gpsConfig->provider == GPS_UBLOX || gpsState.gpsConfig->provider == GPS_UBLOX7PLUS)) { if ((millis() - gpsState.lastCapaPoolMs) > GPS_CAPA_INTERVAL) { diff --git a/src/main/io/osd.c b/src/main/io/osd.c index b799ab5c562..422171f92c6 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -546,7 +546,7 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt) buff[0] = ' '; } -#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values +#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())) { totalDigits++; digits++; @@ -641,7 +641,7 @@ static inline void osdFormatFlyTime(char *buff, textAttributes_t *attr) } /** - * Trim whitespace from string. + * Trim whitespace from string. * Used in Stats screen on lines with multiple values. */ char *osdFormatTrimWhiteSpace(char *buff) @@ -652,7 +652,7 @@ char *osdFormatTrimWhiteSpace(char *buff) while(isspace((unsigned char)*buff)) buff++; // All spaces? - if(*buff == 0) + if(*buff == 0) return buff; // Trim trailing spaces @@ -1099,7 +1099,7 @@ void osdCrosshairPosition(uint8_t *x, uint8_t *y) * Check if this OSD layout is using scaled or unscaled throttle. * If both are used, it will default to scaled. */ -bool osdUsingScaledThrottle(void) +bool osdUsingScaledThrottle(void) { bool usingScaledThrottle = OSD_VISIBLE(osdLayoutsConfig()->item_pos[currentLayout][OSD_SCALED_THROTTLE_POS]); bool usingRCThrottle = OSD_VISIBLE(osdLayoutsConfig()->item_pos[currentLayout][OSD_THROTTLE_POS]); @@ -1323,7 +1323,11 @@ static void osdDrawMap(int referenceHeading, uint16_t referenceSym, uint16_t cen } } - if (STATE(GPS_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); @@ -1437,7 +1441,11 @@ static void osdDisplayTelemetry(void) static uint16_t trk_bearing = 0; if (ARMING_FLAG(ARMED)) { - if (STATE(GPS_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; @@ -1785,8 +1793,8 @@ static bool osdDrawSingleElement(uint8_t item) osdFormatCentiNumber(buff + 1, getBatteryRemainingCapacity() / 10, 0, 2, 0, 3, false); if (!unitsDrawn) { - buff[4] = currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH; - buff[5] = '\0'; + buff[4] = currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH; + buff[5] = '\0'; } if (batteryUsesCapacityThresholds()) { @@ -1815,6 +1823,12 @@ 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 +#endif if (!STATE(GPS_FIX)) { hardwareSensorStatus_e sensorStatus = getHwGPSStatus(); if (sensorStatus == HW_SENSOR_UNAVAILABLE || sensorStatus == HW_SENSOR_UNHEALTHY) { @@ -1872,7 +1886,11 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_HOME_DIR: { - if (STATE(GPS_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); } @@ -2094,8 +2112,8 @@ static bool osdDrawSingleElement(uint8_t item) int8_t blankPos; for (blankPos = 2; blankPos >= 0; blankPos--) { if (buff[blankPos] == SYM_BLANK) { - break; - } + break; + } } if (blankPos >= 0 || OSD_ALTERNATING_CHOICES(600, 2) == 0) { blankPos = blankPos < 0 ? 0 : blankPos; @@ -2418,11 +2436,19 @@ static bool osdDrawSingleElement(uint8_t item) osdCrosshairPosition(&elemPosX, &elemPosY); osdHudDrawCrosshair(osdGetDisplayPortCanvas(), elemPosX, elemPosY); - if (osdConfig()->hud_homing && STATE(GPS_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) && 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(); @@ -3046,7 +3072,11 @@ static bool osdDrawSingleElement(uint8_t item) digits = 4U; } #endif - if (STATE(GPS_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)); @@ -3068,7 +3098,7 @@ static bool osdDrawSingleElement(uint8_t item) tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI); } if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = buff[3] = '-'; + buff[0] = buff[1] = buff[2] = buff[3] = '-'; buff[digits] = SYM_MAH_MI_0; // This will overwrite the "-" at buff[3] if not in BFCOMPAT mode buff[digits + 1] = SYM_MAH_MI_1; buff[digits + 2] = '\0'; @@ -3082,7 +3112,7 @@ static bool osdDrawSingleElement(uint8_t item) tfp_sprintf(buff, "%s%c", buff, SYM_AH_NM); } if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = buff[3] = '-'; + buff[0] = buff[1] = buff[2] = buff[3] = '-'; buff[digits] = SYM_MAH_NM_0; buff[digits + 1] = SYM_MAH_NM_1; buff[digits + 2] = '\0'; @@ -3098,7 +3128,7 @@ static bool osdDrawSingleElement(uint8_t item) tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM); } if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = buff[3] = '-'; + buff[0] = buff[1] = buff[2] = buff[3] = '-'; buff[digits] = SYM_MAH_KM_0; buff[digits + 1] = SYM_MAH_KM_1; buff[digits + 2] = '\0'; @@ -3117,7 +3147,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) && 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)); @@ -3269,7 +3303,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)) { + 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 @@ -3997,7 +4035,7 @@ uint8_t drawLogos(bool singular, uint8_t row) { if (logoSpacing > 0 && ((osdDisplayPort->cols % 2) != (logoSpacing % 2))) { logoSpacing++; // Add extra 1 character space between logos, if the odd/even of the OSD cols doesn't match the odd/even of the logo spacing - } +} // Draw Logo(s) if (usePilotLogo && !singular) { @@ -4025,9 +4063,9 @@ uint8_t drawLogos(bool singular, uint8_t row) { logoRow = row; if (singular) { logo_x = logoColOffset; - } else { + } else { logo_x = logoColOffset + SYM_LOGO_WIDTH + logoSpacing; - } + } for (uint8_t lRow = 0; lRow < SYM_LOGO_HEIGHT; lRow++) { for (uint8_t lCol = 0; lCol < SYM_LOGO_WIDTH; lCol++) { @@ -4038,7 +4076,7 @@ uint8_t drawLogos(bool singular, uint8_t row) { } return logoRow; -} + } uint8_t drawStats(uint8_t row) { #ifdef USE_STATS @@ -4269,7 +4307,7 @@ static void osdUpdateStats(void) static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) { const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "KILLSW", "FAILSAFE", "NAV SYS", "LANDING"}; - uint8_t top = 1; // Start one line down leaving space at the top of the screen. + uint8_t top = 1; // Start one line down leaving space at the top of the screen. size_t multiValueLengthOffset = 0; const uint8_t statNameX = osdDisplayIsHD() ? 11 : 1; @@ -4298,7 +4336,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) osdLeftAlignString(buff); strcat(osdFormatTrimWhiteSpace(buff),"/"); multiValueLengthOffset = strlen(buff); - displayWrite(osdDisplayPort, statValuesX, top, buff); + displayWrite(osdDisplayPort, statValuesX, top, buff); osdGenerateAverageVelocityStr(buff); osdLeftAlignString(buff); displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); @@ -4335,7 +4373,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) osdLeftAlignString(buff); strcat(osdFormatTrimWhiteSpace(buff), "%/"); multiValueLengthOffset = strlen(buff); - displayWrite(osdDisplayPort, statValuesX, top, buff); + displayWrite(osdDisplayPort, statValuesX, top, buff); itoa(stats.min_rssi_dbm, buff, 10); tfp_sprintf(buff, "%s%c", buff, SYM_DBM); osdLeftAlignString(buff); @@ -4350,7 +4388,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) 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); @@ -4362,7 +4400,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) itoa(stats.min_rssi, buff, 10); strcat(buff, "%"); displayWrite(osdDisplayPort, statValuesX, top++, buff); - } + } displayWrite(osdDisplayPort, statNameX, top, "FLY TIME :"); uint16_t flySeconds = getFlightTime(); @@ -4414,13 +4452,13 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) 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 + 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; @@ -4509,9 +4547,9 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:"); osdFormatCentiNumber(buff, acc_extremes_min * 100, 0, 2, 0, 4, false); osdLeftAlignString(buff); - strcat(osdFormatTrimWhiteSpace(buff),"/"); - multiValueLengthOffset = strlen(buff); - displayWrite(osdDisplayPort, statValuesX, top, buff); + strcat(osdFormatTrimWhiteSpace(buff),"/"); + multiValueLengthOffset = strlen(buff); + displayWrite(osdDisplayPort, statValuesX, top, buff); osdFormatCentiNumber(buff, acc_extremes_max * 100, 0, 2, 0, 3, false); osdLeftAlignString(buff); displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); @@ -4533,10 +4571,10 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) // HD arming screen. based on the minimum HD OSD grid size of 50 x 18 static void osdShowHDArmScreen(void) { - dateTime_t dt; + dateTime_t dt; char buf[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)]; char buf2[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)]; - char craftNameBuf[MAX_NAME_LENGTH]; + char craftNameBuf[MAX_NAME_LENGTH]; char versionBuf[osdDisplayPort->cols]; uint8_t safehomeRow = 0; uint8_t armScreenRow = 2; // Start at row 2 @@ -4576,7 +4614,7 @@ static void osdShowHDArmScreen(void) #if defined(USE_GPS) if (feature(FEATURE_GPS)) { if (STATE(GPS_FIX_HOME)) { - if (osdConfig()->osd_home_position_arm_screen) { + if (osdConfig()->osd_home_position_arm_screen){ osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat); osdFormatCoordinate(buf2, SYM_LON, GPS_home.lon); uint8_t gap = 1; @@ -4632,7 +4670,7 @@ static void osdShowHDArmScreen(void) if (armScreenRow < (osdDisplayPort->rows - 4)) armScreenRow = drawStats(armScreenRow); #endif // USE_STATS -} + } static void osdShowSDArmScreen(void) { @@ -4859,27 +4897,27 @@ static void osdRefresh(timeUs_t currentTimeUs) // Manual paging stick commands are only applicable to multi-page stats. // ****************************** - // For single-page stats, this effectively disables the ability to cancel the + // For single-page stats, this effectively disables the ability to cancel the // automatic paging/updates with the stick commands. So unless stats_page_auto_swap_time - // is set to 0 or greater than 4 (saved settings display interval is 5 seconds), then - // "Saved Settings" should display if it is active within the refresh interval. + // is set to 0 or greater than 4 (saved settings display interval is 5 seconds), then + // "Saved Settings" should display if it is active within the refresh interval. // ****************************** // With multi-page stats, "Saved Settings" could also be missed if the user - // has canceled automatic paging using the stick commands, because that is only - // updated when osdShowStats() is called. So, in that case, they would only see - // the "Saved Settings" message if they happen to manually change pages using the - // stick commands within the interval the message is displayed. + // has canceled automatic paging using the stick commands, because that is only + // updated when osdShowStats() is called. So, in that case, they would only see + // the "Saved Settings" message if they happen to manually change pages using the + // stick commands within the interval the message is displayed. bool manualPageUpRequested = false; - bool manualPageDownRequested = false; + bool manualPageDownRequested = false; if (!statsSinglePageCompatible) { // These methods ensure the paging stick commands are held for a brief period - // Otherwise it can result in a race condition where the stats are - // updated too quickly and can result in partial blanks, etc. - if (osdIsPageUpStickCommandHeld()) { + // Otherwise it can result in a race condition where the stats are + // updated too quickly and can result in partial blanks, etc. + if (osdIsPageUpStickCommandHeld()) { manualPageUpRequested = true; statsAutoPagingEnabled = false; } else if (osdIsPageDownStickCommandHeld()) { - manualPageDownRequested = true; + manualPageDownRequested = true; statsAutoPagingEnabled = false; } } @@ -4902,16 +4940,16 @@ static void osdRefresh(timeUs_t currentTimeUs) // Process manual page change events for multi-page stats. if (manualPageUpRequested) { osdShowStats(statsSinglePageCompatible, 1); - statsCurrentPage = 1; + statsCurrentPage = 1; } else if (manualPageDownRequested) { osdShowStats(statsSinglePageCompatible, 0); statsCurrentPage = 0; - } } } + } // Handle events when either "Splash", "Armed" or "Stats" screens are displayed. - if ((currentTimeUs > resumeRefreshAt) || OSD_RESUME_UPDATES_STICK_COMMAND) { + if ((currentTimeUs > resumeRefreshAt) || OSD_RESUME_UPDATES_STICK_COMMAND) { // Time elapsed or canceled by stick commands. // Exit to normal OSD operation. displayClearScreen(osdDisplayPort); @@ -4921,7 +4959,7 @@ static void osdRefresh(timeUs_t currentTimeUs) // Continue "Splash", "Armed" or "Stats" screens. displayHeartbeat(osdDisplayPort); } - + return; } @@ -5183,7 +5221,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } } if (isFixedWingLevelTrimActive()) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLEVEL); + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLEVEL); } if (FLIGHT_MODE(HEADFREE_MODE)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_HEADFREE); diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index c040b7b762f..38db724c8e1 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -787,7 +787,11 @@ static void osdDJIEfficiencyMahPerKM(char *buff) timeUs_t currentTimeUs = micros(); timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); - if (STATE(GPS_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 c566c94bfad..7049b215bf9 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2505,8 +2505,19 @@ 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 + posControl.rthSanityChecker.minimalDistanceToHome = 1e10f; + //schedule check in 5 seconds after getting real GPS fix, when position estimation coords stabilise after jump + posControl.rthSanityChecker.lastCheckTime = currentTimeMs + 5000; + return true; + } +#endif + // Check at 10Hz rate - if ((currentTimeMs - posControl.rthSanityChecker.lastCheckTime) > 100) { + if ( ((int32_t)(currentTimeMs - posControl.rthSanityChecker.lastCheckTime)) > 100) { const float currentDistanceToHome = calculateDistanceToDestination(&posControl.rthState.homePosition.pos); posControl.rthSanityChecker.lastCheckTime = currentTimeMs; @@ -4141,7 +4152,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); + 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 d4ee6e078ed..0e111941641 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -209,6 +209,10 @@ typedef struct positionEstimationConfig_s { float baro_epv; // Baro position error 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 e6084e0972e..d4da630f343 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -725,7 +725,7 @@ bool isFixedWingLandingDetected(void) // Check horizontal and vertical velocities are low (cm/s) bool velCondition = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < (50.0f * sensitivity) && - posControl.actualState.velXY < (100.0f * sensitivity); + ( posControl.actualState.velXY < (100.0f * sensitivity)); // Check angular rates are low (degs/s) bool gyroCondition = averageAbsGyroRates() < (2.0f * sensitivity); DEBUG_SET(DEBUG_LANDING, 2, velCondition); diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index d7ef73c9382..221f6cc76c5 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -92,7 +92,10 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, .w_acc_bias = SETTING_INAV_W_ACC_BIAS_DEFAULT, .max_eph_epv = SETTING_INAV_MAX_EPH_EPV_DEFAULT, - .baro_epv = SETTING_INAV_BARO_EPV_DEFAULT + .baro_epv = SETTING_INAV_BARO_EPV_DEFAULT, +#ifdef USE_GPS_FIX_ESTIMATION + .allow_gps_fix_estimation = SETTING_INAV_ALLOW_GPS_FIX_ESTIMATION_DEFAULT +#endif ); #define resetTimer(tim, currentTimeUs) { (tim)->deltaTime = 0; (tim)->lastTriggeredTime = currentTimeUs; } @@ -170,6 +173,15 @@ 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; } @@ -221,8 +233,16 @@ void onNewGPSData(void) newLLH.lon = gpsSol.llh.lon; newLLH.alt = gpsSol.llh.alt; - if (sensors(SENSOR_GPS)) { - if (!STATE(GPS_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; } @@ -502,7 +522,11 @@ static bool navIsAccelerationUsable(void) static bool navIsHeadingUsable(void) { - if (sensors(SENSOR_GPS)) { + 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(); } @@ -517,7 +541,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) && 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) { @@ -714,7 +742,11 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) static void estimationCalculateGroundCourse(timeUs_t currentTimeUs) { - if (STATE(GPS_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 f0e54d90138..7d8a67081eb 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -97,7 +97,7 @@ static int logicConditionCompute( int temporaryValue; #if defined(USE_VTX_CONTROL) vtxDeviceCapability_t vtxDeviceCapability; -#endif +#endif switch (operation) { @@ -154,7 +154,7 @@ static int logicConditionCompute( case LOGIC_CONDITION_NOR: return !(operandA || operandB); - break; + break; case LOGIC_CONDITION_NOT: return !operandA; @@ -170,7 +170,7 @@ static int logicConditionCompute( return false; } - //When both operands are not met, keep current value + //When both operands are not met, keep current value return currentValue; break; @@ -238,7 +238,7 @@ static int logicConditionCompute( gvSet(operandA, operandB); return operandB; break; - + case LOGIC_CONDITION_GVAR_INC: temporaryValue = gvGet(operandA) + operandB; gvSet(operandA, temporaryValue); @@ -270,7 +270,7 @@ static int logicConditionCompute( return operandA; } break; - + case LOGIC_CONDITION_OVERRIDE_ARMING_SAFETY: LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY); return true; @@ -288,10 +288,10 @@ static int logicConditionCompute( break; case LOGIC_CONDITION_SET_VTX_POWER_LEVEL: -#if defined(USE_VTX_CONTROL) +#if defined(USE_VTX_CONTROL) #if(defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)) if ( - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] != operandA && + logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] != operandA && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability) ) { logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount); @@ -339,18 +339,18 @@ static int logicConditionCompute( LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_PITCH); return true; break; - + case LOGIC_CONDITION_INVERT_YAW: LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_YAW); return true; break; - + case LOGIC_CONDITION_OVERRIDE_THROTTLE: logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE] = operandA; LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE); return operandA; break; - + case LOGIC_CONDITION_SET_OSD_LAYOUT: logicConditionValuesByType[LOGIC_CONDITION_SET_OSD_LAYOUT] = operandA; LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT); @@ -366,18 +366,18 @@ static int logicConditionCompute( case LOGIC_CONDITION_SIN: temporaryValue = (operandB == 0) ? 500 : operandB; - return sin_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; + return sin_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; break; - + case LOGIC_CONDITION_COS: temporaryValue = (operandB == 0) ? 500 : operandB; - return cos_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; + return cos_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; break; break; - + case LOGIC_CONDITION_TAN: temporaryValue = (operandB == 0) ? 500 : operandB; - return tan_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; + return tan_approx(DEGREES_TO_RADIANS(operandA)) * temporaryValue; break; case LOGIC_CONDITION_MIN: @@ -387,11 +387,11 @@ static int logicConditionCompute( case LOGIC_CONDITION_MAX: return (operandA > operandB) ? operandA : operandB; break; - + case LOGIC_CONDITION_MAP_INPUT: return scaleRange(constrain(operandA, 0, operandB), 0, operandB, 0, 1000); break; - + case LOGIC_CONDITION_MAP_OUTPUT: return scaleRange(constrain(operandA, 0, 1000), 0, 1000, 0, operandB); break; @@ -486,9 +486,20 @@ static int logicConditionCompute( return operandA; break; #endif +#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); + } else { + LOGIC_CONDITION_GLOBAL_FLAG_DISABLE(LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX); + } + return true; + break; +#endif + default: return false; - break; + break; } } @@ -497,7 +508,7 @@ void logicConditionProcess(uint8_t i) { const int activatorValue = logicConditionGetValue(logicConditions(i)->activatorId); if (logicConditions(i)->enabled && activatorValue && !cliMode) { - + /* * Process condition only when latch flag is not set * Latched LCs can only go from OFF to ON, not the other way @@ -506,13 +517,13 @@ void logicConditionProcess(uint8_t i) { const int operandAValue = logicConditionGetOperandValue(logicConditions(i)->operandA.type, logicConditions(i)->operandA.value); const int operandBValue = logicConditionGetOperandValue(logicConditions(i)->operandB.type, logicConditions(i)->operandB.value); const int newValue = logicConditionCompute( - logicConditionStates[i].value, - logicConditions(i)->operation, - operandAValue, + logicConditionStates[i].value, + logicConditions(i)->operation, + operandAValue, operandBValue, i ); - + logicConditionStates[i].value = newValue; /* @@ -587,7 +598,7 @@ static int logicConditionGetWaypointOperandValue(int operand) { return distance; } break; - + case LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION: return (NAV_Status.activeWpIndex > 0) ? ((posControl.waypointList[NAV_Status.activeWpIndex-1].p3 & NAV_WP_USER1) == NAV_WP_USER1) : 0; break; @@ -633,11 +644,11 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER: // in s return constrain((uint32_t)getFlightTime(), 0, INT16_MAX); break; - + case LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE: //in m return constrain(GPS_distanceToHome, 0, INT16_MAX); break; - + case LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE: //in m return constrain(getTotalTravelDistance() / 100, 0, INT16_MAX); break; @@ -645,7 +656,7 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_RSSI: return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99); break; - + case LOGIC_CONDITION_OPERAND_FLIGHT_VBAT: // V / 100 return getBatteryVoltage(); break; @@ -661,19 +672,24 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT: // Amp / 100 return getAmperage(); break; - + case LOGIC_CONDITION_OPERAND_FLIGHT_MAH_DRAWN: // mAh return getMAhDrawn(); break; case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS: +#ifdef USE_GPS_FIX_ESTIMATION + if ( STATE(GPS_ESTIMATED_FIX) ){ + return gpsSol.numSat; //99 + } else +#endif if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { return 0; } else { return gpsSol.numSat; } break; - + case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID: // 0/1 return STATE(GPS_FIX) ? 1 : 0; break; @@ -725,15 +741,15 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_IS_AUTOLAUNCH: // 0/1 return (navGetCurrentStateFlags() & NAV_CTL_LAUNCH) ? 1 : 0; - break; - + break; + case LOGIC_CONDITION_OPERAND_FLIGHT_IS_ALTITUDE_CONTROL: // 0/1 return (navGetCurrentStateFlags() & NAV_CTL_ALT) ? 1 : 0; - break; + break; case LOGIC_CONDITION_OPERAND_FLIGHT_IS_POSITION_CONTROL: // 0/1 return (navGetCurrentStateFlags() & NAV_CTL_POS) ? 1 : 0; - break; + break; case LOGIC_CONDITION_OPERAND_FLIGHT_IS_EMERGENCY_LANDING: // 0/1 return (navGetCurrentStateFlags() & NAV_CTL_EMERG) ? 1 : 0; @@ -741,7 +757,7 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_IS_RTH: // 0/1 return (navGetCurrentStateFlags() & NAV_AUTO_RTH) ? 1 : 0; - break; + break; case LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING: // 0/1 return (navGetCurrentStateFlags() & NAV_CTL_LAND) ? 1 : 0; @@ -750,16 +766,16 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE: // 0/1 return (failsafePhase() != FAILSAFE_IDLE) ? 1 : 0; break; - - case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW: // + + case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW: // return axisPID[YAW]; break; - - case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL: // + + case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL: // return axisPID[ROLL]; break; - - case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH: // + + case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH: // return axisPID[PITCH]; break; @@ -786,7 +802,7 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE: // int return getConfigProfile() + 1; break; - + case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE: // int return currentMixerProfileIndex + 1; break; @@ -801,14 +817,14 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS: return isEstimatedAglTrusted(); break; - + case LOGIC_CONDITION_OPERAND_FLIGHT_AGL: return getEstimatedAglPosition(); - break; - + break; + case LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW: return rangefinderGetLatestRawAltitude(); - break; + break; default: return 0; @@ -909,7 +925,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) { //Extract RC channel raw value if (operand >= 1 && operand <= MAX_SUPPORTED_RC_CHANNEL_COUNT) { retVal = rxGetChannelValue(operand - 1); - } + } break; case LOGIC_CONDITION_OPERAND_TYPE_FLIGHT: @@ -937,7 +953,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) { retVal = programmingPidGetOutput(operand); } break; - + case LOGIC_CONDITION_OPERAND_TYPE_WAYPOINTS: retVal = logicConditionGetWaypointOperandValue(operand); break; @@ -951,7 +967,7 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) { /* * conditionId == -1 is always evaluated as true - */ + */ int logicConditionGetValue(int8_t conditionId) { if (conditionId >= 0) { return logicConditionStates[conditionId].value; @@ -1033,7 +1049,7 @@ int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue) { uint32_t getLoiterRadius(uint32_t loiterRadius) { #ifdef USE_PROGRAMMING_FRAMEWORK - if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS) && + if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS) && !(FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding())) { return constrain(logicConditionValuesByType[LOGIC_CONDITION_LOITER_OVERRIDE], loiterRadius, 100000); } else { diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index bb62881b780..b46aeb44340 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -82,7 +82,8 @@ typedef enum { LOGIC_CONDITION_DELTA = 50, LOGIC_CONDITION_APPROX_EQUAL = 51, LOGIC_CONDITION_LED_PIN_PWM = 52, - LOGIC_CONDITION_LAST = 53, + LOGIC_CONDITION_DISABLE_GPS_FIX = 53, + LOGIC_CONDITION_LAST = 54, } logicOperation_e; typedef enum logicOperandType_s { @@ -190,6 +191,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 b3d7a1ad4ee..56c4d12417b 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,29 +62,25 @@ 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 { + } else { return HW_SENSOR_UNHEALTHY; } - } + } #endif if (detectedSensors[SENSOR_INDEX_MAG] != MAG_NONE) { if (compassIsHealthy()) { return HW_SENSOR_OK; - } - else { + } else { return HW_SENSOR_UNHEALTHY; } - } - else { + } else { if (requestedSensors[SENSOR_INDEX_MAG] != MAG_NONE) { // Selected but not detected return HW_SENSOR_UNAVAILABLE; - } - else { + } else { // Not selected and not detected return HW_SENSOR_NONE; } @@ -100,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()) { @@ -108,7 +102,7 @@ hardwareSensorStatus_e getHwBarometerStatus(void) } else { return HW_SENSOR_UNHEALTHY; } - } + } #endif if (detectedSensors[SENSOR_INDEX_BARO] != BARO_NONE) { if (baroIsHealthy()) { @@ -139,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; } } @@ -148,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; } @@ -165,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; } } @@ -174,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; } @@ -191,8 +181,7 @@ hardwareSensorStatus_e getHwGPSStatus(void) if (sensors(SENSOR_GPS)) { if (isGPSHealthy()) { return HW_SENSOR_OK; - } - else { + } else { return HW_SENSOR_UNHEALTHY; } } @@ -200,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; } @@ -217,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; } } @@ -226,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 8d2b29c075c..13b9fb54255 100755 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -238,14 +238,14 @@ STATIC_PROTOTHREAD(pitotThread) pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, &pitotTemperatureTmp); #ifdef USE_SIMULATOR - if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { - pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE; - } + 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(); @@ -276,14 +276,14 @@ STATIC_PROTOTHREAD(pitotThread) } #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 d2d135cfa2e..f01b596bdac 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 85f6f85b962..c468d2cfac5 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)) { + 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 9e1f9f36226..8e2e7d8c075 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -234,7 +234,12 @@ 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)) { +#ifdef USE_GPS_FIX_ESTIMATION + if (!(STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX))) +#else + if (!(STATE(GPS_FIX))) +#endif + { hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE; return; } @@ -476,7 +481,11 @@ static bool processBinaryModeRequest(uint8_t address) switch (address) { #ifdef USE_GPS case 0x8A: - if (sensors(SENSOR_GPS)) { + 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 f0819f7cb19..3c7dfd2875a 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)) { + 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)) { + 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)) 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)) 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)) 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)) 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)) 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)) 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)) 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)) 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)) 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)) 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)) 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)) 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)) 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)) 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 c9b68fa27f7..b0ffad84347 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)) { + 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 00e99a80472..84dccddb869 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)) { + 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 187f959b178..f06eda57df6 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -525,7 +525,11 @@ void mavlinkSendPosition(timeUs_t currentTimeUs) { uint8_t gpsFixType = 0; - if (!sensors(SENSOR_GPS)) + if (!(sensors(SENSOR_GPS) +#ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) +#endif + )) return; if (gpsSol.fixType == GPS_NO_FIX) @@ -640,7 +644,11 @@ void mavlinkSendHUDAndHeartbeat(void) #if defined(USE_GPS) // use ground speed if source available - if (sensors(SENSOR_GPS)) { + 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 af58cd5c438..5a74780d630 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)) { + 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 72d91024bcf..9d804be52d8 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) || !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 8e01e47ef8e..c341363f160 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) || 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) || 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; }