Support DSM bind via QGroundControl

5 files changed, 26 insertions, 33 deletions

diff --git a/Documentation/dsm_bind.odt b/Documentation/dsm_bind.odt
index 66ea1f1be..587a38883 100644
--- a/Documentation/dsm_bind.odt
+++ b/Documentation/dsm_bind.odt
diff --git a/Documentation/dsm_bind.pdf b/Documentation/dsm_bind.pdf
index e62d1ed83..76155569e 100644
--- a/Documentation/dsm_bind.pdf
+++ b/Documentation/dsm_bind.pdf
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 5089ce3c7..cb61d4aae 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -189,6 +189,12 @@ private:
 	bool			_dsm_vcc_ctl;
 
 	/**
+	 * System armed
+	 */
+
+	bool			_system_armed;
+
+	/**
 	 * Trampoline to the worker task
 	 */
 	static void		task_main_trampoline(int argc, char *argv[]);
@@ -355,7 +361,8 @@ PX4IO::PX4IO() :
 	_battery_amp_bias(0),
 	_battery_mamphour_total(0),
 	_battery_last_timestamp(0),
-	_dsm_vcc_ctl(false)
+	_dsm_vcc_ctl(false),
+	_system_armed(false)
 {
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
@@ -672,6 +679,17 @@ PX4IO::task_main()
 			 */
 			if (fds[3].revents & POLLIN) {
 				parameter_update_s pupdate;
+				int32_t dsm_bind_param;
+
+				// See if bind parameter has been set, and reset it to 0
+				param_get(param_find("RC_DSM_BIND"), &dsm_bind_param);
+				if (dsm_bind_param) {
+					if (((dsm_bind_param == 1) || (dsm_bind_param == 2)) && !_system_armed) {
+						ioctl(nullptr, DSM_BIND_START, dsm_bind_param == 1 ? 3 : 7);
+					}
+					dsm_bind_param = 0;
+					param_set(param_find("RC_DSM_BIND"), &dsm_bind_param);
+				}
 
 				/* copy to reset the notification */
 				orb_copy(ORB_ID(parameter_update), _t_param, &pupdate);
@@ -737,6 +755,8 @@ PX4IO::io_set_arming_state()
 	uint16_t set = 0;
 	uint16_t clear = 0;
 
+	_system_armed = vstatus.flag_system_armed;
+
 	if (armed.armed && !armed.lockdown) {
 		set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 	} else {
@@ -1448,6 +1468,8 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
 		usleep(50000);
 		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4));
+		usleep(50000);
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
 		break;
 
 	case DSM_BIND_STOP:
@@ -1695,30 +1717,12 @@ bind(int argc, char *argv[])
 	else 
 		errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]);
 
-	/* 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.");
 
 	g_dev->ioctl(nullptr, DSM_BIND_START, pulses);
 
-	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");
-				g_dev->ioctl(nullptr, DSM_BIND_STOP, 0);
-				g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0);
-				close(console);
-	            exit(0);
-            }
-		}
-	}
+	exit(0);
+
 }
 
 void
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 133cda8d6..bd431c9eb 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -156,6 +156,7 @@ 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 */
+PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */
 
 /* 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 f7b41b120..42268b971 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -257,8 +257,6 @@ private:
 
 		float battery_voltage_scaling;
 
-		int   rc_rl1_DSM_VCC_control;
-
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -308,8 +306,6 @@ private:
 
 		param_t battery_voltage_scaling;
 
-		param_t rc_rl1_DSM_VCC_control;
-
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -544,9 +540,6 @@ 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();
 }
@@ -738,11 +731,6 @@ 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;
 }