Merged paparazzi master into paparazzi dev and resolved conflicts

conf/control_panel.xml.example

@@ -53,6 +53,10 @@
       <arg flag="-b" variable="ivy_bus"/>
     </program>
 
+    <program name="Joystick" command="sw/ground_segment/joystick/input2ivy">
+      <arg flag="-b" variable="ivy_bus"/>
+    </program>
+
     <program name="Hardware in the Loop" command="sw/simulator/simhitl">
       <arg flag="-fbw" variable="fbw_serial_port"/>
       <arg flag="-ap" variable="ap_serial_port"/>

conf/flight_plans/versatile_carto_fixe_muret.xml

@@ -54,6 +54,12 @@ float varsweepsize=110;
       <set value="NAV_MODE_ROLL" var="nav_mode"/>
       <attitude roll="0" throttle="0" vmode="throttle"/>
     </block>
+    <block name="Geo init">
+      <while cond="LessThan(NavBlockTime(), 10)"/>
+      <call fun="NavSetGroundReferenceHere()"/>
+      <deroute block="Holding point"/>
+    </block>
+   
     <block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
       <exception cond="estimator_z > ground_alt+25" deroute="Standby"/>
       <set value="0" var="kill_throttle"/>
@@ -111,10 +117,12 @@ float varsweepsize=110;
 
   
     <block name="Carto survey S1-S2-S3" strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
+      <call fun="nav_survey_computefourth_corner(WP_S1,WP_S2,WP_S3,WP_S4)"/>
       <call fun="nav_survey_losange_carto_init(WP_S1,WP_S2,WP_S3, 50,150)"/>
       <call fun="nav_survey_losange_carto()"/>
     </block>
     <block name="Carto survey S1-S3-S2" strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
+      <call fun="nav_survey_computefourth_corner(WP_S1,WP_S2,WP_S3,WP_S4)"/>
       <call fun="nav_survey_losange_carto_init(WP_S1,WP_S3,WP_S2, 0,150)"/>
       <call fun="nav_survey_losange_carto()"/>
     </block>
@@ -136,10 +144,12 @@ float varsweepsize=110;
     </block>
 
     <block name="Carto survey SP1-SP2-SP3 fin" strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
+      <call fun="nav_survey_computefourth_corner(WP_S1,WP_S2,WP_S3,WP_S4)"/>
       <call fun="nav_survey_losange_carto_init(WP_SP1,WP_SP2,WP_SP3, 20,150)"/>
       <call fun="nav_survey_losange_carto()"/>
     </block>
     <block name="Carto survey SP1-SP3-SP2 fin " strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
+      <call fun="nav_survey_computefourth_corner(WP_S1,WP_S2,WP_S3,WP_S4)"/>
       <call fun="nav_survey_losange_carto_init(WP_SP1,WP_SP3,WP_SP2, 0,150)"/>
       <call fun="nav_survey_losange_carto()"/>
     </block>
@@ -148,11 +158,6 @@ float varsweepsize=110;
     </block>
 
 
-    <block name="Geo init">
-      <while cond="LessThan(NavBlockTime(), 10)"/>
-      <call fun="NavSetGroundReferenceHere()"/>
-    </block>
-   
     <block name="Survey S1-S2" strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
       <survey_rectangle grid="150" wp1="S1" wp2="S2"/>
     </block>

conf/joystick/cockpit_sx.xml

@@ -12,10 +12,10 @@
 
     <message class="datalink" name="RC_4CH" send_always="true">
       <field name="mode" value="mode + 1"/> <!-- only AUTO1 and AUTO2 available -->
-      <field name="throttle" value="Fit(throttle,0-100,100,0,127)"/>
+      <field name="throttle" value="Fit(throttle,-100,100,0,127)"/>
-      <field name="roll" value="roll"/>
+      <field name="roll" value="Fit(roll,-100,100,-127,127)"/>
-      <field name="pitch" value="pitch"/>
+      <field name="pitch" value="Fit(pitch,-100,100,-127,127)"/>
-      <field name="yaw" value="yaw"/>
+      <field name="yaw" value="Fit(yaw,-100,100,-127,127)"/>
     </message>
 
   </messages>

conf/messages.xml

@@ -794,10 +794,14 @@
     <field name="speed_sp" type="float"/>
   </message>
 
- <!--126 is free -->
+  <!--126 is free -->
- <!--127 is free -->
+  <!--127 is free -->
- <!--128 is free -->
+
- <!--129 is free -->
+  <message name="CAMERA_SNAPSHOT" id="128">
+    <field name="snapshot_image_number" type="uint16"/>
+  </message>
+
+  <!--129 is free -->
 
   <message name="STAB_ATTITUDE_FLOAT" id="130">
     <field name="est_p"         type="float" alt_unit="degres/s" alt_unit_coef="57.29578"/>

