Fixed drivers

src/drivers/boards/px4fmu-v4/board_config.h

@@ -214,6 +214,8 @@ __BEGIN_DECLS
 #define HRT_PPM_CHANNEL		3	/* use capture/compare channel 2 */
 #define GPIO_PPM_IN			(GPIO_ALT|GPIO_AF2|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0)
 
+#define SBUS_SERIAL_PORT	"/dev/ttyS4"	/* XXX not vetted */
+
 /* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */
 #define PWMIN_TIMER			4
 #define PWMIN_TIMER_CHANNEL	2

src/drivers/px4fmu/fmu.cpp

@@ -71,6 +71,11 @@
 #include <drivers/drv_mixer.h>
 #include <drivers/drv_rc_input.h>
 
+#include <lib/rc/sbus.h>
+#include <lib/rc/dsm.h>
+#include <lib/rc/st24.h>
+#include <lib/rc/sumd.h>
+
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_controls_0.h>
 #include <uORB/topics/actuator_controls_1.h>
@@ -799,6 +804,45 @@ PX4FMU::cycle()
 		update_pwm_rev_mask();
 	}
 
+	bool rc_updated = false;
+
+#ifdef SBUS_SERIAL_PORT
+	bool sbus_failsafe, sbus_frame_drop;
+	uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+	uint16_t raw_rc_count;
+	bool sbus_updated = sbus_input(_sbus_fd, &raw_rc_values[0], &raw_rc_count, &sbus_failsafe, &sbus_frame_drop,
+				       input_rc_s::RC_INPUT_MAX_CHANNELS);
+
+	if (sbus_updated) {
+		// we have a new PPM frame. Publish it.
+		_rc_in.channel_count = raw_rc_count;
+
+		if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
+			_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
+		}
+
+		for (uint8_t i = 0; i < _rc_in.channel_count; i++) {
+			_rc_in.values[i] = raw_rc_values[i];
+		}
+
+		_rc_in.timestamp_publication = hrt_absolute_time();
+		_rc_in.timestamp_last_signal = _rc_in.timestamp_publication;
+
+		_rc_in.rc_ppm_frame_length = 0;
+		_rc_in.rssi = (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : 0;
+		_rc_in.rc_failsafe = false;
+		_rc_in.rc_lost = false;
+		_rc_in.rc_lost_frame_count = 0;
+		_rc_in.rc_total_frame_count = 0;
+
+		rc_updated = true;
+	}
+#endif
+
+#ifdef DSM_SERIAL_PORT
+	//_dsm_fd = dsm_init(DSM_SERIAL_PORT);
+#endif
+
 #ifdef HRT_PPM_CHANNEL
 
 	// see if we have new PPM input data
@@ -824,6 +868,12 @@ PX4FMU::cycle()
 		_rc_in.rc_lost_frame_count = 0;
 		_rc_in.rc_total_frame_count = 0;
 
+		rc_updated = true;
+	}
+
+#endif
+
+	if (rc_updated) {
 		/* lazily advertise on first publication */
 		if (_to_input_rc == nullptr) {
 			_to_input_rc = orb_advertise(ORB_ID(input_rc), &_rc_in);
@@ -833,7 +883,6 @@ PX4FMU::cycle()
 		}
 	}
 
-#endif
 	work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, USEC2TICK(CONTROL_INPUT_DROP_LIMIT_MS * 1000));
 }
 

src/modules/px4iofirmware/controls.c

@@ -77,7 +77,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
 	uint16_t temp_count = r_raw_rc_count;
 	uint8_t n_bytes = 0;
 	uint8_t *bytes;
-	*dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes);
+	*dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes, PX4IO_RC_INPUT_CHANNELS);
 
 	if (*dsm_updated) {
 		r_raw_rc_count = temp_count & 0x7fff;