From 4e126c061c1f47fe0960b6f7902b38ffed41d371 Mon Sep 17 00:00:00 2001
From: Nik Langrind <langrind@gmail.com>
Date: Fri, 15 Nov 2019 20:01:40 -0500
Subject: [PATCH] px4io: When running HITL, don't publish actuator_outputs.
 Fixes #13471.

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 <langrind@gmail.com>
---
 ROMFS/px4fmu_common/init.d/rc.io | 10 +++++++++-
 src/drivers/px4io/px4io.cpp      | 33 +++++++++++++++++++++++---------
 2 files changed, 33 insertions(+), 10 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io
index 2ee6f8a05035..2f0aadb96f45 100644
--- a/ROMFS/px4fmu_common/init.d/rc.io
+++ b/ROMFS/px4fmu_common/init.d/rc.io
@@ -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
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 4f7592709de6..182a1620d6b2 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -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.
@@ -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
@@ -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;
@@ -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();
 }
 
@@ -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);
@@ -2328,6 +2336,10 @@ PX4IO::print_status(bool extended_status)
 		}
 	}
 
+	if (_hitl_mode) {
+		printf("\nHITL Mode");
+	}
+
 	printf("\n");
 }
 
@@ -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");