DSM parser: Keep local copy of channel data

src/lib/rc/dsm.c

@@ -46,6 +46,7 @@
 #include <fcntl.h>
 #include <unistd.h>
 #include <termios.h>
+#include <string.h>
 
 #include "dsm.h"
 #include <drivers/drv_hrt.h>
@@ -57,7 +58,7 @@
 #define dsm_udelay(arg)    up_udelay(arg)
 #endif
 
-//#define DSM_DEBUG
+// #define DSM_DEBUG
 
 static enum DSM_DECODE_STATE {
 	DSM_DECODE_STATE_DESYNC = 0,
@@ -69,6 +70,7 @@ static hrt_abstime dsm_last_rx_time;            /**< Timestamp when we last rece
 static hrt_abstime dsm_last_frame_time;		/**< Timestamp for start of last valid dsm frame */
 static uint8_t dsm_frame[DSM_BUFFER_SIZE];	/**< DSM dsm frame receive buffer */
 static uint8_t dsm_buf[DSM_FRAME_SIZE * 2];
+static uint16_t dsm_chan_buf[DSM_MAX_CHANNEL_COUNT];
 static unsigned dsm_partial_frame_count;	/**< Count of bytes received for current dsm frame */
 static unsigned dsm_channel_shift = 0;			/**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */
 static unsigned dsm_frame_drops = 0;			/**< Count of incomplete DSM frames */
@@ -263,6 +265,19 @@ dsm_config(int fd)
 	return ret;
 }
 
+void
+dsm_proto_init()
+{
+	dsm_channel_shift = 0;
+	dsm_frame_drops = 0;
+	dsm_chan_count = 0;
+	dsm_decode_state = DSM_DECODE_STATE_DESYNC;
+
+	for (unsigned i = 0; i < DSM_MAX_CHANNEL_COUNT; i++) {
+		dsm_chan_buf[i] = 0;
+	}
+}
+
 /**
  * Initialize the DSM receive functionality
  *
@@ -278,10 +293,7 @@ dsm_init(const char *device)
 		dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
 	}
 
-	dsm_channel_shift = 0;
+	dsm_proto_init();
-	dsm_frame_drops = 0;
-	dsm_chan_count = 0;
-	dsm_decode_state = DSM_DECODE_STATE_DESYNC;
 
 	int ret = dsm_config(dsm_fd);
 
@@ -494,7 +506,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
 		/* if the value is unrealistic, fail the parsing entirely */
 		if (values[i] < 600 || values[i] > 2400) {
 #ifdef DSM_DEBUG
-			printf("DSM: VALUE RANGE FAIL\n");
+			printf("DSM: VALUE RANGE FAIL: %d: %d\n", (int)i, (int)values[i]);
 #endif
 			dsm_chan_count = 0;
 			return false;
@@ -581,6 +593,11 @@ dsm_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values,
 	 */
 	bool decode_ret = false;
 
+	/* ensure there can be no overflows */
+	if (max_channels > sizeof(dsm_chan_buf) / sizeof(dsm_chan_buf[0])) {
+		max_channels = sizeof(dsm_chan_buf) / sizeof(dsm_chan_buf[0]);
+	}
+
 	/* keep decoding until we have consumed the buffer */
 	for (unsigned d = 0; d < len; d++) {
 
@@ -636,7 +653,7 @@ dsm_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values,
 				 * Great, it looks like we might have a frame.  Go ahead and
 				 * decode it.
 				 */
-				decode_ret = dsm_decode(now, values, &dsm_chan_count, dsm_11_bit, max_channels);
+				decode_ret = dsm_decode(now, &dsm_chan_buf[0], &dsm_chan_count, dsm_11_bit, max_channels);
 
 				/* we consumed the partial frame, reset */
 				dsm_partial_frame_count = 0;
@@ -665,6 +682,14 @@ dsm_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values,
 
 	if (decode_ret) {
 		*num_values = dsm_chan_count;
+		memcpy(&values[0], &dsm_chan_buf[0], dsm_chan_count * sizeof(dsm_chan_buf[0]));
+#ifdef DSM_DEBUG
+
+		for (unsigned i = 0; i < dsm_chan_count; i++) {
+			printf("dsm_decode: %u: %u\n", i, values[0]);
+		}
+
+#endif
 	}
 
 	dsm_last_rx_time = now;

src/lib/rc/dsm.h

@@ -54,6 +54,7 @@ __BEGIN_DECLS
 #define DSM_BUFFER_SIZE		(DSM_FRAME_SIZE + DSM_FRAME_SIZE / 2)
 
 __EXPORT int	dsm_init(const char *device);
+__EXPORT void	dsm_proto_init(void);
 __EXPORT int	dsm_config(int dsm_fd);
 __EXPORT bool	dsm_input(int dsm_fd, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, uint8_t *n_bytes,
 			  uint8_t **bytes, unsigned max_values);

src/lib/rc/rc_tests/RCTest.cpp

@@ -48,12 +48,12 @@ bool RCTest::run_tests(void)
 
 bool RCTest::dsmTest10Ch()
 {
-	return dsmTest(TEST_DATA_PATH "dsm_x_data.txt", 10, 6, 0);
+	return dsmTest(TEST_DATA_PATH "dsm_x_data.txt", 10, 6, 1500);
 }
 
 bool RCTest::dsmTest12Ch()
 {
-	return dsmTest(TEST_DATA_PATH "dsm_x_dx9_data.txt", 12, 11, 1500);
+	return dsmTest(TEST_DATA_PATH "dsm_x_dx9_data.txt", 12, 6, 1500);
 }
 
 bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned expected_dropcount, unsigned chan0)
@@ -76,7 +76,7 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned
 	}
 
 	// Init the parser
-	uint8_t frame[20];
+	uint8_t frame[30];
 	uint16_t rc_values[18];
 	uint16_t num_values;
 	bool dsm_11_bit;
@@ -86,6 +86,8 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned
 	int rate_limiter = 0;
 	unsigned last_drop = 0;
 
+	dsm_proto_init();
+
 	while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
 
 		ut_test(ret > 0);
@@ -100,9 +102,7 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned
 		if (result) {
 			ut_test(num_values == expected_chancount);
 
-			if (chan0 > 0) {
+			ut_test(abs((int)chan0 - (int)rc_values[0]) < 30);
-				ut_test((chan0 - rc_values[0]) < 3);
-			}
 
 			//PX4_INFO("decoded packet with %d channels and %s encoding:", num_values, (dsm_11_bit) ? "11 bit" : "10 bit");
 
@@ -112,7 +112,7 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned
 		}
 
 		if (last_drop != (dsm_frame_drops)) {
-			//PX4_INFO("frame dropped, now #%d", (dsm_frame_drops));
+			PX4_INFO("frame dropped, now #%d", (dsm_frame_drops));
 			last_drop = dsm_frame_drops;
 		}