Support binding DSM2 and DSMX satellite receivers

The px4io bind command allows you to put a DSM satellite receiver into
bind mode. Since this feature requires that
the dsm VCC line (red wire) be cut and routed through relay one, it is
not enabled by default in order not to
affect those not using a DSM satellite receiver or wising to use relay
one for other purposes.

NOTE: Binding DSM2 satellites in 11-bit mode is not supported due to
potential bug in some DSM2 receiver streams
when in 11-bit mode. Furthermore the px4io software folds 11 bit data
down to 10 bits so there is no resolution
advantage to to 11-bit mode.

To enable the feature the RC_RL1_DSM_VCC parameter must be set to a non
zero value from the console, or using
QGroundControl:

param set RC_RL1_DSM_VCC 1

From the console you can initiate DSM bind mode with:

uorb start
param set RC_RL1_DSM_VCC 1
px4io start
px4io bind dsm2

For binding a DSMX satellite to a DSMX transmitter you would instead
use:

px4io bind dsmx

Your receiver module should start a rapid flash and you can follow the
normal binding sequence of your
transmitter.

Note: The value of parameter RC_RL1_DSM_VCC defaults to 0, so none of
this will have any effect on an unmodified
DSM receiver connection. For this feature to work, the power wire (red)
must be cut and each side connected to a
terminal on relay1 of the px4io board.

This has been tested using Spektrum as well as Hobby King 'Orange' DSM
satellite receivers.

Both px4fmu and px4io images are updated.

9 files changed, 178 insertions, 10 deletions

diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 56af71059..52a667403 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -115,6 +115,15 @@ ORB_DECLARE(output_pwm);
 /** clear the 'ARM ok' bit, which deactivates the safety switch */
 #define PWM_SERVO_CLEAR_ARM_OK	_IOC(_PWM_SERVO_BASE, 6)
 
+/** start DSM bind */
+#define DSM_BIND_START	_IOC(_PWM_SERVO_BASE, 7)
+
+/** stop DSM bind */
+#define DSM_BIND_STOP	_IOC(_PWM_SERVO_BASE, 8)
+
+/** Power up DSM receiver */
+#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 9)
+
 /** set a single servo to a specific value */
 #define PWM_SERVO_SET(_servo)	_IOC(_PWM_SERVO_BASE, 0x20 + _servo)
 
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 19163cebe..54b9d50e4 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -89,6 +89,8 @@
 #define PX4IO_SET_DEBUG			_IOC(0xff00, 0)
 #define PX4IO_INAIR_RESTART_ENABLE	_IOC(0xff00, 1)
 
+static int dsm_vcc_ctl;
+
 class PX4IO : public device::I2C
 {
 public:
@@ -700,8 +702,6 @@ PX4IO::io_set_control_state()
 int
 PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
 {
-	uint16_t 		regs[_max_actuators];
-
 	if (len > _max_actuators)
 		/* fail with error */
 		return E2BIG;
@@ -1271,13 +1271,14 @@ PX4IO::print_status()
 	printf("%u bytes free\n",
 		io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
 	uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
-	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n",
+	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
 		flags,
 		((flags & PX4IO_P_STATUS_FLAGS_ARMED)    ? " ARMED" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_RC_OK)    ? " RC_OK" : " RC_FAIL"),
 		((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)   ? " PPM" : ""),
-		((flags & PX4IO_P_STATUS_FLAGS_RC_DSM)   ? " DSM" : ""),
+		(((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""),
+		(((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)  ? " SBUS" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_FMU_OK)   ? " FMU_OK" : " FMU_FAIL"),
 		((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)  ? " RAW_PPM" : ""),
@@ -1424,6 +1425,26 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		*(unsigned *)arg = _max_actuators;
 		break;
 
+	case DSM_BIND_START:
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); 
+		usleep(500000);
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
+		usleep(1000);
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
+		usleep(100000);
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4));
+		break;
+
+	case DSM_BIND_STOP:
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
+		usleep(500000);
+		break;
+
+	case DSM_BIND_POWER_UP:
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
+		break;
+
 	case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS - 1): {
 
 		/* TODO: we could go lower for e.g. TurboPWM */
@@ -1614,10 +1635,72 @@ start(int argc, char *argv[])
 		errx(1, "driver init failed");
 	}
 
+	if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) {
+		if (dsm_vcc_ctl) {
+			int fd = open("/dev/px4io", O_WRONLY);
+			if (fd < 0)
+				errx(1, "failed to open device");
+			ioctl(fd, DSM_BIND_POWER_UP, 0);
+			close(fd);
+		}
+	}
 	exit(0);
 }
 
 void
