diff --git a/msg/geofence_result.msg b/msg/geofence_result.msg index 25022bbd56..fcae7a28c4 100644 --- a/msg/geofence_result.msg +++ b/msg/geofence_result.msg @@ -4,6 +4,7 @@ uint8 GF_ACTION_WARN = 1 # critical mavlink message uint8 GF_ACTION_LOITER = 2 # switch to AUTO|LOITER uint8 GF_ACTION_RTL = 3 # switch to AUTO|RTL uint8 GF_ACTION_TERMINATE = 4 # flight termination +uint8 GF_ACTION_LAND = 5 # switch to AUTO|LAND bool geofence_violated # true if the geofence is violated uint8 geofence_action # action to take when geofence is violated diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index abcee12ac9..2b90df7131 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1765,6 +1765,15 @@ Commander::run() break; } + case (geofence_result_s::GF_ACTION_LAND) : { + if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags, + &_internal_state)) { + _geofence_land_on = true; + } + + break; + } + case (geofence_result_s::GF_ACTION_TERMINATE) : { PX4_WARN("Flight termination because of geofence"); mavlink_log_critical(&mavlink_log_pub, "Geofence violation! Flight terminated"); @@ -1794,12 +1803,21 @@ Commander::run() _geofence_rtl_on = false; } - _geofence_warning_action_on = _geofence_warning_action_on || (_geofence_loiter_on || _geofence_rtl_on); + // reset if no longer in LAND or if manually switched to LAND + const bool in_land_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND; + + if (!in_land_mode) { + _geofence_land_on = false; + } + + _geofence_warning_action_on = _geofence_warning_action_on || (_geofence_loiter_on || _geofence_rtl_on + || _geofence_land_on); } else { // No geofence checks, reset flags _geofence_loiter_on = false; _geofence_rtl_on = false; + _geofence_land_on = false; _geofence_warning_action_on = false; _geofence_violated_prev = false; } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 1f5aa14841..5af52af240 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -304,6 +304,7 @@ private: bool _geofence_loiter_on{false}; bool _geofence_rtl_on{false}; + bool _geofence_land_on{false}; bool _geofence_warning_action_on{false}; bool _geofence_violated_prev{false}; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 88e3a7d8df..e42b32da80 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -53,12 +53,13 @@ * to be reset to 0 to really shut down the system. * * @min 0 - * @max 4 + * @max 5 * @value 0 None * @value 1 Warning * @value 2 Hold mode * @value 3 Return mode * @value 4 Terminate + * @value 5 Land mode * @group Geofence */ PARAM_DEFINE_INT32(GF_ACTION, 1);