From 3ac8c23dd0516b81df50d835d5bd506d6ea2de9c Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Thu, 6 May 2021 14:43:13 +0300 Subject: [PATCH] commander: added prearm check for geofence violation - if geofence action is not none, then don't allow arming outside of geofence Signed-off-by: RomanBapst --- .../commander/Arming/PreFlightCheck/PreFlightCheck.hpp | 1 + .../Arming/PreFlightCheck/checks/preArmCheck.cpp | 8 ++++++++ src/modules/commander/Commander.cpp | 2 ++ 3 files changed, 11 insertions(+) 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;