+bind(int argc, char *argv[])
+{
+	int fd, pulses;
+
+	if (g_dev == nullptr)
+		errx(1, "px4io must be started first");
+
+	if (dsm_vcc_ctl == 0)
+		errx(1, "DSM bind feature not enabled");
+
+	if (argc < 3)
+		errx(0, "needs argument, use dsm2 or dsmx");
+
+	if (!strcmp(argv[2], "dsm2"))
+		pulses = 3;
+	else if (!strcmp(argv[2], "dsmx"))
+		pulses = 7;
+	else 
+		errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]);
+
+	fd = open("/dev/px4io", O_WRONLY);
+
+	if (fd < 0)
+		errx(1, "failed to open device");
+
+	ioctl(fd, DSM_BIND_START, pulses);
+
+	/* Open console directly to grab CTRL-C signal */
+	int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
+	if (!console)
+		errx(1, "failed opening console");
+
+	warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1.");
+	warnx("Press CTRL-C or 'c' when done.");
+
+	for (;;) {
+		usleep(500000L);
+		/* Check if user wants to quit */
+		char c;
+		if (read(console, &c, 1) == 1) {
+			if (c == 0x03 || c == 0x63) {
+				warnx("Done\n");
+				ioctl(fd, DSM_BIND_STOP, 0);
+				ioctl(fd, DSM_BIND_POWER_UP, 0);
+				close(fd);
+				close(console);
+	                        exit(0);
+                        }
+		}
+	}
+}
+
+void
 test(void)
 {
 	int		fd;
@@ -1918,6 +2001,9 @@ px4io_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "monitor"))
 		monitor();
 
+	if (!strcmp(argv[1], "bind"))
+		bind(argc, argv);
+
 	out:
-	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe' or 'update'");
+	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe', 'bind', or 'update'");
 }
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 3cf9ca149..9561c9b1e 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -95,9 +95,16 @@ controls_tick() {
 	 */
 
 	perf_begin(c_gather_dsm);
-	bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count);
-	if (dsm_updated)
+	uint16_t temp_count = r_raw_rc_count;
+	bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
+	if (dsm_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
+		r_raw_rc_count = temp_count & 0x7fff;
+		if (temp_count & 0x8000)
+			r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11;
+		else
+			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11;
+	}
 	perf_end(c_gather_dsm);
 
 	perf_begin(c_gather_sbus);
