ST24 integration in IO firmware

4 files changed, 54 insertions, 32 deletions

diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 3aa3d28ff..1b7be692a 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -52,7 +52,7 @@
 #define RC_CHANNEL_LOW_THRESH		-8000	/* 10% threshold */
 
 static bool	ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
-static bool	dsm_port_input(void);
+static bool	dsm_port_input(uint8_t *rssi);
 
 static perf_counter_t c_gather_dsm;
 static perf_counter_t c_gather_sbus;
@@ -60,9 +60,49 @@ static perf_counter_t c_gather_ppm;
 
 static int _dsm_fd;
 
-bool dsm_port_input()
+bool dsm_port_input(uint8_t *rssi)
 {
+	perf_begin(c_gather_dsm);
+	uint16_t temp_count = r_raw_rc_count;
+	uint8_t n_bytes = 0;
+	uint8_t *bytes;
+	bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes);
+	if (dsm_updated) {
+		r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
+		r_raw_rc_count = temp_count & 0x7fff;
+		if (temp_count & 0x8000)
+			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
+		else
+			r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
+
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+
+	}
+	perf_end(c_gather_dsm);
+
 	/* get data from FD and attempt to parse with DSM and ST24 libs */
+	uint8_t st24_rssi, rx_count;
+	uint16_t st24_channel_count;
+	uint8_t st24_maxchans = 18;
+
+	bool st24_updated = false;
+
+	for (unsigned i = 0; i < n_bytes; i++) {
+		/* set updated flag if one complete packet was parsed */
+		st24_updated |= st24_decode(bytes[i], &st24_rssi, &rx_count,
+					&st24_channel_count, r_raw_rc_values, st24_maxchans);
+	}
+
+	if (st24_updated) {
+
+		*rssi = st24_rssi;
+		r_raw_rc_count = st24_channel_count;
+
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+	}
 
 	return false;
 }
@@ -127,20 +167,7 @@ controls_tick() {
 #endif
 
 	perf_begin(c_gather_dsm);
-	uint16_t temp_count = r_raw_rc_count;
-	bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
-	if (dsm_updated) {
-		r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
-		r_raw_rc_count = temp_count & 0x7fff;
-		if (temp_count & 0x8000)
-			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
-		else
-			r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
-
-		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
-		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
-
-	}
+	(void)dsm_port_input(&rssi);
 	perf_end(c_gather_dsm);
 
 	perf_begin(c_gather_sbus);
@@ -186,18 +213,6 @@ controls_tick() {
 	}
 	perf_end(c_gather_ppm);
 
-	uint8_t st24_rssi, rx_count;
-	uint8_t st24_maxchans = 18;
-	bool st24_updated = st24_decode(0, &st24_rssi, &rx_count, r_raw_rc_values, st24_maxchans);
-	if (st24_updated) {
-
-		rssi = st24_rssi;
-
-		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
-		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
-		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
-	}
-
 	/* limit number of channels to allowable data size */
 	if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
 		r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
@@ -402,7 +417,7 @@ controls_tick() {
 			r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
 
 			/* mix new RC input control values to servos */
-			if (dsm_updated || sbus_updated || ppm_updated)
+			if (dsm_updated || sbus_updated || ppm_updated || st24_updated)
 				mixer_tick();
 
 		} else {
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
index d3f365822..6e57c9ec7 100644
--- a/src/modules/px4iofirmware/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
@@ -452,10 +452,12 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
  *
  * @param[out] values pointer to per channel array of decoded values
  * @param[out] num_values pointer to number of raw channel values returned, high order bit 0:10 bit data, 1:11 bit data
+ * @param[out] n_butes number of bytes read
+ * @param[out] bytes pointer to the buffer of read bytes
  * @return true=decoded raw channel values updated, false=no update
  */
 bool
-dsm_input(uint16_t *values, uint16_t *num_values)
+dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes)
 {
 	ssize_t		ret;
 	hrt_abstime	now;
@@ -478,8 +480,12 @@ dsm_input(uint16_t *values, uint16_t *num_values)
 	ret = read(dsm_fd, &dsm_frame[dsm_partial_frame_count], DSM_FRAME_SIZE - dsm_partial_frame_count);
 
 	/* if the read failed for any reason, just give up here */
-	if (ret < 1)
+	if (ret < 1) {
 		return false;
+	} else {
+		*n_bytes = ret;
+		*bytes = &dsm_frame[dsm_partial_frame_count];
+	}
 
 	dsm_last_rx_time = now;
 
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 4739f6e40..e907d2959 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -112,6 +112,7 @@
 #define PX4IO_P_STATUS_FLAGS_FAILSAFE		(1 << 11) /* failsafe is active */
 #define PX4IO_P_STATUS_FLAGS_SAFETY_OFF		(1 << 12) /* safety is off */
 #define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED	(1 << 13) /* FMU was initialized and OK once */
+#define PX4IO_P_STATUS_FLAGS_RC_ST24		(1 << 14) /* SBUS input is valid */
 
 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
 #define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* [1] VBatt is very close to regulator dropout */
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index b00a96717..e32fcc72b 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -215,7 +215,7 @@ extern uint16_t	adc_measure(unsigned channel);
 extern void	controls_init(void);
 extern void	controls_tick(void);
 extern int	dsm_init(const char *device);
-extern bool	dsm_input(uint16_t *values, uint16_t *num_values);
+extern bool	dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes);
 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);