Skip to content

Commit

Permalink
px4io: When running HITL, don't publish actuator_outputs. Fixes #13471.
Browse files Browse the repository at this point in the history
When running in HITL mode, pwm_out_sim publishes actuator_outputs.

px4io unconditionally publishes the uOrb actuator_outputs. When HITL
is configured, the px4io copy of the uOrb has all zeros.

The result is that there are two publications, one valid, and one
all-zeros. This causes the HIL_ACTUATOR_CONTROLS mavlink message
to be incorrect (all-zeros) and the SERVO_OUTPUTS_RAW mavlink
message to be inconsistent, as it takes the data from one or the
other uOrb randomly each cycle.

The fix is to let px4io know that HITL is in effect when it is
started, and modify px4io to suppress publication in this case.

This is a bigger more complicated fix than I would like, but I
think it is conceptually correct.

Signed-off-by: Nik Langrind <[email protected]>
  • Loading branch information
langrind authored and bkueng committed Nov 20, 2019
1 parent 1b313c6 commit 4e126c0
Show file tree
Hide file tree
Showing 2 changed files with 33 additions and 10 deletions.
10 changes: 9 additions & 1 deletion ROMFS/px4fmu_common/init.d/rc.io
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,17 @@
#
# PX4IO interface init script.
#

# If $OUTPUT_MODE indicated Hardware-int-the-loop simulation, px4io should not publish actuator_outputs,
# instead, pwm_out_sim will publish that uORB
if [ $OUTPUT_MODE = hil ]
then
set HIL_ARG $OUTPUT_MODE
fi

if [ $USE_IO = yes -a $IO_PRESENT = yes ]
then
if px4io start
if px4io start $HIL_ARG
then
# Allow PX4IO to recover from midair restarts.
px4io recovery
Expand Down
33 changes: 24 additions & 9 deletions src/drivers/px4io/px4io.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -148,8 +148,9 @@ class PX4IO : public cdev::CDev
* Retrieve relevant initial system parameters. Initialize PX4IO registers.
*
* @param disable_rc_handling set to true to forbid override / RC handling on IO
* @param hitl_mode set to suppress publication of actuator_outputs - instead defer to pwm_out_sim
*/
int init(bool disable_rc_handling);
int init(bool disable_rc_handling, bool hitl_mode);

/**
* Detect if a PX4IO is connected.
Expand Down Expand Up @@ -286,7 +287,8 @@ class PX4IO : public cdev::CDev
bool _analog_rc_rssi_stable; ///< true when analog RSSI input is stable
float _analog_rc_rssi_volt; ///< analog RSSI voltage

bool _test_fmu_fail; ///< To test what happens if IO looses FMU
bool _test_fmu_fail; ///< To test what happens if IO loses FMU
bool _hitl_mode; ///< Hardware-in-the-loop simulation mode - don't publish actuator_outputs

/**
* Trampoline to the worker task
Expand Down Expand Up @@ -489,7 +491,8 @@ PX4IO::PX4IO(device::Device *interface) :
_thermal_control(-1),
_analog_rc_rssi_stable(false),
_analog_rc_rssi_volt(-1.0f),
_test_fmu_fail(false)
_test_fmu_fail(false),
_hitl_mode(false)
{
/* we need this potentially before it could be set in task_main */
g_dev = this;
Expand Down Expand Up @@ -559,9 +562,10 @@ PX4IO::detect()
}

int
PX4IO::init(bool rc_handling_disabled)
PX4IO::init(bool rc_handling_disabled, bool hitl_mode)
{
_rc_handling_disabled = rc_handling_disabled;
_hitl_mode = hitl_mode;
return init();
}

Expand Down Expand Up @@ -1837,6 +1841,10 @@ PX4IO::io_publish_raw_rc()
int
PX4IO::io_publish_pwm_outputs()
{
if (_hitl_mode) {
return OK;
}

/* get servo values from IO */
uint16_t ctl[_max_actuators];
int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators);
Expand Down Expand Up @@ -2328,6 +2336,10 @@ PX4IO::print_status(bool extended_status)
}
}

if (_hitl_mode) {
printf("\nHITL Mode");
}

printf("\n");
}

Expand Down Expand Up @@ -2891,19 +2903,22 @@ start(int argc, char *argv[])
}

bool rc_handling_disabled = false;
bool hitl_mode = false;

/* disable RC handling on request */
if (argc > 1) {
if (!strcmp(argv[1], "norc")) {

/* disable RC handling and/or actuator_output publication on request */
for (int extra_args = 1; extra_args < argc; extra_args++) {
if (!strcmp(argv[extra_args], "norc")) {
rc_handling_disabled = true;

} else if (!strcmp(argv[extra_args], "hil")) {
hitl_mode = true;

} else {
warnx("unknown argument: %s", argv[1]);
}
}

if (OK != g_dev->init(rc_handling_disabled)) {
if (OK != g_dev->init(rc_handling_disabled, hitl_mode)) {
delete g_dev;
g_dev = nullptr;
errx(1, "driver init failed");
Expand Down

0 comments on commit 4e126c0

Please sign in to comment.