diff --git a/conf/Makefile.stm32-upload b/conf/Makefile.stm32-upload
index acc0446293..dba6f10a21 100644
--- a/conf/Makefile.stm32-upload
+++ b/conf/Makefile.stm32-upload
@@ -158,9 +158,8 @@ else ifeq ($(FLASH_MODE),PX4_BOOTLOADER)
 	# Program the device and start it.
 upload: $(OBJDIR)/$(TARGET).bin
 	$(PAPARAZZI_SRC)/sw/tools/px4/px_mkfw.py --prototype $(PX4_PROTOTYPE) --image $(OBJDIR)/$(TARGET).bin > $(OBJDIR)/$(TARGET).px4
-	$(PAPARAZZI_SRC)/sw/tools/px4/set_target.py $(PX4_TARGET)
+	$(PAPARAZZI_SRC)/sw/tools/px4/set_target.py $(PX4_TARGET) $(OBJDIR)/$(TARGET).px4
 	$(PAPARAZZI_SRC)/sw/tools/px4/px_uploader.py --port $(PX4_BL_PORT) $(OBJDIR)/$(TARGET).px4
-
 #
 # no known flash mode
 else
diff --git a/conf/boards/px4fmu_2.4.makefile b/conf/boards/px4fmu_2.4.makefile
index 89ed0eca31..8673d6a8e7 100644
--- a/conf/boards/px4fmu_2.4.makefile
+++ b/conf/boards/px4fmu_2.4.makefile
@@ -34,7 +34,7 @@ RADIO_CONTROL_LED  ?= none
 BARO_LED           ?= none
 AHRS_ALIGNER_LED   ?= none
 GPS_LED            ?= none
-SYS_TIME_LED       ?= none
+SYS_TIME_LED       ?= 1
 
 #
 # default UART configuration (modem, gps, spektrum)
diff --git a/conf/boards/px4io_2.4.makefile b/conf/boards/px4io_2.4.makefile
index 1041cf3a76..c3f978b844 100644
--- a/conf/boards/px4io_2.4.makefile
+++ b/conf/boards/px4io_2.4.makefile
@@ -21,7 +21,6 @@ PX4_PROTOTYPE ?= "${PAPARAZZI_HOME}/sw/tools/px4/px4io-v2.prototype"
 PX4_TARGET = "fbw"
 FLASH_MODE ?= PX4_BOOTLOADER
 
-
 #
 # default LED configuration
 #
diff --git a/sw/airborne/modules/px4io_flash/px4io_flash.c b/sw/airborne/modules/px4io_flash/px4io_flash.c
index d0eaf8b170..ae6839fd84 100644
--- a/sw/airborne/modules/px4io_flash/px4io_flash.c
+++ b/sw/airborne/modules/px4io_flash/px4io_flash.c
@@ -38,32 +38,38 @@
 
 #include "libopencm3/cm3/scb.h"
 
+#include "mcu_periph/sys_time.h"
+tid_t px4iobl_tid; ///< id for time out of the px4 bootloader reset
+
 // define coms link for px4io f1
 #define PX4IO_PORT   (&((PX4IO_UART).device))
 #define TELEM2_PORT   (&((TELEM2_UART).device))
 
 // weird that these below are not in protocol.h, which is from the firmware px4 repo
 // below is copied from qgroundcontrol:
-#define PROTO_INSYNC            0x12 //< 'in sync' byte sent before status
-#define PROTO_EOC               0x20 //< end of command
+#define PROTO_INSYNC            0x12 ///< 'in sync' byte sent before status
+#define PROTO_EOC               0x20 ///< end of command
 // Reply bytes
-#define PROTO_OK                0x10 //< INSYNC/OK      - 'ok' response
-#define PROTO_FAILED            0x11 //< INSYNC/FAILED  - 'fail' response
-#define PROTO_INVALID           0x13 //< INSYNC/INVALID - 'invalid' response for bad commands
+#define PROTO_OK                0x10 ///< INSYNC/OK      - 'ok' response
+#define PROTO_FAILED            0x11 ///< INSYNC/FAILED  - 'fail' response
+#define PROTO_INVALID           0x13 ///< INSYNC/INVALID - 'invalid' response for bad commands
 // Command bytes
