4 files changed, 33 insertions, 40 deletions

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index c4abd07d2..6da03402e 100755
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -108,25 +108,17 @@ then
 	nshterm /dev/ttyACM0 &
 	
 	#
-	# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell)
+	# Upgrade PX4IO firmware
 	#
-	if [ -f /fs/microsd/px4io.bin ]
+	if px4io update
 	then
-		echo "PX4IO Firmware found. Checking Upgrade.."
-		if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.cur
+		if [ -d /fs/microsd ]
 		then
-			echo "No newer version, skipping upgrade."
-		else
-			echo "Loading /fs/microsd/px4io.bin"
-			if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io.log
-			then
-				cp /fs/microsd/px4io.bin /fs/microsd/px4io.cur
-				echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io.log
-			else
-				echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io.log
-				echo "Failed to upgrade px4io firmware - check if px4io is in bootloader mode"
-			fi
+			echo "Flashed PX4IO Firmware OK" > /fs/microsd/px4io.log
 		fi
+
+		# Allow IO to safely kick back to app
+		usleep 200000
 	fi
 	
 	#
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 15873f79b..392bc9f0a 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -2096,10 +2096,10 @@ px4io_main(int argc, char *argv[])
 			fn[1] = nullptr;
 
 		} else {
-			fn[0] = "/fs/microsd/px4io.bin";
-			fn[1] =	"/etc/px4io.bin";
-			fn[2] = "/fs/microsd/px4io2.bin";
-			fn[3] =	"/etc/px4io2.bin";
+			fn[0] = "/etc/extras/px4io-v2_default.bin";
+			fn[1] = "/etc/extras/px4io-v1_default.bin";
+			fn[2] =	"/fs/microsd/px4io.bin";
+			fn[3] =	"/fs/microsd/px4io2.bin";
 			fn[4] =	nullptr;
 		}
 
diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp
index c7ce60b5e..7db28ecad 100644
--- a/src/drivers/px4io/px4io_uploader.cpp
+++ b/src/drivers/px4io/px4io_uploader.cpp
@@ -82,6 +82,27 @@ PX4IO_Uploader::upload(const char *filenames[])
 #error Must define PX4IO_SERIAL_DEVICE in board configuration to support firmware upload
 #endif
 
+	/* allow an early abort and look for file first */
+	for (unsigned i = 0; filenames[i] != nullptr; i++) {
+		_fw_fd = open(filenames[i], O_RDONLY);
+
+		if (_fw_fd < 0) {
+			log("failed to open %s", filenames[i]);
+			continue;
+		}
+
+		log("using firmware from %s", filenames[i]);
+		filename = filenames[i];
+		break;
+	}
+
+	if (filename == NULL) {
+		log("no firmware found");
+		close(_io_fd);
+		_io_fd = -1;
+		return -ENOENT;
+	}
+
 	_io_fd = open(PX4IO_SERIAL_DEVICE, O_RDWR);
 
 	if (_io_fd < 0) {
@@ -106,26 +127,6 @@ PX4IO_Uploader::upload(const char *filenames[])
 		return -EIO;
 	}
 
-	for (unsigned i = 0; filenames[i] != nullptr; i++) {
-		_fw_fd = open(filenames[i], O_RDONLY);
-
-		if (_fw_fd < 0) {
-			log("failed to open %s", filenames[i]);
-			continue;
-		}
-
-		log("using firmware from %s", filenames[i]);
-		filename = filenames[i];
-		break;
-	}
-
-	if (filename == NULL) {
-		log("no firmware found");
-		close(_io_fd);
-		_io_fd = -1;
-		return -ENOENT;
-	}
-
 	struct stat st;
 	if (stat(filename, &st) != 0) {
 		log("Failed to stat %s - %d\n", filename, (int)errno);
diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h
index 135202dd1..a4a8a88fe 100644
--- a/src/drivers/px4io/uploader.h
+++ b/src/drivers/px4io/uploader.h
@@ -91,7 +91,7 @@ private:
 	void			drain();
 	int			send(uint8_t c);
 	int			send(uint8_t *p, unsigned count);
-	int			get_sync(unsigned timeout = 1000);
+	int			get_sync(unsigned timeout = 100);
 	int			sync();
 	int			get_info(int param, uint32_t &val);
 	int			erase();