Add prototypical support for Spektrum satellite remotes to PX4IO.

1 files changed, 177 insertions, 22 deletions

diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c
index fab577383..471965fd7 100644
--- a/apps/px4io/mixer.c
+++ b/apps/px4io/mixer.c
@@ -40,10 +40,13 @@
 
 #include <sys/types.h>
 #include <stdbool.h>
-
+#include <string.h>
 #include <assert.h>
 #include <errno.h>
 #include <fcntl.h>
+#include <termios.h>
+#include <unistd.h>
+#include <fcntl.h>
 
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_hrt.h>
@@ -65,6 +68,11 @@ 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;
@@ -98,12 +106,47 @@ 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);
 
 	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)
 {
@@ -181,22 +224,145 @@ mixer_tick(void *arg)
 }
 
 static void
-mixer_get_rc_input(void)
+mixer_update(int mixer, uint16_t *inputs, int input_count)
 {
+	/* simple passthrough for now */
+	if (mixer < input_count) {
+		mixers[mixer].current_value = inputs[mixer];
+	} else {
+		mixers[mixer].current_value = 0;
+	}
+}
+
+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];
+
 	/*
-	 * XXX currently only supporting PPM
+	 * 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:
 	 *
-	 * XXX check timestamp to ensure current
+	 * 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 (ppm_decoded_channels > 0) {
-		mixer_input_drops = 0;
-		system_state.fmu_report_due = true;
+	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;
 
-		/* 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];
+	case RX_MODE_FUTABA_SBUS:
+		got_input = mixer_get_sbus_input();
+		break;
 
+	default:
+		break;
+	}
+
+	if (got_input) {
+		mixer_input_drops = 0;
+		system_state.fmu_report_due = true;
 	} else {
 		/*
 		 * No data; count the 'frame drops' and once we hit the limit
@@ -213,14 +379,3 @@ mixer_get_rc_input(void)
 		}
 	}
 }
-
-static void
-mixer_update(int mixer, uint16_t *inputs, int input_count)
-{
-	/* simple passthrough for now */
-	if (mixer < input_count) {
-		mixers[mixer].current_value = inputs[mixer];
-	} else {
-		mixers[mixer].current_value = 0;
-	}
-}