diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index c1c1ee2f78..e5d290e163 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -4257,9 +4257,23 @@ 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) { diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 4690c6182f..d9c69c0c52 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -260,7 +260,9 @@ private: (ParamInt) _param_com_flt_time_max, (ParamFloat) _param_com_wind_max, - (ParamFloat) _param_com_spoolup_time + (ParamFloat) _param_com_spoolup_time, + + (ParamBool) _param_com_gnss_ivd_rtl ) // optional parameters diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 15c98f8c09..e8d2a2fd68 100644 --- a/src/modules/commander/commander_params.c +++ b/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);