Implement Spektrum DSM pairing in V2

- Bind control for V2
- Relays and accessory power not supported on V2 hardware

8 files changed, 83 insertions, 30 deletions

diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h
index 818b64436..4d41d0d07 100644
--- a/src/drivers/boards/px4io-v2/board_config.h
+++ b/src/drivers/boards/px4io-v2/board_config.h
@@ -84,7 +84,7 @@
 
 /* Power switch controls ******************************************************/
 
-#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
+#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
 
 #define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15)
 
diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c
index 0ea95bded..ccd01edf5 100644
--- a/src/drivers/boards/px4io-v2/px4iov2_init.c
+++ b/src/drivers/boards/px4io-v2/px4iov2_init.c
@@ -111,9 +111,7 @@ __EXPORT void stm32_boardinitialize(void)
 
 	stm32_configgpio(GPIO_BTN_SAFETY);
 
-	/* spektrum power enable is active high - disable it by default */
-	/* XXX might not want to do this on warm restart? */
-	stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, false);
+	/* spektrum power enable is active high - enable it by default */
 	stm32_configgpio(GPIO_SPEKTRUM_PWR_EN);
 
 	stm32_configgpio(GPIO_SERVO_FAULT_DETECT);
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 5fff1feac..f9fb9eea9 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -204,6 +204,7 @@ public:
 	 */
 	int			disable_rc_handling();
 
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	/**
 	 * Set the DSM VCC is controlled by relay one flag
 	 *
@@ -223,6 +224,9 @@ public:
 	{
 		return _dsm_vcc_ctl;
 	};
+#endif
+
+	inline uint16_t		system_status() const {return _status;}
 
 private:
 	device::Device		*_interface;
@@ -274,8 +278,9 @@ private:
 	float			_battery_mamphour_total;///<amp hours consumed so far
 	uint64_t		_battery_last_timestamp;///<last amp hour calculation timestamp
 
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	bool			_dsm_vcc_ctl;		///<true if relay 1 controls DSM satellite RX power
-
+#endif
 
 	/**
 	 * Trampoline to the worker task
@@ -461,8 +466,11 @@ PX4IO::PX4IO(device::Device *interface) :
 	_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
 	_battery_amp_bias(0),
 	_battery_mamphour_total(0),
-	_battery_last_timestamp(0),
-	_dsm_vcc_ctl(false)
+	_battery_last_timestamp(0)
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+	,_dsm_vcc_ctl(false)
+#endif
+
 {
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
@@ -855,7 +863,7 @@ PX4IO::task_main()
 
 				// See if bind parameter has been set, and reset it to -1
 				param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val);
-				if (dsm_bind_val >= 0) {
+				if (dsm_bind_val > -1) {
 					dsm_bind_ioctl(dsm_bind_val);
 					dsm_bind_val = -1;
 					param_set(dsm_bind_param, &dsm_bind_val);
@@ -1169,7 +1177,7 @@ PX4IO::dsm_bind_ioctl(int dsmMode)
 {
 	if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
 		/* 0: dsm2, 1:dsmx */
-		if ((dsmMode >= 0) && (dsmMode <= 1)) {
+		if ((dsmMode == 0) || (dsmMode == 1)) {
 			mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%c rx", (dsmMode == 0) ? '2' : 'x');
 			ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : DSMX_BIND_PULSES);
 		} else {
@@ -1638,11 +1646,19 @@ PX4IO::print_status()
 		((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)   ? " FAILSAFE_CUSTOM" : ""),
 		((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)   ? " INAIR_RESTART_OK" : ""),
 		((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)  ? " ALWAYS_PWM_ENABLE" : ""));
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE),
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS));
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+	printf("rates 0x%04x default %u alt %u\n",
+		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
+		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
+		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE));
+#endif
 	printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG));
 	printf("controls");
 	for (unsigned i = 0; i < _max_controls; i++)
