Merge branch 'master' into obc_svinfo

conf/boards/opa_ap_1.0.makefile

@@ -42,7 +42,7 @@ GPS_PORT ?= UART1
 GPS_BAUD ?= B57600
 
 INTERMCU_PORT ?= UART3
-INTERMCU_BAUD ?= B230400
+INTERMCU_BAUD ?= B460800
 
 #
 # default IMU configuration

conf/boards/opa_ftd_1.0.makefile

@@ -40,7 +40,10 @@ MODEM_PORT ?= UART3
 MODEM_BAUD ?= B19200
 
 INTERMCU_PORT ?= UART2
-INTERMCU_BAUD ?= B230400
+INTERMCU_BAUD ?= B460800
+
+GPS_PORT ?= UART4
+GPS_BAUD ?= B57600
 
 #
 # default actuator configuration

conf/firmwares/subsystems/shared/radio_control_spektrum.makefile

@@ -1,6 +1,7 @@
 #
 # Makefile for shared radio_control spektrum susbsytem
 #
+# Define USE_DSMX on STM32 microcontrollers to bind in DSMX instead of DSM2
 
 RADIO_CONTROL_LED ?= none
 

sw/airborne/arch/stm32/mcu_periph/uart_arch.h

@@ -39,6 +39,7 @@
 #define B100000   100000
 #define B115200   115200
 #define B230400   230400
+#define B460800   460800
 #define B921600   921600
 #define B1500000 1500000
 #define B3000000 3000000

sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c

@@ -49,8 +49,13 @@ INFO("Radio-Control now follows PPRZ sign convention: this means you might need
 #define ONE_MHZ 1000000
 
 /* Number of low pulses sent to satellite receivers */
+#ifdef USE_DSMX
+#define MASTER_RECEIVER_PULSES 9
+#define SLAVE_RECEIVER_PULSES 10
+#else
 #define MASTER_RECEIVER_PULSES 5
 #define SLAVE_RECEIVER_PULSES 6
+#endif
 
 #define TIM_TICS_FOR_100us 100
 #define MIN_FRAME_SPACE  70  // 7ms

sw/airborne/firmwares/rotorcraft/navigation.c

@@ -307,7 +307,7 @@ void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
 
 bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time)
 {
-  int32_t dist_to_point;
+  float dist_to_point;
   struct Int32Vect2 diff;
   struct EnuCoor_i *pos = stateGetPositionEnu_i();
 
@@ -326,13 +326,14 @@ bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t
     VECT2_DIFF(diff, *wp, *pos);
   }
   /* compute distance of estimated/current pos to target wp
-   * distance with half metric precision (6.25 cm)
+   * POS_FRAC resolution
+   * convert to float to compute the norm without overflow in 32bit
    */
-  INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC / 2);
-  dist_to_point = int32_vect2_norm(&diff);
+  struct FloatVect2 diff_f = {POS_FLOAT_OF_BFP(diff.x), POS_FLOAT_OF_BFP(diff.y)};
+  dist_to_point = float_vect2_norm(&diff_f);
 
   /* return TRUE if we have arrived */
-  if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC / 2)) {
+  if (dist_to_point < ARRIVED_AT_WAYPOINT) {
     return true;
   }
 
@@ -341,8 +342,8 @@ bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t
     /* return TRUE if normal line at the end of the segment is crossed */
     struct Int32Vect2 from_diff;
     VECT2_DIFF(from_diff, *wp, *from);
-    INT32_VECT2_RSHIFT(from_diff, from_diff, INT32_POS_FRAC / 2);
-    return (diff.x * from_diff.x + diff.y * from_diff.y < 0);
+    struct FloatVect2 from_diff_f = {POS_FLOAT_OF_BFP(from_diff.x), POS_FLOAT_OF_BFP(from_diff.y)};
+    return (diff_f.x * from_diff_f.x + diff_f.y * from_diff_f.y < 0);
   }
 
   return false;
