Add prototypical support for Spektrum satellite remotes to PX4IO.

apps/px4io/comms.c

@@ -116,18 +116,32 @@ int frame_rx;
 int frame_bad;
 
 static void
-comms_handle_frame(void *arg, const void *buffer, size_t length)
+comms_handle_config(const void *buffer, size_t length)
 {
-	struct px4io_command *cmd = (struct px4io_command *)buffer;
+	const struct px4io_config *cfg = (struct px4io_config *)buffer;
 
-	/* make sure it's what we are expecting */
-	if ((length != sizeof(struct px4io_command)) || 
-	    (cmd->f2i_magic != F2I_MAGIC)) {
-	    	frame_bad++;
+	if (length != sizeof(*cfg)) {
+		frame_bad++;
 		return;
 	}
+
 	frame_rx++;
 
+	mixer_set_serial_mode(cfg->serial_rx_mode);
+
+}
+
+static void
+comms_handle_command(const void *buffer, size_t length)
+{
+	const struct px4io_command *cmd = (struct px4io_command *)buffer;
+
+	if (length != sizeof(*cmd)) {
+		frame_bad++;
+		return;
+	}
+
+	frame_rx++;
 	irqstate_t flags = irqsave();
 
 	/* fetch new PWM output values */
@@ -146,6 +160,29 @@ comms_handle_frame(void *arg, const void *buffer, size_t length)
 	for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
 		system_state.relays[i] = cmd->relay_state[i];
 
-out:
 	irqrestore(flags);
 }
+
+
+static void
+comms_handle_frame(void *arg, const void *buffer, size_t length)
+{
+	const uint16_t *type = (const uint16_t *)buffer;
+
+
+	/* make sure it's what we are expecting */
+	if (length > 2) {
+		switch (*type) {
+		case F2I_MAGIC:
+			comms_handle_command(buffer, length);
+			break;
+		case F2I_CONFIG_MAGIC:
+			comms_handle_config(buffer, length);
+			break;
+		default:
+			break;
+		}
+	}
+    	frame_bad++;
+}
+

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>
@@ -64,6 +67,11 @@ static unsigned mixer_input_drops;
 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.
  */
@@ -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)
 {
@@ -180,23 +223,146 @@ mixer_tick(void *arg)
 	}
 }
 
+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;
+	}
+}
+
+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)
 {
-	/*
-	 * XXX currently only supporting PPM
-	 *
-	 * XXX check timestamp to ensure current
-	 */
-	if (ppm_decoded_channels > 0) {
+	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;
 		system_state.fmu_report_due = true;
-
-		/* 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];
-
 	} 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;
-	}
-}

apps/px4io/protocol.h

@@ -62,6 +62,18 @@ struct px4io_command {
 	bool		arm_ok;
 };
 
+/* config message from FMU to IO */
+struct px4io_config {
+	uint16_t f2i_config_magic;
+#define F2I_CONFIG_MAGIC 0x6366
+
+	uint8_t		serial_rx_mode;
+#define RX_MODE_PPM_ONLY	0
+#define RX_MODE_SPEKTRUM_6	1
+#define RX_MODE_SPEKTRUM_7	2
+#define RX_MODE_FUTABA_SBUS	3
+};
+
 /* report from IO to FMU */
 struct px4io_report {
 	uint16_t	i2f_magic;

apps/px4io/px4io.c

@@ -102,7 +102,7 @@ int user_start(int argc, char *argv[])
 	safety_init();
 
 	/* set up some timers for the main loop */
-	timers[TIMER_BLINK_AMBER] = 250;	/* heartbeat blink @ 2Hz */
+	timers[TIMER_BLINK_BLUE] = 250;		/* heartbeat blink @ 2Hz */
 	timers[TIMER_STATUS_PRINT] = 1000;	/* print status message @ 1Hz */
 
 	/*
@@ -114,21 +114,21 @@ int user_start(int argc, char *argv[])
 		comms_check();
 
 		/* blink the heartbeat LED */
-		if (timers[TIMER_BLINK_AMBER] == 0) {
-			timers[TIMER_BLINK_AMBER] = 250;
-			LED_AMBER(heartbeat = !heartbeat);
+		if (timers[TIMER_BLINK_BLUE] == 0) {
+			timers[TIMER_BLINK_BLUE] = 250;
+			LED_BLUE(heartbeat = !heartbeat);
 		}
 
 		/* blink the failsafe LED if we don't have FMU input */
 		if (!system_state.mixer_use_fmu) {
-			if (timers[TIMER_BLINK_BLUE] == 0) {
-				timers[TIMER_BLINK_BLUE] = 125;
+			if (timers[TIMER_BLINK_AMBER] == 0) {
+				timers[TIMER_BLINK_AMBER] = 125;
 				failsafe = !failsafe;
 			}
 		} else {
 			failsafe = false;
 		}
-		LED_BLUE(failsafe);
+		LED_AMBER(failsafe);
 		
 		/* print some simple status */
 		if (timers[TIMER_STATUS_PRINT] == 0) {

apps/px4io/px4io.h

@@ -101,6 +101,12 @@ struct sys_state_s
 	 * If true, new control data from the FMU has been received.
 	 */
 	bool		fmu_data_received;
+
+	/*
+	 * Current serial interface mode, per the serial_rx_mode parameter
+	 * in the config packet.
+	 */
+	uint8_t		serial_rx_mode;
 };
 
 extern struct sys_state_s system_state;
@@ -141,6 +147,7 @@ extern volatile int	timers[TIMER_NUM_TIMERS];
  * Mixer
  */
 extern int	mixer_init(void);
+extern void	mixer_set_serial_mode(uint8_t newmode);
 
 /*
  * Safety switch/LED.