Add PX4IOv2 support to the uploader.

3 files changed, 37 insertions, 4 deletions

diff --git a/src/drivers/boards/px4fmu/px4fmu_internal.h b/src/drivers/boards/px4fmu/px4fmu_internal.h
index 671a58f8f..5a73c10bf 100644
--- a/src/drivers/boards/px4fmu/px4fmu_internal.h
+++ b/src/drivers/boards/px4fmu/px4fmu_internal.h
@@ -58,6 +58,9 @@ __BEGIN_DECLS
  ****************************************************************************************************/
 /* Configuration ************************************************************************************/
 
+/* PX4IO connection configuration */
+#define PX4IO_SERIAL_DEVICE	"/dev/ttyS2"
+
 //#ifdef CONFIG_STM32_SPI2
 //#  error "SPI2 is not supported on this board"
 //#endif
diff --git a/src/drivers/boards/px4fmuv2/px4fmu_internal.h b/src/drivers/boards/px4fmuv2/px4fmu_internal.h
index 1698336b4..dd291b9b7 100644
--- a/src/drivers/boards/px4fmuv2/px4fmu_internal.h
+++ b/src/drivers/boards/px4fmuv2/px4fmu_internal.h
@@ -59,6 +59,7 @@ __BEGIN_DECLS
 /* Configuration ************************************************************************************/
 
 /* PX4IO connection configuration */
+#define PX4IO_SERIAL_DEVICE	"/dev/ttyS5"
 #define PX4IO_SERIAL_TX_GPIO	GPIO_USART6_TX
 #define PX4IO_SERIAL_RX_GPIO	GPIO_USART6_RX
 #define PX4IO_SERIAL_BASE	STM32_USART6_BASE	/* hardwired on the board */
diff --git a/src/drivers/px4io/uploader.cpp b/src/drivers/px4io/uploader.cpp
index 15524c3ae..67298af32 100644
--- a/src/drivers/px4io/uploader.cpp
+++ b/src/drivers/px4io/uploader.cpp
@@ -49,10 +49,21 @@
 #include <unistd.h>
 #include <fcntl.h>
 #include <poll.h>
+#include <termios.h>
 #include <sys/stat.h>
 
 #include "uploader.h"
 
+#ifdef CONFIG_ARCH_BOARD_PX4FMUV2
+#include <drivers/boards/px4fmuv2/px4fmu_internal.h>
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU
+#include <drivers/boards/px4fmu/px4fmu_internal.h>
+#endif
+
+// define for comms logging
+//#define UDEBUG
+
 static const uint32_t crctab[] =
 {
 	0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
@@ -114,13 +125,23 @@ PX4IO_Uploader::upload(const char *filenames[])
 	const char *filename = NULL;
 	size_t fw_size;
 
-	_io_fd = open("/dev/ttyS2", O_RDWR);
+#ifndef PX4IO_SERIAL_DEVICE
+#error Must define PX4IO_SERIAL_DEVICE in board configuration to support firmware upload
+#endif
+
+	_io_fd = open(PX4IO_SERIAL_DEVICE, O_RDWR);
 
 	if (_io_fd < 0) {
 		log("could not open interface");
 		return -errno;
 	}
 
+	/* adjust line speed to match bootloader */
+	struct termios t;
+	tcgetattr(_io_fd, &t);
+	cfsetspeed(&t, 115200);
+	tcsetattr(_io_fd, TCSANOW, &t);
+
 	/* look for the bootloader */
 	ret = sync();
 
@@ -251,12 +272,16 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
 	int ret = ::poll(&fds[0], 1, timeout);
 
 	if (ret < 1) {
+#ifdef UDEBUG
 		log("poll timeout %d", ret);
+#endif
 		return -ETIMEDOUT;
 	}
 
 	read(_io_fd, &c, 1);
-	//log("recv 0x%02x", c);
+#ifdef UDEBUG
+	log("recv 0x%02x", c);
+#endif
 	return OK;
 }
 
@@ -282,16 +307,20 @@ PX4IO_Uploader::drain()
 	do {
 		ret = recv(c, 1000);
 
+#ifdef UDEBUG
 		if (ret == OK) {
-			//log("discard 0x%02x", c);
+			log("discard 0x%02x", c);
 		}
+#endif
 	} while (ret == OK);
 }
 
 int
 PX4IO_Uploader::send(uint8_t c)
 {
-	//log("send 0x%02x", c);
+#ifdef UDEBUG
+	log("send 0x%02x", c);
+#endif
 	if (write(_io_fd, &c, 1) != 1)
 		return -errno;