conf/modules/cartography.xml

@@ -5,18 +5,9 @@
     <file name="cartography.h"/>
   </header>
   <init fun="init_carto()"/>
-  <periodic fun="periodic_1Hz_carto()" freq="1." start="start_carto()" stop="stop_carto()" autorun="TRUE"/>
+  <periodic fun="periodic_downlink_carto()" freq="2." autorun="FALSE"/>
-  <periodic fun="periodic_10Hz_carto()" period="0.1" start="start_carto()" stop="stop_carto()" autorun="FALSE"/>
   <makefile>
-    <raw>
-#Exemple of RAW makefile part
-    </raw>
-    <define name="DEMO_MODULE_LED" value="2"/>
     <file name="cartography.c"/>
   </makefile>
-  <makefile target="sim">
-    <define name="SOME_FLAG"/>
-    <configure name="SOME_DEFINE" value="bla"/>
-  </makefile>
 </module>
 

conf/telemetry/default_fixedwing_imu.xml

@@ -29,6 +29,9 @@
       <message name="IMU_ACCEL"      period=".8"/>
       <message name="IMU_GYRO"       period=".6"/>
       <message name="IMU_MAG"        period="1.3"/>
+      <message name="IMU_ACCEL_RAW"  period="5.2"/>
+      <message name="IMU_GYRO_RAW"   period="5.3"/>
+      <message name="IMU_MAG_RAW"    period="5.4"/>
     </mode>
     <mode name="minimal">
       <message name="ALIVE"          period="5"/>

sw/airborne/firmwares/fixedwing/main_ap.c

@@ -744,3 +744,4 @@ static inline void on_mag_event(void)
 #endif
 }
 #endif // USE_AHRS
+

sw/airborne/math/pprz_algebra_float.h

@@ -518,7 +518,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
     FLOAT_QUAT_NORMALIZE(_a2c);						\
   }
 
-/* _a2c = _a2b comp _b2c , aka  _a2c = _a2b * _b2c */
+/* _a2c = _a2b comp _b2c , aka  _a2c = _b2c * _a2b */
 #define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c) {				\
     (_a2c).qi = (_a2b).qi*(_b2c).qi - (_a2b).qx*(_b2c).qx - (_a2b).qy*(_b2c).qy - (_a2b).qz*(_b2c).qz; \
     (_a2c).qx = (_a2b).qi*(_b2c).qx + (_a2b).qx*(_b2c).qi + (_a2b).qy*(_b2c).qz - (_a2b).qz*(_b2c).qy; \
@@ -526,7 +526,8 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
     (_a2c).qz = (_a2b).qi*(_b2c).qz + (_a2b).qx*(_b2c).qy - (_a2b).qy*(_b2c).qx + (_a2b).qz*(_b2c).qi; \
   }
 
-#define FLOAT_QUAT_MULT(_a2c, _a2b, _b2c) FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)
+/* Quaternion multiplication q_a2c = q_b2c * q_a2b */
+#define FLOAT_QUAT_MULT(_a2c, _b2c, _a2b) FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)
 
 /* _a2b = _a2c comp_inv _b2c , aka  _a2b = _a2c * inv(_b2c) */
 #define FLOAT_QUAT_COMP_INV_NORM_SHORTEST(_a2b, _a2c, _b2c) {		\

sw/airborne/modules/cartography/cartography.c

@@ -90,8 +90,7 @@ uint16_t railnumberSinceBoot=1; //used to count the number of rails the plane ha
 #define USE_ONBOARD_CAMERA
 
 #ifdef USE_ONBOARD_CAMERA
-bool_t CAMERA_SNAPSHOT_REQUIERED=FALSE;  //declared in main_ap.c
+uint16_t camera_snapshot_image_number=0;
-uint16_t camera_snapshot_image_number=0; //declared in main_ap.c
 #endif
 
 
@@ -137,8 +136,11 @@ bool_t ProjectionInsideLimitOfRail;
 
 
 #include "modules/cartography/cartography.h"
-#include "led.h"
+#include "generated/modules.h"
 
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
 #include "downlink.h"
 #include "mcu_periph/uart.h"
 #include "std.h"
@@ -147,33 +149,16 @@ bool_t ProjectionInsideLimitOfRail;
 ////////////////////////////////////////////////////////////////////////////////////////////////
 
 void init_carto(void) {
-  // this part is already done by led_init in fact
-  LED_INIT(DEMO_MODULE_LED);
-  LED_OFF(DEMO_MODULE_LED);
 }
 
