Switched to usb_ser for flashing

conf/Makefile.stm32-upload

@@ -158,7 +158,7 @@ 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/print_message.py
+	$(PAPARAZZI_SRC)/sw/tools/px4/set_target.py $(PX4_TARGET)
 	$(PAPARAZZI_SRC)/sw/tools/px4/px_uploader.py --port $(PX4_BL_PORT) $(OBJDIR)/$(TARGET).px4
 
 #

conf/boards/px4fmu_2.4.makefile

@@ -22,6 +22,7 @@ HARD_FLOAT=yes
 # default flash mode is the PX4 bootloader
 # possibilities: DFU, SWD, PX4 bootloader
 FLASH_MODE ?= PX4_BOOTLOADER
+PX4_TARGET = "ap"
 PX4_PROTOTYPE ?= "${PAPARAZZI_HOME}/sw/tools/px4/px4fmu-v2.prototype"
 PX4_BL_PORT ?= "/dev/serial/by-id/usb-3D_Robotics*,/dev/serial/by-id/pci-3D_Robotics*"
 
@@ -33,7 +34,7 @@ RADIO_CONTROL_LED  ?= none
 BARO_LED           ?= none
 AHRS_ALIGNER_LED   ?= none
 GPS_LED            ?= none
-SYS_TIME_LED       ?= 1
+SYS_TIME_LED       ?= none
 
 #
 # default UART configuration (modem, gps, spektrum)

conf/boards/px4io_2.4.makefile

@@ -16,9 +16,9 @@ $(TARGET).LDSCRIPT=$(SRC_ARCH)/px4io_2.4.ld
 
 # default flash mode is via usb dfu bootloader
 # possibilities: DFU, SWD, PX4_BOOTLOADER
-PX4_BL_PORT ?= "/dev/serial/by-id/usb-FTDI_*"
+PX4_BL_PORT ?= "/dev/serial/by-id/usb-Paparazzi_UAV_CDC_Serial_STM32_*"
 PX4_PROTOTYPE ?= "${PAPARAZZI_HOME}/sw/tools/px4/px4io-v2.prototype"
-
+PX4_TARGET = "fbw"
 FLASH_MODE ?= PX4_BOOTLOADER
 
 

conf/modules/px4io_flash.xml

@@ -14,13 +14,14 @@
 
     </raw>
     <file name="px4io_flash.c"/>
+    <file name="usb_ser_hw.c"  dir="arch/stm32"/>
     <configure name="PX4IO_UART" default="uart6" case="upper|lower"/>
     <configure name="PX4IO_BAUD" default="B1500000"/>
     <define name="USE_$(PX4IO_UART_UPPER)"/>
     <define name="PX4IO_UART" value="$(PX4IO_UART_LOWER)"/>
     <define name="$(PX4IO_UART_UPPER)_BAUD" value="$(PX4IO_BAUD)"/>
 
-    <configure name="TELEM2_UART" default="uart3" case="upper|lower"/>
+    <configure name="TELEM2_UART" default="usb_serial" case="upper|lower"/>
     <configure name="TELEM2_BAUD" default="B115200"/>
     <define name="USE_$(TELEM2_UART_UPPER)"/>
     <define name="TELEM2_UART" value="$(TELEM2_UART_LOWER)"/>

sw/airborne/arch/stm32/usb_ser_hw.c

@@ -39,11 +39,14 @@
 
 #include "mcu_periph/usb_serial.h"
 
+#include "mcu_periph/sys_time_arch.h"
 
 /* Max packet size for USB transfer */
 #define MAX_PACKET_SIZE          64
 /* Max fifo size for storing data */
-#define VCOM_FIFO_SIZE          128
+#define VCOM_FIFO_SIZE          256
+
+#define TX_TIMEOUT_CNT 20 //TODO, make dynamic with event period
 
 typedef struct {
   int         head;
@@ -62,7 +65,7 @@ bool_t fifo_put(fifo_t *fifo, uint8_t c);
 bool_t fifo_get(fifo_t *fifo, uint8_t *pc);
 int  fifo_avail(fifo_t *fifo);
 int  fifo_free(fifo_t *fifo);
-
+int tx_timeout;
 
 usbd_device *my_usbd_dev;
 
@@ -387,10 +390,13 @@ int VCOM_putchar(int c)
     if (VCOM_check_free_space(2)) {
       // if yes, add char
       fifo_put(&txfifo, c);
+      tx_timeout = TX_TIMEOUT_CNT;
     } else {
       // less than 2 bytes available, add byte and send data now
       fifo_put(&txfifo, c);
+      sys_time_usleep(10); //far from optimal, increase fifo size to prevent this problem
       VCOM_send_message();
+      tx_timeout = TX_TIMEOUT_CNT;
     }
     return c;
   }
