Merge remote-tracking branch 'upstream/master' into attitude_filter_improvement

5 files changed, 151 insertions, 76 deletions

diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp
index bc5c74de1..fc929284c 100644
--- a/apps/drivers/blinkm/blinkm.cpp
+++ b/apps/drivers/blinkm/blinkm.cpp
@@ -126,7 +126,7 @@ static const int LED_NOBLINK = 0;
 class BlinkM : public device::I2C
 {
 public:
-	BlinkM(int bus);
+	BlinkM(int bus, int blinkm);
 	~BlinkM();
 
 
@@ -245,8 +245,8 @@ const char *BlinkM::script_names[] = {
 
 extern "C" __EXPORT int blinkm_main(int argc, char *argv[]);
 
-BlinkM::BlinkM(int bus) :
-	I2C("blinkm", BLINKM_DEVICE_PATH, bus, 0x09, 100000),
+BlinkM::BlinkM(int bus, int blinkm) :
+	I2C("blinkm", BLINKM_DEVICE_PATH, bus, blinkm, 100000),
 	led_color_1(LED_OFF),
 	led_color_2(LED_OFF),
 	led_color_3(LED_OFF),
@@ -830,14 +830,42 @@ BlinkM::get_firmware_version(uint8_t version[2])
 	return transfer(&msg, sizeof(msg), version, sizeof(version));
 }
 
+void blinkm_usage() {
+	fprintf(stderr, "missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}\n");
+	fprintf(stderr, "options:\n");
+	fprintf(stderr, "\t-b --bus i2cbus (3)\n");
+	fprintf(stderr, "\t-a --blinkmaddr blinkmaddr (9)\n");
+}
+
 int
 blinkm_main(int argc, char *argv[])
 {
+
+	int i2cdevice = 3;
+	int blinkmadr = 9;
+
+	int x;
+
+	for (x = 1; x < argc; x++) {
+		if (strcmp(argv[x], "-b") == 0 || strcmp(argv[x], "--bus") == 0) {
+			if (argc > x + 1) {
+				i2cdevice = atoi(argv[x + 1]);
+			}
+		}
+
+		if (strcmp(argv[x], "-a") == 0 || strcmp(argv[x], "--blinkmaddr") == 0) {
+			if (argc > x + 1) {
+				blinkmadr = atoi(argv[x + 1]);
+			}
+		}
+
+	}
+
 	if (!strcmp(argv[1], "start")) {
 		if (g_blinkm != nullptr)
 			errx(1, "already started");
 
-		g_blinkm = new BlinkM(3);
+		g_blinkm = new BlinkM(i2cdevice, blinkmadr);
 
 		if (g_blinkm == nullptr)
 			errx(1, "new failed");
@@ -852,8 +880,11 @@ blinkm_main(int argc, char *argv[])
 	}
 
 
-	if (g_blinkm == nullptr)
-		errx(1, "not started");
+	if (g_blinkm == nullptr) {
+		fprintf(stderr, "not started\n");
+		blinkm_usage();
+		exit(0);
+	}
 
 	if (!strcmp(argv[1], "systemstate")) {
 		g_blinkm->setMode(1);
@@ -882,5 +913,6 @@ blinkm_main(int argc, char *argv[])
 	if (ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK)
 		exit(0);
 
-	errx(1, "missing command, try 'start', 'systemstate', 'ledoff', 'list' or a script name.");
+	blinkm_usage();
+	exit(0);
 }
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
diff --git a/apps/hott_telemetry/hott_telemetry_main.c b/apps/hott_telemetry/hott_telemetry_main.c
index d67ab06a9..31c9247aa 100644
--- a/apps/hott_telemetry/hott_telemetry_main.c
+++ b/apps/hott_telemetry/hott_telemetry_main.c
@@ -51,6 +51,7 @@
 #include <stdio.h>
 #include <string.h>
 #include <termios.h>
+#include <sys/ioctl.h>
 #include <unistd.h>
 #include <systemlib/systemlib.h>
 
diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c
index ccca8bdc6..510d2f3e4 100644
--- a/apps/mavlink/mavlink_receiver.c
+++ b/apps/mavlink/mavlink_receiver.c
@@ -586,12 +586,12 @@ receive_thread(void *arg)
 		struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } };
 
 		if (poll(fds, 1, timeout) > 0) {
-			/* non-blocking read */
-			size_t nread = read(uart_fd, buf, sizeof(buf));
-			ASSERT(nread > 0)
+			/* non-blocking read. read may return negative values */
+			ssize_t nread = read(uart_fd, buf, sizeof(buf));
 
-			for (size_t i = 0; i < nread; i++) {
-				if (mavlink_parse_char(chan, buf[i], &msg, &status)) { //parse the char
+			/* if read failed, this loop won't execute */
+			for (ssize_t i = 0; i < nread; i++) {
+				if (mavlink_parse_char(chan, buf[i], &msg, &status)) {
 					/* handle generic messages and commands */
 					handle_message(&msg);