mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-01 19:07:45 +08:00
commander: remove unused HITL transition function
To my knowledge this hil transition function is not used anymore, however, it makes sending the DO_SET_MODE command unnecessarily complex. In my opinion the DO_SET_MODE command should only change the mode but not other things like arming (already removed) and HITL state (this commit). Often times, I was seeing the error message "Set SYS_HITL to 1 and reboot to enable HITL." when using QGC with a vehicle in HITL. HITL is set via parameter which then has an impact in the startup script where the CLI argument `-hil` is added to some of the commands that require it (like commander as well).
This commit is contained in:
@@ -593,10 +593,6 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
|
|||||||
|
|
||||||
transition_result_t main_ret = TRANSITION_NOT_CHANGED;
|
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
|
// We ignore base_mode & VEHICLE_MODE_FLAG_SAFETY_ARMED because
|
||||||
// the command VEHICLE_CMD_COMPONENT_ARM_DISARM should be used
|
// the command VEHICLE_CMD_COMPONENT_ARM_DISARM should be used
|
||||||
// instead according to the latest mavlink spec.
|
// 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;
|
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|||||||
@@ -368,65 +368,6 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai
|
|||||||
return ret;
|
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
|
* Enable failsafe and report to user
|
||||||
*/
|
*/
|
||||||
|
|||||||
@@ -88,9 +88,6 @@ transition_result_t
|
|||||||
main_state_transition(const vehicle_status_s &status, const main_state_t new_main_state,
|
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);
|
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);
|
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,
|
bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_state_s *internal_state,
|
||||||
|
|||||||
Reference in New Issue
Block a user