[intermcu] PX4: Dial down baud after start up period

closes #1587

conf/airframes/TUDELFT/tudelft_conf.xml

@@ -116,7 +116,7 @@
    radio="radios/FrSky3dr.xml"
    telemetry="telemetry/default_rotorcraft_slow.xml"
    flight_plan="flight_plans/TUDELFT/tudelft_delft_basic.xml"
-   settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml [settings/control/stabilization_att_int_quat.xml] settings/control/stabilization_indi.xml"
+   settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml [settings/control/stabilization_att_int_quat.xml] settings/control/stabilization_indi.xml settings/estimation/body_to_imu.xml"
    settings_modules="modules/geo_mag.xml modules/air_data.xml modules/gps_ubx_ucenter.xml"
    gui_color="#ffffcccaccca"
   />

conf/airframes/TUDELFT/tudelft_iris_indi.xml

@@ -35,7 +35,7 @@
     </subsystem>
     <subsystem name="intermcu" type="uart">
       <configure name="INTERMCU_PORT" value="UART6" />
-      <configure name="INTERMCU_BAUD" value="B1500000" />
+      <configure name="INTERMCU_BAUD" value="B1500000" /> <!-- This is only during first 10s start up, afterwards it is set to 230400-->
     </subsystem>
   </firmware>
   <firmware name="rotorcraft">
@@ -58,7 +58,7 @@
     <define name="RC_LOST_FBW_MODE" value="FBW_MODE_FAILSAFE" />
     <!-- Switch to Failsafe or to Autopilot on RC loss? -->
     <define name="RC_LOST_IN_AUTO_FBW_MODE" value="FBW_MODE_AUTO" />
-    <define name="AP_LOST_FBW_MODE" value="FBW_MODE_MANUAL" />
+    <define name="AP_LOST_FBW_MODE" value="FBW_MODE_FAILSAFE" />
     <!-- Switch to Failsafe or to Manual on AP loss? -->
     <define name="INTERMCU_LOST_CNT" value="100" />
     <subsystem name="intermcu" type="uart">

sw/airborne/firmwares/rotorcraft/main_fbw.c

@@ -37,19 +37,17 @@
 #include "subsystems/actuators/motor_mixing.h"
 #endif
 
-
 #include "subsystems/electrical.h"
-
 #include "subsystems/radio_control.h"
-
 #include "subsystems/intermcu/intermcu_fbw.h"
-
 #include "firmwares/rotorcraft/main_fbw.h"
 #include "firmwares/rotorcraft/autopilot_rc_helpers.h"
 
 #define MODULES_C
 #include "generated/modules.h"
 
+
+
 /** Fly by wire modes */
 typedef enum {FBW_MODE_MANUAL = 0, FBW_MODE_AUTO = 1, FBW_MODE_FAILSAFE = 2} fbw_mode_enum;
 fbw_mode_enum fbw_mode;
@@ -178,18 +176,31 @@ STATIC_INLINE void main_periodic(void)
     }
   }
 
+#ifdef BOARD_PX4IO
+  //due to a baud rate issue on PX4, for a few seconds the baud is 1500000 however this may result in package loss, causing the motors to spin at random
+  //to prevent this situation:
+  if (inter_mcu.stable_px4_baud != PPRZ_BAUD) {
+    fbw_mode = FBW_MODE_FAILSAFE;
+    autopilot_motors_on = false;
+    //signal to user whether fbw can be flashed:
+#ifdef FBW_MODE_LED
+    LED_OFF(FBW_MODE_LED); // causes really fast blinking
+#endif
+  }
+#endif
 
   static uint16_t dv = 0;
   // TODO make module out of led blink?
   /* set failsafe commands     */
   if (fbw_mode == FBW_MODE_FAILSAFE) {
+    autopilot_motors_on = false;
     SetCommands(commands_failsafe);
 #ifdef FBW_MODE_LED
     if (!(dv++ % (PERIODIC_FREQUENCY / 20))) { LED_TOGGLE(FBW_MODE_LED);}
   } else if (fbw_mode == FBW_MODE_MANUAL) {
     if (!(dv++ % (PERIODIC_FREQUENCY))) { LED_TOGGLE(FBW_MODE_LED);}
   } else if (fbw_mode == FBW_MODE_AUTO) {
-    LED_TOGGLE(FBW_MODE_LED); // toggle instead of on, because then it is still visible when fbw_mode switches very fast
+    intermcu_blink_fbw_led(dv++);
 #endif // FWB_MODE_LED
   }
 
