diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bf7304c7fb..99d53ed700 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -251,8 +251,7 @@ static uint64_t rc_signal_lost_timestamp; // Time at which the RC reception was static float avionics_power_rail_voltage; // voltage of the avionics power rail -static bool arm_without_gps = false; -static bool arm_mission_required = false; +static uint8_t arm_requirements = ARM_REQ_NONE; static bool _last_condition_global_position_valid = false; @@ -468,10 +467,10 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "check")) { int checkres = 0; - checkres = preflight_check(&status, &mavlink_log_pub, false, true, &status_flags, &battery, true, false, hrt_elapsed_time(&commander_boot_timestamp)); + checkres = preflight_check(&status, &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 = preflight_check(&status, &mavlink_log_pub, true, true, &status_flags, &battery, arm_without_gps, arm_mission_required, hrt_elapsed_time(&commander_boot_timestamp)); + checkres = preflight_check(&status, &mavlink_log_pub, true, true, &status_flags, &battery, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED"); return 0; @@ -742,8 +741,7 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co mavlink_log_pub_local, &status_flags, avionics_power_rail_voltage, - arm_without_gps, - arm_mission_required, + arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); if (arming_res == TRANSITION_CHANGED) { @@ -1728,11 +1726,11 @@ int commander_thread_main(int argc, char *argv[]) int32_t arm_without_gps_param = 0; param_get(_param_arm_without_gps, &arm_without_gps_param); - arm_without_gps = (arm_without_gps_param == 1); + arm_requirements = arm_without_gps_param == 1 ? ARM_REQ_NONE : ARM_REQ_GPS_BIT; int32_t arm_mission_required_param = 0; param_get(_param_arm_mission_required, &arm_mission_required_param); - arm_mission_required = (arm_mission_required_param == 1); + arm_requirements |= (arm_mission_required_param & (ARM_REQ_MISSION_BIT | ARM_REQ_ARM_AUTH_BIT)); status.rc_input_mode = rc_in_off; if (status.hil_state == vehicle_status_s::HIL_STATE_ON) { @@ -1880,10 +1878,9 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_arm_switch_is_button, &arm_switch_is_button); param_get(_param_arm_without_gps, &arm_without_gps_param); - arm_without_gps = (arm_without_gps_param == 1); - + arm_requirements = arm_without_gps_param == 1 ? ARM_REQ_NONE : ARM_REQ_GPS_BIT; param_get(_param_arm_mission_required, &arm_mission_required_param); - arm_mission_required = (arm_mission_required_param == 1); + arm_requirements |= (arm_mission_required_param & (ARM_REQ_MISSION_BIT | ARM_REQ_ARM_AUTH_BIT)); /* Autostart id */ param_get(_param_autostart_id, &autostart_id); @@ -2009,7 +2006,7 @@ int commander_thread_main(int argc, char *argv[]) } else { /* check sensors also */ (void)Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, checkAirspeed, - (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !arm_without_gps, + (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), arm_requirements & ARM_REQ_GPS_BIT, /* checkDynamic */ true, is_vtol(&status), /* reportFailures */ hotplug_timeout, /* prearm */ false, hrt_elapsed_time(&commander_boot_timestamp)); } @@ -2125,8 +2122,7 @@ int commander_thread_main(int argc, char *argv[]) &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, - arm_without_gps, - arm_mission_required, + arm_requirements, hrt_elapsed_time(&commander_boot_timestamp))) { arming_state_changed = true; } @@ -2437,8 +2433,7 @@ int commander_thread_main(int argc, char *argv[]) &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, - arm_without_gps, - arm_mission_required, + arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); if (arming_ret == TRANSITION_CHANGED) { @@ -2734,8 +2729,7 @@ int commander_thread_main(int argc, char *argv[]) &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, - arm_without_gps, - arm_mission_required, + arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); if (arming_ret == TRANSITION_CHANGED) { @@ -2789,8 +2783,7 @@ int commander_thread_main(int argc, char *argv[]) &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, - arm_without_gps, - arm_mission_required, + arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); if (arming_ret == TRANSITION_CHANGED) { @@ -4234,8 +4227,7 @@ void *commander_low_prio_loop(void *arg) &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, - arm_without_gps, - arm_mission_required, + arm_requirements, hrt_elapsed_time(&commander_boot_timestamp))) { answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub); @@ -4330,7 +4322,7 @@ void *commander_low_prio_loop(void *arg) } status_flags.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, checkAirspeed, - !(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), !arm_without_gps, + !(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), arm_requirements & ARM_REQ_GPS_BIT, /* checkDynamic */ true, is_vtol(&status), /* reportFailures */ hotplug_timeout, /* prearm */ false, hrt_elapsed_time(&commander_boot_timestamp)); arming_state_transition(&status, @@ -4342,8 +4334,7 @@ void *commander_low_prio_loop(void *arg) &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, - arm_without_gps, - arm_mission_required, + arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); } else { diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 86c43f985d..46d5b4f45e 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -297,8 +297,7 @@ bool StateMachineHelperTest::armingStateTransitionTest() nullptr /* no mavlink_log_pub */, &status_flags, 5.0f, /* avionics rail voltage */ - check_gps, - false, /* valid mission not required*/ + (check_gps ? ARM_REQ_GPS_BIT : 0), 2e6 /* 2 seconds after boot, everything should be checked */ ); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 9a0f0b7ec7..276ae8e88a 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -124,8 +124,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log status_flags_s *status_flags, float avionics_power_rail_voltage, - bool arm_without_gps, - bool arm_mission_required, + uint8_t arm_requirements, hrt_abstime time_since_boot) { // Double check that our static arrays are still valid @@ -153,7 +152,8 @@ transition_result_t arming_state_transition(vehicle_status_s *status, && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { prearm_ret = preflight_check(status, mavlink_log_pub, true /* pre-arm */, false /* force_report */, - status_flags, battery, arm_without_gps, arm_mission_required, time_since_boot); + status_flags, battery, arm_requirements, + time_since_boot); } /* re-run the pre-flight check as long as sensors are failing */ @@ -164,7 +164,8 @@ transition_result_t arming_state_transition(vehicle_status_s *status, if (last_preflight_check == 0 || hrt_absolute_time() - last_preflight_check > 1000 * 1000) { prearm_ret = preflight_check(status, mavlink_log_pub, false /* pre-flight */, false /* force_report */, - status_flags, battery, arm_without_gps, arm_mission_required, time_since_boot); + status_flags, battery, arm_requirements, + time_since_boot); status_flags->condition_system_sensors_initialized = (prearm_ret == OK); last_preflight_check = hrt_absolute_time(); last_prearm_ret = prearm_ret; @@ -1029,7 +1030,7 @@ void reset_link_loss_globals(struct actuator_armed_s *armed, const bool old_fail } int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, - status_flags_s *status_flags, battery_status_s *battery, bool arm_without_gps, bool arm_mission_required, + status_flags_s *status_flags, battery_status_s *battery, uint8_t arm_requirements, hrt_abstime time_since_boot) { bool reportFailures = force_report || (!status_flags->condition_system_prearm_error_reported && @@ -1047,7 +1048,7 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p bool preflight_ok = Commander::preflightCheck(mavlink_log_pub, sensor_checks, sensor_checks, sensor_checks, sensor_checks, checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), - !arm_without_gps, true, status->is_vtol, reportFailures, prearm, time_since_boot); + arm_requirements & ARM_REQ_GPS_BIT, true, status->is_vtol, reportFailures, prearm, time_since_boot); if (!status_flags->circuit_breaker_engaged_usb_check && status_flags->usb_connected && prearm) { preflight_ok = false; @@ -1066,7 +1067,7 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p } // mission required - if (arm_mission_required && + if ((arm_requirements & ARM_REQ_MISSION_BIT) && (!status_flags->condition_auto_mission_available || !status_flags->condition_home_position_valid || !status_flags->condition_global_position_valid)) { diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 07bf46608a..1938bcddf7 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -67,6 +67,13 @@ enum class link_loss_actions_t { LOCKDOWN = 6, }; +typedef enum { + ARM_REQ_NONE = 0, + ARM_REQ_MISSION_BIT = (1 << 0), + ARM_REQ_ARM_AUTH_BIT = (1 << 1), + ARM_REQ_GPS_BIT = (1 << 2), +} arm_requirements_t; + // This is a struct used by the commander internally. struct status_flags_s { bool condition_calibration_enabled; @@ -118,8 +125,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log status_flags_s *status_flags, float avionics_power_rail_voltage, - bool arm_without_gps, - bool arm_mission_required, + uint8_t arm_requirements, hrt_abstime time_since_boot); transition_result_t @@ -164,6 +170,6 @@ void set_data_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *ar int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, status_flags_s *status_flags, battery_status_s *battery, - bool arm_without_gps, bool arm_mission_required, hrt_abstime time_since_boot); + uint8_t arm_requirements, hrt_abstime time_since_boot); #endif /* STATE_MACHINE_HELPER_H_ */