From e224441ac14b4ed384d77995b7dbdbc047cee6a5 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 6 Jun 2015 10:34:01 -0600 Subject: [PATCH 1/3] special treatment and warning message for HIL platform arming --- src/modules/commander/commander.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 64fd972ba4..c6f5eaa7b5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -437,6 +437,12 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char { transition_result_t arming_res = TRANSITION_NOT_CHANGED; + // For HIL platforms, require that simulated sensors are connected + if (autostart_id < 2000 && status.hil_state != vehicle_status_s::HIL_STATE_ON) { + mavlink_and_console_log_critical(mavlink_fd_local, "HIL platform: Connect to simulator before arming"); + return TRANSITION_DENIED; + } + // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. arming_res = arming_state_transition(&status, &safety, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed, From 71da3976abf593c7d515b8aa78b49e7f8553a51e Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 6 Jun 2015 15:03:03 -0600 Subject: [PATCH 2/3] add HIL autostart ID range macros and remove warnx --- src/modules/commander/commander.cpp | 27 +++++++++++++++------------ 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c6f5eaa7b5..8091a73ece 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -147,6 +147,9 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage #define INAIR_RESTART_HOLDOFF_INTERVAL 2000000 +#define HIL_ID_MIN 1000 +#define HIL_ID_MAX 1999 + enum MAV_MODE_FLAG { MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ MAV_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ @@ -438,7 +441,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char transition_result_t arming_res = TRANSITION_NOT_CHANGED; // For HIL platforms, require that simulated sensors are connected - if (autostart_id < 2000 && status.hil_state != vehicle_status_s::HIL_STATE_ON) { + if (autostart_id >= HIL_ID_MIN && autostart_id <= HIL_ID_MAX + && status.hil_state != vehicle_status_s::HIL_STATE_ON) { mavlink_and_console_log_critical(mavlink_fd_local, "HIL platform: Connect to simulator before arming"); return TRANSITION_DENIED; } @@ -1162,7 +1166,11 @@ int commander_thread_main(int argc, char *argv[]) // Run preflight check param_get(_param_autostart_id, &autostart_id); - if (autostart_id > 1999) { + if (autostart_id >= HIL_ID_MIN && autostart_id <= HIL_ID_MAX) { + // HIL configuration selected: real sensors will be disabled + status.condition_system_sensors_initialized = false; + set_tune_override(TONE_STARTUP_TUNE); //normal boot tune + } else { status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status.rc_input_mode, !status.circuit_breaker_engaged_gpsfailure_check); if (!status.condition_system_sensors_initialized) { set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune @@ -1170,11 +1178,6 @@ int commander_thread_main(int argc, char *argv[]) else { set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } - } else { - // HIL configuration selected: real sensors will be disabled - warnx("HIL configuration; autostart_id: %d, onboard IMU sensors disabled", autostart_id); - status.condition_system_sensors_initialized = false; - set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } commander_boot_timestamp = hrt_absolute_time(); @@ -1345,14 +1348,14 @@ int commander_thread_main(int argc, char *argv[]) } /* provide RC and sensor status feedback to the user */ - if (autostart_id > 1999) { - (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); - } else { + if (autostart_id >= HIL_ID_MIN && autostart_id <= HIL_ID_MAX) { /* HIL configuration: check only RC input */ - warnx("new telemetry link connected: checking RC status"); (void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false, !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false); + } else { + /* check sensors also */ + (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); } } From 7e48c66c221fc281b37dfb0db188440f89823ce6 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 10 Jun 2015 11:05:56 -0600 Subject: [PATCH 3/3] add is_hil_setup() --- src/modules/commander/commander.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8091a73ece..becf585452 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -256,6 +256,15 @@ void *commander_low_prio_loop(void *arg); void answer_command(struct vehicle_command_s &cmd, unsigned result); +/** + * check whether autostart ID is in the reserved range for HIL setups + */ +bool is_hil_setup(int id); + +bool is_hil_setup(int id) { + return (id >= HIL_ID_MIN) && (id <= HIL_ID_MAX); +} + int commander_main(int argc, char *argv[]) { @@ -441,8 +450,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char transition_result_t arming_res = TRANSITION_NOT_CHANGED; // For HIL platforms, require that simulated sensors are connected - if (autostart_id >= HIL_ID_MIN && autostart_id <= HIL_ID_MAX - && status.hil_state != vehicle_status_s::HIL_STATE_ON) { + if (is_hil_setup(autostart_id) && status.hil_state != vehicle_status_s::HIL_STATE_ON) { mavlink_and_console_log_critical(mavlink_fd_local, "HIL platform: Connect to simulator before arming"); return TRANSITION_DENIED; } @@ -1166,7 +1174,7 @@ int commander_thread_main(int argc, char *argv[]) // Run preflight check param_get(_param_autostart_id, &autostart_id); - if (autostart_id >= HIL_ID_MIN && autostart_id <= HIL_ID_MAX) { + if (is_hil_setup(autostart_id)) { // HIL configuration selected: real sensors will be disabled status.condition_system_sensors_initialized = false; set_tune_override(TONE_STARTUP_TUNE); //normal boot tune @@ -1348,7 +1356,7 @@ int commander_thread_main(int argc, char *argv[]) } /* provide RC and sensor status feedback to the user */ - if (autostart_id >= HIL_ID_MIN && autostart_id <= HIL_ID_MAX) { + if (is_hil_setup(autostart_id)) { /* HIL configuration: check only RC input */ (void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false, !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false);