Merge branch 'master' into telemetry

to resolve
Conflicts:
	sw/airborne/firmwares/fixedwing/ap_downlink.h

conf/firmwares/rotorcraft.makefile

@@ -40,9 +40,6 @@ ROTORCRAFT_INC = -I$(SRC_FIRMWARE) -I$(SRC_BOARD)
 
 ap.ARCHDIR = $(ARCH)
 
-# would be better to auto-generate this
-$(TARGET).CFLAGS 	+= -DFIRMWARE=ROTORCRAFT
-
 ap.CFLAGS += $(ROTORCRAFT_INC)
 ap.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -DPERIPHERALS_AUTO_INIT
 ap.srcs    = $(SRC_FIRMWARE)/main.c

conf/firmwares/subsystems/fixedwing/autopilot.makefile

@@ -35,9 +35,6 @@ $(TARGET).CFLAGS 	+= -DBOARD_CONFIG=$(BOARD_CFG)
 $(TARGET).CFLAGS 	+= -DPERIPHERALS_AUTO_INIT
 $(TARGET).CFLAGS 	+= $(FIXEDWING_INC)
 
-# would be better to auto-generate this
-$(TARGET).CFLAGS 	+= -DFIRMWARE=FIXEDWING
-
 $(TARGET).srcs 	+= mcu.c
 $(TARGET).srcs 	+= $(SRC_ARCH)/mcu_arch.c
 

conf/modules/gps_ubx_ucenter.xml

@@ -11,10 +11,12 @@
 - automatic baudrate detection
 
 Warning: you still need to tell the driver
-      a) which paparazzi uart you use
+1. Which paparazzi uart you use
-      b) inside the ublox gps there are also many ports. the tiny/ppzgps use ublox_internal_port1 but if for instance you use a LS-SAM or I2C device you need to configure:
+2. Inside the ublox gps there are also many ports.
+   The tiny/ppzgps use ublox_internal_port1 (UART1) but if for instance you use a LS-SAM
+   or I2C device you need to set this by defining GPS_PORT_ID.
     </description>
-    <define name="GPS_PORT_ID" value="GPS_PORT_UART2" description=""/>
+    <define name="GPS_PORT_ID" value="GPS_PORT_UART1" description="Port inside the Ublox GPS (GPS_PORT_DDC, GPS_PORT_UART1, GPS_PORT_UART2, GPS_PORT_USB, GPS_PORT_SPI)"/>
   </doc>
   <header>
     <file name="gps_ubx_ucenter.h"/>

sw/airborne/boards/umarim/baro_board.c

@@ -33,7 +33,7 @@
 #include "led.h"
 
 // ADC for absolute pressure
-#define BARO_ABS_ADS
+#ifndef BARO_ABS_ADS
 #define BARO_ABS_ADS ads1114_1
 #endif
 

sw/airborne/firmwares/fixedwing/autopilot.h

@@ -32,7 +32,6 @@
 #include <inttypes.h>
 #include "std.h"
 #include "paparazzi.h"
-#include "mcu_periph/sys_time.h"
 #include "generated/airframe.h"
 
 

sw/airborne/modules/gps/gps_ubx_ucenter.c

@@ -27,7 +27,6 @@
  *
  */
 
-
 #include "gps_ubx_ucenter.h"
 
 //////////////////////////////////////////////////////////////////////////////////////
@@ -295,6 +294,9 @@ static inline void gps_ubx_ucenter_config_port(void)
   #if GPS_PORT_ID == GPS_PORT_UART1 || GPS_PORT_ID == GPS_PORT_UART2
     UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, 0x000008D0, 38400, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0);
   #endif
+  #if GPS_PORT_ID == GPS_PORT_USB
+    UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, 0x0, 0, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0);
+  #endif
 }
 
 #define GPS_SBAS_ENABLED       0x01
@@ -319,6 +321,9 @@ static inline void gps_ubx_ucenter_enable_msg(uint8_t class, uint8_t id, uint8_t
   #if GPS_PORT_ID == GPS_PORT_UART2
     UbxSend_CFG_MSG(class, id, 0, 0, rate, 0);
   #endif
+  #if GPS_PORT_ID == GPS_PORT_USB
+    UbxSend_CFG_MSG(class, id, 0, 0, 0, rate);
+  #endif
   #if GPS_PORT_ID == GPS_PORT_DDC
     UbxSend_CFG_MSG(class, id, rate, 0, 0, 0);
   #endif
@@ -385,10 +390,10 @@ static bool_t gps_ubx_ucenter_configure(uint8_t nr)
     gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SVINFO_ID, 4);
     break;
   case 11:
