GPS Fix estimation (dead reconing, RTH without GPS fix) for fixed wing

docs/GPS_fix_estimation.md

@@ -0,0 +1,93 @@
+# 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
+- 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 inreachable spaces, plane will be lost.
+
+GPS fix estimation allows to recover plane using magnetometer and baromener only.
+
+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.
+
+# 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 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 posible to roughtly 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 **roughtly** estimate it's position.
+
+It is assumed, that plane will fly in roughtly 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*.
+
+# Settings
+
+GPS Fix estimation is enabled with CLI command:
+
+```set inav_allow_gps_fix_estimation=ON```
+
+Also you have to specify criuse 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.*
+
+*Note related command: to continue mission without RC signal, see command ```set failsafe_mission=OFF```.*
+
+**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 purpoces, 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.
+
+# 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

docs/Settings.md

@@ -852,6 +852,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].
@@ -1712,6 +1722,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.

src/main/fc/runtime_config.h

@@ -122,6 +122,7 @@ 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),
+    GPS_ESTIMATED_FIX                   = (1 << 10),
     NAV_CRUISE_BRAKING                  = (1 << 11),
     NAV_CRUISE_BRAKING_BOOST            = (1 << 12),
     NAV_CRUISE_BRAKING_LOCKED           = (1 << 13),

src/main/fc/settings.yaml

@@ -840,6 +840,11 @@ 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."
+        default_value: 7
+        min: -1
+        max: 600
 
   - name: PG_LIGHTS_CONFIG
     type: lightsConfig_t
@@ -2188,6 +2193,11 @@ 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."
+        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"

src/main/flight/failsafe.c

@@ -82,6 +82,7 @@ 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)
+    .failsafe_gps_fix_estimation_delay = SETTING_FAILSAFE_GPS_FIX_ESTIMATION_DELAY_DEFAULT, // Time delay before Failsafe activated when GPS Fix estimation is allied
 );
 
 typedef enum {
@@ -349,7 +350,7 @@ 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)) {
+            sensors(SENSOR_GPS) && (STATE(GPS_FIX) || STATE(GPS_ESTIMATED_FIX)) && STATE(GPS_FIX_HOME)) {
 
         // get the distance to the original arming point
         uint32_t distance = calculateDistanceToDestination(&original_rth_home);
@@ -403,6 +404,18 @@ void failsafeUpdateState(void)
                         }
                         reprocessState = true;
                     }
+                    else {
+                        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))) {
+                                failsafeSetActiveProcedure(FAILSAFE_PROCEDURE_RTH);
+                                failsafeActivate(FAILSAFE_RETURN_TO_HOME);
+                                activateForcedRTH();
+                            }
+                      } else
+                          failsafeState.wpModeGPSFixEstimationDelayedFailsafeStart = 0;
+                    }		
                 } else {
                     // When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode)
                     if (!receivingRxDataAndNotFailsafeMode) {

src/main/flight/failsafe.h

@@ -43,6 +43,7 @@ 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)
+	int16_t failsafe_gps_fix_estimation_delay;  // Time delay before Failsafe triggered when GPX Fix estimation is applied (s)
 } failsafeConfig_t;
 
 PG_DECLARE(failsafeConfig_t, failsafeConfig);
@@ -149,6 +150,7 @@ 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
+    timeMs_t wpModeGPSFixEstimationDelayedFailsafeStart;    // waypoint mission delayed failsafe timer start time on GPS fix estimation
     failsafeProcedure_e activeProcedure;
     failsafePhase_e phase;
     failsafeRxLinkState_e rxLinkState;

src/main/flight/wind_estimator.c

@@ -88,7 +88,8 @@ void updateWindEstimator(timeUs_t currentTimeUs)
     if (!STATE(FIXED_WING_LEGACY) ||
         !isGPSHeadingValid() ||
         !gpsSol.flags.validVelNE ||
-        !gpsSol.flags.validVelD) {
+        !gpsSol.flags.validVelD ||
+		STATE(GPS_ESTIMATED_FIX)){
         return;
     }
 

src/main/io/gps.c

@@ -45,21 +45,33 @@
 #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"
 #include "io/gps_private.h"
 
 #include "navigation/navigation.h"
+#include "navigation/navigation_private.h"
 
 #include "config/feature.h"
 
 #include "fc/config.h"
 #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)
@@ -137,6 +149,117 @@ void gpsSetProtocolTimeout(timeMs_t timeoutMs)
     gpsState.timeoutMs = timeoutMs;
 }
 
+
+bool canEstimateGPSFix(void)
+{
+	return positionEstimationConfig()->allow_gps_fix_estimation && STATE(AIRPLANE) && sensors(SENSOR_GPS) && sensors(SENSOR_BARO) && sensors(SENSOR_MAG) && ARMING_FLAG(WAS_EVER_ARMED) && STATE(GPS_FIX_HOME);
+}
+
+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;
+
+	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;
+
+		//freeze coordinates
+		gpsSol.llh.lat = last_lat;
+		gpsSol.llh.lon = last_lon;
+		gpsSol.llh.alt = last_alt;
+
+		DISABLE_STATE(GPS_FIX);
+	}
+	else
+	{
+		last_lat = gpsSol.llh.lat;
+		last_lon = gpsSol.llh.lon;
+		last_alt = gpsSol.llh.alt;
+	}
+
+	if (STATE(GPS_FIX) || !canEstimateGPSFix()) {
+		DISABLE_STATE(GPS_ESTIMATED_FIX);
+		estimated_lat = gpsSol.llh.lat;
+		estimated_lon = gpsSol.llh.lon;
+		estimated_alt = posControl.gpsOrigin.alt + baro.BaroAlt;
+		return;
+	}
+
+	ENABLE_STATE(GPS_ESTIMATED_FIX);
+
+	gpsSol.fixType = GPS_FIX_3D;
+	gpsSol.hdop = 99;
+	gpsSol.flags.hasNewData = true;
+	gpsSol.numSat = 99;
+
+	gpsSol.eph = 100;
+	gpsSol.epv = 100;
+
+	gpsSol.flags.validVelNE = 1;
+	gpsSol.flags.validVelD = 0;  //do not provide velocity.z
+	gpsSol.flags.validEPE = 1;
+
+	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() == 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;
+}
+
+
+
 void gpsProcessNewSolutionData(void)
 {
     // Set GPS fix flag only if we have 3D fix
@@ -154,6 +277,8 @@ void gpsProcessNewSolutionData(void)
     // Set sensor as ready and available
     sensorsSet(SENSOR_GPS);
 
+	updateEstimatedGPSFix();
+
     // Pass on GPS update to NAV and IMU
     onNewGPSData();