Commander: add functionality for RTL due to GNSS loss

System switches to RTL if GNSS data is declared invalid but global position is still valid
(e.g. through dead-reckoning). Only in AUTO_MISSION, AUTO_LOITER, MANUAL_POSITION modes.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/commander/Commander.cpp

@@ -4257,9 +4257,32 @@ void Commander::estimator_check()
 
 		// report GPS failure if flying and the global position estimate is still valid
 		if (!_vehicle_land_detected.landed && _vehicle_status_flags.global_position_valid) {
-			mavlink_log_warning(&_mavlink_log_pub, "GPS no longer valid\t");
-			events::send(events::ID("commander_gps_lost"), {events::Log::Critical, events::LogInternal::Info},
-				     "GPS no longer valid");
+
+			if (_param_com_gnss_ivd_rtl.get() && (_commander_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER
+							      || _commander_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION
+							      || _commander_state.main_state == commander_state_s::MAIN_STATE_POSCTL)) {
+
+				main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_RTL, _vehicle_status_flags, _commander_state);
+				_status_changed = true;
+				mavlink_log_critical(&_mavlink_log_pub, "GPS no longer valid, returning\t");
+				events::send(events::ID("commander_gps_lost_rtl"), {events::Log::Critical, events::LogInternal::Info},
+					     "GPS no longer valid, returning");
+
+			} else {
+				mavlink_log_warning(&_mavlink_log_pub, "GPS no longer valid\t");
+				events::send(events::ID("commander_gps_lost"), {events::Log::Critical, events::LogInternal::Info},
+					     "GPS no longer valid");
+			}
+
+		}
+
+	} else if (!condition_gps_position_was_valid && _vehicle_status_flags.gps_position_valid) {
+
+		// report GPS valid if flying and global position is (still) valid
+		if (!_vehicle_land_detected.landed && _vehicle_status_flags.global_position_valid) {
+			mavlink_log_warning(&_mavlink_log_pub, "GPS valid\t");
+			events::send(events::ID("commander_gps_valid_in_flight"), {events::Log::Warning, events::LogInternal::Info},
+				     "GPS valid");
 		}
 	}
 }

src/modules/commander/Commander.hpp

@@ -260,7 +260,9 @@ private:
 		(ParamInt<px4::params::COM_FLT_TIME_MAX>) _param_com_flt_time_max,
 		(ParamFloat<px4::params::COM_WIND_MAX>) _param_com_wind_max,
 
-		(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time
+		(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time,
+
+		(ParamBool<px4::params::COM_GNSS_IVD_RTL>) _param_com_gnss_ivd_rtl
 	)
 
 	// optional parameters

src/modules/commander/commander_params.c

@@ -1090,3 +1090,16 @@ PARAM_DEFINE_INT32(COM_FLT_TIME_MAX, -1);
  * @unit m/s
  */
 PARAM_DEFINE_FLOAT(COM_WIND_MAX, -1.f);
+
+/**
+ * Trigger RTL when GNSS gets invalid.
+ *
+ * Even after the loss of GNSS sensoring capabilities on the vehicle, its position can still be estimated
+ * by relying on the remaining sensor data. As the quality of the estimate in that case though deteriorates
+ * with time, it is recommended to start returning to launch as soon as the GNSS failure is detected. By setting
+ * this parameter to true, this is done automatically for Mission, Loiter and Position flight mode.
+ *
+ * @group Commander
+ * @boolean
+ */
+PARAM_DEFINE_INT32(COM_GNSS_IVD_RTL, 1);