diff --git a/Makefile b/Makefile
index 801256cfec..d9469bb49a 100644
--- a/Makefile
+++ b/Makefile
@@ -114,10 +114,21 @@ endif
 
 upload:		$(FIRMWARE_BUNDLE) $(UPLOADER)
 	@python -u $(UPLOADER) --port $(SERIAL_PORTS) $(FIRMWARE_BUNDLE)
-	
+
+#
+# JTAG firmware uploading with OpenOCD
+#
+ifeq ($(JTAGCONFIG),)
+JTAGCONFIG=interface/olimex-jtag-tiny.cfg
+endif
+
 upload-jtag-px4fmu:
 	@echo Attempting to flash PX4FMU board via JTAG
-	@openocd -f interface/olimex-jtag-tiny.cfg -f ../Bootloader/stm32f4x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4fmu_bl.elf" -c "reset run" -c shutdown
+	@openocd -f $(JTAGCONFIG) -f ../Bootloader/stm32f4x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4fmu_bl.elf" -c "reset run" -c shutdown
+
+upload-jtag-px4io: all
+	@echo Attempting to flash PX4IO board via JTAG
+	@openocd -f $(JTAGCONFIG) -f ../Bootloader/stm32f1x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4io_bl.elf" -c "reset run" -c shutdown
 
 #
 # Hacks and fixups
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 11f2f6f688..a74437d086 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -348,7 +348,6 @@ PX4IO::task_main()
 		/* this would be bad... */
 		if (ret < 0) {
 			log("poll error %d", errno);
-			usleep(1000000);
 			continue;
 		}
 
diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig
index d354ebfe57..7af9c9e4f7 100755
--- a/nuttx/configs/px4io/io/defconfig
+++ b/nuttx/configs/px4io/io/defconfig
@@ -124,7 +124,7 @@ CONFIG_STM32_TIM7=n
 CONFIG_STM32_WWDG=n
 CONFIG_STM32_SPI2=n
 CONFIG_STM32_USART2=y
-CONFIG_STM32_USART3=n
+CONFIG_STM32_USART3=y
 CONFIG_STM32_I2C1=y
 CONFIG_STM32_I2C2=n
 CONFIG_STM32_BKP=n