diff --git a/nuttx-configs/px4io-v2/include/board.h b/nuttx-configs/px4io-v2/include/board.h
index 4b9c906385..17e77918b4 100755
--- a/nuttx-configs/px4io-v2/include/board.h
+++ b/nuttx-configs/px4io-v2/include/board.h
@@ -103,8 +103,6 @@
 #define GPIO_USART2_RTS	0xffffffff
 #undef GPIO_USART2_CK
 #define GPIO_USART2_CK 	0xffffffff
-#undef GPIO_USART3_TX
-#define GPIO_USART3_TX 	0xffffffff
 #undef GPIO_USART3_CK
 #define GPIO_USART3_CK 	0xffffffff
 #undef GPIO_USART3_CTS
diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig
index 4111e70eba..e6563e5878 100755
--- a/nuttx-configs/px4io-v2/nsh/defconfig
+++ b/nuttx-configs/px4io-v2/nsh/defconfig
@@ -104,9 +104,9 @@ CONFIG_ARMV7M_CMNVECTOR=y
 #   CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled
 #
 CONFIG_STM32_DFU=n
-CONFIG_STM32_JTAG_FULL_ENABLE=y
+CONFIG_STM32_JTAG_FULL_ENABLE=n
 CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n
-CONFIG_STM32_JTAG_SW_ENABLE=n
+CONFIG_STM32_JTAG_SW_ENABLE=y
 
 #
 # Individual subsystems can be enabled:
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 9558198f35..ebf4f3e8e9 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -254,10 +254,25 @@ mixer_tick(void)
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
 			up_pwm_servo_set(i, r_page_servos[i]);
 
+		/* set S.BUS1 or S.BUS2 outputs */
+
+		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
+			sbus2_output(r_page_servos, PX4IO_SERVO_COUNT);
+		} else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
+			sbus1_output(r_page_servos, PX4IO_SERVO_COUNT);
+		}
+
 	} else if (mixer_servos_armed && should_always_enable_pwm) {
 		/* set the disarmed servo outputs. */
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
 			up_pwm_servo_set(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(r_page_servos, PX4IO_SERVO_COUNT);
+
+		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT)
+			sbus2_output(r_page_servos, PX4IO_SERVO_COUNT);
 	}
 }
 
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 4db9484848..ca175bfbc3 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -219,6 +219,8 @@ extern bool	dsm_input(uint16_t *values, uint16_t *num_values);
 extern void	dsm_bind(uint16_t cmd, int pulses);
 extern int	sbus_init(const char *device);
 extern bool	sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
+extern bool	sbus1_output(uint16_t *values, uint16_t num_values);
+extern bool	sbus2_output(uint16_t *values, uint16_t num_values);
 
 /** global debug level for isr_debug() */
 extern volatile uint8_t debug_level;
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 9e5d7e7e20..fd7c6081f6 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -463,9 +463,18 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			#ifdef ENABLE_SBUS_OUT
 			ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT));
 
-			/* disable the conflicting options */
-			if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) {
-				value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | PX4IO_P_SETUP_FEATURES_ADC_RSSI);
+			/* disable the conflicting options with SBUS 1 */
+			if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT)) {
+				value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
+					PX4IO_P_SETUP_FEATURES_ADC_RSSI |
+					PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
+			}
+
+			/* disable the conflicting options with SBUS 2 */
+			if (value & (PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) {
+				value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
+					PX4IO_P_SETUP_FEATURES_ADC_RSSI |
+					PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
 			}
 			#endif
 
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index f6ec542eb0..0e7dc621cf 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -93,7 +93,7 @@ int
 sbus_init(const char *device)
 {
 	if (sbus_fd < 0)
-		sbus_fd = open(device, O_RDONLY | O_NONBLOCK);
+		sbus_fd = open(device, O_RDWR | O_NONBLOCK);
 
 	if (sbus_fd >= 0) {
 		struct termios t;
@@ -113,10 +113,21 @@ sbus_init(const char *device)
 	} else {
 		debug("S.Bus: open failed");
 	}
-
 	return sbus_fd;
 }
 
+bool
+sbus1_output(uint16_t *values, uint16_t num_values)
+{
+	write(sbus_fd, 'A', 1);
+}
+
+bool
+sbus2_output(uint16_t *values, uint16_t num_values)
+{
+	write(sbus_fd, 'B', 1);
+}
+
 bool
 sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels)
 {