diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 5e2733d9a2..16748b0aad 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -200,7 +200,7 @@ void print_status(); bool shutdown_if_allowed(); -transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub, const char *armedBy); +transition_result_t arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink_log_pub, const char *armedBy); /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. @@ -383,7 +383,13 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "arm")) { - if (TRANSITION_CHANGED != arm_disarm(true, &mavlink_log_pub, "command line")) { + bool force_arming = false; + + if (argc > 2 && !strcmp(argv[2], "-f")) { + force_arming = true; + } + + if (TRANSITION_CHANGED != arm_disarm(true, !force_arming, &mavlink_log_pub, "command line")) { PX4_ERR("arming failed"); } @@ -391,7 +397,7 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "disarm")) { - if (TRANSITION_DENIED == arm_disarm(false, &mavlink_log_pub, "command line")) { + if (TRANSITION_DENIED == arm_disarm(false, true, &mavlink_log_pub, "command line")) { PX4_ERR("rejected disarm"); } @@ -405,7 +411,7 @@ int commander_main(int argc, char *argv[]) /* see if we got a home position */ if (status_flags.condition_local_position_valid) { - if (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "command line")) { + if (TRANSITION_DENIED != arm_disarm(true, true, &mavlink_log_pub, "command line")) { ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF); } else { @@ -516,7 +522,7 @@ void usage(const char *reason) PX4_INFO("%s", reason); } - PX4_INFO("usage: commander {start|stop|status|calibrate|check|arm|disarm|takeoff|land|transition|mode}\n"); + PX4_INFO("usage: commander {start|stop|status|calibrate|check|arm [-f]|disarm|takeoff|land|transition|mode}\n"); } void print_status() @@ -531,7 +537,8 @@ bool shutdown_if_allowed() hrt_elapsed_time(&commander_boot_timestamp)); } -transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, const char *armedBy) +transition_result_t arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink_log_pub_local, + const char *armedBy) { transition_result_t arming_res = TRANSITION_NOT_CHANGED; @@ -541,7 +548,7 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co safety, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed, - true /* fRunPreArmChecks */, + run_preflight_checks, mavlink_log_pub_local, &status_flags, arm_requirements, @@ -818,7 +825,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ } } - transition_result_t arming_res = arm_disarm(cmd_arms, &mavlink_log_pub, "Arm/Disarm component command"); + transition_result_t arming_res = arm_disarm(cmd_arms, true, &mavlink_log_pub, "Arm/Disarm component command"); if (arming_res == TRANSITION_DENIED) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; @@ -1022,7 +1029,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ // switch to AUTO_MISSION and ARM if ((TRANSITION_DENIED != main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags, &internal_state)) - && (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "Mission start command"))) { + && (TRANSITION_DENIED != arm_disarm(true, true, &mavlink_log_pub, "Mission start command"))) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; @@ -1683,7 +1690,7 @@ Commander::run() } if (_auto_disarm_landed.get_state()) { - arm_disarm(false, &mavlink_log_pub, "Auto disarm initiated"); + arm_disarm(false, true, &mavlink_log_pub, "Auto disarm initiated"); } } @@ -1691,7 +1698,7 @@ Commander::run() _auto_disarm_killed.set_state_and_update(armed.manual_lockdown, hrt_absolute_time()); if (_auto_disarm_killed.get_state()) { - arm_disarm(false, &mavlink_log_pub, "Kill-switch still engaged, disarming"); + arm_disarm(false, true, &mavlink_log_pub, "Kill-switch still engaged, disarming"); } } else {