Geofence - Do not trigger geofence failsafe while in low battery

failsafe action.
Also move geofence flags from static variables declared into the if
statement to private members of the class.

src/modules/commander/Commander.cpp

@@ -156,7 +156,6 @@ static uint8_t _last_sp_man_arm_switch = 0;
 static struct vtol_vehicle_status_s vtol_status = {};
 static struct cpuload_s cpuload = {};
 
-static bool warning_action_on = false;
 static bool last_overload = false;
 
 static struct vehicle_status_flags_s status_flags = {};
@@ -1738,7 +1737,7 @@ Commander::run()
 		}
 
 
-		if (!warning_action_on) {
+		if (!_warning_action_on) {
 			// store the last good main_state when not in an navigation
 			// hold state
 			main_state_before_rtl = internal_state.main_state;
@@ -1747,7 +1746,7 @@ Commander::run()
 			   && internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LOITER
 			   && internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) {
 			// reset flag again when we switched out of it
-			warning_action_on = false;
+			_warning_action_on = false;
 		}
 
 		orb_check(cpuload_sub, &updated);
@@ -1834,19 +1833,18 @@ Commander::run()
 		}
 
 		// Geofence actions
-		if (armed.armed && (geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE)) {
-
-			static bool geofence_loiter_on = false;
-			static bool geofence_rtl_on = false;
+		// Geofence failsafe is disabled when in a low battery handling action (critical or emergency)
+		if (armed.armed
+		    && (geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE)
+		    && (_battery_warning < battery_status_s::BATTERY_WARNING_CRITICAL)) {
 
 			// check for geofence violation
 			if (geofence_result.geofence_violated) {
-				static hrt_abstime last_geofence_violation = 0;
 				const hrt_abstime geofence_violation_action_interval = 10_s;
 
-				if (hrt_elapsed_time(&last_geofence_violation) > geofence_violation_action_interval) {
+				if (hrt_elapsed_time(&_last_geofence_violation) > geofence_violation_action_interval) {
 
-					last_geofence_violation = hrt_absolute_time();
+					_last_geofence_violation = hrt_absolute_time();
 
 					switch (geofence_result.geofence_action) {
 					case (geofence_result_s::GF_ACTION_NONE) : {
@@ -1862,7 +1860,7 @@ Commander::run()
 					case (geofence_result_s::GF_ACTION_LOITER) : {
 							if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags,
 									&internal_state)) {
-								geofence_loiter_on = true;
+								_geofence_loiter_on = true;
 							}
 
 							break;
@@ -1871,7 +1869,7 @@ Commander::run()
 					case (geofence_result_s::GF_ACTION_RTL) : {
 							if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags,
 									&internal_state)) {
-								geofence_rtl_on = true;
+								_geofence_rtl_on = true;
 							}
 
 							break;
@@ -1889,23 +1887,30 @@ Commander::run()
 			}
 
 			// reset if no longer in LOITER or if manually switched to LOITER
-			geofence_loiter_on = geofence_loiter_on
-					     && (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER)
-					     && (sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_OFF
-						 || sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_NONE);
+			_geofence_loiter_on = _geofence_loiter_on
+					      && (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER)
+					      && (sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_OFF
+						  || sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_NONE);
 
 			// reset if no longer in RTL or if manually switched to RTL
-			geofence_rtl_on = geofence_rtl_on
-					  && (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL)
-					  && (sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_OFF
-					      || sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_NONE);
+			_geofence_rtl_on = _geofence_rtl_on
+					   && (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL)
+					   && (sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_OFF
+					       || sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_NONE);
+
+			_warning_action_on = _warning_action_on || (_geofence_loiter_on || _geofence_rtl_on);
 
-			warning_action_on = warning_action_on || (geofence_loiter_on || geofence_rtl_on);
+		} else {
+			// No geofence checks, reset flags
+			_geofence_loiter_on = false;
+			_geofence_rtl_on = false;
+			_warning_action_on = false;
+			_last_geofence_violation = 0;
 		}
 
 		// revert geofence failsafe transition if sticks are moved and we were previously in a manual mode
 		// but only if not in a low battery handling action
-		if (rc_override != 0 && (_battery_warning < battery_status_s::BATTERY_WARNING_CRITICAL) && (warning_action_on &&
+		if (rc_override != 0 && (_battery_warning < battery_status_s::BATTERY_WARNING_CRITICAL) && (_warning_action_on &&
 				(main_state_before_rtl == commander_state_s::MAIN_STATE_MANUAL ||
 				 main_state_before_rtl == commander_state_s::MAIN_STATE_ALTCTL ||
 				 main_state_before_rtl == commander_state_s::MAIN_STATE_POSCTL ||
@@ -2721,7 +2726,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
 		// if the system now later enters an autonomous state the pilot can move
 		// the sticks to break out of the autonomous state
 
-		if (!warning_action_on
+		if (!_warning_action_on
 		    && (internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL ||
 			internal_state.main_state == commander_state_s::MAIN_STATE_ALTCTL ||
 			internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL ||

src/modules/commander/Commander.hpp

@@ -163,6 +163,12 @@ private:
 	float		_load_factor_ratio{0.5f};	/**< ratio of maximum load factor predicted by stall speed to measured load factor */
 	float		_apsd_innov_integ_state{0.0f};	/**< inegral of excess normalised airspeed innovation (sec) */
 
+	/* geofence flags */
+	bool _geofence_loiter_on{false};
+	bool _geofence_rtl_on{false};
+	bool _warning_action_on{false};
+	hrt_abstime _last_geofence_violation{0};
+
 	FailureDetector _failure_detector;
 	bool _failure_detector_termination_printed{false};