@@ -208,7 +219,10 @@ static void autopilot_on_rc_frame(void)
 {
   /* get autopilot fbw mode as set by RADIO_MODE 3-way switch */
   if (radio_control.values[RADIO_FBW_MODE] < (MIN_PPRZ / 2)) {
-    fbw_mode = FBW_MODE_MANUAL;
+    //TODO, check whether the aircraft can actually be flown in manual mode
+    //most rotory aircraft can't, at least not without additional IMU aid
+    //for now, just turn set to failsafe instead of manual mode.
+    fbw_mode = FBW_MODE_FAILSAFE;
   } else {
     fbw_mode = FBW_MODE_AUTO;
   }
@@ -222,6 +236,7 @@ static void autopilot_on_rc_frame(void)
   /* if manual */
   if (fbw_mode == FBW_MODE_MANUAL) {
     autopilot_motors_on = true;
+    SetCommands(commands_failsafe);
 #ifdef SetCommandsFromRC
     SetCommandsFromRC(commands, radio_control.values);
 #else
@@ -237,8 +252,10 @@ static void autopilot_on_ap_command(void)
 {
   if (fbw_mode != FBW_MODE_MANUAL) {
     SetCommands(intermcu_commands);
-  } else {
+  } else if (fbw_mode == FBW_MODE_AUTO) {
     autopilot_motors_on = true;
+  } else {
+    autopilot_motors_on = false;
   }
 }
 

sw/airborne/modules/px4_flash/px4_flash.c

@@ -74,6 +74,15 @@ void px4flash_init(void)
 
 void px4flash_event(void)
 {
+  if (sys_time_check_and_ack_timer(px4iobl_tid)) {
+    px4ioRebootTimeout = TRUE;
+    sys_time_cancel_timer(px4iobl_tid);
+    //for unknown reasons, 1500000 baud does not work reliably after prolonged times.
+    //I suspect a temperature related issue, combined with the fbw f1 crystal which is out of specs
+    //After a initial period on 1500000, revert to 230400
+    //We still start at 1500000 to remain compatible with original PX4 firmware. (which always runs at 1500000)
+    uart_periph_set_baudrate(PX4IO_PORT->periph, B230400);
+  }
   if (PX4IO_PORT->char_available(PX4IO_PORT->periph)) {
     if (!setToBootloaderMode) {
       //ignore anything coming from IO if not in bootloader mode (which should be nothing)
@@ -128,9 +137,7 @@ void px4flash_event(void)
       //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) {
+      if (px4ioRebootTimeout) {
-        px4ioRebootTimeout = true;
-        sys_time_cancel_timer(px4iobl_tid);
         FLASH_PORT->put_byte(FLASH_PORT->periph, 'T');
         FLASH_PORT->put_byte(FLASH_PORT->periph, 'I');
         FLASH_PORT->put_byte(FLASH_PORT->periph, 'M');
@@ -153,23 +160,22 @@ void px4flash_event(void)
       //stop all intermcu communication:
       disable_inter_comm(true);
 
+      px4iobl_tid = sys_time_register_timer(5.0, NULL); //10 (fbw pprz bl timeout)-5 (px4 fmu bl timeout)
       /*
-      * The progdieshit define is very usefull, if for whatever reason the (normal, not bootloader) firmware on the IO chip became disfunct.
+      * The forceprog 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/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
+      * Optional 5&6:
-      * 4. Start the following command:
-      *    /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 forceprog
 
-#ifndef progdieshit
+#ifndef forceprog
       //send the reboot to bootloader command:
       static struct IOPacket  dma_packet;
       dma_packet.count_code = 0x40 + 0x01;

sw/airborne/subsystems/intermcu.h

@@ -37,17 +37,30 @@
 #define INTERMCU_LOST_CNT 25  /* 50ms with a 512Hz timer TODO fixed value */
 #endif
 
+#include BOARD_CONFIG
+
 enum intermcu_status {
   INTERMCU_OK,
   INTERMCU_LOST
 };
 
+enum intermcu_PX4_baud_status {
+  PX4_BAUD,
+  CHANGING_BAUD,
+  PPRZ_BAUD
+};
+
 struct intermcu_t {
   enum intermcu_status status;
   uint8_t time_since_last_frame;
+#ifdef BOARD_PX4IO
+  enum intermcu_PX4_baud_status stable_px4_baud;
+#endif
 };
 extern struct intermcu_t inter_mcu;
 
+
+
 void intermcu_init(void);
 void intermcu_periodic(void);
 

sw/airborne/subsystems/intermcu/intermcu_fbw.c

@@ -31,13 +31,13 @@
 #include "pprzlink/pprz_transport.h"
 #include "modules/spektrum_soft_bind/spektrum_soft_bind_fbw.h"
 
+#include BOARD_CONFIG
 #ifdef BOARD_PX4IO
 #include "libopencm3/cm3/scb.h"
 #include "led.h"
 #include "mcu_periph/sys_time.h"
 static uint8_t px4RebootSequence[] = {0x41, 0xd7, 0x32, 0x0a, 0x46, 0x39};
 static uint8_t px4RebootSequenceCount = 0;
-static bool px4RebootTimeout = false;
 uint8_t autopilot_motors_on = false;
 tid_t px4bl_tid; ///< id for time out of the px4 bootloader reset
 #endif
@@ -63,7 +63,7 @@ void intermcu_init(void)
 {
   pprz_transport_init(&intermcu_transport);
 #ifdef BOARD_PX4IO
-  px4bl_tid = sys_time_register_timer(20.0, NULL);
+  px4bl_tid = sys_time_register_timer(10.0, NULL);
 #endif
 
 }
@@ -119,6 +119,21 @@ void intermcu_send_status(uint8_t mode)
   //FIXME
 }
 
+
+void intermcu_blink_fbw_led(uint16_t dv)
+{
+#ifdef FBW_MODE_LED
+  static uint16_t idv = 0;
+  if (!autopilot_motors_on) {
+    if (!(dv % (PERIODIC_FREQUENCY))) {
+      if (!(idv++ % 3)) { LED_OFF(FBW_MODE_LED);} else {LED_TOGGLE(FBW_MODE_LED);}
+    }
+  } else {
+    LED_TOGGLE(FBW_MODE_LED); // toggle makes random blinks if intermcu comm problem!
+  }
+#endif
+}
+
 static void intermcu_parse_msg(struct transport_rx *trans, void (*commands_frame_handler)(void))
 {
   /* Parse the Inter MCU message */
@@ -174,13 +189,22 @@ void InterMcuEvent(void (*frame_handler)(void))
 #ifdef BOARD_PX4IO
 static void checkPx4RebootCommand(unsigned char b)
 {
-  if (!px4RebootTimeout) {
+  if (inter_mcu.stable_px4_baud == CHANGING_BAUD && sys_time_check_and_ack_timer(px4bl_tid)) {
-
+    //to prevent a short intermcu comm loss, give some time to changing the baud
+    sys_time_cancel_timer(px4bl_tid);
+    inter_mcu.stable_px4_baud = PPRZ_BAUD;
+  } else if (inter_mcu.stable_px4_baud == PX4_BAUD) {
 
     if (sys_time_check_and_ack_timer(px4bl_tid)) {
       //time out the possibility to reboot to the px4 bootloader, to prevent unwanted restarts during flight
-      px4RebootTimeout = true;
       sys_time_cancel_timer(px4bl_tid);
+      //for unknown reasons, 1500000 baud does not work reliably after prolonged times.
+      //I suspect a temperature related issue, combined with the fbw f1 crystal which is out of specs
+      //After a initial period on 1500000, revert to 230400
+      //We still start at 1500000 to remain compatible with original PX4 firmware. (which always runs at 1500000)
+      uart_periph_set_baudrate(intermcu_device->periph, B230400);
+      inter_mcu.stable_px4_baud = CHANGING_BAUD;
+      px4bl_tid = sys_time_register_timer(1.0, NULL);
       return;
     }
 

sw/airborne/subsystems/intermcu/intermcu_fbw.h

@@ -34,6 +34,7 @@ extern pprz_t intermcu_commands[COMMANDS_NB];
 void intermcu_on_rc_frame(uint8_t fbw_mode);
 void intermcu_send_status(uint8_t mode);
 void InterMcuEvent(void (*frame_handler)(void));
+void intermcu_blink_fbw_led(uint16_t dv);
 
 
 /* We need radio defines for the Autopilot */