src/drivers/px4io/px4io.cpp

@@ -335,7 +335,8 @@ private:
 		(ParamInt<px4::params::RC_RSSI_PWM_CHAN>) _param_rc_rssi_pwm_chan,
 		(ParamInt<px4::params::RC_RSSI_PWM_MAX>) _param_rc_rssi_pwm_max,
 		(ParamInt<px4::params::RC_RSSI_PWM_MIN>) _param_rc_rssi_pwm_min,
-		(ParamInt<px4::params::SENS_EN_THERMAL>) _param_sens_en_themal
+		(ParamInt<px4::params::SENS_EN_THERMAL>) _param_sens_en_themal,
+		(ParamInt<px4::params::SYS_HITL>) _param_sys_hitl
 	)
 };
 
@@ -473,7 +474,9 @@ int PX4IO::init()
 	}
 
 	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
-	_mixing_output.setMaxTopicUpdateRate(MIN_TOPIC_UPDATE_INTERVAL);
+	if (_param_sys_hitl.get() <= 0) {
+		_mixing_output.setMaxTopicUpdateRate(MIN_TOPIC_UPDATE_INTERVAL);
+	}
 
 	_px4io_status_pub.advertise();
 
@@ -522,7 +525,9 @@ void PX4IO::Run()
 	perf_count(_interval_perf);
 
 	/* if we have new control data from the ORB, handle it */
-	_mixing_output.update();
+	if (_param_sys_hitl.get() <= 0) {
+		_mixing_output.update();
+	}
 
 	if (hrt_elapsed_time(&_poll_last) >= 20_ms) {
 		/* run at 50 */
@@ -535,11 +540,13 @@ void PX4IO::Run()
 		io_publish_raw_rc();
 	}
 
-	/* check updates on uORB topics and handle it */
-	if (_t_actuator_armed.updated()) {
-		io_set_arming_state();
+	if (_param_sys_hitl.get() <= 0) {
+		/* check updates on uORB topics and handle it */
+		if (_t_actuator_armed.updated()) {
+			io_set_arming_state();
 
-		// TODO: throttle
+			// TODO: throttle
+		}
 	}
 
 	if (!_mixing_output.armed().armed) {
@@ -807,6 +814,14 @@ PX4IO::io_set_arming_state()
 			clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
 		}
 
+		// XXX this is for future support in the commander
+		// but can be removed if unneeded
+		// if (armed.termination_failsafe) {
+		// 	set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
+		// } else {
+		// 	clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
+		// }
+
 		if (armed.ready_to_arm) {
 			set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
 

src/modules/px4iofirmware/mixer.cpp

@@ -176,14 +176,6 @@ mixer_tick()
 		atomic_modify_clear(&r_status_flags, (PX4IO_P_STATUS_FLAGS_FAILSAFE));
 	}
 
-	const bool armed_output = should_arm || should_arm_nothrottle || (source == MIX_FAILSAFE);
-	const bool disarmed_output = (!armed_output && should_always_enable_pwm)
-				     || (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN);
-
-	if (disarmed_output) {
-		source = MIX_DISARMED;
-	}
-
 	/*
 	 * Run the mixers.
 	 */
@@ -227,7 +219,9 @@ mixer_tick()
 		isr_debug(5, "> PWM disabled");
 	}
 
-	if (mixer_servos_armed && (armed_output || disarmed_output)) {
+	if (mixer_servos_armed
+	    && (should_arm || should_arm_nothrottle || (source == MIX_FAILSAFE))
+	    && !(r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)) {
 		/* update the servo outputs. */
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
 			up_pwm_servo_set(i, r_page_servos[i]);
@@ -240,5 +234,22 @@ mixer_tick()
 		} else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
 			sbus1_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT);
 		}
+
+	} else if (mixer_servos_armed && (should_always_enable_pwm || (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN))) {
+		/* set the disarmed servo outputs. */
+		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
+			up_pwm_servo_set(i, r_page_servo_disarmed[i]);
+			/* copy values into reporting register */
+			r_page_servos[i] = r_page_servo_disarmed[i];
+		}
+
+		/* set S.BUS1 or S.BUS2 outputs */
+		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
+			sbus1_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT);
+		}
+
+		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
+			sbus2_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT);
+		}
 	}
 }