@@ -351,7 +352,7 @@ bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t
 bool nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time)
 {
   uint16_t time_at_wp;
-  uint32_t dist_to_point;
+  float dist_to_point;
   static uint16_t wp_entry_time = 0;
   static bool wp_reached = false;
   static struct EnuCoor_i wp_last = { 0, 0, 0 };
@@ -361,10 +362,11 @@ bool nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time)
     wp_reached = false;
     wp_last = *wp;
   }
+
   VECT2_DIFF(diff, *wp, *stateGetPositionEnu_i());
-  INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC / 2);
-  dist_to_point = int32_vect2_norm(&diff);
-  if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC / 2)) {
+  struct FloatVect2 diff_f = {POS_FLOAT_OF_BFP(diff.x), POS_FLOAT_OF_BFP(diff.y)};
+  dist_to_point = float_vect2_norm(&diff_f);
+  if (dist_to_point < ARRIVED_AT_WAYPOINT) {
     if (!wp_reached) {
       wp_reached = true;
       wp_entry_time = autopilot_flight_time;

sw/airborne/subsystems/imu/imu_bebop.c

@@ -70,6 +70,8 @@ PRINT_CONFIG_VAR(BEBOP_GYRO_RANGE)
 #endif
 PRINT_CONFIG_VAR(BEBOP_ACCEL_RANGE)
 
+struct OrientationReps imu_to_mag_bebop;    ///< IMU to magneto rotation
+
 /** Basic Navstik IMU data */
 struct ImuBebop imu_bebop;
 
@@ -87,6 +89,15 @@ void imu_bebop_init(void)
 
   /* AKM8963 */
   ak8963_init(&imu_bebop.ak, &(BEBOP_MAG_I2C_DEV), AK8963_ADDR);
+
+#if BEBOP_VERSION2
+  //the magnetometer of the bebop2 is located on the gps board,
+  //which is under a slight angle
+  struct FloatEulers imu_to_mag_eulers =
+          {0.0, RadOfDeg(8.5), 0.0};
+  orientationSetEulers_f(&imu_to_mag_bebop, &imu_to_mag_eulers);
+#endif
+
 }
 
 /**
@@ -132,8 +143,13 @@ void imu_bebop_event(void)
 
   if (imu_bebop.ak.data_available) {
 #if BEBOP_VERSION2
+    struct Int32Vect3 mag_temp;
     // In the second bebop version the magneto is turned 90 degrees
-    VECT3_ASSIGN(imu.mag_unscaled, -imu_bebop.ak.data.vect.x, -imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.z);
+    VECT3_ASSIGN(mag_temp, -imu_bebop.ak.data.vect.x, -imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.z);
+
+    // Rotate the magneto
+    struct Int32RMat *imu_to_mag_rmat = orientationGetRMat_i(&imu_to_mag_bebop);
+    int32_rmat_vmult(&imu.mag_unscaled, imu_to_mag_rmat, &mag_temp);
 #else //BEBOP regular first verion
     VECT3_ASSIGN(imu.mag_unscaled, -imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.x, imu_bebop.ak.data.vect.z);
 #endif

sw/lib/ocaml/fp_proc.ml

@@ -121,6 +121,9 @@ let transform_stage = fun prefix reroutes env xml ->
             | "attitude" ->
               let attribs = transform_values ["vmode"] env attribs in
               Xml.Element (tag, attribs, children)
+            | "manual" ->
+              let attribs = transform_values [] env attribs in
+              Xml.Element (tag, attribs, children)
             | "go" ->
               assert (children=[]);
               let attribs = transform_values ["wp";"from";"hmode";"vmode"] env attribs in
@@ -226,6 +229,7 @@ let parse_include = fun dir flight_plan include_xml ->
 
     let waypoints = get_children "waypoints" proc
     and exceptions = get_children "exceptions" proc
+    and modules = get_children "modules" proc
     and blocks = get_children "blocks" proc
     and sectors = get_children "sectors" proc
     and header = get_pc_data "header" proc in
@@ -237,6 +241,7 @@ let parse_include = fun dir flight_plan include_xml ->
       append_children
       ["waypoints", waypoints;
        "blocks", blocks;
+       "modules", modules;
        "exceptions", exceptions;
        "sectors", sectors]
       (append_pc_data "header" header flight_plan)