-void periodic_1Hz_carto(void) {
+void periodic_downlink_carto(void) {
-	/*uint8_t  toto=3;
+	DOWNLINK_SEND_CAMERA_SNAPSHOT(DefaultChannel,&camera_snapshot_image_number);
-	float lat=3.14159;
-	float lon=1.234;
-    LED_TOGGLE(DEMO_MODULE_LED);
-	DOWNLINK_SEND_MARK(DefaultChannel,&toto,&lat,&lon);
-	 */
-}
-
-void periodic_10Hz_carto(void) {
-  /* LED_TOGGLE(DEMO_MODULE_LED);
-   */
 }
 
 void start_carto(void) {
- /* LED_ON(DEMO_MODULE_LED);
-  */
 }
 
 void stop_carto(void) {
- /* LED_OFF(DEMO_MODULE_LED);
-  */
 }
 
 
@@ -189,7 +174,7 @@ bool_t nav_survey_Snapshoot(void)
 {
 	camera_snapshot_image_number=railnumberSinceBoot;
 	PRTDEBSTR(SNAPSHOT)
-	CAMERA_SNAPSHOT_REQUIERED=TRUE;
+  cartography_periodic_downlink_carto_status = MODULES_START;
 	return FALSE; 
 	
 }
@@ -198,7 +183,7 @@ bool_t nav_survey_Snapshoot_Continu(void)
 {
 	camera_snapshot_image_number=railnumberSinceBoot;
 	PRTDEBSTR(SNAPSHOT)
-	CAMERA_SNAPSHOT_REQUIERED=TRUE;
+  cartography_periodic_downlink_carto_status = MODULES_START;
 	return TRUE; 
 	
 }
@@ -207,7 +192,7 @@ bool_t nav_survey_StopSnapshoot(void)
 {
 	camera_snapshot_image_number=0;
 	PRTDEBSTR(STOP SNAPSHOT)
-	CAMERA_SNAPSHOT_REQUIERED=TRUE;
+  cartography_periodic_downlink_carto_status = MODULES_START;
 	return FALSE; 
 	
 }
@@ -735,8 +720,7 @@ bool_t nav_survey_losange_carto(void)
 	
 	
 	
-	CAMERA_SNAPSHOT_REQUIERED=TRUE;
+  cartography_periodic_downlink_carto_status = MODULES_START;
-	
 	
 	return TRUE; //apparament pour les fonctions de tache=> true
 }

sw/airborne/modules/cartography/cartography.h

@@ -7,8 +7,7 @@
 
  
 void init_carto(void);
-void periodic_1Hz_carto(void);
+void periodic_downlink_carto(void);
-void periodic_10Hz_carto(void);
 void start_carto(void);
 void stop_carto(void);
 

sw/airborne/subsystems/ahrs/ahrs_float_dcm.c

@@ -216,11 +216,11 @@ void ahrs_update_accel(void)
 
   // DCM filter uses g-force as positive
   // accelerometer measures [0 0 -g] in a static case
-
   accel_float.x = -accel_float.x;
   accel_float.y = -accel_float.y;
   accel_float.z = -accel_float.z;
 
+
 #ifdef USE_GPS
   if (gps.fix == GPS_FIX_3D) {    //Remove centrifugal acceleration.
     accel_float.y += gps.speed_3d/100. * Omega[2];  // Centrifugal force on Acc_y = GPS_speed*GyroZ

sw/ground_segment/joystick/input2ivy.ml

@@ -349,14 +349,15 @@ let eval_input = fun buttons axis input ->
 let scale = fun x min max ->
   min + ((x - min_input) * (max - min)) / (max_input - min_input)
 
-(** Fit a given interval of value into [min_input; max_input] *)
-let fit = fun x min max min_input max_input ->
-  min_input + ((x - min) * (max_input - min_input)) / (max - min)
-
 (** Scale a value in the given bounds *)
 let bound = fun x min max ->
   if x < min then min else (if x > max then max else x)
 
+(** Fit a given interval of value into [min_input; max_input] *)
+let fit = fun x min max min_input max_input ->
+  let v = min_input + ((x - min) * (max_input - min_input)) / (max - min) in
+  bound v min_input max_input
+
 (** Return a pprz RC mode
   * mode > max -> 2
   * mode < min -> 0
@@ -371,7 +372,8 @@ let pprz_mode = fun mode ->
 (** Eval a function call (TO BE COMPLETED) *)
 let eval_call = fun f args ->
   match f, args with
-    "-", [a1; a2] -> a1 - a2
+    "-", [a] -> - a
+  | "-", [a1; a2] -> a1 - a2
   | "+", [a1; a2] -> a1 + a2
   | "*", [a1; a2] -> a1 * a2
   | "%", [a1; a2] -> a1 / a2