Another take on Spektrum/DSM frame decoding, based on more careful examination of the relevant docs.

1 files changed, 14 insertions, 185 deletions

diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c
index 471965fd7..b614f3fa4 100644
--- a/apps/px4io/mixer.c
+++ b/apps/px4io/mixer.c
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Control channel input/output mixer and failsafe.
+ * @file mixer.c
+ *
+ * Control channel input/output mixer and failsafe.
  */
 
 #include <nuttx/config.h>
@@ -43,8 +45,6 @@
 #include <string.h>
 #include <assert.h>
 #include <errno.h>
-#include <fcntl.h>
-#include <termios.h>
 #include <unistd.h>
 #include <fcntl.h>
 
@@ -56,23 +56,12 @@
 #include "px4io.h"
 
 /*
- * Count of periodic calls in which we have no data.
- */
-static unsigned mixer_input_drops;
-#define MIXER_INPUT_DROP_LIMIT	10
-
-/*
  * Count of periodic calls in which we have no FMU input.
  */
 static unsigned fmu_input_drops;
 #define FMU_INPUT_DROP_LIMIT	20
 
 /*
- * Serial port fd for serial RX protocols
- */
-static int rx_port = -1;
-
-/*
  * HRT periodic call used to check for control input data.
  */
 static struct hrt_call mixer_input_call;
@@ -106,8 +95,7 @@ struct mixer {
 int
 mixer_init(void)
 {
-	/* open the serial port */
-	rx_port = open("/dev/ttyS0", O_RDONLY | O_NONBLOCK);
+
 
 	/* look for control data at 50Hz */
 	hrt_call_every(&mixer_input_call, 1000, 20000, mixer_tick, NULL);
@@ -115,37 +103,6 @@ mixer_init(void)
 	return 0;
 }
 
-void
-mixer_set_serial_mode(uint8_t serial_mode)
-{
-
-	if (serial_mode == system_state.serial_rx_mode)
-		return;
-
-	struct termios t;
-	tcgetattr(rx_port, &t);
-
-	switch (serial_mode) {
-	case RX_MODE_PPM_ONLY:
-		break;
-	case RX_MODE_SPEKTRUM_6:
-	case RX_MODE_SPEKTRUM_7:
-		/* 115200, no parity, one stop bit */
-		cfsetspeed(&t, 115200);
-		t.c_cflag &= ~(CSTOPB | PARENB);
-		break;
-	case RX_MODE_FUTABA_SBUS:
-		/* 100000, even parity, two stop bits */
-		cfsetspeed(&t, 100000);
-		t.c_cflag |= (CSTOPB | PARENB);
-		break;
-	default:
-		return;
-	}
-
-	tcsetattr(rx_port, TCSANOW, &t);
-	system_state.serial_rx_mode = serial_mode;
-}
 
 static void
 mixer_tick(void *arg)
@@ -234,148 +191,20 @@ mixer_update(int mixer, uint16_t *inputs, int input_count)
 	}
 }
 
