Add prototypical support for Spektrum satellite remotes to PX4IO.

6 files changed, 310 insertions, 41 deletions

diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 5050c6ce7..9e2025231 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -87,6 +87,8 @@ public:
 
 	virtual int		ioctl(file *filp, int cmd, unsigned long arg);
 
+	void			set_rx_mode(unsigned mode);
+
 private:
 	static const unsigned	_max_actuators = PX4IO_OUTPUT_CHANNELS;
 
@@ -114,6 +116,9 @@ private:
 	// XXX how should this work?
 
 	bool			_send_needed;	///< If true, we need to send a packet to IO
+	bool			_config_needed;	///< if true, we need to set a config update to IO
+
+	uint8_t			_rx_mode;	///< the current RX mode on IO
 
 	/**
 	 * Trampoline to the worker task
@@ -146,6 +151,11 @@ private:
 	void			io_send();
 
 	/**
+	 * Send a config packet to PX4IO
+	 */
+	void			config_send();
+
+	/**
 	 * Mixer control callback; invoked to fetch a control from a specific
 	 * group/index during mixing.
 	 */
@@ -176,7 +186,9 @@ PX4IO::PX4IO() :
 	_mixers(nullptr),
 	_primary_pwm_device(false),
 	_switch_armed(false),
-	_send_needed(false)
+	_send_needed(false),
+	_config_needed(false),
+	_rx_mode(RX_MODE_PPM_ONLY)
 {
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
@@ -373,6 +385,12 @@ PX4IO::task_main()
 			_send_needed = false;
 			io_send();
 		}
+
+		/* send a config packet to IO if required */
+		if (_config_needed) {
+			_config_needed = false;
+			config_send();
+		}
 	}
 
 out:
@@ -473,6 +491,20 @@ PX4IO::io_send()
 		debug("send error %d", ret);
 }
 
+void
+PX4IO::config_send()
+{
+	px4io_config	cfg;
+	int		ret;
+
+	cfg.f2i_config_magic = F2I_CONFIG_MAGIC;
+	cfg.serial_rx_mode = _rx_mode;
+
+	ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg));
+	if (ret)
+		debug("config error %d", ret);
+}
+
 int
 PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 {
@@ -586,6 +618,15 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 	return ret;
 }
 
+void
+PX4IO::set_rx_mode(unsigned mode)
+{
+	if (mode != _rx_mode) {
+		_rx_mode = mode;
+		_config_needed = true;
+	}
+}
+
 extern "C" __EXPORT int px4io_main(int argc, char *argv[]);
 
 namespace
@@ -642,9 +683,6 @@ px4io_main(int argc, char *argv[])
 		exit(0);
 	}
 
-	if (!strcmp(argv[1], "test"))
-		test();
-
 	/* note, stop not currently implemented */
 
 	if (!strcmp(argv[1], "update")) {
@@ -690,5 +728,25 @@ px4io_main(int argc, char *argv[])
 		return ret;
 	}
 
-	errx(1, "need a verb, only support 'start' and 'update'");
+	if (!strcmp(argv[1], "rx_spektrum6")) {
+		if (g_dev == nullptr)
+			errx(1, "not started");
+		g_dev->set_rx_mode(RX_MODE_SPEKTRUM_6);
+	}
+	if (!strcmp(argv[1], "rx_spektrum7")) {
+		if (g_dev == nullptr)
+			errx(1, "not started");
+		g_dev->set_rx_mode(RX_MODE_SPEKTRUM_7);
+	}
+	if (!strcmp(argv[1], "rx_sbus")) {
+		if (g_dev == nullptr)
+			errx(1, "not started");
+		g_dev->set_rx_mode(RX_MODE_FUTABA_SBUS);
+	}
+
+	if (!strcmp(argv[1], "test"))
+		test();
+
+
+	errx(1, "need a command, try 'start', 'test', 'rx_spektrum6', 'rx_spektrum7', 'rx_sbus' or 'update'");
 }
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index 8fdf7d1c8..1edff25b1 100644
--- a/apps/px4io/comms.c
+++ b/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++;
+}
+
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;
-	}
-}
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index c4a63d886..7467f1adc 100644
--- a/apps/px4io/protocol.h
+++ b/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;
diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c
index dc8e227fc..7039e5d58 100644
--- a/apps/px4io/px4io.c
+++ b/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) {
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index 096b109d6..274b27ff3 100644
--- a/apps/px4io/px4io.h
+++ b/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.