diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 58787d0a64..aa2b80e537 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -633,6 +633,7 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co // For HIL platforms, require that simulated sensors are connected if (arm && hrt_absolute_time() > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL && is_hil_setup(autostart_id) && status.hil_state != vehicle_status_s::HIL_STATE_ON) { + mavlink_log_critical(mavlink_log_pub_local, "HIL platform: Connect to simulator before arming"); return TRANSITION_DENIED; } @@ -651,7 +652,7 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co can_arm_without_gps); if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_log_pub_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); + mavlink_log_info(mavlink_log_pub_local, "%s by %s", arm ? "ARMED" : "DISARMED", armedBy); } else if (arming_res == TRANSITION_DENIED) { tune_negative(true); @@ -848,16 +849,19 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } // Refuse to arm if in manual with non-zero throttle - if ((status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL || status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_STAB || - status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO) && sp_man.z > 0.1f) { + if (cmd_arms + && (status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL + || status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_STAB + || status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO) + && (sp_man.z > 0.1f)) { + mavlink_log_critical(&mavlink_log_pub, "Arming DENIED. Manual throttle non-zero."); cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; break; } - } - 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, &mavlink_log_pub, "arm/disarm component command"); if (arming_res == TRANSITION_DENIED) { mavlink_log_critical(&mavlink_log_pub, "REJECTING component arm cmd");