diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 1ce23e15fb..e79d60ff70 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -570,7 +570,7 @@ out: } bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool checkAirspeed, bool checkRC, bool checkGNSS, - bool checkDynamic, bool isVTOL, bool reportFailures, bool prearm, hrt_abstime time_since_boot) + bool checkDynamic, bool isVTOL, bool reportFailures, bool prearm, const hrt_abstime& time_since_boot) { if (time_since_boot < 2000000) { diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h index 12105240b2..98a94b71e1 100644 --- a/src/modules/commander/PreflightCheck.h +++ b/src/modules/commander/PreflightCheck.h @@ -69,7 +69,7 @@ namespace Preflight * true if the GNSS receiver should be checked **/ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool checkAirspeed, bool checkRC, bool checkGNSS, - bool checkDynamic, bool isVTOL, bool reportFailures, bool prearm, hrt_abstime time_since_boot); + bool checkDynamic, bool isVTOL, bool reportFailures, bool prearm, const hrt_abstime& time_since_boot); static constexpr unsigned max_mandatory_gyro_count = 1; static constexpr unsigned max_optional_gyro_count = 3; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2fff9b2650..8ac1083a3b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -102,6 +102,7 @@ #include #include #include +#include #include #include #include @@ -389,7 +390,7 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "check")) { - bool checkres = prearm_check(&mavlink_log_pub, true, &status_flags, battery, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); + bool checkres = prearm_check(&mavlink_log_pub, status_flags, battery, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); PX4_INFO("Prearm check: %s", checkres ? "OK" : "FAILED"); return 0; @@ -1049,10 +1050,10 @@ Commander::handle_command(vehicle_status_s *status_local, } break; case vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY: { - // only send the acknowledge from the commander, the command actually is handled by each mavlink instance - cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; - } - break; + // only send the acknowledge from the commander, the command actually is handled by each mavlink instance + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + } + break; case vehicle_command_s::VEHICLE_CMD_CUSTOM_0: case vehicle_command_s::VEHICLE_CMD_CUSTOM_1: case vehicle_command_s::VEHICLE_CMD_CUSTOM_2: diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6f7fd41f37..af2f4a2cb8 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -117,7 +117,8 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt /* * Get sensing state if necessary */ - bool prearm_check_ret = true; + bool preflight_check_ret = false; + bool prearm_check_ret = false; bool checkAirspeed = false; bool sensor_checks = !hil_enabled; @@ -132,17 +133,18 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED && !hil_enabled) { - bool preflight_check = 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, true, true, time_since_boot); + preflight_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, true, true, time_since_boot); - prearm_check_ret = prearm_check(mavlink_log_pub, false /* force_report */, status_flags, battery, - arm_requirements, time_since_boot); + if (preflight_check_ret) { + status_flags->condition_system_sensors_initialized = true; - if (!preflight_check) { - prearm_check_ret = false; + // only both with prearm_check if preflight has passed + prearm_check_ret = prearm_check(mavlink_log_pub, *status_flags, battery, arm_requirements, time_since_boot); } + feedback_provided = true; } /* re-run the pre-flight check as long as sensors are failing */ @@ -153,11 +155,11 @@ 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_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 = 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_check_ret; last_preflight_check = hrt_absolute_time(); } } @@ -172,6 +174,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt /* enforce lockdown in HIL */ if (hil_enabled) { armed->lockdown = true; + preflight_check_ret = true; prearm_check_ret = true; status_flags->condition_system_sensors_initialized = true; @@ -197,14 +200,13 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt !hil_enabled) { // Fail transition if pre-arm check fails - if (!prearm_check_ret) { + if (!(preflight_check_ret && prearm_check_ret)) { /* the prearm check already prints the reject reason */ feedback_provided = true; valid_transition = false; - // Fail transition if we need safety switch press - } else if (safety.safety_switch_available && !safety.safety_off) { + // Fail transition if we need safety switch press mavlink_log_critical(mavlink_log_pub, "NOT ARMING: Press safety switch first!"); feedback_provided = true; @@ -1031,15 +1033,13 @@ void reset_link_loss_globals(actuator_armed_s *armed, const bool old_failsafe, c } } -bool prearm_check(orb_advert_t *mavlink_log_pub, 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) +bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, + const battery_status_s &battery, const uint8_t arm_requirements, const hrt_abstime &time_since_boot) { - bool reportFailures = force_report || (!status_flags->condition_system_prearm_error_reported && - status_flags->condition_system_hotplug_timeout); + bool reportFailures = true; bool prearm_ok = true; - if (!status_flags->circuit_breaker_engaged_usb_check && status_flags->usb_connected) { + if (!status_flags.circuit_breaker_engaged_usb_check && status_flags.usb_connected) { prearm_ok = false; if (reportFailures) { @@ -1047,7 +1047,7 @@ bool prearm_check(orb_advert_t *mavlink_log_pub, const bool force_report, } } - if (!status_flags->circuit_breaker_engaged_power_check && battery.warning >= battery_status_s::BATTERY_WARNING_LOW) { + if (!status_flags.circuit_breaker_engaged_power_check && battery.warning >= battery_status_s::BATTERY_WARNING_LOW) { prearm_ok = false; if (reportFailures) { @@ -1057,7 +1057,7 @@ bool prearm_check(orb_advert_t *mavlink_log_pub, const bool force_report, // mission required if ((arm_requirements & ARM_REQ_MISSION_BIT) - && (!status_flags->condition_auto_mission_available || !status_flags->condition_global_position_valid)) { + && (!status_flags.condition_auto_mission_available || !status_flags.condition_global_position_valid)) { prearm_ok = false; @@ -1066,10 +1066,5 @@ bool prearm_check(orb_advert_t *mavlink_log_pub, const bool force_report, } } - /* report once, then set the flag */ - if (reportFailures && !prearm_ok) { - status_flags->condition_system_prearm_error_reported = true; - } - return prearm_ok; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index b63787650f..8f77e69fd1 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -106,8 +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); -bool prearm_check(orb_advert_t *mavlink_log_pub, 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); +bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, + const battery_status_s &battery, const uint8_t arm_requirements, const hrt_abstime &time_since_boot); #endif /* STATE_MACHINE_HELPER_H_ */