Fix the DSM (spektrum) protocol decoder, and add some format auto-detection to it.

1 files changed, 134 insertions, 20 deletions

diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c
index 79b6301c7..4384ab523 100644
--- a/apps/px4io/dsm.c
+++ b/apps/px4io/dsm.c
@@ -49,12 +49,15 @@
  
 #include <drivers/drv_hrt.h>
 
+#define DEBUG
+
 #include "px4io.h"
 #include "protocol.h"
 
 #define DSM_FRAME_SIZE		16
 #define DSM_FRAME_CHANNELS	7
 
+static hrt_abstime last_rx_time;
 static hrt_abstime last_frame_time;
 
 static uint8_t	frame[DSM_FRAME_SIZE];
@@ -63,21 +66,21 @@ static unsigned partial_frame_count;
 static bool	insync;
 static unsigned	channel_shift;
 
-static void dsm_decode(void);
+static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value);
+static void dsm_guess_format(bool reset);
+static void dsm_decode(hrt_abstime now);
 
 void
 dsm_init(unsigned mode)
 {
 	insync = false;
 	partial_frame_count = 0;
+	last_rx_time = hrt_absolute_time();
 
-	if (mode == RX_MODE_DSM_10BIT) {
-		channel_shift = 10;
-	} else {
-		channel_shift = 11;
-	}
+	/* reset the format detector */
+	dsm_guess_format(true);
 
-	last_frame_time = hrt_absolute_time();
+	debug("DSM: enabled and waiting\n");
 }
 
 void
@@ -97,10 +100,17 @@ dsm_input(int fd)
 	 * We expect to only be called when bytes arrive for processing,
 	 * and if an interval of more than 5ms passes between calls, 
 	 * the first byte we read will be the first byte of a frame.
+	 *
+	 * In the case where byte(s) are dropped from a frame, this also
+	 * provides a degree of protection. Of course, it would be better
+	 * if we didn't drop bytes...
 	 */
 	now = hrt_absolute_time();
-	if ((now - last_frame_time) > 5000)
+	if ((now - last_rx_time) > 5000) {
+		if (partial_frame_count > 0)
+			debug("DSM: reset @ %d", partial_frame_count);
 		partial_frame_count = 0;
+	}
 
 	/*
 	 * Fetch bytes, but no more than we would need to complete
@@ -111,6 +121,7 @@ dsm_input(int fd)
 	/* if the read failed for any reason, just give up here */
 	if (ret < 1)
 		return;
+	last_rx_time = now;
 
 	/*
 	 * Add bytes to the current frame
@@ -123,20 +134,125 @@ dsm_input(int fd)
 	 */
 	if (partial_frame_count < DSM_FRAME_SIZE)
 	 	return;
-	last_frame_time = now;
 
 	/*
 	 * Great, it looks like we might have a frame.  Go ahead and
 	 * decode it.
 	 */
-	dsm_decode();
+	dsm_decode(now);
 	partial_frame_count = 0;
 }
 
+static bool
+dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value)
+{
+
+	if (raw == 0xffff)
+		return false;
+
+	*channel = (raw >> shift) & 0xf;
+
+	uint16_t data_mask = (1 << shift) - 1;
+	*value = raw & data_mask;
+
+	//debug("DSM: %d 0x%04x -> %d %d", shift, raw, *channel, *value);
+
+	return true;
+}
+
 static void
-dsm_decode(void)
+dsm_guess_format(bool reset)
 {
-	uint16_t	data_mask = (1 << channel_shift) - 1;
+	static uint32_t	cs10;
+	static uint32_t	cs11;
+	static unsigned samples;
+
+	/* reset the 10/11 bit sniffed channel masks */
+	if (reset) {
+		cs10 = 0;
+		cs11 = 0;
+		samples = 0;
+		channel_shift = 0;
+		return;
+	}
+
+	/* scan the channels in the current frame in both 10- and 11-bit mode */
+	for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
+
+		uint8_t *dp = &frame[2 + (2 * i)];
+		uint16_t raw = (dp[0] << 8) | dp[1];
+		unsigned channel, value;
+
+		/* if the channel decodes, remember the assigned number */
+		if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31))
+			cs10 |= (1 << channel);
+		if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31))
+			cs11 |= (1 << channel);
+
+		/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-frame format */
+	}
+
+	/* wait until we have seen plenty of frames - 2 should normally be enough */
+	if (samples++ < 5)
+		return;
+
+	/* 
+	 * Iterate the set of sensible sniffed channel sets and see whether
+	 * decoding in 10 or 11-bit mode has yielded anything we recognise.
+	 */
+	static uint32_t masks[] = { 
+		0x3f,	/* 6 channels (DX6) */
+		0x7f,	/* 7 channels (DX7) */
+		0xff,	/* 8 channels (DX8) */
+		0x3ff,	/* 10 channels (DX10) */
+		0x3fff	/* 18 channels (DX10) */
+	};
+	unsigned votes10 = 0;
+	unsigned votes11 = 0;
+
+	for (unsigned i = 0; i < (sizeof(masks) / sizeof(masks[0])); i++) {
+
+		if (cs10 == masks[i])
+			votes10++;
+		if (cs11 == masks[i])
+			votes11++;
+	}
+	if ((votes11 == 1) && (votes10 == 0)) {
+		channel_shift = 11;
+		debug("DSM: detected 11-bit format");
+		return;
+	}
+	if ((votes10 == 1) && (votes11 == 0)) {
+		channel_shift = 10;
+		debug("DSM: detected 10-bit format");
+		return;
+	}
+
+	/* call ourselves to reset our state ... we have to try again */
+	debug("DSM: format detector failed, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11);
+	dsm_guess_format(true);
+}
+
+static void
+dsm_decode(hrt_abstime frame_time)
+{
+
+/*
+	debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
+	      frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7],
+	      frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]);
+*/
+	/*
+	 * If we have lost signal for at least a second, reset the 
+	 * format guessing heuristic.
+	 */
+	if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0))
+		dsm_guess_format(true);
+	last_frame_time = frame_time;
+	if (channel_shift == 0) {
+		dsm_guess_format(false);
+		return;
+	}
 
 	/*
 	 * The encoding of the first byte is uncertain, so we're going
@@ -159,26 +275,24 @@ dsm_decode(void)
 
 		uint8_t *dp = &frame[2 + (2 * i)];
 		uint16_t raw = (dp[0] << 8) | dp[1];
+		unsigned channel, value;
 
-		/* ignore pad channels */
-		if (raw == 0xffff)
+		if (!dsm_decode_channel(raw, channel_shift, &channel, &value))
 			continue;
 
-		unsigned channel = (raw >> channel_shift) & 0xf;
-
 		/* ignore channels out of range */
 		if (channel >= PX4IO_INPUT_CHANNELS)
 			continue;
 
+		/* update the decoded channel count */
 		if (channel > ppm_decoded_channels)
 			ppm_decoded_channels = channel;
 
 		/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
-		unsigned data = raw & data_mask;
 		if (channel_shift == 11)
-			data /= 2;
-		ppm_buffer[channel] = 988 + data;
-
+			value /= 2;
+		ppm_buffer[channel] = 988 + value;
 	}
+
 	ppm_last_valid_decode = hrt_absolute_time();
 }