-#define PROTO_GET_SYNC          0x21 //< NOP for re-establishing sync
-#define PROTO_GET_DEVICE        0x22 //< get device ID bytes
-#define PROTO_CHIP_ERASE        0x23 //< erase program area and reset program address
-#define PROTO_LOAD_ADDRESS      0x24 //< set next programming address
-#define PROTO_PROG_MULTI        0x27 //< write bytes at program address and increment
-#define PROTO_GET_CRC           0x29 //< compute & return a CRC
-#define PROTO_BOOT              0x30 //< boot the application
+#define PROTO_GET_SYNC          0x21 ///< NOP for re-establishing sync
+#define PROTO_GET_DEVICE        0x22 ///< get device ID bytes
+#define PROTO_CHIP_ERASE        0x23 ///< erase program area and reset program address
+#define PROTO_LOAD_ADDRESS      0x24 ///< set next programming address
+#define PROTO_PROG_MULTI        0x27 ///< write bytes at program address and increment
+#define PROTO_GET_CRC           0x29 ///< compute & return a CRC
+#define PROTO_BOOT              0x30 ///< boot the application
 
 bool_t setToBootloaderMode;
+bool_t px4ioRebootTimeout;
 
 void px4ioflash_init(void)
 {
   setToBootloaderMode = FALSE;
+  px4ioRebootTimeout = FALSE;
+  px4iobl_tid = sys_time_register_timer(15.0, NULL); //20 (fbw pprz bl timeout)-5 (px4 fmu bl timeout)
 }
 
 void px4ioflash_event(void)