@@ -433,7 +439,19 @@ int VCOM_check_available(void)
  */
 void VCOM_event(void)
 {
+  if (tx_timeout == 1) { // send a remaining bytes that still hangs arround in the tx fifo, after a timeout
+    if (fifo_avail(&txfifo)) {
+      VCOM_send_message();
+      tx_timeout = TX_TIMEOUT_CNT;
+    } else {
+      tx_timeout = 0;
+    }
+  } else if (tx_timeout > 1) {
+    tx_timeout--;
+  }
+
   usbd_poll(my_usbd_dev);
+
 }
 
 /**
@@ -443,6 +461,7 @@ void VCOM_event(void)
 void VCOM_send_message(void)
 {
   if (usb_connected) {
+
     uint8_t buf[MAX_PACKET_SIZE];
     uint8_t i;
     for (i = 0; i < MAX_PACKET_SIZE; i++) {
@@ -450,7 +469,10 @@ void VCOM_send_message(void)
         break;
       }
     }
+
+    while (usbd_ep_stall_get(my_usbd_dev, 0x82)) {};
     usbd_ep_write_packet(my_usbd_dev, 0x82, buf, i);
+
   }
 }
 
@@ -532,4 +554,6 @@ void VCOM_init(void)
   usb_serial.device.send_message = (send_message_t) usb_serial_send;
   usb_serial.device.char_available = (char_available_t) usb_serial_char_available;
   usb_serial.device.get_byte = (get_byte_t) usb_serial_getch;
+
+  tx_timeout = -1;
 }

sw/airborne/mcu_periph/uart.h

@@ -36,7 +36,12 @@
 #define UART_RX_BUFFER_SIZE 128
 #endif
 #ifndef UART_TX_BUFFER_SIZE
+#ifdef STM32F4  //the F4 has enough memory, and the PX4 bootloader needs more then 128
+#define UART_TX_BUFFER_SIZE 256
+#else
 #define UART_TX_BUFFER_SIZE 128
+#endif
+
 #endif
 #define UART_DEV_NAME_SIZE 16
 

sw/airborne/modules/px4io_flash/px4io_flash.c

@@ -32,6 +32,11 @@
 
 // Serial Port
 #include "mcu_periph/uart.h"
+#include "mcu_periph/usb_serial.h"
+
+#include "led.h"
+
+#include "libopencm3/cm3/scb.h"
 
 // define coms link for px4io f1
 #define PX4IO_PORT   (&((PX4IO_UART).device))
@@ -39,20 +44,20 @@
 
 // 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;
 
@@ -63,82 +68,111 @@ void px4ioflash_init(void)
 
 void px4ioflash_event(void)
 {
-  // setToBootloaderMode=true;
   if (PX4IO_PORT->char_available(PX4IO_PORT->periph)) {
     if (!setToBootloaderMode) {
       //ignore anything coming from IO if not in bootloader mode (which should be nothing)
     } else {
       //relay everything from IO to the laptop
-      unsigned char b = PX4IO_PORT->get_byte(PX4IO_PORT->periph);
-      TELEM2_PORT->put_byte(TELEM2_PORT->periph, b);
+      while (PX4IO_PORT->char_available(PX4IO_PORT->periph)) {
+        unsigned char b = PX4IO_PORT->get_byte(PX4IO_PORT->periph);
+        TELEM2_PORT->put_byte(TELEM2_PORT->periph, b);
+      }
     }
   }
 
-  //TODO: check if timeout was surpassed
+  //TODO: check if bootloader timeout was surpassed
   if (TELEM2_PORT->char_available(TELEM2_PORT->periph) && !setToBootloaderMode) {
-    //data was received on the pc uart, so
-    //stop all intermcu comminication:
-    disable_inter_comm(true);
-    //send the reboot to bootloader command:
+    // TMP TEST
+    //    while (TELEM2_PORT->char_available(TELEM2_PORT->periph)) {
+    //      unsigned char bla = TELEM2_PORT->get_byte(TELEM2_PORT->periph);
+    //      TELEM2_PORT->put_byte(TELEM2_PORT->periph,bla);
+    //    }
+    //    return;
 
-    /*
+    //check whether this is flash related communication, and for who (ap/fbw)
+    int state = 0;
+    while (state < 4 && TELEM2_PORT->char_available(TELEM2_PORT->periph)) {
+      unsigned char b = TELEM2_PORT->get_byte(TELEM2_PORT->periph);
+      switch (state) {
+      case (0) :
+        if (b == 'p') { state++; } else { return; }
+        break;
+      case (1) :
+        if (b == 'p') { state++; } else { return; }
+        break;
+      case (2) :
+        if (b == 'r') { state++; } else { return; }
+        break;
+      case (3) :
+        if (b == 'z') { state++; } else { return; }
+        break;
+      default :
+        break;
+      }
+    }
+
+    if (state != 4) {return;}
+    //TODO: check if/how this interferes with flashing original PX4 firmware
+    unsigned char target = TELEM2_PORT->get_byte(TELEM2_PORT->periph);
+    if (target == '1') { //target ap
+      //the target is the ap, so reboot to PX4 bootloader
+      scb_reset_system();
+
+    } else { // target fbw
+      //the target is the fbw, so reboot the fbw and switch to relay mode
+
+      //stop all intermcu communication:
+      disable_inter_comm(true);
+
+      /*
       * The progdieshit define is very usefull, if for whatever reason the (normal, not bootloader) firmware on the IO chip became disfunct.
       * In that case:
       * 1. enable this define
       * 2. build and upload  the fmu f4 chip (ap target in pprz center)
       * 3. build the io code, and convert the firmware using the following command:
-      *       /home/houjebek/paparazzi/sw/tools/pixhawk/px_mkfw.py --prototype "/home/houjebek/px4/Firmware/Images/px4io-v2.prototype" --image /home/houjebek/paparazzi/var/aircrafts/Iris/fbw/fbw.bin > /home/houjebek/paparazzi/var/aircrafts/Iris/fbw/fbw.px4
+      *       /home/houjebek/paparazzi/sw/tools/px4/px_mkfw.py --prototype "/home/houjebek/px4/Firmware/Images/px4io-v2.prototype" --image /home/houjebek/paparazzi/var/aircrafts/Iris/fbw/fbw.bin > /home/houjebek/paparazzi/var/aircrafts/Iris/fbw/fbw.px4
       * 4. Start the following command:
-      *    /home/houjebek/paparazzi/sw/tools/pixhawk/px_uploader.py --port "/dev/serial/by-id/usb-FTDI_TTL232R-3V3_FT906KBO-if00-port0" /home/houjebek/paparazzi/var/aircrafts/Iris/fbw/fbw.px4
+      *    /home/houjebek/paparazzi/sw/tools/px4/px_uploader.py --port "/dev/ttyACM0" /home/houjebek/paparazzi/var/aircrafts/Iris/fbw/fbw.px4
       * 5a. Either, boot the Pixhawk (reconnect usb) holding the IO reset button until the FMU led stops blinking fast (i.e. exits its own bootloader)
       * 5b  Or, press the IO reset button on the pixhawk
       * 6. Watch the output of the command of step 4, it should recognize the IO bootloader and start flashing. If not try repeating step 5a.
       * 7. Don forget to disable the define and upload the ap again :)
       */