-static bool
-mixer_get_spektrum_input(void)
-{
-	static uint8_t buf[16];
-	static unsigned count;
-
-	/* always read as much data as we can into the buffer */
-	if (count >= sizeof(buf))
-		count = 0;
-	ssize_t result = read(rx_port, buf, sizeof(buf) - count);
-	/* no data or an error */
-	if (result <= 0)
-		return false;
-	count += result;
-
-	/* if there are more than two bytes in the buffer, check for sync */
-	if (count >= 2) {
-		if ((buf[0] != 0x3) || (buf[1] != 0x1)) {
-			/* not in sync; look for a possible sync marker */
-			for (unsigned i = 1; i < count; i++) {
-				if (buf[i] == 0x3) {
-					/* could be a frame marker; move buffer bytes */
-					count -= i;
-					memmove(buf, buf + i, count);
-					break;
-				}
-			}
-		}
-	}
-	if (count < sizeof(buf))
-		return false;
-
-	/* we got a frame; decode it */
-	const uint16_t *channels = (const uint16_t *)&buf[2];
-
-	/*
-	 * Channel assignment for DX6i vs. DX7 is different.
-	 *
-	 * DX7 etc. is:
-	 *
-	 * 0: Aileron
-	 * 1: Flaps
-	 * 2: Gear
-	 * 3: Elevator
-	 * 4: Aux2
-	 * 5: Throttle
-	 * 6: Rudder
-	 *
-	 * DX6i is:
-	 *
-	 * 0: Aileron	
-	 * 1: Flaps
-	 * 2: Elevator
-	 * 3: Rudder
-	 * 4: Throttle
-	 * 5: Gear
-	 * 6: <notused>
-	 *
-	 * We convert these to our standard Futaba-style assignment:
-	 *
-	 * 0: Throttle  (Throttle)
-	 * 1: Roll      (Aileron)
-	 * 2: Pitch     (Elevator)
-	 * 3: Yaw       (Rudder)
-	 * 4: Override  (Flaps)
-	 * 5: FUNC_0    (Gear)
-	 * 6: FUNC_1    (Aux2)
-	 */
-	if (system_state.serial_rx_mode == RX_MODE_SPEKTRUM_7) {
-		system_state.rc_channel_data[0] = channels[5]; /* Throttle */
-		system_state.rc_channel_data[1] = channels[0]; /* Roll */
-		system_state.rc_channel_data[2] = channels[3]; /* Pitch */
-		system_state.rc_channel_data[3] = channels[6]; /* Yaw */
-		system_state.rc_channel_data[4] = channels[1]; /* Override */
-		system_state.rc_channel_data[5] = channels[2]; /* FUNC_0 */
-		system_state.rc_channel_data[6] = channels[4]; /* FUNC_1 */
-		system_state.rc_channels = 7;
-	} else {
-		system_state.rc_channel_data[0] = channels[4]; /* Throttle */
-		system_state.rc_channel_data[1] = channels[0]; /* Roll */
-		system_state.rc_channel_data[2] = channels[2]; /* Pitch */
-		system_state.rc_channel_data[3] = channels[3]; /* Yaw */
-		system_state.rc_channel_data[4] = channels[1]; /* Override */
-		system_state.rc_channel_data[5] = channels[5]; /* FUNC_0 */
-		system_state.rc_channels = 6;
-	}
-	count = 0;
-	return true;
-}
-
-static bool
-mixer_get_sbus_input(void)
-{
-	/* XXX not implemented yet */
-	return false;
-}
-
 static void
 mixer_get_rc_input(void)
 {
-	bool got_input = false;
-
-	switch (system_state.serial_rx_mode) {
-	case RX_MODE_PPM_ONLY:
-		if (ppm_decoded_channels > 0) {
-			/* copy channel data */
-			system_state.rc_channels = ppm_decoded_channels;
-			for (unsigned i = 0; i < ppm_decoded_channels; i++)
-				system_state.rc_channel_data[i] = ppm_buffer[i];
-			got_input = true;
-		}
-		break;
-
-	case RX_MODE_SPEKTRUM_6:
-	case RX_MODE_SPEKTRUM_7:
-		got_input = mixer_get_spektrum_input();
-		break;
-
-	case RX_MODE_FUTABA_SBUS:
-		got_input = mixer_get_sbus_input();
-		break;
-
-	default:
-		break;
-	}
 
-	if (got_input) {
-		mixer_input_drops = 0;
+	/* if we haven't seen any new data in 200ms, assume we have lost input and tell FMU */
+	if ((hrt_absolute_time() - ppm_last_valid_decode) > 200000) {
+		system_state.rc_channels = 0;
 		system_state.fmu_report_due = true;
-	} else {
-		/*
-		 * No data; count the 'frame drops' and once we hit the limit
-		 * assume that we have lost input.
-		 */
-		if (mixer_input_drops < MIXER_INPUT_DROP_LIMIT) {
-			mixer_input_drops++;
-
-			/* if we hit the limit, stop pretending we have input and let the FMU know */
-			if (mixer_input_drops == MIXER_INPUT_DROP_LIMIT) {
-				system_state.rc_channels = 0;
-				system_state.fmu_report_due = true;
-			}
-		}
+		return;
 	}
+
+	/* otherwise, copy channel data */
+	system_state.rc_channels = ppm_decoded_channels;
+	for (unsigned i = 0; i < ppm_decoded_channels; i++)
+		system_state.rc_channel_data[i] = ppm_buffer[i];
+	system_state.fmu_report_due = true;
 }