state_machine_helper: change main state directly also for low battery RTL

src/modules/commander/state_machine_helper.cpp

@@ -1140,35 +1140,29 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
 	}
 
 	// Failsafe action
-	const bool rtl_possible = status_flags.condition_global_position_valid && status_flags.condition_home_position_valid;
 	const bool already_landing = internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND
 				     || internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND;
 	const bool already_landing_or_rtl = already_landing
 					    || internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL;
 
+	// The main state is directly changed for the action because we need the fallbacks by the navigation state.
 	switch (battery_warning) {
 	case battery_status_s::BATTERY_WARNING_CRITICAL:
 		switch (low_battery_action) {
 		case LOW_BAT_ACTION::RETURN:
 		case LOW_BAT_ACTION::RETURN_OR_LAND:
-			if (rtl_possible) {
 			if (!already_landing_or_rtl) {
 				internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_RTL;
+				internal_state.main_state_changes++;
 				internal_state.timestamp = hrt_absolute_time();
 			}
 
-			} else {
-				if (!already_landing) {
-					internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
-					internal_state.timestamp = hrt_absolute_time();
-				}
-			}
-
 			break;
 
 		case LOW_BAT_ACTION::LAND:
 			if (!already_landing) {
 				internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
+				internal_state.main_state_changes++;
 				internal_state.timestamp = hrt_absolute_time();
 			}
 
@@ -1182,25 +1176,19 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
 	case battery_status_s::BATTERY_WARNING_EMERGENCY:
 		switch (low_battery_action) {
 		case LOW_BAT_ACTION::RETURN:
-			if (rtl_possible) {
 			if (!already_landing_or_rtl) {
 				internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_RTL;
+				internal_state.main_state_changes++;
 				internal_state.timestamp = hrt_absolute_time();
 			}
 
-			} else {
-				if (!already_landing) {
-					internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
-					internal_state.timestamp = hrt_absolute_time();
-				}
-			}
-
 			break;
 
 		case LOW_BAT_ACTION::RETURN_OR_LAND:
 		case LOW_BAT_ACTION::LAND:
 			if (!already_landing) {
 				internal_state.main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
+				internal_state.main_state_changes++;
 				internal_state.timestamp = hrt_absolute_time();
 			}