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 <bapstroman@gmail.com>

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,

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) {

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;