commander state machine fix preflight and prearm error

-  fixes #9155

src/modules/commander/commander.cpp

@@ -389,12 +389,8 @@ int commander_main(int argc, char *argv[])
 	}
 
 	if (!strcmp(argv[1], "check")) {
-		int checkres = 0;
-		checkres = prearm_check(&mavlink_log_pub, false, true, &status_flags, battery, ARM_REQ_GPS_BIT, hrt_elapsed_time(&commander_boot_timestamp));
-		warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED");
-
-		checkres = prearm_check(&mavlink_log_pub, true, true, &status_flags, battery, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp));
-		warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED");
+		bool checkres = prearm_check(&mavlink_log_pub, true, true, &status_flags, battery, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp));
+		PX4_INFO("Prearm check: %s", checkres ? "OK" : "FAILED");
 
 		return 0;
 	}

src/modules/commander/state_machine_helper.cpp

@@ -85,7 +85,6 @@ const char * const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = {
 };
 
 static hrt_abstime last_preflight_check = 0;	///< initialize so it gets checked immediately
-static int last_prearm_ret = 1;			///< initialize to fail
 
 void set_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed,
 			     const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const link_loss_actions_t link_loss_act,
@@ -118,7 +117,8 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt
 		/*
 		 * Get sensing state if necessary
 		 */
-		int prearm_ret = OK;
+		bool prearm_check_ret = true;
+
 		bool checkAirspeed = false;
 		bool sensor_checks = !hil_enabled;
 
@@ -136,11 +136,11 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt
 					       (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), arm_requirements & ARM_REQ_GPS_BIT, true,
 					       status->is_vtol, true, true, time_since_boot);
 
-			prearm_ret = prearm_check(mavlink_log_pub, true /* pre-arm */, false /* force_report */, status_flags, battery,
+			prearm_check_ret = prearm_check(mavlink_log_pub, true /* pre-arm */, false /* force_report */, status_flags, battery,
 						  arm_requirements, time_since_boot);
 
 			if (!preflight_check) {
-				prearm_ret = false;
+				prearm_check_ret = false;
 			}
 
 		}
@@ -153,16 +153,12 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt
 
 			if (last_preflight_check == 0 || hrt_absolute_time() - last_preflight_check > 1000 * 1000) {
 
-				prearm_ret = Preflight::preflightCheck(mavlink_log_pub, sensor_checks, checkAirspeed,
+				prearm_check_ret = Preflight::preflightCheck(mavlink_log_pub, sensor_checks, checkAirspeed,
 								       (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), arm_requirements & ARM_REQ_GPS_BIT, true,
 								       status->is_vtol, false, false, time_since_boot);
 
-				status_flags->condition_system_sensors_initialized = (prearm_ret == OK);
+				status_flags->condition_system_sensors_initialized = prearm_check_ret;
 				last_preflight_check = hrt_absolute_time();
-				last_prearm_ret = prearm_ret;
-
-			} else {
-				prearm_ret = last_prearm_ret;
 			}
 		}
 
@@ -176,7 +172,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt
 		/* enforce lockdown in HIL */
 		if (hil_enabled) {
 			armed->lockdown = true;
-			prearm_ret = OK;
+			prearm_check_ret = true;
 			status_flags->condition_system_sensors_initialized = true;
 
 			/* recover from a prearm fail */
@@ -201,7 +197,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt
 				    !hil_enabled) {
 
 					// Fail transition if pre-arm check fails
-					if (prearm_ret) {
+					if (!prearm_check_ret) {
 						/* the prearm check already prints the reject reason */
 						feedback_provided = true;
 						valid_transition = false;
@@ -1035,7 +1031,7 @@ void reset_link_loss_globals(actuator_armed_s *armed, const bool old_failsafe, c
 	}
 }
 
-int prearm_check(orb_advert_t *mavlink_log_pub, const bool prearm, const bool force_report,
+bool prearm_check(orb_advert_t *mavlink_log_pub, const bool prearm, const bool force_report,
 		 vehicle_status_flags_s *status_flags, const battery_status_s &battery, const uint8_t arm_requirements,
 		 const hrt_abstime &time_since_boot)
 {
@@ -1075,5 +1071,5 @@ int prearm_check(orb_advert_t *mavlink_log_pub, const bool prearm, const bool fo
 		status_flags->condition_system_prearm_error_reported = true;
 	}
 
-	return !prearm_ok;
+	return prearm_ok;
 }

src/modules/commander/state_machine_helper.h

@@ -106,7 +106,7 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
 bool check_invalid_pos_nav_state(vehicle_status_s *status, bool old_failsafe, orb_advert_t *mavlink_log_pub,
 				 const vehicle_status_flags_s &status_flags, const bool use_rc, const bool using_global_pos);
 
-int prearm_check(orb_advert_t *mavlink_log_pub, const bool prearm, const bool force_report,
+bool prearm_check(orb_advert_t *mavlink_log_pub, const bool prearm, const bool force_report,
 		 vehicle_status_flags_s *status_flags, const battery_status_s &battery, const uint8_t arm_requirements,
 		 const hrt_abstime &time_since_boot);