forked from Archive/PX4-Autopilot
HITL handling: Enforce the use of the activation parameter for HITL configuration
This commit is contained in:
parent
8aa1382e08
commit
5838556742
|
@ -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,
|
||||
|
|
|
@ -44,6 +44,7 @@
|
|||
#include <math.h>
|
||||
|
||||
#include <px4_posix.h>
|
||||
#include <px4_shutdown.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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");
|
||||
|
|
|
@ -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
|
||||
*
|
||||
|
|
Loading…
Reference in New Issue