Merge branch 'master' of https://github.com/PX4/Firmware into fmuv2_bringup

5 files changed, 67 insertions, 8 deletions

diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 5a95a8aa9..74037d1c2 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);
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
index ea35e5513..ab6e3fec4 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
@@ -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 fa57dfc3f..dc5c40638 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -106,6 +106,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) /* [1] VBatt is very close to regulator dropout */
@@ -169,7 +170,14 @@
 
 #define PX4IO_P_SETUP_VBATT_SCALE		6	/* [1] battery voltage correction factor (float) */
 #define PX4IO_P_SETUP_VSERVO_SCALE		6	/* [2] servo voltage correction factor (float) */
- 					     /*	7 */
+#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
+};
  					     /*	8 */
 #define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
 
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index ccf175e45..bd78f2638 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -200,6 +200,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 873ee73f1..9606faa86 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -360,16 +360,16 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			value &= PX4IO_P_SETUP_RELAYS_VALID;
 			r_setup_relays = value;
 #ifdef POWER_RELAY1
-			POWER_RELAY1(value & (1 << 0) ? 1 : 0);
+			POWER_RELAY1((value & PX4IO_P_SETUP_RELAYS_POWER1) ? 1 : 0);
 #endif
 #ifdef POWER_RELAY2
-			POWER_RELAY2(value & (1 << 1) ? 1 : 0);
+			POWER_RELAY2((value & PX4IO_P_SETUP_RELAYS_POWER2) ? 1 : 0);
 #endif
 #ifdef POWER_ACC1
-			POWER_ACC1(value & (1 << 2) ? 1 : 0);
+			POWER_ACC1((value & PX4IO_P_SETUP_RELAYS_ACC1) ? 1 : 0);
 #endif
 #ifdef POWER_ACC2
-			POWER_ACC2(value & (1 << 3) ? 1 : 0);
+			POWER_ACC2((value & PX4IO_P_SETUP_RELAYS_ACC2) ? 1 : 0);
 #endif
 			break;
 
@@ -382,6 +382,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;
 		}