Merge branch 'master' of github.com:PX4/Firmware into px4io-i2c

5 files changed, 160 insertions, 65 deletions

diff --git a/ROMFS/Makefile b/ROMFS/Makefile
index 3b024de06..d827fa491 100644
--- a/ROMFS/Makefile
+++ b/ROMFS/Makefile
@@ -23,7 +23,8 @@ ROMFS_FSSPEC	:= $(SRCROOT)/scripts/rcS~init.d/rcS \
 		   $(SRCROOT)/scripts/rc.PX4IOAR~init.d/rc.PX4IOAR \
 		   $(SRCROOT)/scripts/rc.FMU_quad_x~init.d/rc.FMU_quad_x \
 		   $(SRCROOT)/mixers/FMU_pass.mix~mixers/FMU_pass.mix \
-		   $(SRCROOT)/mixers/FMU_delta.mix~mixers/FMU_delta.mix \
+		   $(SRCROOT)/mixers/FMU_Q.mix~mixers/FMU_Q.mix \
+		   $(SRCROOT)/mixers/FMU_X5.mix~mixers/FMU_X5.mix \
 		   $(SRCROOT)/mixers/FMU_AERT.mix~mixers/FMU_AERT.mix \
 		   $(SRCROOT)/mixers/FMU_AET.mix~mixers/FMU_AET.mix \
 		   $(SRCROOT)/mixers/FMU_RET.mix~mixers/FMU_ERT.mix \
diff --git a/ROMFS/mixers/FMU_Q.mix b/ROMFS/mixers/FMU_Q.mix
new file mode 100644
index 000000000..a35d299fd
--- /dev/null
+++ b/ROMFS/mixers/FMU_Q.mix
@@ -0,0 +1,52 @@
+Delta-wing mixer for PX4FMU
+===========================
+
+Designed for Bormatec Camflyer Q
+
+This file defines mixers suitable for controlling a delta wing aircraft using
+PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU
+servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is
+assumed to be unused.
+
+Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
+(roll), 1 (pitch) and 3 (thrust).
+
+See the README for more information on the scaler format.
+
+Elevon mixers
+-------------
+Three scalers total (output, roll, pitch).
+
+On the assumption that the two elevon servos are physically reversed, the pitch
+input is inverted between the two servos.
+
+The scaling factor for roll inputs is adjusted to implement differential travel
+for the elevons.
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 0 0  -5000  -8000      0 -10000  10000
+S: 0 1  -8000  -8000      0 -10000  10000
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 0 0  -8000  -5000      0 -10000  10000
+S: 0 1  8000   8000       0 -10000  10000
+
+Output 2
+--------
+This mixer is empty.
+
+Z:
+
+Motor speed mixer
+-----------------
+Two scalers total (output, thrust).
+
+This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
+range.  Inputs below zero are treated as zero.
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 3      0  20000 -10000 -10000  10000
+
diff --git a/ROMFS/mixers/FMU_delta.mix b/ROMFS/mixers/FMU_X5.mix
index 981466704..981466704 100644
--- a/ROMFS/mixers/FMU_delta.mix
+++ b/ROMFS/mixers/FMU_X5.mix
diff --git a/apps/drivers/px4io/uploader.cpp b/apps/drivers/px4io/uploader.cpp
index 2de33f410..abf59216a 100644
--- a/apps/drivers/px4io/uploader.cpp
+++ b/apps/drivers/px4io/uploader.cpp
@@ -48,6 +48,7 @@
 #include <unistd.h>
 #include <fcntl.h>
 #include <poll.h>
+#include <sys/stat.h>
 
 #include "uploader.h"
 
@@ -109,6 +110,8 @@ int
 PX4IO_Uploader::upload(const char *filenames[])
 {
 	int	ret;
+	const char *filename = NULL;
+	size_t fw_size;
 
 	_io_fd = open("/dev/ttyS2", O_RDWR);
 
@@ -135,9 +138,22 @@ PX4IO_Uploader::upload(const char *filenames[])
 		}
 
 		log("using firmware from %s", filenames[i]);
+		filename = filenames[i];
 		break;
 	}
 
+	if (filename == NULL) {
+		log("no firmware found");
+		return -ENOENT;
+	}
+
+	struct stat st;
+	if (stat(filename, &st) != 0) {
+		log("Failed to stat %s - %d\n", filename, (int)errno);
+		return -errno;
+	}
+	fw_size = st.st_size;
+
 	if (_fw_fd == -1)
 		return -ENOENT;
 
@@ -172,7 +188,7 @@ PX4IO_Uploader::upload(const char *filenames[])
 			continue;
 		}
 
-		ret = program();
+		ret = program(fw_size);
 
 		if (ret != OK) {
 			log("program failed");
@@ -180,9 +196,9 @@ PX4IO_Uploader::upload(const char *filenames[])
 		}
 
 		if (bl_rev <= 2)