@@ -1783,36 +1799,58 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 	}
 
 	case GPIO_RESET: {
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 		uint32_t bits = (1 << _max_relays) - 1;
 		/* don't touch relay1 if it's controlling RX vcc */
 		if (_dsm_vcc_ctl)
 			bits &= ~PX4IO_P_SETUP_RELAYS_POWER1;
 		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0);
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		ret = -EINVAL;
+#endif
 		break;
 	}
 
 	case GPIO_SET:
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 		arg &= ((1 << _max_relays) - 1);
 		/* don't touch relay1 if it's controlling RX vcc */
-		if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1))
+		if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) {
 			ret = -EINVAL;
-		else
-			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
+			break;
+		}
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		ret = -EINVAL;
+#endif
 		break;
 
 	case GPIO_CLEAR:
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 		arg &= ((1 << _max_relays) - 1);
 		/* don't touch relay1 if it's controlling RX vcc */
-		if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1))
+		if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) {
 			ret = -EINVAL;
-		else
+			break;
+		}
 			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0);
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		ret = -EINVAL;
+#endif
 		break;
 
 	case GPIO_GET:
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 		*(uint32_t *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS);
 		if (*(uint32_t *)arg == _io_reg_get_error)
 			ret = -EIO;
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		ret = -EINVAL;
+#endif
 		break;
 
 	case MIXERIOCGETOUTPUTCOUNT:
@@ -1990,6 +2028,7 @@ start(int argc, char *argv[])
 		}
 	}
 
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	int dsm_vcc_ctl;
 
 	if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) {
@@ -1998,6 +2037,7 @@ start(int argc, char *argv[])
 			g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0);
 		}
 	}
+#endif
 	exit(0);
 }
 
@@ -2037,8 +2077,10 @@ bind(int argc, char *argv[])
 	if (g_dev == nullptr)
 		errx(1, "px4io must be started first");
 
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	if (!g_dev->get_dsm_vcc_ctl())
 		errx(1, "DSM bind feature not enabled");
+#endif
 
 	if (argc < 3)
 		errx(0, "needs argument, use dsm2 or dsmx");
@@ -2049,9 +2091,12 @@ bind(int argc, char *argv[])
 		pulses = DSMX_BIND_PULSES;
 	else 
 		errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]);