-#if defined FIRMWARE && FIRMWARE == ROTORCRAFT
+#if GPS_UBX_UCENTER_SLOW_NAV_SOL
-    gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SOL_ID, 1);
-#else
     gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SOL_ID, 8);
+#else
+    gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SOL_ID, 1);
 #endif
     break;
   case 12:
@@ -416,7 +421,3 @@ static bool_t gps_ubx_ucenter_configure(uint8_t nr)
   }
   return TRUE; // Continue, except for the last case
 }
-
-
-
-

sw/airborne/modules/sensors/baro_mpl3115.c

@@ -20,7 +20,7 @@
  */
 
 /**
- * @file modules/sensors/baro_mpl3155.c
+ * @file modules/sensors/baro_mpl3115.c
  *
  * Module for the baro MPL3115A2 from Freescale (i2c)
  *

sw/airborne/modules/sensors/baro_mpl3115.h

@@ -20,7 +20,7 @@
  */
 
 /**
- * @file modules/sensors/baro_mpl3155.h
+ * @file modules/sensors/baro_mpl3115.h
  *
  * Module for the baro MPL3115A2 from Freescale (i2c)
  *

sw/airborne/test/ahrs/Makefile

@@ -3,14 +3,13 @@ Q=@
 
 
 CC = gcc
-CFLAGS = -std=c99 -I.. -I../.. -I../../../include -I../../booz -I../../../booz  -Wall
+CFLAGS = -std=c99 -I.. -I../.. -I../../../include  -Wall
 LDFLAGS = -lm -lgsl -lgslcblas
 
 CFLAGS +=
 
 # imu wants airframe to fetch its neutrals
 # ahrs wants airframe to fetch IMU_BODY_TO_IMU_ANGLES
-#CFLAGS += -I../../../../var/BOOZ2_A7
 CFLAGS += -I../../../../var/ahrs_test
 
 
@@ -51,28 +50,15 @@ AHRS_TYPE = AHRS_TYPE_ICQ
 #AHRS_TYPE = AHRS_TYPE_FCR2
 #AHRS_TYPE = AHRS_TYPE_FCQ
 endif
-ifndef PROPAGATE_LOW_PASS_RATES
-PROPAGATE_LOW_PASS_RATES = 0
-endif
-ifndef GRAVITY_UPDATE_NORM_HEURISTIC
-GRAVITY_UPDATE_NORM_HEURISTIC = 0
-endif
-ifndef GRAVITY_UPDATE_COORDINATED_TURN
-GRAVITY_UPDATE_COORDINATED_TURN = 0
-endif
-ifndef MAG_UPDATE_YAW_ONLY
-MAG_UPDATE_YAW_ONLY = 0
-endif
-ifndef DISABLE_MAG_UPDATE
-DISABLE_MAG_UPDATE = 0
-endif
-ifndef USE_GPS_HEADING
-USE_GPS_HEADING = 0
-endif
 
-ifndef FREQUENCY
+PROPAGATE_LOW_PASS_RATES ?= 0
-FREQENCY = 512
+GRAVITY_UPDATE_NORM_HEURISTIC ?= 0
-endif
+GRAVITY_UPDATE_COORDINATED_TURN ?= 0
+MAG_UPDATE_YAW_ONLY ?= 0
+DISABLE_MAG_UPDATE ?= 0
+USE_GPS_HEADING ?= 0
+
+FREQENCY ?= 512
 
 
 AHRS_CFLAGS += -DAHRS_TYPE=$(AHRS_TYPE)
@@ -114,10 +100,6 @@ ifeq ($(AHRS_TYPE), AHRS_TYPE_ICQ)
 AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat.h\"
 AHRS_SRCS   += ../../subsystems/ahrs/ahrs_int_cmpl_quat.c
 endif
-ifeq ($(AHRS_TYPE), AHRS_TYPE_FLQ)
-AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_lkf_quat.h\"
-AHRS_SRCS   += ../../subsystems/ahrs/ahrs_float_lkf_quat.c
-endif
 ifeq ($(AHRS_TYPE), AHRS_TYPE_FCR)
 AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\"
 AHRS_SRCS   += ../../subsystems/ahrs/ahrs_float_dcm.c
@@ -135,9 +117,14 @@ endif
 
 
 RAOS_SRCS =  ./ahrs_on_synth.c			 \
+			 ../../state.c \
 			 ../../subsystems/imu.c              \
 			 ../../subsystems/imu/imu_dummy.c    \
 			 ../../math/pprz_trig_int.c          \
+			 ../../math/pprz_orientation_conversion.c \
+			 ../../math/pprz_geodetic_int.c \
+			 ../../math/pprz_geodetic_float.c \
+			 ../../math/pprz_geodetic_double.c \
 			 ../../../simulator/nps/nps_random.c
 
 

sw/airborne/test/ahrs/ahrs_utils.py

@@ -31,6 +31,7 @@ from array import array
 import numpy
 import matplotlib.pyplot as plt
 
+
 def run_simulation(ahrs_type, build_opt, traj_nb):
     print "\nBuilding ahrs"
     args = ["make", "clean", "run_ahrs_on_synth", "AHRS_TYPE=AHRS_TYPE_" + ahrs_type] + build_opt
@@ -44,7 +45,8 @@ def run_simulation(ahrs_type, build_opt, traj_nb):
 
     print "Running simulation"
     print "   using traj " + str(traj_nb)
-    p = subprocess.Popen(args=["./run_ahrs_on_synth",str(traj_nb)], stdout=subprocess.PIPE, stderr=subprocess.STDOUT, shell=False)
+    p = subprocess.Popen(args=["./run_ahrs_on_synth", str(traj_nb)], stdout=subprocess.PIPE, stderr=subprocess.STDOUT,
+                         shell=False)
     outputlines = p.stdout.readlines()
     p.wait()
     #    for i in outputlines:
@@ -57,13 +59,8 @@ def run_simulation(ahrs_type, build_opt, traj_nb):
                       ('bp_true', 'float32'), ('bq_true', 'float32'), ('br_true', 'float32'),
                       ('phi_ahrs', 'float32'), ('theta_ahrs', 'float32'), ('psi_ahrs', 'float32'),
                       ('p_ahrs', 'float32'), ('q_ahrs', 'float32'), ('r_ahrs', 'float32'),
-                      ('bp_ahrs',  'float32'), ('bq_ahrs',    'float32'), ('br_ahrs',  'float32')
+                      ('bp_ahrs', 'float32'), ('bq_ahrs', 'float32'), ('br_ahrs', 'float32')]
-                     ]
+
-    pos_data_type = [ ('x0_true', 'float32'), ('y0_true', 'float32'), ('z0_true', 'float32'),
-                      ('x1_true', 'float32'), ('y1_true', 'float32'), ('z1_true', 'float32'),
-                      ('x2_true', 'float32'), ('y2_true', 'float32'), ('z2_true', 'float32'),
-                      ('x3_true', 'float32'), ('y3_true', 'float32'), ('z3_true', 'float32'),
-                    ]
     mydescr = numpy.dtype(ahrs_data_type)
     data = [[] for dummy in xrange(len(mydescr))]
     #    import code; code.interact(local=locals())
@@ -71,7 +68,7 @@ def run_simulation(ahrs_type, build_opt, traj_nb):
         if line.startswith("#"):
             print "   " + line,
         else:
-            fields = line.strip().split(' ');
+            fields = line.strip().split(' ')
             #        print fields
             for i, number in enumerate(fields):
                 data[i].append(number)
@@ -83,7 +80,6 @@ def run_simulation(ahrs_type, build_opt, traj_nb):
     return numpy.rec.array(data, dtype=mydescr)
 
 
-
 def plot_simulation_results(plot_true_state, lsty, type, sim_res):
     print "Plotting Results"
 
@@ -132,7 +128,6 @@ def plot_simulation_results(plot_true_state, lsty, type, sim_res):
     xlabel('time in s')
     title('br')
 
-
     if plot_true_state:
         subplot(3, 3, 1)
         plt.plot(sim_res.time, sim_res.phi_true, 'r--')
@@ -154,6 +149,5 @@ def plot_simulation_results(plot_true_state, lsty, type, sim_res):
         plot(sim_res.time, sim_res.br_true, 'r--')
 
 
-
 def show_plot():
-    plt.show();
+    plt.show()

sw/airborne/test/ahrs/compare_ahrs.py

@@ -24,51 +24,51 @@ import ahrs_utils
 
 def main():
 
-#   traj_nb = 0; # static
+#   traj_nb = 0  # static
-#   traj_nb = 1; # sine orientation
+#   traj_nb = 1  # sine orientation
-#   traj_nb = 2; # sine X quad
+#   traj_nb = 2  # sine X quad
-#   traj_nb = 3; # step_phi
+#   traj_nb = 3  # step_phi
-#   traj_nb = 4; # step_phi_2nd_order
+#   traj_nb = 4  # step_phi_2nd_order
-#   traj_nb = 5; # step_bias
+#   traj_nb = 5  # step_bias
-#   traj_nb = 6; # coordinated circle
+#   traj_nb = 6  # coordinated circle
-#   traj_nb = 7; # stop stop X quad
+#   traj_nb = 7  # stop stop X quad
-    traj_nb = 8; # bungee takeoff
+    traj_nb = 8  # bungee takeoff
 
-    build_opt1 = [];
+    build_opt1 = []
-#   build_opt1 = ["Q="];
+#   build_opt1 = ["Q="]
-    build_opt1 += ["FREQUENCY=120"];
+    build_opt1 += ["FREQUENCY=120"]
-#   build_opt1 += ["PROPAGATE_LOW_PASS_RATES=1"];
+#   build_opt1 += ["PROPAGATE_LOW_PASS_RATES=1"]
-    build_opt1 += ["GRAVITY_UPDATE_COORDINATED_TURN=1"];
+    build_opt1 += ["GRAVITY_UPDATE_COORDINATED_TURN=1"]
-    build_opt1 += ["GRAVITY_UPDATE_NORM_HEURISTIC=1"];
+    build_opt1 += ["GRAVITY_UPDATE_NORM_HEURISTIC=1"]
-#   build_opt1 += ["DISABLE_MAG_UPDATE=1"];
+#   build_opt1 += ["DISABLE_MAG_UPDATE=1"]
-#   build_opt1 += ["USE_GPS_HEADING=1"];
+#   build_opt1 += ["USE_GPS_HEADING=1"]
-#   build_opt1 += ["MAG_UPDATE_YAW_ONLY=1"];
+#   build_opt1 += ["MAG_UPDATE_YAW_ONLY=1"]
 
-#   ahrs_type1 = "FCQ";
+#   ahrs_type1 = "FCQ"
-#   ahrs_type1 = "FCR2";
+#   ahrs_type1 = "FCR2"
-#   ahrs_type1 = "FLQ";
+#   ahrs_type1 = "FLQ"
-    ahrs_type1 = "ICQ";
+    ahrs_type1 = "ICQ"
-#   ahrs_type1 = "FCR";
+#   ahrs_type1 = "FCR"
 
     sim_res1 = ahrs_utils.run_simulation(ahrs_type1, build_opt1, traj_nb)
 #   import code; code.interact(local=locals())
 
-    build_opt2 = [];
+    build_opt2 = []
-    build_opt2 += ["FREQUENCY=120"];
+    build_opt2 += ["FREQUENCY=120"]
-    build_opt2 += ["GRAVITY_UPDATE_COORDINATED_TURN=1"];
+    build_opt2 += ["GRAVITY_UPDATE_COORDINATED_TURN=1"]
-    build_opt2 += ["GRAVITY_UPDATE_NORM_HEURISTIC=1"];
+    build_opt2 += ["GRAVITY_UPDATE_NORM_HEURISTIC=1"]
-    build_opt2 += ["USE_AHRS_GPS_ACCELERATIONS=1"];
+    build_opt2 += ["USE_AHRS_GPS_ACCELERATIONS=1"]
-#   build_opt2 += ["DISABLE_MAG_UPDATE=1"];
+#   build_opt2 += ["DISABLE_MAG_UPDATE=1"]
-#   build_opt2 += ["USE_GPS_HEADING=1"];
+#   build_opt2 += ["USE_GPS_HEADING=1"]
-#   build_opt2 += ["MAG_UPDATE_YAW_ONLY=0"];
+#   build_opt2 += ["MAG_UPDATE_YAW_ONLY=0"]
-#   build_opt2 += ["PROPAGATE_LOW_PASS_RATES=1"];
+#   build_opt2 += ["PROPAGATE_LOW_PASS_RATES=1"]
-#   build_opt2 = build_opt1;
+#   build_opt2 = build_opt1
 
-#   ahrs_type2 = "FLQ";
+#   ahrs_type2 = "FLQ"
-#   ahrs_type2 = "FCR2";
+#   ahrs_type2 = "FCR2"
-#   ahrs_type2 = "ICQ";
+#   ahrs_type2 = "ICQ"
-    ahrs_type2 = "FCR";
+    ahrs_type2 = "FCR"
-#   ahrs_type2 = ahrs_type1;
+#   ahrs_type2 = ahrs_type1
 
     sim_res2 = ahrs_utils.run_simulation(ahrs_type2, build_opt2, traj_nb)
 

sw/airborne/test/ahrs/run_ahrs_on_synth.c

@@ -37,10 +37,6 @@ static void report(void) {
   int output_sensors = FALSE;
   int output_pos = FALSE;
 
-#if AHRS_TYPE == AHRS_TYPE_ICE || AHRS_TYPE == AHRS_TYPE_ICQ
-  EULERS_FLOAT_OF_BFP(ahrs_float.ltp_to_imu_euler, ahrs.ltp_to_imu_euler);
-  RATES_FLOAT_OF_BFP(ahrs_float.imu_rate, ahrs.imu_rate);
-#endif
   printf("%f ", aos.time);
 
   printf("%f %f %f ", DegOfRad(aos.ltp_to_imu_euler.phi),
@@ -55,23 +51,37 @@ static void report(void) {
 	              DegOfRad(aos.gyro_bias.q),
 	              DegOfRad(aos.gyro_bias.r));
 
-
-  printf("%f %f %f ", DegOfRad(ahrs_float.ltp_to_imu_euler.phi),
-	              DegOfRad(ahrs_float.ltp_to_imu_euler.theta),
-                      DegOfRad(ahrs_float.ltp_to_imu_euler.psi));
-
-  printf("%f %f %f ", DegOfRad(ahrs_float.imu_rate.p),
-	              DegOfRad(ahrs_float.imu_rate.q),
-	              DegOfRad(ahrs_float.imu_rate.r));
-
 #if AHRS_TYPE == AHRS_TYPE_ICQ
+  struct Int32Eulers ltp_to_imu_euler_i;
+  INT32_EULERS_OF_QUAT(ltp_to_imu_euler_i, ahrs_impl.ltp_to_imu_quat);
+  struct FloatEulers ltp_to_imu_euler_f;
+  EULERS_FLOAT_OF_BFP(ltp_to_imu_euler_f, ltp_to_imu_euler_i);
+  printf("%f %f %f ", DegOfRad(ltp_to_imu_euler_f.phi),
+         DegOfRad(ltp_to_imu_euler_f.theta),
+         DegOfRad(ltp_to_imu_euler_f.psi));
+
+  struct FloatRates imu_rate_f;
+  RATES_FLOAT_OF_BFP(imu_rate_f, ahrs_impl.imu_rate);
+  printf("%f %f %f ", DegOfRad(imu_rate_f.p),
+         DegOfRad(imu_rate_f.q),
+         DegOfRad(imu_rate_f.r));
+
   struct FloatRates bias;
   RATES_FLOAT_OF_BFP(bias, ahrs_impl.gyro_bias);
   printf("%f %f %f ", DegOfRad(bias.p),
 	             DegOfRad(bias.q),
 	             DegOfRad(bias.r));
-#endif
+
-#if AHRS_TYPE == AHRS_TYPE_FLQ || AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FCR
+#elif AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FCR
+
+  printf("%f %f %f ", DegOfRad(ahrs_impl.ltp_to_imu_euler.phi),
+         DegOfRad(ahrs_impl.ltp_to_imu_euler.theta),
+         DegOfRad(ahrs_impl.ltp_to_imu_euler.psi));
+
+  printf("%f %f %f ", DegOfRad(ahrs_impl.imu_rate.p),
+         DegOfRad(ahrs_impl.imu_rate.q),
+         DegOfRad(ahrs_impl.imu_rate.r));
+
   printf("%f %f %f ", DegOfRad(ahrs_impl.gyro_bias.p),
 	             DegOfRad(ahrs_impl.gyro_bias.q),
 	             DegOfRad(ahrs_impl.gyro_bias.r));