AP_HAL: added standalone test program for dsm

useful for debugging

libraries/AP_HAL/utility/dsm.cpp

@@ -38,8 +38,7 @@
 
 #include <stdint.h>
 #include <stdbool.h>
-
+#include <stdio.h>
-#include <AP_Common/AP_Common.h>
 
 #include "dsm.h"
 
@@ -47,7 +46,9 @@
 #define DSM_FRAME_CHANNELS	7		/**<Max supported DSM channels*/
 
 static uint64_t dsm_last_frame_time;		/**< Timestamp for start of last dsm frame */
-static unsigned dsm_channel_shift;			/**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */
+static unsigned dsm_channel_shift;			/**< Channel resolution, 0=unknown, 10=10 bit, 11=11 bit */
+
+//#define DEBUG
 
 #ifdef DEBUG
 # define debug(fmt, args...)	printf(fmt "\n", ##args)
@@ -162,7 +163,7 @@ dsm_guess_format(bool reset, const uint8_t dsm_frame[16])
 	unsigned votes10 = 0;
 	unsigned votes11 = 0;
 
-	for (unsigned i = 0; i < ARRAY_SIZE(masks); i++) {
+	for (unsigned i = 0; i < sizeof(masks)/sizeof(masks[0]); i++) {
 
 		if (cs10 == masks[i])
 			votes10++;
@@ -311,3 +312,262 @@ dsm_decode(uint64_t frame_time, const uint8_t dsm_frame[16], uint16_t *values, u
 	 */
 	return true;
 }
+
+#if defined(TEST_MAIN_PROGRAM) || defined(TEST_HEX_STRING)
+static uint8_t dsm_partial_frame_count;
+static uint8_t dsm_frame[DSM_FRAME_SIZE];
+static enum DSM_DECODE_STATE {
+	DSM_DECODE_STATE_DESYNC = 0,
+	DSM_DECODE_STATE_SYNC
+} dsm_decode_state = DSM_DECODE_STATE_DESYNC;
+static uint64_t dsm_last_rx_time;            /**< Timestamp when we last received data */
+static uint16_t dsm_chan_count;
+static uint16_t dsm_frame_drops;
+
+static bool
+dsm_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values,
+	  uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, uint16_t max_channels)
+{
+
+	/* this is set by the decoding state machine and will default to false
+	 * once everything that was decodable has been decoded.
+	 */
+	bool decode_ret = false;
+
+	/* keep decoding until we have consumed the buffer */
+	for (unsigned d = 0; d < len; d++) {
+
+		/* overflow check */
+		if (dsm_partial_frame_count == sizeof(dsm_frame) / sizeof(dsm_frame[0])) {
+			dsm_partial_frame_count = 0;
+			dsm_decode_state = DSM_DECODE_STATE_DESYNC;
+#ifdef DSM_DEBUG
+			printf("DSM: RESET (BUF LIM)\n");
+#endif
+		}
+
+		if (dsm_partial_frame_count == DSM_FRAME_SIZE) {
+			dsm_partial_frame_count = 0;
+			dsm_decode_state = DSM_DECODE_STATE_DESYNC;
+#ifdef DSM_DEBUG
+			printf("DSM: RESET (PACKET LIM)\n");
+#endif
+		}
+
+#ifdef DSM_DEBUG
+#if 1
+		printf("dsm state: %s%s, count: %d, val: %02x\n",
+		       (dsm_decode_state == DSM_DECODE_STATE_DESYNC) ? "DSM_DECODE_STATE_DESYNC" : "",
+		       (dsm_decode_state == DSM_DECODE_STATE_SYNC) ? "DSM_DECODE_STATE_SYNC" : "",
+		       dsm_partial_frame_count,
+		       (unsigned)frame[d]);
+#endif
+#endif
+
+		switch (dsm_decode_state) {
+		case DSM_DECODE_STATE_DESYNC:
+
+			/* we are de-synced and only interested in the frame marker */
+			if ((now - dsm_last_rx_time) > 5000) {
+				printf("resync %u\n", dsm_partial_frame_count);
+				dsm_decode_state = DSM_DECODE_STATE_SYNC;
+				dsm_partial_frame_count = 0;
+				dsm_chan_count = 0;
+				dsm_frame[dsm_partial_frame_count++] = frame[d];
+			}
+
+			break;
+
+		case DSM_DECODE_STATE_SYNC: {
+				dsm_frame[dsm_partial_frame_count++] = frame[d];
+
+				/* decode whatever we got and expect */
+				if (dsm_partial_frame_count < DSM_FRAME_SIZE) {
+					break;
+				}
+
+				/*
+				 * Great, it looks like we might have a frame.  Go ahead and
+				 * decode it.
+				 */
+				decode_ret = dsm_decode(now, dsm_frame, values, &dsm_chan_count, max_channels);
+
+#if 1
+                                printf("%u %u: ", ((unsigned)(now/1000)) % 1000000, len);
+                                for (uint8_t i=0; i<DSM_FRAME_SIZE; i++) {
+                                    printf("%02x ", (unsigned)dsm_frame[i]);
+                                }
+                                printf("\n");
+#endif
+	
+                                
+				/* we consumed the partial frame, reset */
+				dsm_partial_frame_count = 0;
+
+				/* if decoding failed, set proto to desync */
+				if (decode_ret == false) {
+					dsm_decode_state = DSM_DECODE_STATE_DESYNC;
+					dsm_frame_drops++;
+					printf("drop ");
+					for (uint8_t i=0; i<DSM_FRAME_SIZE; i++) {
+						printf("%02x ", (unsigned)dsm_frame[i]);
+					}
+					printf("\n");
+				}
+			}
+			break;
+
+		default:
+#ifdef DSM_DEBUG
+			printf("UNKNOWN PROTO STATE");
+#endif
+			decode_ret = false;
+		}
+
+
+	}
+
+	if (frame_drops) {
+		*frame_drops = dsm_frame_drops;
+	}
+
+	if (decode_ret) {
+		*num_values = dsm_chan_count;
+	}
+
+	dsm_last_rx_time = now;
+
+	/* return false as default */
+	return decode_ret;
+}
+#endif
+
+
+#ifdef TEST_MAIN_PROGRAM
+/*
+  test harness for use under Linux with USB serial adapter
+ */
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <time.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <termios.h>
+#include <string.h>
+
+static uint64_t micros64(void)
+{
+    struct timespec ts;
+    clock_gettime(CLOCK_MONOTONIC, &ts);
+    return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)));
+}
+
+int main(int argc, const char *argv[])
+{
+    int fd = open(argv[1], O_RDONLY);
+    if (fd == -1) {
+        perror(argv[1]);
+        exit(1);
+    }
+
+    struct termios options;
+
+    tcgetattr(fd, &options);
+
+    cfsetispeed(&options, B115200);
+    cfsetospeed(&options, B115200);
+
+    options.c_cflag &= ~(PARENB|CSTOPB|CSIZE);
+    options.c_cflag |= CS8;
+
+    options.c_lflag &= ~(ICANON|ECHO|ECHOE|ISIG);
+    options.c_iflag &= ~(IXON|IXOFF|IXANY);
+    options.c_oflag &= ~OPOST;
+
+    if (tcsetattr(fd, TCSANOW, &options) != 0) {
+        perror("tcsetattr");
+        exit(1);
+    }
+    tcflush(fd, TCIOFLUSH);
+
+    uint16_t values[18];
+    memset(values, 0, sizeof(values));
+
+    while (true) {
+        uint8_t b[16];
+        uint16_t num_values = 0;
+        fd_set fds;
+        struct timeval tv;
+    
+        FD_ZERO(&fds);
+        FD_SET(fd, &fds);
+
+        tv.tv_sec = 1;
+        tv.tv_usec = 0;
+
+        // check if any bytes are available
+        if (select(fd+1, &fds, NULL, NULL, &tv) != 1) {
+            break;
+        }
+
+        ssize_t nread;
+        if ((nread = read(fd, b, sizeof(b))) < 1) {
+            break;
+        }
+
+        bool dsm_11_bit;
+        unsigned frame_drops;
+        
+        if (dsm_parse(micros64(), b, nread, values, &num_values, &dsm_11_bit, &frame_drops, 18)) {
+#if 1
+            printf("%u: ", num_values);
+            for (uint8_t i=0; i<num_values; i++) {
+                printf("%u:%4u ", i+1, values[i]);
+            }
+            printf("\n");
+#endif
+        }
+    }
+}
+#elif defined(TEST_HEX_STRING)
+/*
+  test harness providing hex string to decode
+ */
+#include <string.h>
+
+int main(int argc, const char *argv[])
+{
+    uint8_t b[16];
+    uint64_t t = 0;
+
+    for (uint8_t i=1; i<argc; i++) {
+        unsigned v;
+        if (sscanf(argv[i], "%02x", &v) != 1 || v > 255) {
+            printf("Bad hex value at %u : %s\n", (unsigned)i, argv[i]);
+            return 1;
+        }
+        b[i-1] = v;
+    }
+    uint16_t values[18];
+    memset(values, 0, sizeof(values));
+    
+    while (true) {
+        uint16_t num_values = 0;
+        bool dsm_11_bit;
+        unsigned frame_drops;
+
+        t += 11000;
+        
+        if (dsm_parse(t, b, sizeof(b), values, &num_values, &dsm_11_bit, &frame_drops, 18)) {
+#if 1
+            printf("%u: ", num_values);
+            for (uint8_t i=0; i<num_values; i++) {
+                printf("%u:%4u ", i+1, values[i]);
+            }
+            printf("\n");
+#endif
+        }
+    }
+}
+#endif