@@ -138,7 +145,7 @@ controls_tick() {
 
 		/* map raw inputs to mapped inputs */
 		/* XXX mapping should be atomic relative to protocol */
-		for (unsigned i = 0; i < r_raw_rc_count; i++) {
+		for (unsigned i = 0; i < (r_raw_rc_count & 0x7fff); i++) {
 
 			/* map the input channel */
 			uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
index ea35e5513..79e892503 100644
--- a/src/modules/px4iofirmware/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
@@ -40,6 +40,7 @@
  */
 
 #include <nuttx/config.h>
+#include <nuttx/arch.h>
 
 #include <fcntl.h>
 #include <unistd.h>
@@ -101,6 +102,41 @@ dsm_init(const char *device)
 	return dsm_fd;
 }
 
+void
+dsm_bind(uint16_t cmd, int pulses)
+{
+	const uint32_t usart1RxAsOutp = GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10;
+
+	if (dsm_fd < 0)
+		return;
+
+	switch (cmd) {
+	case dsm_bind_power_down:
+		// power down DSM satellite
+		POWER_RELAY1(0);
+		break;
+	case dsm_bind_power_up:
+		POWER_RELAY1(1);
+		dsm_guess_format(true);
+		break;
+	case dsm_bind_set_rx_out:
+		stm32_configgpio(usart1RxAsOutp); 
+		break;
+	case dsm_bind_send_pulses:
+		for (int i = 0; i < pulses; i++) {
+			stm32_gpiowrite(usart1RxAsOutp, false);
+			up_udelay(50);
+			stm32_gpiowrite(usart1RxAsOutp, true);
+			up_udelay(50);
+		}
+		break;
+	case dsm_bind_reinit_uart:
+		// Restore USART rx pin
+		stm32_configgpio(GPIO_USART1_RX);
+		break;
+	}
+}
+
 bool
 dsm_input(uint16_t *values, uint16_t *num_values)
 {
@@ -218,7 +254,7 @@ dsm_guess_format(bool reset)
 
 	/*
 	 * Iterate the set of sensible sniffed channel sets and see whether
-	 * decoding in 10 or 11-bit mode has yielded anything we recognise.
+	 * decoding in 10 or 11-bit mode has yielded anything we recognize.
 	 *
 	 * XXX Note that due to what seem to be bugs in the DSM2 high-resolution
 	 *     stream, we may want to sniff for longer in some cases when we think we
@@ -303,7 +339,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 	for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
 
 		uint8_t *dp = &frame[2 + (2 * i)];
-		uint16_t raw = (dp[0] << 8) | dp[1];
+		uint16_t raw = ((uint16_t)dp[0] << 8) | dp[1];
 		unsigned channel, value;
 
 		if (!dsm_decode_channel(raw, channel_shift, &channel, &value))
@@ -349,6 +385,9 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 		values[channel] = value;
 	}
 
+	if (channel_shift == 11)
+		*num_values |= 0x8000;
+
 	/*
 	 * XXX Note that we may be in failsafe here; we need to work out how to detect that.
 	 */
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 674f9dddd..6ee5c2834 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -105,6 +105,7 @@
 #define PX4IO_P_STATUS_FLAGS_ARM_SYNC		(1 << 9) /* the arming state between IO and FMU is in sync */
 #define PX4IO_P_STATUS_FLAGS_INIT_OK		(1 << 10) /* initialisation of the IO completed without error */
 #define PX4IO_P_STATUS_FLAGS_FAILSAFE		(1 << 11) /* failsafe is active */
+#define PX4IO_P_STATUS_FLAGS_RC_DSM11		(1 << 12) /* DSM input is 11 bit data */
 
 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
 #define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* VBatt is very close to regulator dropout */
@@ -157,6 +158,14 @@
 #define PX4IO_P_SETUP_PWM_ALTRATE		4	/* 'high' PWM frame output rate in Hz */
 #define PX4IO_P_SETUP_RELAYS			5	/* bitmask of relay/switch outputs, 0 = off, 1 = on */
 #define PX4IO_P_SETUP_VBATT_SCALE		6	/* battery voltage correction factor (float) */
+#define PX4IO_P_SETUP_DSM				7	/* DSM bind state */
+enum {                                      /* DSM bind states */
+	dsm_bind_power_down = 0,
+	dsm_bind_power_up,
+	dsm_bind_set_rx_out,
+	dsm_bind_send_pulses,
+	dsm_bind_reinit_uart
+};
 #define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
 
 /* autopilot control values, -10000..10000 */
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 272cdb7bf..83feeb9b6 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -184,6 +184,7 @@ extern void	controls_init(void);
 extern void	controls_tick(void);
 extern int	dsm_init(const char *device);
 extern bool	dsm_input(uint16_t *values, uint16_t *num_values);
+extern void     dsm_bind(uint16_t cmd, int pulses);
 extern int	sbus_init(const char *device);
 extern bool	sbus_input(uint16_t *values, uint16_t *num_values);
 
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index df7d6dcd3..805eb7ecc 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -360,6 +360,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
 			break;
 
+		case PX4IO_P_SETUP_DSM:
+			dsm_bind(value & 0x0f, (value >> 4) & 7);
+			break;
+
 		default:
 			return -1;
 		}
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 230060148..a11c13568 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -151,6 +151,7 @@ PARAM_DEFINE_FLOAT(RC14_REV, 1.0f);
 PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f);
 
 PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */
+PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
 
 /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f));
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 6b6aeedee..626cbade4 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -229,6 +229,8 @@ private:
 		float rc_scale_flaps;
 
 		float battery_voltage_scaling;
+
+		int   rc_rl1_DSM_VCC_control;
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -276,6 +278,8 @@ private:
 		param_t rc_scale_flaps;
 
 		param_t battery_voltage_scaling;
+
+		param_t rc_rl1_DSM_VCC_control;
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -509,6 +513,9 @@ Sensors::Sensors() :
 
 	_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
 
+	/* DSM VCC relay control */
+	_parameter_handles.rc_rl1_DSM_VCC_control = param_find("RC_RL1_DSM_VCC");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -722,6 +729,11 @@ Sensors::parameters_update()
 		warnx("Failed updating voltage scaling param");
 	}
 
+	/* relay 1 DSM VCC control */
+	if (param_get(_parameter_handles.rc_rl1_DSM_VCC_control, &(_parameters.rc_rl1_DSM_VCC_control)) != OK) {
+		warnx("Failed updating relay 1 DSM VCC control");
+	}
+
 	return OK;
 }