-    //#define progdieshit
+      //    #define progdieshit
 
 #ifndef progdieshit
-    static struct IOPacket  dma_packet;
-    dma_packet.count_code = 0x40 + 0x01;
-    dma_packet.crc = 0;
-    dma_packet.page = PX4IO_PAGE_SETUP;
-    dma_packet.offset = PX4IO_P_SETUP_REBOOT_BL;
-    dma_packet.regs[0] = PX4IO_REBOOT_BL_MAGIC;
-    dma_packet.crc = crc_packet(&dma_packet);
-    struct IOPacket *pkt = &dma_packet;
-    uint8_t *p = (uint8_t *)pkt;
-    PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[0]);
-    PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[1]);
-    PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[2]);
-    PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[3]);
-    PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[4]);
-    PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[5]);
+      //send the reboot to bootloader command:
+      static struct IOPacket  dma_packet;
+      dma_packet.count_code = 0x40 + 0x01;
+      dma_packet.crc = 0;
+      dma_packet.page = PX4IO_PAGE_SETUP;
+      dma_packet.offset = PX4IO_P_SETUP_REBOOT_BL;
+      dma_packet.regs[0] = PX4IO_REBOOT_BL_MAGIC;
+      dma_packet.crc = crc_packet(&dma_packet);
+      struct IOPacket *pkt = &dma_packet;
+      uint8_t *p = (uint8_t *)pkt;
+      PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[0]);
+      PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[1]);
+      PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[2]);
+      PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[3]);
+      PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[4]);
+      PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[5]);
 
