diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 765c390908..bb09aff3e3 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -593,10 +593,6 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ transition_result_t main_ret = TRANSITION_NOT_CHANGED; - /* set HIL state */ - hil_state_t new_hil_state = (base_mode & VEHICLE_MODE_FLAG_HIL_ENABLED) ? vehicle_status_s::HIL_STATE_ON : vehicle_status_s::HIL_STATE_OFF; - transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, &mavlink_log_pub); - // We ignore base_mode & VEHICLE_MODE_FLAG_SAFETY_ARMED because // the command VEHICLE_CMD_COMPONENT_ARM_DISARM should be used // instead according to the latest mavlink spec. @@ -704,7 +700,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ } } - if ((hil_ret != TRANSITION_DENIED) && (arming_ret != TRANSITION_DENIED) && (main_ret != TRANSITION_DENIED)) { + if ((arming_ret != TRANSITION_DENIED) && (main_ret != TRANSITION_DENIED)) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3db75a9d78..098bee455e 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -368,65 +368,6 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai return ret; } -/** - * Transition from one hil state to another - */ -transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, - vehicle_status_s *current_status, orb_advert_t *mavlink_log_pub) -{ - transition_result_t ret = TRANSITION_DENIED; - - if (current_status->hil_state == new_state) { - ret = TRANSITION_NOT_CHANGED; - - } else { - switch (new_state) { - case vehicle_status_s::HIL_STATE_OFF: - /* The system is in HITL mode and unexpected things can happen if we disable HITL without rebooting. */ - mavlink_log_critical(mavlink_log_pub, "Set SYS_HITL to 0 and reboot to disable HITL."); - ret = TRANSITION_DENIED; - break; - - case vehicle_status_s::HIL_STATE_ON: - if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT - || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY - || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { - - ret = TRANSITION_CHANGED; - - int32_t hitl_on = 0; - param_get(param_find("SYS_HITL"), &hitl_on); - - if (hitl_on) { - mavlink_log_info(mavlink_log_pub, "Enabled Hardware-in-the-loop simulation (HITL)."); - - } else { - mavlink_log_critical(mavlink_log_pub, "Set SYS_HITL to 1 and reboot to enable HITL."); - ret = TRANSITION_DENIED; - } - - } else { - mavlink_log_critical(mavlink_log_pub, "Not switching to HITL when armed."); - ret = TRANSITION_DENIED; - } - - break; - - default: - PX4_WARN("Unknown HITL state"); - break; - } - } - - if (ret == TRANSITION_CHANGED) { - current_status->hil_state = new_state; - current_status->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - } - - return ret; -} - /** * Enable failsafe and report to user */ diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index bfb4b648f3..6a7839ce77 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -88,9 +88,6 @@ transition_result_t main_state_transition(const vehicle_status_s &status, const main_state_t new_main_state, const vehicle_status_flags_s &status_flags, commander_state_s *internal_state); -transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, - vehicle_status_s *current_status, orb_advert_t *mavlink_log_pub); - void enable_failsafe(vehicle_status_s *status, bool old_failsafe, orb_advert_t *mavlink_log_pub, const char *reason); bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_state_s *internal_state,