-			ret = verify_rev2();
+			ret = verify_rev2(fw_size);
 		else if(bl_rev == 3) {
-			ret = verify_rev3();
+			ret = verify_rev3(fw_size);
 		}
 
 		if (ret != OK) {
@@ -219,7 +235,7 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
 	int ret = ::poll(&fds[0], 1, timeout);
 
 	if (ret < 1) {
-		//log("poll timeout %d", ret);
+		log("poll timeout %d", ret);
 		return -ETIMEDOUT;
 	}
 
@@ -232,7 +248,7 @@ int
 PX4IO_Uploader::recv(uint8_t *p, unsigned count)
 {
 	while (count--) {
-		int ret = recv(*p++);
+		int ret = recv(*p++, 5000);
 
 		if (ret != OK)
 			return ret;
@@ -248,7 +264,7 @@ PX4IO_Uploader::drain()
 	int ret;
 
 	do {
-		ret = recv(c, 250);
+		ret = recv(c, 1000);
 
 		if (ret == OK) {
 			//log("discard 0x%02x", c);
@@ -343,24 +359,55 @@ PX4IO_Uploader::erase()
 	return get_sync(10000);		/* allow 10s timeout */
 }
 
+
+static int read_with_retry(int fd, void *buf, size_t n)
+{
+	int ret;
+	uint8_t retries = 0;
+	do {
+		ret = read(fd, buf, n);
+	} while (ret == -1 && retries++ < 100);
+	if (retries != 0) {
+		printf("read of %u bytes needed %u retries\n",
+		       (unsigned)n,
+		       (unsigned)retries);
+	}
+	return ret;
+}
+
 int
-PX4IO_Uploader::program()
+PX4IO_Uploader::program(size_t fw_size)
 {
 	uint8_t	file_buf[PROG_MULTI_MAX];
 	ssize_t count;
 	int ret;
+	size_t sent = 0;
 
-	log("program...");
-	lseek(_fw_fd, 0, SEEK_SET);
+	log("programming %u bytes...", (unsigned)fw_size);
+
+	ret = lseek(_fw_fd, 0, SEEK_SET);
 
-	while (true) {
+	while (sent < fw_size) {
 		/* get more bytes to program */
-		//log("  %d", (int)lseek(_fw_fd, 0, SEEK_CUR));
-		count = read(_fw_fd, file_buf, sizeof(file_buf));
+		size_t n = fw_size - sent;
+		if (n > sizeof(file_buf)) {
+			n = sizeof(file_buf);
+		}
+		count = read_with_retry(_fw_fd, file_buf, n);
+
+		if (count != (ssize_t)n) {
+			log("firmware read of %u bytes at %u failed -> %d errno %d", 
+			    (unsigned)n,
+			    (unsigned)sent,
+			    (int)count,
+			    (int)errno);
+		}
 
 		if (count == 0)
 			return OK;
 
+		sent += count;
+
 		if (count < 0)
 			return -errno;
 
@@ -376,14 +423,16 @@ PX4IO_Uploader::program()
 		if (ret != OK)
 			return ret;
 	}
+	return OK;
 }
 
 int
-PX4IO_Uploader::verify_rev2()
+PX4IO_Uploader::verify_rev2(size_t fw_size)
 {
-	uint8_t	file_buf[PROG_MULTI_MAX];
+	uint8_t	file_buf[4];
 	ssize_t count;
 	int ret;
+	size_t sent = 0;
 
 	log("verify...");
 	lseek(_fw_fd, 0, SEEK_SET);
@@ -395,14 +444,27 @@ PX4IO_Uploader::verify_rev2()
 	if (ret != OK)
 		return ret;
 
-	while (true) {
+	while (sent < fw_size) {
 		/* get more bytes to verify */
-		int base = (int)lseek(_fw_fd, 0, SEEK_CUR);
-		count = read(_fw_fd, file_buf, sizeof(file_buf));
+		size_t n = fw_size - sent;
+		if (n > sizeof(file_buf)) {
+			n = sizeof(file_buf);
+		}
+		count = read_with_retry(_fw_fd, file_buf, n);
+
+		if (count != (ssize_t)n) {
+			log("firmware read of %u bytes at %u failed -> %d errno %d", 
+			    (unsigned)n,
+			    (unsigned)sent,
+			    (int)count,
+			    (int)errno);
+		}
 
 		if (count == 0)
 			break;
 
+		sent += count;
+
 		if (count < 0)
 			return -errno;
 
@@ -415,15 +477,15 @@ PX4IO_Uploader::verify_rev2()
 		for (ssize_t i = 0; i < count; i++) {
 			uint8_t c;
 
-			ret = recv(c);
+			ret = recv(c, 5000);
 
 			if (ret != OK) {
-				log("%d: got %d waiting for bytes", base + i, ret);
+				log("%d: got %d waiting for bytes", sent + i, ret);
 				return ret;
 			}
 
 			if (c != file_buf[i]) {
-				log("%d: got 0x%02x expected 0x%02x", base + i, c, file_buf[i]);
+				log("%d: got 0x%02x expected 0x%02x", sent + i, c, file_buf[i]);
 				return -EINVAL;
 			}
 		}
@@ -440,21 +502,21 @@ PX4IO_Uploader::verify_rev2()
 }
 
 int
-PX4IO_Uploader::verify_rev3()
+PX4IO_Uploader::verify_rev3(size_t fw_size_local)
 {
 	int ret;
 	uint8_t	file_buf[4];
 	ssize_t count;
 	uint32_t sum = 0;
 	uint32_t bytes_read = 0;
-	uint32_t fw_size = 0;
 	uint32_t crc = 0;
+	uint32_t fw_size_remote;
 	uint8_t fill_blank = 0xff;
 
 	log("verify...");
 	lseek(_fw_fd, 0, SEEK_SET);
 
-	ret = get_info(INFO_FLASH_SIZE, fw_size);
+	ret = get_info(INFO_FLASH_SIZE, fw_size_remote);
 	send(PROTO_EOC);
 
 	if (ret != OK) {
@@ -463,9 +525,20 @@ PX4IO_Uploader::verify_rev3()
 	}
 
 	/* read through the firmware file again and calculate the checksum*/
-	while (true) {
-		lseek(_fw_fd, 0, SEEK_CUR);
-		count = read(_fw_fd, file_buf, sizeof(file_buf));
+	while (bytes_read < fw_size_local) {
+		size_t n = fw_size_local - bytes_read;
+		if (n > sizeof(file_buf)) {
+			n = sizeof(file_buf);
+		}
+		count = read_with_retry(_fw_fd, file_buf, n);
+
+		if (count != (ssize_t)n) {
+			log("firmware read of %u bytes at %u failed -> %d errno %d", 
+			    (unsigned)n,
+			    (unsigned)bytes_read,
+			    (int)count,
+			    (int)errno);
+		}
 
 		/* set the rest to ff */
 		if (count == 0) {
@@ -482,7 +555,7 @@ PX4IO_Uploader::verify_rev3()
 	}
 
 	/* fill the rest with 0xff */
-	while (bytes_read < fw_size) {
+	while (bytes_read < fw_size_remote) {
 		sum = crc32(&fill_blank, sizeof(fill_blank), sum);
 		bytes_read += sizeof(fill_blank);
 	}
@@ -516,36 +589,6 @@ PX4IO_Uploader::reboot()
 	return OK;
 }
 
-int
-PX4IO_Uploader::compare(bool &identical)
-{
-	uint32_t file_vectors[15];
-	uint32_t fw_vectors[15];
-	int ret;
-
-	lseek(_fw_fd, 0, SEEK_SET);
-	ret = read(_fw_fd, &file_vectors[0], sizeof(file_vectors));
-
-	send(PROTO_CHIP_VERIFY);
-	send(PROTO_EOC);
-	ret = get_sync();
-
-	if (ret != OK)
-		return ret;
-
-	send(PROTO_READ_MULTI);
-	send(sizeof(fw_vectors));
-	send(PROTO_EOC);
-	ret = recv((uint8_t *)&fw_vectors[0], sizeof(fw_vectors));
-
-	if (ret != OK)
-		return ret;
-
-	identical = (memcmp(&file_vectors[0], &fw_vectors[0], sizeof(file_vectors))) ? true : false;
-
-	return OK;
-}
-
 void
 PX4IO_Uploader::log(const char *fmt, ...)
 {
@@ -557,4 +600,4 @@ PX4IO_Uploader::log(const char *fmt, ...)
 	va_end(ap);
 	printf("\n");
 	fflush(stdout);
-}
\ No newline at end of file
+}
diff --git a/apps/drivers/px4io/uploader.h b/apps/drivers/px4io/uploader.h
index b8a3a2794..915ee9259 100644
--- a/apps/drivers/px4io/uploader.h
+++ b/apps/drivers/px4io/uploader.h
@@ -85,7 +85,7 @@ private:
 
 	void			log(const char *fmt, ...);
 
-	int			recv(uint8_t &c, unsigned timeout = 1000);
+	int			recv(uint8_t &c, unsigned timeout);
 	int			recv(uint8_t *p, unsigned count);
 	void			drain();
 	int			send(uint8_t c);
@@ -94,11 +94,10 @@ private:
 	int			sync();
 	int			get_info(int param, uint32_t &val);
 	int			erase();
-	int			program();
-	int			verify_rev2();
-	int			verify_rev3();
+	int			program(size_t fw_size);
+	int			verify_rev2(size_t fw_size);
+	int			verify_rev3(size_t fw_size);
 	int			reboot();
-	int			compare(bool &identical);
 };
 
-#endif
\ No newline at end of file
+#endif