@@ -121,6 +127,29 @@ void px4ioflash_event(void)
     } else { // target fbw
       //the target is the fbw, so reboot the fbw and switch to relay mode
 
+      //first check if the bootloader has not timeout:
+      if (sys_time_check_and_ack_timer(px4iobl_tid) || px4ioRebootTimeout) {
+        px4ioRebootTimeout= TRUE;
+        sys_time_cancel_timer(px4iobl_tid);
+        TELEM2_PORT->put_byte(TELEM2_PORT->periph, 'T');
+        TELEM2_PORT->put_byte(TELEM2_PORT->periph, 'I');
+        TELEM2_PORT->put_byte(TELEM2_PORT->periph, 'M');
+        TELEM2_PORT->put_byte(TELEM2_PORT->periph, 'E');
+        TELEM2_PORT->put_byte(TELEM2_PORT->periph, 'O');
+        TELEM2_PORT->put_byte(TELEM2_PORT->periph, 'U');
+        TELEM2_PORT->put_byte(TELEM2_PORT->periph, 'T'); // use 7 chars as answer        
+        return;
+      }  { // FBW OK OK hollay hollay :)
+        TELEM2_PORT->put_byte(TELEM2_PORT->periph, 'F');
+        TELEM2_PORT->put_byte(TELEM2_PORT->periph, 'B');
+        TELEM2_PORT->put_byte(TELEM2_PORT->periph, 'W');
+        TELEM2_PORT->put_byte(TELEM2_PORT->periph, 'O');
+        TELEM2_PORT->put_byte(TELEM2_PORT->periph, 'K');
+        TELEM2_PORT->put_byte(TELEM2_PORT->periph, 'O');
+        TELEM2_PORT->put_byte(TELEM2_PORT->periph, 'K'); // use 7 chars as answer
+      }
+
+
       //stop all intermcu communication:
       disable_inter_comm(true);
 
@@ -222,6 +251,13 @@ void px4ioflash_event(void)
         }
       } else {
         TELEM2_PORT->put_byte(TELEM2_PORT->periph, 'E'); //TODO: find out what the PX4 protocol for error feedback is...
+        TELEM2_PORT->put_byte(TELEM2_PORT->periph, 'R');
+        TELEM2_PORT->put_byte(TELEM2_PORT->periph, 'R');
+        TELEM2_PORT->put_byte(TELEM2_PORT->periph, 'O');
+        TELEM2_PORT->put_byte(TELEM2_PORT->periph, 'R');
+        TELEM2_PORT->put_byte(TELEM2_PORT->periph, '!');
+        TELEM2_PORT->put_byte(TELEM2_PORT->periph, ' '); // use 7 chars as answer
+
       }
     }
   } else if (TELEM2_PORT->char_available(TELEM2_PORT->periph)) {
diff --git a/sw/tools/px4/set_target.py b/sw/tools/px4/set_target.py
index c83efe22a8..99589339dd 100755
--- a/sw/tools/px4/set_target.py
+++ b/sw/tools/px4/set_target.py
@@ -1,15 +1,77 @@
-#!/usr/bin/python
+#!/usr/bin/env python
+import os
 import sys
 import serial
+import glob
+import time
 
-print sys.argv[2]
-portname = sys.argv[1]
-if sys.argv[2] == "fbw" :
-  line = "pprz0"
-else :
-  line = "pprz1"
+target = sys.argv[1]
+firmware_file = sys.argv[2]
 
-#print line.rstrip() + " " + str(len(line))
-ser = serial.Serial(portname)
-ser.write(line)
+print "Target: " + target
+print "Firmware file: " + firmware_file
+
+#test if pprz cdm is connected
+mode =-1
+port = ""
+try :
+  port = "/dev/serial/by-id/usb-Paparazzi_UAV_CDC_Serial_STM32_*"
+  if len(glob.glob(port)) > 1:
+    print("Warning: multiple Paparazzi cdc devices found. Selecting the first one.")
+  port = glob.glob(port)[0]
+  ser = serial.Serial(port,timeout=0.5)
+  mode = 1
+  print ("Paparazzi CDC device found at port: " + port)
+except (serial.serialutil.SerialException,IndexError) :
+  print("No Paparazzi CDC device found, looking further.")
+
+if mode == 1 :
+  if target == "fbw" :
+      line = "pprz0"
+  else :
+    line = "pprz1"
+
+  print ("Sending target command to Paparazzi firmware...")
+  ser.flush()
+  ser.write(line)
+
+  if target == "fbw" :
+    try :
+      c = ser.read(7)
+      print ("AP responded with: " + c)
+      if c=="TIMEOUT" :
+        print("Error: FBW bootloader TIMEOUT. Power cycle the board and wait between 5 seconds to 20 seconds to retry.")
+        sys.exit(1)
+      elif c != "FBWOKOK" :
+        print("Error: unknown error. Power cycle the board and wait between 5 seconds to 20 seconds to retry.")
+        sys.exit(1)
+    except serial.serialutil.SerialException :
+      pass
+
+  print("Uploading using Paparazzi firmware...")
+  sys.exit(0)
+
+if mode == -1 : #no pprz cdc was found, look for PX4
+    ports = glob.glob("/dev/serial/by-id/usb-3D_Robotics*")
+    ports.append(glob.glob("/dev/serial/by-id/pci-3D_Robotics*"))
+    for p in ports :
+      if len(p) > 0 :
+        try :
+          ser = serial.Serial(p,timeout=0.5)
+          port = p
+          mode = 2
+          print ("Original PX4 firmware CDC device found at port: " + port)
+        except serial.serialutil.SerialException :
+          print("Non working PX4 port found, continuing...")
+
+    if mode == -1 :
+      print("No original PX4 CDC firmware found either.")
+      print("Error: no compatible usb device found...")
+      sys.exit(1)
+
+    if target == "fbw" :
+      print("Error: original firmware cannot be used to upload the fbw code. Wait for the PX4 bootloader to exit (takes 5 seconds), or in case this is the first upload; first upload the Paparazzi ap target.")
+      sys.exit(1)
+    else :
+      print("Uploading AP using original PX4 firmware...")