Merge branch '#61-px4io-spektrum-decoder' into fixedwing_outdoor

2 files changed, 51 insertions, 71 deletions

diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 2385385a2..efd396fa6 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -53,6 +53,7 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <unistd.h>
+#include <termios.h>
 #include <fcntl.h>
 
 #include <arch/board/board.h>
@@ -88,15 +89,13 @@ 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;
 
 	int			_serial_fd;	///< serial interface to PX4IO
 	hx_stream_t		_io_stream;	///< HX protocol stream
 
-	int			_task;		///< worker task
+	volatile int		_task;		///< worker task
 	volatile bool		_task_should_exit;
 	volatile bool		_connected;	///< true once we have received a valid frame
 
@@ -125,8 +124,6 @@ private:
 	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
 	 */
@@ -195,8 +192,7 @@ PX4IO::PX4IO() :
 	_primary_pwm_device(false),
 	_switch_armed(false),
 	_send_needed(false),
-	_config_needed(false),
-	_rx_mode(RX_MODE_PPM_ONLY)
+	_config_needed(false)
 {
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
@@ -206,33 +202,17 @@ PX4IO::PX4IO() :
 
 PX4IO::~PX4IO()
 {
-	if (_task != -1) {
-		/* tell the task we want it to go away */
-		_task_should_exit = true;
-
-		/* spin waiting for the thread to stop */
-		unsigned i = 10;
-
-		do {
-			/* wait 50ms - it should wake every 100ms or so worst-case */
-			usleep(50000);
-
-			/* if we have given up, kill it */
-			if (--i == 0) {
-				task_delete(_task);
-				break;
-			}
+	/* tell the task we want it to go away */
+	_task_should_exit = true;
 
-		} while (_task != -1);
+	/* spin waiting for the task to stop */
+	for (unsigned i = 0; (i < 10) && (_task != -1); i++) {
+		/* give it another 100ms */
+		usleep(100000);
 	}
-
-	/* clean up the alternate device node */
-	if (_primary_pwm_device)
-		unregister_driver(PWM_OUTPUT_DEVICE_PATH);
-
-	/* kill the HX stream */
-	if (_io_stream != nullptr)
-		hx_stream_free(_io_stream);
+	/* well, kill it anyway, though this will probably crash */
+	if (_task != -1)
+		task_delete(_task);
 
 	g_dev = nullptr;
 }
@@ -300,6 +280,16 @@ PX4IO::task_main()
 		goto out;
 	}
 
+	/* 115200bps, no parity, one stop bit */
+	{
+		struct termios t;
+
+		tcgetattr(_serial_fd, &t);
+		cfsetspeed(&t, 115200);
+		t.c_cflag &= ~(CSTOPB | PARENB);
+		tcsetattr(_serial_fd, TCSANOW, &t);
+	}
+
 	/* protocol stream */
 	_io_stream = hx_stream_init(_serial_fd, &PX4IO::rx_callback_trampoline, this);
 	if (_io_stream == nullptr) {
@@ -358,7 +348,6 @@ PX4IO::task_main()
 		/* this would be bad... */
 		if (ret < 0) {
 			log("poll error %d", errno);
-			usleep(1000000);
 			continue;
 		}
 
@@ -418,8 +407,16 @@ PX4IO::task_main()
 	}
 
 out:
+	debug("exiting");
+
+	/* kill the HX stream */
 	if (_io_stream != nullptr)
 		hx_stream_free(_io_stream);
+	::close(_serial_fd);
+
+	/* clean up the alternate device node */
+	if (_primary_pwm_device)
+		unregister_driver(PWM_OUTPUT_DEVICE_PATH);
 
 	/* tell the dtor that we are exiting */
 	_task = -1;
@@ -480,15 +477,17 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
 	}
 	_connected = true;
 
-	/* publish raw rc channel values from IO */
-	_input_rc.timestamp = hrt_absolute_time();
-	_input_rc.channel_count = rep->channel_count;
-	for (int i = 0; i < rep->channel_count; i++)
-	{
-		_input_rc.values[i] = rep->rc_channel[i];
-	}
+	/* publish raw rc channel values from IO if valid channels are present */
+	if (rep->channel_count > 0) {
+		_input_rc.timestamp = hrt_absolute_time();
+		_input_rc.channel_count = rep->channel_count;
+		for (int i = 0; i < rep->channel_count; i++)
+		{
+			_input_rc.values[i] = rep->rc_channel[i];
+		}
 
-	orb_publish(ORB_ID(input_rc), _to_input_rc, &_input_rc);
+		orb_publish(ORB_ID(input_rc), _to_input_rc, &_input_rc);
+	}
 
 	/* remember the latched arming switch state */
 	_switch_armed = rep->armed;
@@ -531,7 +530,6 @@ PX4IO::config_send()
 	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)
@@ -651,15 +649,6 @@ 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
@@ -761,25 +750,14 @@ px4io_main(int argc, char *argv[])
 		return ret;
 	}
 
-	if (!strcmp(argv[1], "rx_dsm_10bit")) {
-		if (g_dev == nullptr)
-			errx(1, "not started");
-		g_dev->set_rx_mode(RX_MODE_DSM_10BIT);
-	}
-	if (!strcmp(argv[1], "rx_dsm_11bit")) {
-		if (g_dev == nullptr)
-			errx(1, "not started");
-		g_dev->set_rx_mode(RX_MODE_DSM_11BIT);
-	}
-	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], "rx_dsm") ||
+	    !strcmp(argv[1], "rx_dsm_10bit") ||
+	    !strcmp(argv[1], "rx_dsm_11bit") ||
+	    !strcmp(argv[1], "rx_sbus") ||
+	    !strcmp(argv[1], "rx_ppm"))
+		errx(0, "receiver type is automatically detected, option '%s' is deprecated", argv[1]);
 	if (!strcmp(argv[1], "test"))
 		test();
 
-
-	errx(1, "need a command, try 'start', 'test', 'rx_dsm_10bit', 'rx_dsm_11bit', 'rx_sbus' or 'update'");
+	errx(1, "need a command, try 'start', 'test', 'rx_ppm', 'rx_dsm', 'rx_sbus' or 'update'");
 }
diff --git a/apps/drivers/px4io/uploader.cpp b/apps/drivers/px4io/uploader.cpp
index 5669aeb01..8b354ff60 100644
--- a/apps/drivers/px4io/uploader.cpp
+++ b/apps/drivers/px4io/uploader.cpp
@@ -189,8 +189,10 @@ PX4IO_Uploader::drain()
 	int ret;
 
 	do {
-		ret = recv(c, 10);
-		//log("discard 0x%02x", c);
+		ret = recv(c, 250);
+		if (ret == OK) {
+			//log("discard 0x%02x", c);
+		}
 	} while (ret == OK);
 }