+	if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) 
+		errx(1, "system must not be armed");
 
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1.");
-
+#endif
 	g_dev->ioctl(nullptr, DSM_BIND_START, pulses);
 
 	exit(0);
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
index 206e27db5..fd6bca62a 100644
--- a/src/modules/px4iofirmware/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
@@ -243,28 +243,35 @@ dsm_init(const char *device)
 void
 dsm_bind(uint16_t cmd, int pulses)
 {
+#if !(defined(CONFIG_ARCH_BOARD_PX4IO_V1) || defined(CONFIG_ARCH_BOARD_PX4IO_V2))
+	#warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM
+#else
 	const uint32_t usart1RxAsOutp =
 		GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10;
 
 	if (dsm_fd < 0)
 		return;
 
-#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
-	// XXX implement
-	#warning DSM BIND NOT IMPLEMENTED ON PX4IO V2
-#else
 	switch (cmd) {
 
 	case dsm_bind_power_down:
 
 		/*power down DSM satellite*/
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 		POWER_RELAY1(0);
+#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */
+		POWER_SPEKTRUM(0);
+#endif
 		break;
 
 	case dsm_bind_power_up:
 
 		/*power up DSM satellite*/
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 		POWER_RELAY1(1);
+#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */
+		POWER_SPEKTRUM(1);
+#endif
 		dsm_guess_format(true);
 		break;
 
@@ -387,8 +394,10 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 		values[channel] = value;
 	}
 
-	if (dsm_channel_shift == 11)
+	if (dsm_channel_shift == 11) {
+		/* Set the 11-bit data indicator */
 		*num_values |= 0x8000;
+	}
 
 	/*
 	 * XXX Note that we may be in failsafe here; we need to work out how to detect that.
@@ -412,7 +421,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
  * Upon receiving a full dsm frame we attempt to decode it.
  *
  * @param[out] values pointer to per channel array of decoded values
- * @param[out] num_values pointer to number of raw channel values returned
+ * @param[out] num_values pointer to number of raw channel values returned, high order bit 0:10 bit data, 1:11 bit data
  * @return true=decoded raw channel values updated, false=no update
  */
 bool
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index f5fa0ba75..0e2cd1689 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -165,11 +165,13 @@
 #define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
 #define PX4IO_P_SETUP_PWM_ALTRATE		4	/* 'high' PWM frame output rate in Hz */
 
+#if defined(CONFIG_ARCH_BOARD_PX4IO_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 #define PX4IO_P_SETUP_RELAYS			5	/* bitmask of relay/switch outputs, 0 = off, 1 = on */
 #define PX4IO_P_SETUP_RELAYS_POWER1		(1<<0)	/* hardware rev [1] power relay 1 */
 #define PX4IO_P_SETUP_RELAYS_POWER2		(1<<1)	/* hardware rev [1] power relay 2 */
 #define PX4IO_P_SETUP_RELAYS_ACC1		(1<<2)	/* hardware rev [1] accessory power 1 */
 #define PX4IO_P_SETUP_RELAYS_ACC2		(1<<3)	/* hardware rev [1] accessory power 2 */
+#endif
 
 #define PX4IO_P_SETUP_VBATT_SCALE		6	/* hardware rev [1] battery voltage correction factor (float) */
 #define PX4IO_P_SETUP_VSERVO_SCALE		6	/* hardware rev [2] servo voltage correction factor (float) */
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 11f4d053d..66c4ca906 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -100,7 +100,9 @@ extern uint16_t			r_page_servo_idle[];	/* PX4IO_PAGE_IDLE_PWM */
 #define r_setup_pwm_rates	r_page_setup[PX4IO_P_SETUP_PWM_RATES]
 #define r_setup_pwm_defaultrate	r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE]
 #define r_setup_pwm_altrate	r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE]
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 #define r_setup_relays		r_page_setup[PX4IO_P_SETUP_RELAYS]
+#endif
 
 #define r_control_values	(&r_page_controls[0])
 
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 9c95fd1c5..8cb21e54f 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -145,7 +145,9 @@ volatile uint16_t	r_page_setup[] =
 	[PX4IO_P_SETUP_PWM_RATES]		= 0,
 	[PX4IO_P_SETUP_PWM_DEFAULTRATE]		= 50,
 	[PX4IO_P_SETUP_PWM_ALTRATE]		= 200,
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 	[PX4IO_P_SETUP_RELAYS]			= 0,
+#endif
 #ifdef ADC_VSERVO
 	[PX4IO_P_SETUP_VSERVO_SCALE]		= 10000,
 #else
@@ -462,22 +464,16 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value);
 			break;
 
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 		case PX4IO_P_SETUP_RELAYS:
 			value &= PX4IO_P_SETUP_RELAYS_VALID;
 			r_setup_relays = value;
-#ifdef POWER_RELAY1
 			POWER_RELAY1((value & PX4IO_P_SETUP_RELAYS_POWER1) ? 1 : 0);
-#endif
-#ifdef POWER_RELAY2
 			POWER_RELAY2((value & PX4IO_P_SETUP_RELAYS_POWER2) ? 1 : 0);
-#endif
-#ifdef POWER_ACC1
 			POWER_ACC1((value & PX4IO_P_SETUP_RELAYS_ACC1) ? 1 : 0);
-#endif
-#ifdef POWER_ACC2
 			POWER_ACC2((value & PX4IO_P_SETUP_RELAYS_ACC2) ? 1 : 0);
-#endif
 			break;
+#endif
 
 		case PX4IO_P_SETUP_VBATT_SCALE:
 			r_page_setup[PX4IO_P_SETUP_VBATT_SCALE] = value;
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index ed4fcdd46..4d719e0e1 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -164,8 +164,9 @@ PARAM_DEFINE_FLOAT(RC15_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC15_REV, 1.0f);
 PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f);
 
-
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
+#endif
 PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */
 
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2