-    // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'E');
-    // for (int i=0;i<6;i++) {
-    //  unsigned char tmp[3];
-    //  itoa(p[i],tmp,16);
-    //  TELEM2_PORT->put_byte(TELEM2_PORT->periph,tmp[0]);
-    //  TELEM2_PORT->put_byte(TELEM2_PORT->periph,tmp[1]);
-    //  TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\n');
-    //  TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\r');
-    // }
+      sys_time_usleep(5000); // this seems to be close to the minimum delay necessary to process this packet at the IO side
+      //the pixhawk IO chip should respond with:
+      // 0x00 ( PKT_CODE_SUCCESS )
+      // 0xe5
+      // 0x32
+      // 0x0a
+      //After that, the IO chips reboots into bootloader mode, in which it will stay for a short period
+      //The baudrate in bootloader mode ic changed to 115200 (normal operating baud is 1500000, at least for original pixhawk fmu firmware)
 
-    sys_time_usleep(5000); // this seems to be close to the minimum delay necessary to process this packet at the IO side
-    //the pixhawk IO chip should respond with:
-    // 0x00 ( PKT_CODE_SUCCESS )
-    // 0xe5
-    // 0x32
-    // 0x0a
-    //After that, the IO chips reboots into bootloader mode, in which it will stay for a short period
-    //The baudrate in bootloader mode ic changed to 115200 (normal operating baud is 1500000, at least for original pixhawk fmu firmware)
+      //state machine
+      state = 0;
+      while (state < 4 && PX4IO_PORT->char_available(PX4IO_PORT->periph)) {
 
-    //state machine
-    int state = 0;
-    while (state < 4 && PX4IO_PORT->char_available(PX4IO_PORT->periph)) {
-
-      unsigned char b = PX4IO_PORT->get_byte(PX4IO_PORT->periph);
-      switch (state) {
+        unsigned char b = PX4IO_PORT->get_byte(PX4IO_PORT->periph);
+        switch (state) {
         case (0) :
           if (b == PKT_CODE_SUCCESS) { state++; } else { state = 0; }
           break;
@@ -152,107 +186,50 @@ void px4ioflash_event(void)
           if (b == 0x0a) { state++; } else { state = 0; }
           break;
         default :
-          TELEM2_PORT->put_byte(TELEM2_PORT->periph, 'b');
           break;
-      }
-      // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'S');
-      // TELEM2_PORT->put_byte(TELEM2_PORT->periph,state+48);
-      // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\n');
-      // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\r');
-    }
-#else
-    int state = 4;
-#endif
-    if (state == 4) {
-#ifndef progdieshit
-      // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'S');
-      // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'6');
-      // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\n');
-      // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\r');
-#endif
-      uart_periph_set_baudrate(PX4IO_PORT->periph, B115200);
-      /* look for the bootloader for 150 ms */
-      int ret = 0;
-      for (int i = 0; i < 15 && !ret ; i++) {
-        sys_time_usleep(10000);
-
-
-        //send a get_sync command in order to keep the io in bootloader mode
-        PX4IO_PORT->put_byte(PX4IO_PORT->periph, PROTO_GET_SYNC);
-        PX4IO_PORT->put_byte(PX4IO_PORT->periph, PROTO_EOC);
-
-
-#ifndef progdieshit
-        // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'S');
-        //    TELEM2_PORT->put_byte(TELEM2_PORT->periph,'6');
-        //    TELEM2_PORT->put_byte(TELEM2_PORT->periph,'a');
-        //    TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\n');
-        //    TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\r');
-#endif
-
-        //get_sync should be replied with, so check if that happens and
-        //all other bytes are discarded, hopefully those were not important
-        //(they may be caused by sending multiple syncs)
-        while (PX4IO_PORT->char_available(PX4IO_PORT->periph)) {
-          unsigned char b = PX4IO_PORT->get_byte(PX4IO_PORT->periph);
-
-#ifndef progdieshit
-          // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'S');
-          //    TELEM2_PORT->put_byte(TELEM2_PORT->periph,'6');
-          //    TELEM2_PORT->put_byte(TELEM2_PORT->periph,'b');
-          //    TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\n');
-          //    TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\r');
-#endif
-
-          if (b == PROTO_INSYNC) {
-#ifndef progdieshit
-            // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'S');
-            // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'7');
-            // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\n');
-            // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\r');
-#endif
-
-            setToBootloaderMode = true;
-            ret = 1;
-            break;
-          }
         }
       }
-      if (setToBootloaderMode) {
-
-#ifndef progdieshit
-        // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'S');
-        // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'8');
-        // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\n');
-        // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\r');
+#else
+      int state = 4;
 #endif
+      if (state == 4) {
+        uart_periph_set_baudrate(PX4IO_PORT->periph, B115200);
+        /* look for the bootloader for 150 ms */
+        int ret = 0;
+        for (int i = 0; i < 15 && !ret ; i++) {
+          sys_time_usleep(10000);
 
-        //if successfully entered bootloader mode, clear any remaining bytes (which may have a function, but I did not check)
-        while (PX4IO_PORT->char_available(PX4IO_PORT->periph)) {PX4IO_PORT->get_byte(PX4IO_PORT->periph);}
+          //send a get_sync command in order to keep the io in bootloader mode
+          PX4IO_PORT->put_byte(PX4IO_PORT->periph, PROTO_GET_SYNC);
+          PX4IO_PORT->put_byte(PX4IO_PORT->periph, PROTO_EOC);
+
+          //get_sync should be replied with, so check if that happens and
+          //all other bytes are discarded, hopefully those were not important
+          //(they may be caused by sending multiple syncs)
+          while (PX4IO_PORT->char_available(PX4IO_PORT->periph)) {
+            unsigned char b = PX4IO_PORT->get_byte(PX4IO_PORT->periph);
+
+            if (b == PROTO_INSYNC) {
+              setToBootloaderMode = true;
+              ret = 1;
+              break;
+            }
+          }
+        }
+        if (setToBootloaderMode) {
+          //if successfully entered bootloader mode, clear any remaining bytes (which may have a function, but I did not check)
+          while (PX4IO_PORT->char_available(PX4IO_PORT->periph)) {PX4IO_PORT->get_byte(PX4IO_PORT->periph);}
+        }
+      } else {
+        TELEM2_PORT->put_byte(TELEM2_PORT->periph, 'E'); //TODO: find out what the PX4 protocol for error feedback is...
       }
-
-#ifndef progdieshit
-      // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'S');
-      // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'9');
-      // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\n');
-      // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\r');
-#endif
-
-    } else {
-      TELEM2_PORT->put_byte(TELEM2_PORT->periph, 'E');
-      // 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,'\n');
-      // TELEM2_PORT->put_byte(TELEM2_PORT->periph,'\r');
     }
   } else if (TELEM2_PORT->char_available(TELEM2_PORT->periph)) {
     //already in bootloader mode, just directly relay data
-    unsigned char b = TELEM2_PORT->get_byte(TELEM2_PORT->periph);
-    PX4IO_PORT->put_byte(PX4IO_PORT->periph, b);
-    // TELEM2_PORT->put_byte(TELEM2_PORT->periph,b);
+    while (TELEM2_PORT->char_available(TELEM2_PORT->periph)) {
+      unsigned char b = TELEM2_PORT->get_byte(TELEM2_PORT->periph);
+      PX4IO_PORT->put_byte(PX4IO_PORT->periph, b);
+    }
   }
-
 }
 

sw/tools/px4/set_target.py

@@ -0,0 +1,15 @@
+#!/usr/bin/python
+import sys
+import serial
+
+print sys.argv[2]
+portname = sys.argv[1]
+if sys.argv[2] == "fbw" :
+  line = "pprz0"
+else :
+  line = "pprz1"
+
+#print line.rstrip() + " " + str(len(line))
+ser = serial.Serial(portname)
+ser.write(line)
+