diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp index 5fb1b6647f..392f3bb9c8 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp @@ -86,6 +86,7 @@ public: bool esc_check = false; bool global_position = false; bool mission = false; + bool geofence = false; }; static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp index bb3479502e..d0d113ece8 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp @@ -168,6 +168,14 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st } } + if (arm_requirements.geofence && status.geofence_violated) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "Arming denied, vehicle outside geofence"); + } + + prearm_ok = false; + } + // Arm Requirements: authorization // check last, and only if everything else has passed if (arm_requirements.arm_authorization && prearm_ok) { diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 525c9fedbd..69b1a0b62b 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1733,6 +1733,7 @@ Commander::run() _arm_requirements.esc_check = _param_escs_checks_required.get(); _arm_requirements.global_position = !_param_arm_without_gps.get(); _arm_requirements.mission = _param_arm_mission_required.get(); + _arm_requirements.geofence = _param_geofence_action.get() > geofence_result_s::GF_ACTION_NONE; /* flight mode slots */ _flight_mode_slots[0] = _param_fltmode_1.get(); @@ -2114,6 +2115,7 @@ Commander::run() /* start geofence result check */ _geofence_result_sub.update(&_geofence_result); + _status.geofence_violated = _geofence_result.geofence_violated; const bool in_low_battery_failsafe = _battery_warning > battery_status_s::BATTERY_WARNING_LOW;