From 58385567424d6f57afce7c7d212ea48d5c120ac8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 29 Jul 2017 17:45:35 +0200 Subject: [PATCH] HITL handling: Enforce the use of the activation parameter for HITL configuration --- src/modules/commander/commander.cpp | 18 +++++------------- .../commander/state_machine_helper.cpp | 19 ++++++++++++++----- src/modules/sensors/parameters.cpp | 1 + src/modules/systemlib/system_params.c | 12 ++++++++++++ 4 files changed, 32 insertions(+), 18 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index dc625310d8..01313a4ad0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -165,6 +165,8 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage #define POSVEL_PROBATION_MAX 100E6 /**< maximum probation duration (usec) */ #define POSVEL_VALID_PROBATION_FACTOR 10 /**< the rate at which the probation duration is increased while checks are failing */ +static bool startup_in_hil = false; + /* * Probation times for position and velocity validity checks to pass if failed * Signed integers are used because these can become negative values before constraints are applied @@ -344,14 +346,6 @@ static int power_button_state_notification_cb(board_power_button_state_notificat return PWR_BUTTON_RESPONSE_SHUT_DOWN_PENDING; } -/** - * check whether autostart ID is in the reserved range for HIL setups - */ -static bool is_hil_setup(int id) { - return (id >= HIL_ID_MIN) && (id <= HIL_ID_MAX); -} - - int commander_main(int argc, char *argv[]) { if (argc < 2) { @@ -700,7 +694,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) { + startup_in_hil && 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; @@ -1288,8 +1282,6 @@ int commander_thread_main(int argc, char *argv[]) bool was_falling = false; bool was_armed = false; - bool startup_in_hil = false; - // XXX for now just set sensors as initialized status_flags.condition_system_sensors_initialized = true; @@ -1711,7 +1703,7 @@ int commander_thread_main(int argc, char *argv[]) arm_mission_required = (arm_mission_required_param == 1); status.rc_input_mode = rc_in_off; - if (is_hil_setup(autostart_id)) { + if (startup_in_hil) { // HIL configuration selected: real sensors will be disabled status_flags.condition_system_sensors_initialized = false; set_tune_override(TONE_STARTUP_TUNE); //normal boot tune @@ -1968,7 +1960,7 @@ int commander_thread_main(int argc, char *argv[]) telemetry_preflight_checks_reported[i] = hotplug_timeout; /* provide RC and sensor status feedback to the user */ - if (is_hil_setup(autostart_id)) { + if (startup_in_hil) { /* HIL configuration: check only RC input */ (void)Commander::preflightCheck(&mavlink_log_pub, false, false, false, false, false, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), false, diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6b5fab9f87..4d17306e92 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -44,6 +44,7 @@ #include #include +#include #include #include @@ -481,8 +482,8 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta } else { switch (new_state) { case vehicle_status_s::HIL_STATE_OFF: - /* we're in HIL and unexpected things can happen if we disable HIL now */ - mavlink_log_critical(mavlink_log_pub, "Not switching off HIL (safety)"); + /* we're in HIL and unexpected things can happen if we dis HITL. Sable HIL now */ + mavlink_log_critical(mavlink_log_pub, "Set SYS_HITL to 0 and reboot to disable HITL."); ret = TRANSITION_DENIED; break; @@ -492,17 +493,25 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { ret = TRANSITION_CHANGED; - mavlink_log_critical(mavlink_log_pub, "Switched to ON hil state"); + + 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."); + } else { + mavlink_log_critical(mavlink_log_pub, "Set parameter SYS_HITL to 1 before starting HITL."); + } } else { - mavlink_log_critical(mavlink_log_pub, "Not switching to HIL when armed"); + mavlink_log_critical(mavlink_log_pub, "Not switching to HITL when armed."); ret = TRANSITION_DENIED; } break; default: - warnx("Unknown HIL state"); + PX4_WARN("Unknown HIL state"); break; } } diff --git a/src/modules/sensors/parameters.cpp b/src/modules/sensors/parameters.cpp index 17155b4bd6..65ff620a41 100644 --- a/src/modules/sensors/parameters.cpp +++ b/src/modules/sensors/parameters.cpp @@ -225,6 +225,7 @@ int initialize_parameter_handles(ParameterHandles ¶meter_handles) (void)param_find("SYS_PARAM_VER"); (void)param_find("SYS_AUTOSTART"); (void)param_find("SYS_AUTOCONFIG"); + (void)param_find("SYS_HITL"); (void)param_find("PWM_RATE"); (void)param_find("PWM_MIN"); (void)param_find("PWM_MAX"); diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index dd8e9c1647..d65fcc5113 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -64,6 +64,18 @@ PARAM_DEFINE_INT32(SYS_AUTOSTART, 0); */ PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0); +/** + * Enable HITL mode on next boot + * + * Set to 1 to enable HITL on next boot. The system will automatically + * reset it on reboot. + * + * @value 0 HITL off + * @value 1 HITL on + * @group System + */ +PARAM_DEFINE_INT32(SYS_HITL, 0); + /** * Set usage of IO board *