Merge remote-tracking branch 'paparazzi/master' into corsica_merge

conf/airframes/ENAC/quadrotor/blender.xml

@@ -29,6 +29,7 @@
     <subsystem name="actuators"     type="asctec"/>
     <subsystem name="imu"           type="b2_v1.1"/>
     <subsystem name="gps"           type="ublox"/>
+    <subsystem name="stabilization"   type="euler"/>
     <subsystem name="ahrs"          type="cmpl"/>
     <subsystem name="ins"           type="hff"/>
   </firmware>

conf/airframes/ENAC/quadrotor/booz2_g1.xml

@@ -34,6 +34,7 @@
     <subsystem name="gps"           type="ublox">
       <configure name="GPS_BAUD"    value="B57600"/>
     </subsystem>
+    <subsystem name="stabilization"   type="euler"/>
     <subsystem name="ahrs"          type="cmpl"/>
     <subsystem name="ins"           type="hff"/>
   </firmware>

conf/airframes/ENAC/quadrotor/mkk1.xml

@@ -16,6 +16,7 @@
     <subsystem name="actuators"     type="mkk"/>
     <subsystem name="imu"           type="b2_v1.1"/>
     <subsystem name="gps"           type="ublox"/>
+    <subsystem name="stabilization"   type="euler"/>
     <subsystem name="ahrs"          type="cmpl"/>
     <subsystem name="ins"           type="hff"/>
   </firmware>

conf/airframes/ENAC/quadrotor/nova1.xml

@@ -15,6 +15,7 @@
     <subsystem name="actuators"     type="mkk"/>
     <subsystem name="imu"           type="b2_v1.1"/>
     <subsystem name="gps"           type="ublox"/>
+    <subsystem name="stabilization"   type="euler"/>
     <subsystem name="ahrs"          type="cmpl"/>
     <subsystem name="ins"           type="hff"/>
   </firmware>

conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml

@@ -206,13 +206,12 @@
     -->
     <!--    <load name="digital_cam_i2c.xml"/>  -->
 
- <!--     -->
+ <!--    <load name="ins_ppzuavimu.xml" /> -->
 
 <!--
     <load name="digital_cam.xml" >
       <define name="DC_SHUTTER_LED" value="3"/>
     </load>
-    <load name="ins_ppzuavimu.xml" />
     -->
   </modules>
 
@@ -243,13 +242,13 @@
       <define name="AHRS_GRAVITY_UPDATE_COORDINATED_TURN" />
     </subsystem>
     <subsystem name="imu" type="aspirin_i2c"/>
--->
+    -->
-    
     <subsystem name="imu" type="ppzuav"/>
     <subsystem name="attitude" type="dcm">
-      <define name="USE_MAGNETOMETER" />
+<!--      <define name="USE_MAGNETOMETER" /> -->
     </subsystem>
 
+
     <subsystem name="radio_control" type="ppm"/>
 
     <!-- Communication -->

conf/airframes/Poine/beth.xml

@@ -142,7 +142,7 @@ main_stm32.srcs += subsystems/imu.c             \
                         arch/$(ARCH)/subsystems/imu/imu_crista_arch.c
 main_stm32.CFLAGS += -DUSE_DMA1_C4_IRQ
 
-main_stm32.srcs += $(SRC_BOOZ)/booz2_commands.c
+main_stm32.srcs += $(SRC_FIRMWARE)/commands.c
 main_stm32.srcs += firmwares/rotorcraft/actuators/actuators_asctec.c 
 #\
 #                             $(SRC_BOOZ_ARCH)/actuators/actuators_asctec_arch.c

conf/airframes/Poine/booz2_a1.xml

@@ -206,6 +206,7 @@
     <subsystem name="actuators"     type="mkk"/>
     <subsystem name="imu"           type="b2_v1.0"/>
     <subsystem name="gps"           type="ublox"/>
+    <subsystem name="stabilization"   type="euler"/>
     <subsystem name="ahrs"          type="cmpl"/>
     <subsystem name="ins"           type="hff"/>
   </firmware>

conf/airframes/Poine/booz2_a7.xml

@@ -235,6 +235,7 @@
       <configure name="GPS_BAUD"    value="B57600"/>
     </subsystem>
     <!--subsystem name="ahrs"          type="cmpl"/-->
+    <subsystem name="stabilization"   type="euler"/>
     <subsystem name="ahrs"          type="ic"/>
     <subsystem name="ins"           type="hff"/>
   </firmware>

conf/airframes/Poine/h_hex.xml

@@ -185,6 +185,7 @@
 <!--    <subsystem name="imu"           type="b2_v1.1"/> -->
     <subsystem name="imu"           type="aspirin"/>
     <subsystem name="gps"           type="ublox"/>
+    <subsystem name="stabilization"   type="euler"/>
     <subsystem name="ahrs"          type="cmpl"/>
   </firmware>
 

conf/airframes/TU_Delft/MicrojetBR.xml

@@ -5,9 +5,9 @@
 <!-- commands section -->
   <servos>
     <servo name="THROTTLE" no="0" min="1100" neutral="1100" max="1700"/>
-    <servo name="AILEVON_LEFT" no="1" min="1000" neutral="1615" max="2000"/>
+    <servo name="AILEVON_LEFT" no="1" min="2000" neutral="1500" max="1000"/>
-    <servo name="AILEVON_RIGHT" no="3" min="2000" neutral="1465" max="1000"/>
+    <servo name="AILEVON_RIGHT" no="3" min="950" neutral="1450" max="1950"/>
-    <servo name="CAM_PAN" no="5" min="2000" neutral="1550" max="1000"/>
+    <servo name="CAM_PAN" no="5" min="1950" neutral="1450" max="950"/>
     <servo name="CAM_TILT" no="2" min="1000" neutral="1550" max="2000"/>
   </servos>
 
@@ -253,7 +253,7 @@
 
     <!-- Communication -->
     <subsystem name="telemetry" 	type="transparent">
-      <configure name="MODEM_BAUD" 		value="57600"/>
+      <configure name="MODEM_BAUD" 		value="B57600"/>
     </subsystem>
 
     <!-- Sensors -->

conf/airframes/TU_Delft/MicrojetCDW.xml

@@ -5,7 +5,7 @@
 <!-- commands section -->
   <servos>
     <servo name="THROTTLE" no="0" min="1050" neutral="1050" max="1900"/>
-    <servo name="AILEVON_RIGHT" no="1" min="1850" neutral="1480" max="1100"/> <!-- 400 - 380 --> 
+    <servo name="AILEVON_RIGHT" no="1" min="1850" neutral="1480" max="1100"/> <!-- 400 - 380 -->
     <servo name="AILEVON_LEFT" no="2" min="1250" neutral="1580" max="1980"/> <!-- 300 - 400 -->
   </servos>
 
@@ -28,7 +28,7 @@
 
   <command_laws>
     <let var="aileron" value="@ROLL  * AILEVON_AILERON_RATE"/>
-    <let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/>  
+    <let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/>
 
     <set servo="THROTTLE" value="@THROTTLE"/>
     <set servo="AILEVON_LEFT" value="$elevator + $aileron"/>
@@ -42,7 +42,7 @@
 
 
   <section name="IMU" prefix="IMU_">
-    <!-- MAX1168 ADC CHANNELS --> 
+    <!-- MAX1168 ADC CHANNELS -->
     <define name="GYRO_P_CHAN" value="0"/>
     <define name="GYRO_Q_CHAN" value="1"/>
     <define name="GYRO_R_CHAN" value="2"/>
@@ -72,18 +72,20 @@
     <define name="GYRO_Q_SIGN" value="-1"/>
     <define name="GYRO_R_SIGN" value="-1"/>
 
-    <define name="ACCEL_X_NEUTRAL" value="26424"/>
+    <define name="ACCEL_X_NEUTRAL" value="26584"/>
-    <define name="ACCEL_Y_NEUTRAL" value="26640"/>
+    <define name="ACCEL_Y_NEUTRAL" value="26660"/>
     <define name="ACCEL_Z_NEUTRAL" value="26732"/>
 
-    <!-- SENS = 660mV/g / 9.81 ms2/g = 67.27 mV/ms2 * 16 LSB/mV = 1076 LSB/ms2 / 10bit FRAC -->
+    <!-- 3.5G: SENS = 330mV/g / 9.81 ms2/g = 33.63 mV/ms2 * 16 LSB/mV = 538.22 LSB/ms2 / 10bit FRAC -->
-    <define name="ACCEL_X_SENS" value="0.9372" integer="16"/>
+    <!-- 2G: SENS = 660mV/g / 9.81 ms2/g = 67.27 mV/ms2 * 16 LSB/mV = 1076 LSB/ms2 / 10bit FRAC -> 0.9372-->
-    <define name="ACCEL_Y_SENS" value="0.9346" integer="16"/>
+    <!-- 6G: SENS = 220mV/g / 9.81 ms2/g = 67.27 mV/ms2 * 16 LSB/mV = 358.8 LSB/ms2 / 10bit FRAC -> 2.853818182-->
-    <define name="ACCEL_Z_SENS" value="0.9178" integer="16"/>
+    <define name="ACCEL_X_SENS" value="2.85382" integer="10"/>
+    <define name="ACCEL_Y_SENS" value="2.85382" integer="10"/>
+    <define name="ACCEL_Z_SENS" value="2.85382" integer="10"/>
 
-    <define name="ACCEL_X_SIGN" value="-1"/>
+    <define name="ACCEL_X_SIGN" value="1"/>
-    <define name="ACCEL_Y_SIGN" value="1"/>
+    <define name="ACCEL_Y_SIGN" value="-1"/>
-    <define name="ACCEL_Z_SIGN" value="1"/>
+    <define name="ACCEL_Z_SIGN" value="-1"/>
 
     <define name="MAG_X_NEUTRAL" value="0"/>
     <define name="MAG_Y_NEUTRAL" value="0"/>
@@ -92,7 +94,7 @@
     <define name="MAG_X_SENS" value="1" integer="16"/>
     <define name="MAG_Y_SENS" value="1" integer="16"/>
     <define name="MAG_Z_SENS" value="1" integer="16"/>
-    
+
 <!--    <define name="MAG_45_HACK" value="1"/> -->
 
     <define name="BODY_TO_IMU_PHI" value="0"/>
@@ -161,7 +163,7 @@
     <define name="ROLL_ATTITUDE_GAIN" value="-6000."/>
     <define name="ROLL_RATE_GAIN" value="-600."/>
 
-    <define name="ROLL_SLEW" value="0.02"/> <!-- Maximal roll angle change per 1/60 of second: 0.02 rad/loop * 180/pi * 60 loop/sec = 60 deg/sec --> 
+    <define name="ROLL_SLEW" value="0.02"/> <!-- Maximal roll angle change per 1/60 of second: 0.02 rad/loop * 180/pi * 60 loop/sec = 60 deg/sec -->
   </section>
 
   <section name="AGGRESSIVE" prefix="AGR_">
@@ -190,7 +192,7 @@
   <firmware name="fixedwing">
     <target name="ap" board="tiny_1.1">
       <configure name="PERIODIC_FREQUENCY" value="960"/> <!-- IMU FREQ -->
-      <configure name="AHRS_PROPAGATE_FREQUENCY" value="240"/>
+      <configure name="AHRS_PROPAGATE_FREQUENCY" value="120"/>
       <configure name="AHRS_CORRECT_FREQUENCY" value="60"/>
       <configure name="AHRS_ALIGNER_LED" value="1"/>
       <configure name="CPU_LED" value="2"/>

conf/airframes/TU_Delft/skywalker.xml

@@ -0,0 +1,242 @@
+<!DOCTYPE airframe SYSTEM "../airframe.dtd">
+
+<!--
+	YAPA + XSens + XBee
+-->
+
+<airframe name="Yapa v1">
+
+  <servos>
+    <servo name="THROTTLE" no="0" min="1100" neutral="1100" max="1900"/>
+    <servo name="AILERON_LEFT" no="2" min="900" neutral="1500" max="2100"/>
+    <servo name="AILERON_RIGHT" no="6" min="900" neutral="1500" max="2100"/>
+    <servo name="ELEVATOR" no="7" min="1900" neutral="1500" max="1100"/>
+    <servo name="RUDDER" no="3" min="1100" neutral="1500" max="1900"/>
+  </servos>
+
+  <commands>
+    <axis name="THROTTLE" failsafe_value="0"/>
+    <axis name="ROLL" failsafe_value="0"/>
+    <axis name="YAW" failsafe_value="0"/>
+    <axis name="PITCH" failsafe_value="0"/>
+    <axis name="BRAKE" failsafe_value="9600"/>	<!-- both elerons up as butterfly brake ? -->
+  </commands>
+
+  <rc_commands>
+    <set command="THROTTLE" value="@THROTTLE"/>
+    <set command="ROLL" value="@ROLL"/>
+    <set command="YAW" value="@YAW"/>
+    <set command="PITCH" value="@PITCH"/>
+    <set command="BRAKE" value="@FLAPS"/>
+  </rc_commands>
+
+  <section name="SERVO_MIXER_GAINS">
+    <define name="AILERON_NEUTRAL" value="0.3f"/>
+
+    <define name="AILERON_RATE_UP" value="1.0f"/>
+    <define name="AILERON_RATE_DOWN" value="0.5f"/>
+
+    <define name="AILERON_RATE_UP_BRAKE" value="1.0f"/>
+    <define name="AILERON_RATE_DOWN_BRAKE" value="1.0f"/>
+
+    <define name="PITCH_GAIN" value="0.9f"/>
+
+    <define name="BRAKE_AILEVON" value="-0.7f"/>
+    <define name="BRAKE_PITCH" value="0.1f"/>
+    <define name="MAX_BRAKE_RATE" value="130"/>
+
+    <define name="RUDDER_OF_AILERON" value="0.3"/>
+
+    <define name="LIMIT(X,XL,XH)" value="(    ((X)>(XH))   ?    (XH)    :    ( ((X)>(XL)) ? (X) : (XL) )    )"/>
+
+</section>
+
+  <command_laws>
+    <!-- Brake Rate Limiter -->
+    <let var="brake_value_nofilt" value="LIMIT(-@BRAKE, 0, MAX_PPRZ)"/>
+    <let var="test; \
+  	      static int16_t _var_brake_value = 0; \
+              _var_brake_value += LIMIT(_var_brake_value_nofilt - _var_brake_value,-MAX_BRAKE_RATE,MAX_BRAKE_RATE); \
+              int verwaarloos_deze_warning_CDW" value="0"/>
+
+    <!-- Differential Aileron Depending on Brake Value -->
+    <let var="aileron_up_rate" value="(AILERON_RATE_UP   * (MAX_PPRZ - $brake_value)) + (AILERON_RATE_UP_BRAKE   * $brake_value)"/>
+    <let var="aileron_down_rate" value="(AILERON_RATE_DOWN * (MAX_PPRZ - $brake_value)) + (AILERON_RATE_DOWN_BRAKE * $brake_value)"/>
+    <let var="aileron_up" value="(@ROLL  *  (((float)$aileron_up_rate) / ((float)MAX_PPRZ)))"/>
+    <let var="aileron_down" value="(@ROLL  * (((float)$aileron_down_rate) / ((float)MAX_PPRZ)))"/>
+    <let var="leftturn" value="(@ROLL >= 0? 1 : 0)"/>
+    <let var="rightturn" value="(1 - $leftturn)"/>
+    <set servo="AILERON_LEFT" value="($aileron_up * $leftturn) + ($aileron_down * $rightturn) - $brake_value*(BRAKE_AILEVON) - (MAX_PPRZ * AILERON_NEUTRAL)"/>
+    <set servo="AILERON_RIGHT" value="($aileron_up * $rightturn) + ($aileron_down * $leftturn) + $brake_value*(BRAKE_AILEVON) + (MAX_PPRZ *AILERON_NEUTRAL)"/>
+
+    <set servo="RUDDER" value="@YAW + @ROLL * RUDDER_OF_AILERON"/>
+    <set servo="THROTTLE" value="@THROTTLE"/>
+
+    <!-- Pitch with Brake-Trim Function -->
+    <set servo="ELEVATOR" value="@PITCH * PITCH_GAIN - BRAKE_PITCH * $brake_value"/>
+  </command_laws>
+
+  <!-- Local magnetic field -->
+  <section name="AHRS" prefix="AHRS_">
+    <define name="H_X" value="0.51562740288882"/>
+    <define name="H_Y" value="-0.05707735220832"/>
+    <define name="H_Z" value="0.85490967783446"/>
+  </section>
+
+  <section name="XSENS">
+    <define name="GPS_IMU_LEVER_ARM_X" value="-0.285f"/>
+    <define name="GPS_IMU_LEVER_ARM_Y" value="0.0f"/>
+    <define name="GPS_IMU_LEVER_ARM_Z" value="0.0f"/>
+  </section>
+
+  <section name="AUTO1" prefix="AUTO1_">
+    <define name="MAX_ROLL" value="RadOfDeg(75)"/>
+    <define name="MAX_PITCH" value="RadOfDeg(45)"/>
+  </section>
+
+  <section name="BAT">
+<!--    <define name="MilliAmpereOfAdc(adc)" value="((adc) - 505) * 124.0f"/> -->
+
+    <define name="LOW_BAT_LEVEL" value="10.5" unit="V"/>
+    <define name="CRITIC_BAT_LEVEL" value="10" unit="V"/>
+    <define name="CATASTROPHIC_BAT_LEVEL" value="9.1" unit="V"/>
+  </section>
+
+  <section name="INS" prefix="INS_">
+    <define name="ROLL_NEUTRAL_DEFAULT" value="0." unit="deg"/>
+    <define name="PITCH_NEUTRAL_DEFAULT" value="-0.027" unit="deg"/>
+  </section>
+
+  <section name="MISC">
+    <define name="NOMINAL_AIRSPEED" value="14." unit="m/s"/>
+    <define name="CARROT" value="5." unit="s"/>
+    <define name="CONTROL_RATE" value="60" unit="Hz"/>
+    <define name="XBEE_INIT" value="&quot;ATPL2\rATRN5\rATTT80\r&quot;"/>
+<!--    <define name="NO_XBEE_API_INIT" value="TRUE"/> -->
+<!--    <define name="ALT_KALMAN_ENABLED" value="TRUE"/> -->
+
+    <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
+
+    <define name="GLIDE_AIRSPEED" value="10"/>
+    <define name="GLIDE_VSPEED" value="3."/>
+    <define name="GLIDE_PITCH" value="45" unit="deg"/>
+  </section>
+
+  <section name="VERTICAL CONTROL" prefix="V_CTL_">
+    <define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
+    <!-- outer loop proportional gain -->
+    <define name="ALTITUDE_PGAIN" value="-0.108000002801"/>
+    <!-- outer loop saturation -->
+    <define name="ALTITUDE_MAX_CLIMB" value="2."/>
+
+    <!-- auto throttle inner loop -->
+    <define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.25"/>
+    <define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
+    <define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.65"/>
+    <define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
+    <define name="AUTO_THROTTLE_DASH_TRIM" value="-4000"/>
+    <define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.250999987125" unit="%/(m/s)"/>
+    <define name="AUTO_THROTTLE_PGAIN" value="-0.00700000021607"/>
+    <define name="AUTO_THROTTLE_IGAIN" value="0.0309999994934"/>
+    <define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.171000003815"/>
+
+    <define name="THROTTLE_SLEW_LIMITER" value="2" unit="s"/>
+  </section>
+
+  <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
+    <define name="COURSE_PGAIN" value="-1.16799998283"/>
+    <define name="COURSE_DGAIN" value="0.280999988317"/>
+    <define name="COURSE_PRE_BANK_CORRECTION" value="1.0039999485"/>
+
+    <define name="ROLL_MAX_SETPOINT" value="0.851999998093" unit="radians"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+
+    <define name="PITCH_PGAIN" value="-15429.6865234"/>
+    <define name="PITCH_DGAIN" value="7.73400020599"/>
+
+    <define name="ELEVATOR_OF_ROLL" value="3007.81298828"/>
+
+    <define name="ROLL_SLEW" value="1."/>
+
+    <define name="ROLL_ATTITUDE_GAIN" value="-11718.75"/>
+    <define name="ROLL_RATE_GAIN" value="-820.312011719"/>
+  </section>
+
+  <section name="AGGRESSIVE" prefix="AGR_">
+    <define name="BLEND_START" value="30"/>
+    <define name="BLEND_END" value="15"/>
+    <define name="CLIMB_THROTTLE" value="1."/>
+    <define name="CLIMB_PITCH" value="0.4"/>
+    <define name="DESCENT_THROTTLE" value="0."/>
+    <define name="DESCENT_PITCH" value="-0.1"/>
+    <define name="CLIMB_NAV_RATIO" value="0.800000011921"/>
+    <define name="DESCENT_NAV_RATIO" value="0.834999978542"/>
+  </section>
+
+  <section name="FAILSAFE" prefix="FAILSAFE_">
+    <define name="DEFAULT_THROTTLE" value="0.35" unit="%"/>
+    <define name="DEFAULT_ROLL" value="0.17" unit="rad"/>
+    <define name="DEFAULT_PITCH" value="0.08" unit="rad"/>
+
+    <define name="HOME_RADIUS" value="DEFAULT_CIRCLE_RADIUS" unit="m"/>
+    <define name="KILL_MODE_DISTANCE" value="(MAX_DIST_FROM_HOME*1.5)"/>
+    <define name="DELAY_WITHOUT_GPS" value="3" unit="s"/>
+  </section>
+
+  <section name="DIGITAL_CAMERA" prefix="DC_">
+    <define name="AUTOSHOOT_QUARTERSEC_PERIOD" value="6" unit="quarter_second"/>
+    <define name="AUTOSHOOT_METER_GRID" value="50" unit="meter"/>
+  </section>
+
+
+  <modules>
+    <load name="ins_xsens_MTiG_fixedwing.xml">
+      <configure name="XSENS_UART_NR" value="0"/>
+    </load>
+
+    <load name="light.xml">
+      <define name="LIGHT_LED_STROBE" value="3"/>
+      <define name="LIGHT_LED_NAV" value="2"/>
+      <define name="STROBE_LIGHT_MODE_DEFAULT" value="6"/>
+      <define name="NAV_LIGHT_MODE_DEFAULT" value="4"/>
+    </load>
+
+<!--    <load name="digital_cam_i2c.xml"/>  -->
+  <!--    <load name="ins_ppzuavimu.xml"/>  -->
+    <load name="digital_cam.xml">
+      <define name="DC_SHUTTER_LED" value="2"/>
+    </load>
+
+  </modules>
+
+  <firmware name="fixedwing">
+
+    <target name="ap" board="tiny_2.11">
+      <define name="STRONG_WIND"/>
+      <define name="WIND_INFO"/>
+      <define name="WIND_INFO_RET"/>
+      <define name="LOITER_TRIM"/>
+      <define name="TUNE_AGRESSIVE_CLIMB"/>
+      <define name="AGR_CLIMB"/>
+    </target>
+    <target name="sim" board="pc"/>
+
+    <subsystem name="radio_control" type="ppm"/>
+
+    <!-- Communication -->
+    <subsystem name="telemetry" type="xbee_api">
+      <configure name="MODEM_BAUD" value="B9600"/>
+    </subsystem>
+
+    <!-- Actuators -->
+    <subsystem name="control"/>
+    <!-- Sensors -->
+    <subsystem name="navigation"/>
+    <subsystem name="gps" type="xsens"/>
+
+    <subsystem name="i2c"/>
+
+</firmware>
+
+</airframe>

conf/airframes/TU_Delft/yapa_xsens.xml

@@ -1,6 +1,6 @@
 <!DOCTYPE airframe SYSTEM "../airframe.dtd">
 
-<!-- 
+<!--
 	YAPA + XSens + XBee
 -->
 
@@ -42,16 +42,21 @@
     <define name="BRAKE_PITCH" value="0.0f"/>
     <define name="MAX_BRAKE_RATE" value="150"/>
 
-    <define name="LIMIT(X,XL,XH)" value="(    ((X)>(XH))   ?    (XH)    :    ( ((X)>(XL)) ? (X) : (XL) )    )"/>
+    <define name="RATELIMIT(X,RATE)" value="( _rate_limited_value + Chop( (X) - _rate_limited_value, -(RATE), (RATE)  )); \
-  </section>
+       int16_t _rate_limited_value = 0;"/>
+
+</section>
 
   <command_laws>
     <!-- Brake Rate Limiter -->
-    <let var="brake_value_nofilt" value="LIMIT(-@BRAKE, 0, MAX_PPRZ)"/>
+    <let var="brake_value" value="Chop(-@BRAKE, 0, MAX_PPRZ)"/>
+    <!--<let var="brake_value" value="RATELIMIT(  $brake_value , MAX_BRAKE_RATE )"/>
+
     <let var="test; \
   	      static int16_t _var_brake_value = 0; \
               _var_brake_value += LIMIT(_var_brake_value_nofilt - _var_brake_value,-MAX_BRAKE_RATE,MAX_BRAKE_RATE); \
               int verwaarloos_deze_warning_CDW" value="0"/>
+		-->
 
     <!-- Differential Aileron Depending on Brake Value -->
     <let var="aileron_up_rate" value="(AILERON_RATE_UP   * (MAX_PPRZ - $brake_value)) + (AILERON_RATE_UP_BRAKE   * $brake_value)"/>
@@ -83,6 +88,11 @@
     <define name="CATASTROPHIC_BAT_LEVEL" value="9.1" unit="V"/>
   </section>
 
+  <section name="INS" prefix="INS_">
+    <define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+    <define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+  </section>
+
   <section name="MISC">
     <define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
     <define name="CARROT" value="5." unit="s"/>
@@ -177,14 +187,17 @@
       <define name="STROBE_LIGHT_MODE_DEFAULT" value="6"/>
       <define name="NAV_LIGHT_MODE_DEFAULT" value="4"/>
     </load>
+
 <!--    <load name="digital_cam_i2c.xml"/>  -->
+  <!--    <load name="ins_ppzuavimu.xml"/>  -->
     <load name="digital_cam.xml" >
-      <define name="DC_SHUTTER_LED" value="3"/>
+      <define name="DC_SHUTTER_LED" value="2"/>
     </load>
 
   </modules>
 
   <firmware name="fixedwing">
+
     <target name="ap" board="tiny_2.11">
       <define name="STRONG_WIND"/>
       <define name="WIND_INFO"/>
@@ -204,11 +217,11 @@
     <!-- Actuators -->
     <subsystem name="control"/>
     <!-- Sensors -->
-    <subsystem name="navigation" type="extra"/>
+    <subsystem name="navigation"/>
     <subsystem name="gps" type="xsens"/>
 
-    <!--<subsystem name="i2c"/>-->
+    <subsystem name="i2c"/>
 
-  </firmware>
+</firmware>
 
 </airframe>

conf/airframes/UofAdelaide/A1000_BOOZ.xml

@@ -293,6 +293,7 @@ second attempt
 	<subsystem name="actuators"     type="mkk"/>
 	<subsystem name="imu"           type="crista"/>
 	<subsystem name="gps"           type="ublox"/>
+    <subsystem name="stabilization"   type="euler"/>
 	<subsystem name="ahrs"          type="cmpl"/>
 	<subsystem name="ins"           type="hff"/>
 

conf/airframes/UofAdelaide/A1000_LISA.xml

@@ -202,6 +202,7 @@
     <subsystem name="actuators"     type="heli"/>
     <subsystem name="imu"           type="b2_v1.1"/>
     <subsystem name="gps"           type="ublox"/>
+    <subsystem name="stabilization"   type="euler"/>
     <subsystem name="ahrs"          type="cmpl"/>
   </firmware>
 

conf/airframes/UofAdelaide/A1000_NOVA.xml

@@ -252,6 +252,7 @@
 	<subsystem name="actuators"     type="mkk"/>
 	<subsystem name="imu"           type="b2_v1.1"/>
 	<subsystem name="gps"           type="ublox"/>
+    <subsystem name="stabilization"   type="euler"/>
 	<subsystem name="ahrs"          type="cmpl"/>
 	<subsystem name="ins"           type="hff"/>
   </firmware>

conf/airframes/booz2_flixr.xml

@@ -4,7 +4,7 @@
     <target name="ap" board="booz_1.0">
       <!--<define name="KILL_MOTORS"/>-->       <!-- prevent motors from ever starting -->
       <!--<define name="RADIO_KILL_SWITCH" value="RADIO_CONTROL_SWITCH1"/>-->
-
+      <define name="USE_INS_NAV_INIT"/>
     </target>
 
     <target name="sim" board="pc">
@@ -18,7 +18,8 @@
     <subsystem name="gps"           type="ublox"/>
     <subsystem name="actuators"     type="mkk"/>
     <subsystem name="imu"           type="b2_v1.0"/>
-    <subsystem name="ahrs"          type="cmpl"/>
+    <subsystem name="stabilization"   type="euler"/>
+    <subsystem name="ahrs"          type="ic"/>
   </firmware>
 
   <firmware name="booz2_test_progs">
@@ -107,6 +108,12 @@
     <define name="BODY_TO_IMU_PSI" value="RadOfDeg(0.)" />
   </section>
 
+  <section name="AHRS" prefix="AHRS_">
+    <define name="PROPAGATE_FREQUENCY" value="512"/>
+    <define name="H_X" value=" 0.51562740288882"/>
+    <define name="H_Y" value="-0.05707735220832"/>
+    <define name="H_Z" value=" 0.85490967783446"/>
+  </section>
 
   <section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
     <!-- setpoints -->

conf/airframes/booz2_ppzuav.xml

@@ -208,6 +208,7 @@
     <subsystem name="actuators"     type="asctec"/>
     <subsystem name="imu"           type="b2_v1.1"/>
     <subsystem name="gps"           type="ublox"/>
+    <subsystem name="stabilization"   type="euler"/>
     <subsystem name="ahrs"          type="cmpl"/>
 </firmware>
 <firmware name="booz_test_progs">

conf/airframes/esden/jt_lisam.xml

@@ -12,15 +12,15 @@
   </servos>
 
   <commands>
-    <axis name="PITCH"  failsafe_value="0"/>
     <axis name="ROLL"   failsafe_value="0"/>
+    <axis name="PITCH"  failsafe_value="0"/>
     <axis name="YAW"    failsafe_value="0"/>
     <axis name="THRUST" failsafe_value="0"/>
   </commands>
 
   <command_laws>
-    <set servo="AILEVON_LEFT"         value="@YAW"/>
+    <set servo="AILEVON_LEFT"         value="-@ROLL"/>
-    <set servo="AILEVON_RIGHT"        value="@YAW"/>
+    <set servo="AILEVON_RIGHT"        value="-@ROLL"/>
   </command_laws>
   
   <!-- for the sim -->
@@ -30,17 +30,20 @@
   </section>
 
 
+  <section name="ROTORCRAFT" prefix="ROTORCRAFT_">
+    <define name="COMMANDS_YAW_ALWAYS_ENABLED" value="TRUE"/>
+ </section>
   <section name="SUPERVISION" prefix="SUPERVISION_">
     <define name="MIN_MOTOR" value="1100"/>
-    <define name="MAX_MOTOR" value="1850"/>
+    <define name="MAX_MOTOR" value="1800"/>
     <define name="TRIM_A" value="0"/>
     <define name="TRIM_E" value="0"/>
     <define name="TRIM_R" value="0"/>
     <define name="NB_MOTOR" value="4"/>
     <define name="SCALE" value="256"/>
-    <define name="ROLL_COEF"   value="{  256, -256,  256, -256 }"/>
     <define name="PITCH_COEF"  value="{ -256, -256,  256,  256 }"/>
-    <define name="YAW_COEF"    value="{    0,    0,    0,    0 }"/>
+    <define name="ROLL_COEF"    value="{    0,    0,    0,    0 }"/>
+    <define name="YAW_COEF"   value="{  256, -256,  256, -256 }"/>
     <define name="THRUST_COEF" value="{  256,  256,  256,  256 }"/>
   </section>
 
@@ -49,7 +52,7 @@
   <section name="IMU" prefix="IMU_">
     
     <define name="BODY_TO_IMU_PHI"   value="RadOfDeg( 180. )"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(  90. )"/>
+    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(   0. )"/>
     <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(   0. )"/>
 
   </section>
@@ -58,6 +61,7 @@
    <define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
    <define name="MODE_AUTO1"  value="AP_MODE_ATTITUDE_RC_CLIMB"/>
    <define name="MODE_AUTO2"  value="AP_MODE_HOVER_Z_HOLD"/>
+   <define name="AUTOPILOT_KILL_WITHOUT_AHRS"  value="TRUE"/>
  </section>
 
  <section name="BAT">
@@ -80,41 +84,43 @@
   </section>
 
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
-
     <!-- setpoints -->
     <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
     <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
+    <define name="SP_MAX_PSI"     value="RadOfDeg(45.)"/>
     <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_P"       value="RadOfDeg(90.)"/>
     <define name="DEADBAND_R"     value="250"/>
+    <define name="DEADBAND_A"     value="250"/>
 
     <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(600)"/>
+    <define name="REF_OMEGA_R"  value="RadOfDeg(600)"/>
-    <define name="REF_ZETA_P"   value="0.90"/>
+    <define name="REF_ZETA_R"   value="0.90"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(400.)"/>
+    <define name="REF_MAX_R"    value="RadOfDeg(400.)"/>
-    <define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
+    <define name="REF_MAX_RDOT" value="RadOfDeg(8000.)"/>
 
     <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
     <define name="REF_ZETA_Q"   value="0.90"/>
     <define name="REF_MAX_Q"    value="RadOfDeg(500.)"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(600)"/>
+    <define name="REF_OMEGA_P"  value="RadOfDeg(600)"/>
-    <define name="REF_ZETA_R"   value="0.85"/>
+    <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(220.)"/>
+    <define name="REF_MAX_P"    value="RadOfDeg(220.)"/>
-    <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
+    <define name="REF_MAX_PDOT" value="RadOfDeg(1800.)"/>
 
     <!-- feedback -->
-    <define name="PHI_PGAIN"  value="-750"/>
+    <define name="PSI_PGAIN"  value="-750"/>
-    <define name="PHI_DGAIN"  value="-370"/>
+    <define name="PSI_DGAIN"  value="-370"/>
-    <define name="PHI_IGAIN"  value="-100"/>
+    <define name="PSI_IGAIN"  value="-100"/>
 
     <define name="THETA_PGAIN"  value="-800"/>
     <define name="THETA_DGAIN"  value="-240"/>
     <define name="THETA_IGAIN"  value="-100"/>
 
-    <define name="PSI_PGAIN"  value="-6000"/>
+    <define name="PHI_PGAIN"  value="-4000"/>
-    <define name="PSI_DGAIN"  value="-400"/>
+    <define name="PHI_DGAIN"  value="-600"/>
-    <define name="PSI_IGAIN"  value="-10"/>
+    <define name="PHI_IGAIN"  value="-10"/>
 
     <!-- feedforward -->
     <define name="PHI_DDGAIN"   value=" 300"/>
@@ -145,6 +151,7 @@
 
   <section name="AHRS" prefix="AHRS_">
     <define name="PROPAGATE_FREQUENCY" value="512"/>
+    <define name="MAG_UPDATE_YAW_ONLY" value="1"/>
     <!-- magnetic field for Santa cruz from http://www.ngdc.noaa.gov/geomagmodels/IGRFWMM.jsp -->
     <define name="H_X" value=" 0.47577"/>
     <define name="H_Y" value=" 0.11811"/>
@@ -192,6 +199,7 @@
     <subsystem name="imu"           type="aspirin"/>
     <subsystem name="gps"           type="ublox"/>
     <subsystem name="ahrs"          type="ic"/>
+    <subsystem name="stabilization" type="quaternion"/>
   </firmware>
 
 

conf/airframes/esden/lisa_asctec.xml

@@ -202,6 +202,7 @@
 
     <subsystem name="imu"           type="b2_v1.1"/>
     <subsystem name="gps"           type="ublox"/>
+    <subsystem name="stabilization"   type="euler"/>
     <subsystem name="ahrs"          type="cmpl"/>
   </firmware>
 

conf/airframes/esden/lisa_asctec_aspirin.xml

@@ -204,6 +204,7 @@
 
     <subsystem name="imu"           type="aspirin"/>
     <subsystem name="gps"           type="ublox"/>
+    <subsystem name="stabilization"   type="euler"/>
     <subsystem name="ahrs"          type="cmpl"/>
   </firmware>
 

conf/airframes/esden/lisa_m_pwm.xml

@@ -225,6 +225,7 @@
 
     <subsystem name="imu"           type="aspirin"/>
     <subsystem name="gps"           type="ublox"/>
+    <subsystem name="stabilization"   type="euler"/>
     <subsystem name="ahrs"          type="ic"/>
   </firmware>
 

conf/airframes/esden/lisa_pwm_aspirin.xml

@@ -204,6 +204,7 @@
     <subsystem name="gps"           type="ublox">
 	    <configure name="GPS_BAUD"    value="B57600"/>
     </subsystem>
+    <subsystem name="stabilization"   type="euler"/>
     <subsystem name="ahrs"          type="cmpl"/>
     <!--<subsystem name="ahrs"          type="ic"/>-->
   </firmware>

conf/airframes/esden/synerani_4B.xml

@@ -208,6 +208,7 @@
 	    <configure name="GPS_BAUD"    value="B57600"/>
     </subsystem>
     <!--subsystem name="ahrs"          type="cmpl"/-->
+    <subsystem name="stabilization"   type="euler"/>
     <subsystem name="ahrs"          type="ic"/>
   </firmware>
 

conf/airframes/flixr_discovery.xml

@@ -0,0 +1,366 @@
+<!DOCTYPE airframe SYSTEM "airframe.dtd">
+<!-- ST Model Discovery (http://www.sheng-teng.com/)
+     TWOG1  (http://paparazzi.enac.fr/wiki/index.php/)
+     * uBlox LEA5H and Sarantel helix GPS antenna
+
+READ this first
+http://paparazzi.enac.fr/w/index.php?title=Theory_of_Operation
+-->
+
+<airframe name="Discovery">
+
+  <firmware name="fixedwing">
+    <target name="ap" 			board="twog_1.0">
+      <configure name="PERIODIC_FREQUENCY" value="960"/> <!-- IMU FREQ -->
+      <configure name="AHRS_PROPAGATE_FREQUENCY" value="120"/>
+      <configure name="AHRS_CORRECT_FREQUENCY" value="60"/>
+      <configure name="AHRS_ALIGNER_LED" value="1"/>
+      <configure name="CPU_LED" value="2"/>
+      <!--define name="IMU_B2_VERSION_1_0"/-->
+      <define name="USE_ADC_3"/>
+    </target>
+    <target name="sim" 			board="pc"/>
+
+    <define name="AGR_CLIMB"/>
+    <define name="LOITER_TRIM"/>
+    <!--define name="STRONG_WIND"/-->
+
+    <define name="WIND_INFO"/>
+    <define name="ALT_KALMAN"/>
+    <!-- <define name="USE_I2C0"/> -->
+    <!-- <define name="USE_AIRSPEED"/> -->
+    <!-- <define name="USE_BARO_ETS"/> -->
+
+    <subsystem name="radio_control" type="ppm"/>
+    <subsystem name="telemetry" type="transparent"/>
+    <!--<define name="TELEMETRY_MODE_FBW" value="1"/>-->
+    <subsystem name="control">
+
+    </subsystem>
+
+    <subsystem name="attitude" type="dcm"/>
+    <subsystem name="imu" type="booz"/>
+    <subsystem name="gps" type="ublox_lea5h"/>
+    <subsystem name="navigation" type="extra"/>
+    <!-- <subsystem name="i2c"/> -->
+  </firmware>
+
+  <firmware name="setup">
+    <target name="tunnel"          board="twog_1.0" />
+    <target name="usb_tunnel_0"    board="twog_1.0" />
+    <target name="setup_actuators" board="twog_1.0" />
+  </firmware>
+
+
+<!-- ******************* MODULES ********************************** --> 
+  <modules main_freq="60">
+    <load name="servo_switch.xml">
+      <define name="SERVO_SWITCH_ON_VALUE"  value="1100"/>
+      <define name="SERVO_SWITCH_OFF_VALUE" value="1900"/>
+      <define name="SERVO_SWITCH_SERVO"     value="SERVO_HATCH"/>
+    </load>
+    <load name="sys_mon.xml"/>
+    <load name="adc_generic.xml">
+      <configure name="ADC_CHANNEL_GENERIC1" value="ADC_4"/> <!-- voltage sensor -->
+    </load>
+  </modules>
+
+<!-- Define here to which CONNECTOR NUMBER the servo is connected to, on the autopilot cicuit board -->
+  <servos>
+    <servo name="MOTOR"         no="0" min="1100" neutral="1100" max="1900"/>
+    <servo name="ELEVATOR"      no="2" min="1100" neutral="1500" max="1900"/>
+    <servo name="AILERON_RIGHT" no="3" min="2000" neutral="1500" max="1000"/>
+    <servo name="AILERON_LEFT"  no="4" min="2000" neutral="1500" max="1000"/>
+    <servo name="RUDDER"        no="6" min="1000" neutral="1500" max="2000"/>
+    <servo name="HATCH"   	no="7" min="1100" neutral="1100" max="1900"/>
+  </servos>
+
+<!-- commands section -->
+  <commands>
+    <axis name="THROTTLE" 	failsafe_value="0"/>
+    <axis name="ROLL"     	failsafe_value="0"/>
+    <axis name="PITCH"    	failsafe_value="0"/>
+    <axis name="YAW"      	failsafe_value="2000"/>
+    <axis name="HATCH"    	failsafe_value="-9599"/>
+<!-- maybe set to open position so we can insert the payload -->
+  </commands>
+
+  <rc_commands>
+    <set command="THROTTLE" value="@THROTTLE"/>
+    <set command="ROLL"     value="@ROLL"/>
+    <set command="PITCH"    value="@PITCH"/>
+    <set command="YAW"      value="@YAW"/>
+  </rc_commands>
+
+  <auto_rc_commands>
+  <!-- To still be able to use rudder, which is needed with sidewind landing in auto1 stabilization mode only YAW -->
+    <set command="YAW"      value="@YAW"/>
+  </auto_rc_commands>
+
+<!--For mixed controlflaps -->
+  <section name="MIXER">
+    <define name="AILERON_DIFF" value="0.7"/>
+    <define name="COMBI_SWITCH" value="0.3"/>
+  </section>
+
+  <command_laws>
+    <set servo="MOTOR"         value="@THROTTLE"/>
+    <set servo="ELEVATOR"      value="@PITCH"/>
+    <set servo="RUDDER" value="@YAW + @ROLL*COMBI_SWITCH"/>
+
+    <let var="roll" value="@ROLL"/>
+    <set servo="AILERON_LEFT" value="($roll > 0 ? AILERON_DIFF : 1) * $roll"/>
+    <set servo="AILERON_RIGHT" value="($roll > 0 ? 1 : AILERON_DIFF) * $roll"/>
+    <!--set servo="HATCH"         value="@HATCH"/-->
+  </command_laws>
+
+
+  <section name="IMU" prefix="IMU_">
+    <define name="GYRO_P_CHAN"  value="1"/>
+    <define name="GYRO_Q_CHAN"  value="2"/>
+    <define name="GYRO_R_CHAN"  value="0"/>
+
+    <define name="GYRO_P_SIGN"  value="1"/>
+    <define name="GYRO_Q_SIGN"  value="1"/>
+    <define name="GYRO_R_SIGN"  value="-1"/>
+
+    <define name="GYRO_P_NEUTRAL" value="32362"/>
+    <define name="GYRO_R_NEUTRAL" value="32080"/>
+    <define name="GYRO_Q_NEUTRAL" value="32096"/>
+
+    <define name="GYRO_P_SENS" value="1.1032765" integer="16"/>
+    <define name="GYRO_R_SENS" value="1.1360802599" integer="16"/>
+    <define name="GYRO_Q_SENS" value="1.1249874614" integer="16"/>
+
+    <define name="ACCEL_X_CHAN"  value="3"/>
+    <define name="ACCEL_Y_CHAN"  value="6"/>
+    <define name="ACCEL_Z_CHAN"  value="5"/>
+
+    <define name="ACCEL_X_SIGN"  value="-1"/>
+    <define name="ACCEL_Y_SIGN"  value="1"/>
+    <define name="ACCEL_Z_SIGN"  value="-1"/>
+
+    <define name="ACCEL_X_SENS" value="2.45932966" integer="16"/>
+    <define name="ACCEL_Z_SENS" value="2.45106376" integer="16"/>
+    <define name="ACCEL_Y_SENS" value="2.47825717" integer="16"/>
+
+    <define name="ACCEL_X_NEUTRAL" value="31886"/>
+    <define name="ACCEL_Z_NEUTRAL" value="32162"/>
+    <define name="ACCEL_Y_NEUTRAL" value="32941"/>
+
+    <define name="GYRO_P_Q" value="0."/>
+    <define name="GYRO_P_R" value="0."/>
+    <define name="GYRO_Q_P" value="0."/>
+    <define name="GYRO_Q_R" value="0."/>
+    <define name="GYRO_R_P" value="0."/>
+    <define name="GYRO_R_Q" value="0."/>
+
+    <define name="MAG_X_NEUTRAL" value="0"/>
+    <define name="MAG_Y_NEUTRAL" value="0"/>
+    <define name="MAG_Z_NEUTRAL" value="0"/>
+
+    <define name="MAG_X_SENS" value="1" integer="16"/>
+    <define name="MAG_Y_SENS" value="1" integer="16"/>
+    <define name="MAG_Z_SENS" value="1" integer="16"/>
+
+    <!-- <define name="MAG_45_HACK" value="1"/> -->
+
+    <define name="BODY_TO_IMU_PHI" value="0"/>
+    <define name="BODY_TO_IMU_THETA" value="0"/>
+    <define name="BODY_TO_IMU_PSI" value="0"/>
+  </section>
+
+  <section name="INS" prefix="INS_">
+    <define name="ROLL_NEUTRAL_DEFAULT" value="0."/>
+    <define name="PITCH_NEUTRAL_DEFAULT" value="0."/>
+  </section>
+
+<!-- Do not set MAX_ROLL, MAX_PITCH to small of a value, otherwise one can NOT control the plane very well manually -->
+<!-- If you have dual rate swith it of with same swtch as mode switch thus auto1 means dualrate is switched off also -->
+  <section name="AUTO1" prefix="AUTO1_">
+    <define name="MAX_ROLL" value="RadOfDeg(55)"/>
+    <define name="MAX_PITCH" value="RadOfDeg(35)"/>
+  </section>
+
+
+  <section name="BAT">
+    <define name="ADC_CHANNEL_CURRENT" value="ADC_3"/>
+    <define name="MilliAmpereOfAdc(adc)" value="(adc*88)"/>
+
+<!-- <define name="ADC_CHANNEL_VOLTAGE" value="ADC_4" /> -->
+<!-- The real multiplier is unknown we take 2 as test example -->
+<!--  <define name="VoltageOfAdc(adc)" value ="(adc*2)"/>  -->
+
+<!-- If NO current-volts sensor installed uncomment below -->
+  <!--define name="MILLIAMP_AT_FULL_THROTTLE" value="17000" unit="mA"/-->
+
+<!--
+    <define name="VOLTAGE_ADC_A" value="0.0177531"/>
+    <define name="VOLTAGE_ADC_B" value="0.173626"/>
+    <define name="VoltageOfAdc(adc)" value ="(VOLTAGE_ADC_A * adc + VOLTAGE_ADC_B)"/> 
+-->
+
+    <define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/> <!-- 3S lipo 3.1*3=9.3 -->
+    <define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
+    <define name="LOW_BAT_LEVEL" value="9.7" unit="V"/>
+    <define name="MAX_BAT_LEVEL" value="12.4" unit="V"/> <!-- 3S lipo 4.2*3=12.6 -->
+  </section>
+
+  <section name="MISC">
+    <define name="MINIMUM_AIRSPEED" value="9.0" unit="m/s"/>
+    <define name="NOMINAL_AIRSPEED" value="14.0" unit="m/s"/>
+    <define name="MAXIMUM_AIRSPEED" value="22.0" unit="m/s"/>
+    <define name="CARROT" value="5.0" unit="s"/>
+    <define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
+    <define name="CONTROL_RATE" value="60" unit="Hz"/>
+
+<!--UNLOCKED_HOME_MODE if set to TRUE means that HOME mode does not get stuck.
+If not set before when you would enter home mode you had to flip a bit via the GCS to get out. -->
+    <!--define name="UNLOCKED_HOME_MODE" value="TRUE"/-->
+<!-- RC_LOST_MODE means that if your RC Transmitter signal is not received anymore in the autopilot, e.g. you switch it off
+     or  fly a long range mission you define the wanted mode behaviour here. 
+     If you do not define it, it defaults to flying to the flightplan HOME -->
+    <!--define name="RC_LOST_MODE" value="PPRZ_MODE_AUTO2"/-->
+
+
+    <define name="ALT_KALMAN_ENABLED" value="TRUE"/>
+    <define name="DEFAULT_CIRCLE_RADIUS" value="50.0"/>
+
+    <!-- only for spiral -->
+    <define name="MIN_CIRCLE_RADIUS" value="35.0"/>
+
+<!-- The Glide definitions are used for calculating the touch down point during auto landing -->
+<!--
+    <define name="GLIDE_AIRSPEED" value="10."/>
+    <define name="GLIDE_VSPEED" value="3."/>
+    <define name="GLIDE_PITCH" value="10." unit="deg"/>
+-->
+  </section>
+
+<!-- ******************* VERTICAL CONTROL ********************************** --> 
+
+  <section name="VERTICAL CONTROL" prefix="V_CTL_">
+    <define name="POWER_CTL_BAT_NOMINAL" value="11.2" unit="volt"/>
+
+    <!-- outer loop proportional gain -->
+    <define name="ALTITUDE_PGAIN" value="-0.04" unit="(m/s)/m"/>
+
+    <!-- outer loop saturation -->
+    <define name="ALTITUDE_MAX_CLIMB" value="2.5" unit="m/s"/>
+
+    <!-- auto throttle inner loop -->
+    <define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.65" unit="%"/>
+    <define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.50" unit="%"/>
+    <define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.90" unit="%"/>
+    <define name="AUTO_THROTTLE_LOITER_TRIM" value="1000" unit="pprz_t"/>
+    <define name="AUTO_THROTTLE_DASH_TRIM" value="-2000" unit="pprz_t"/>
+    <define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.05" unit="%/(m/s)"/>
+    <define name="AUTO_THROTTLE_PGAIN" value="-0.007" unit="%/(m/s)"/>
+    <define name="AUTO_THROTTLE_IGAIN" value="0.01"/>
+    <define name="AUTO_THROTTLE_DGAIN" value="0.0"/>
+    <define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.20" unit="rad/(m/s)"/>
+    <define name="THROTTLE_SLEW_LIMITER" value="0.7" unit="s"/>
+
+    <!-- NOT YET USED auto airspeed and altitude inner loop -->
+    <!-- <define name="AIRSPEED_ETS_OFFSET" value="1542"/> -->
+
+    <!-- NEVER set AUTO_AIRSPEED_SETPOINT lower than airframe stall speed -->
+
+    <!--
+    <define name="AUTO_AIRSPEED_SETPOINT" value="11.5" unit="m/s"/>  
+    <define name="AUTO_AIRSPEED_PGAIN" value="0.1"/>
+    <define name="AUTO_AIRSPEED_IGAIN" value="0.05"/>  
+
+    <define name="AUTO_AIRSPEED_PITCH_PGAIN" value="0" unit="degree/(m/s)"/>
+    <define name="AUTO_AIRSPEED_THROTTLE_PGAIN" value="0" unit="%/(m/s)"/>
+    <define name="AUTO_AIRSPEED_THROTTLE_IGAIN" value="0"/>
+    -->
+
+    <!--
+    <define name="AUTO_GROUNDSPEED_SETPOINT" value="9.0" unit="m/s"/>  
+    <define name="AUTO_GROUNDSPEED_PGAIN" value="0.75"/>
+    <define name="AUTO_GROUNDSPEED_IGAIN" value="0.25"/> 
+    -->
+
+    <!-- auto pitch inner loop -->
+    <define name="AUTO_PITCH_PGAIN" value="-0.052"/>
+    <define name="AUTO_PITCH_IGAIN" value="0.065"/>
+    <define name="AUTO_PITCH_MAX_PITCH" value="0.45"/> <!-- INVESTIGATE make it variable? so it can be set higher for a stalled landing -->
+    <define name="AUTO_PITCH_MIN_PITCH" value="-0.40"/>
+    <!-- <define name="THROTTLE_SLEW" value="0.2"/> -->
+  </section>
+
+<!-- ******************* HORIZONTAL CONTROL ******************************** --> 
+  <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
+
+    <define name="COURSE_PGAIN" value="-1.1"/>
+    <!-- <define name="COURSE_IGAIN" value="0.01"/> -->
+    <!-- <define name="COURSE_DGAIN" value="0.02"/> -->
+    <!-- <define name="COURSE_PRE_BANK_CORRECTION" value="0.5"/> -->
+
+    <!-- maybe handy to avoid over corner with lots of wind and small airframe -->
+    <!-- <define name="COURSE_SLEW_INCREMENT" value="RadOfDeg(4)"/> -->
+
+    <define name="ROLL_MAX_SETPOINT" value="RadOfDeg(50)" unit="radians"/>
+    <define name="PITCH_MAX_SETPOINT" value="RadOfDeg(35)" unit="radians"/>
+    <define name="PITCH_MIN_SETPOINT" value="RadOfDeg(-30)" unit="radians"/>
+
+    <define name="ROLL_ATTITUDE_GAIN" value="-7500."/><!-- Roll Angle PD control -->
+    <define name="ROLL_RATE_GAIN" value="-1500"/>
+    <!-- ROLL_PGAIN is only used when ROLL_ATTITUDE_GAIN is not defined, P control + rate loop if defined -->
+    <!-- <define name="ROLL_PGAIN" value="7000."/> -->
+
+    <!-- ** Maximal roll angle change per 1/60 of second ** --> 
+    <!--define name="ROLL_SLEW" value="0.6"/--> <!-- was 0.05 -->
+
+    <define name="PITCH_PGAIN" value="-7000."/>
+
+    <!-- only for stabilization adaptive -->
+    <define name="PITCH_DGAIN" value="1.3"/>
+    <define name="PITCH_IGAIN" value="-500"/>
+    <define name="PITCH_KFFA" value="0."/>
+    <define name="PITCH_KFFD" value="0."/>
+
+    <define name="AILERON_OF_THROTTLE" value="0.0"/>
+
+    <define name="ELEVATOR_OF_ROLL" value="1500"/>  
+    <!-- Alternative way of adding some elevator when roll initiated -->
+    <!-- <define name="PITCH_OF_ROLL" value="RadOfDeg(0.1)"/> -->
+  
+  </section>
+
+<!-- ******************************** NAV ********************************** -->
+  <section name="NAV">
+    <define name="NAV_PITCH" value="0."/>
+    <define name="NAV_GLIDE_PITCH_TRIM" value="0"/>
+  </section>
+
+<!-- ***************************** AGGRESIVE ******************************* -->
+  <section name="AGGRESSIVE" prefix="AGR_">
+    <define name="BLEND_START" value="18"/> <!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
+    <define name="BLEND_END" value="8"/> <!-- Altitude Error to Blend Aggressive to Regular Climb Modes  CANNOT BE ZERO!!-->
+    <define name="CLIMB_THROTTLE" value="0.99"/> <!-- Gaz for Aggressive Climb -->
+    <define name="CLIMB_PITCH" value="RadOfDeg(45)"/> <!-- Pitch for Aggressive Climb -->
+    <define name="DESCENT_THROTTLE" value="0.5"/> <!-- Gaz for Aggressive Decent -->
+    <define name="DESCENT_PITCH" value="RadOfDeg(-30)"/> <!-- Pitch for Aggressive Decent -->
+    <define name="CLIMB_NAV_RATIO" value="0.8"/> <!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
+    <define name="DESCENT_NAV_RATIO" value="0.99"/>
+  </section>
+
+<!-- ****************************** FAILSAFE ******************************* -->
+  <section name="FAILSAFE" prefix="FAILSAFE_">
+    <define name="DELAY_WITHOUT_GPS" value="3" unit="s"/>
+    <define name="DEFAULT_THROTTLE" value="0" unit="%"/>
+    <define name="DEFAULT_ROLL" value="0.15" unit="rad"/>
+    <define name="DEFAULT_PITCH" value="-0.1" unit="rad"/>
+    <define name="HOME_RADIUS" value="60" unit="m"/>
+  </section>
+
+
+<!-- ******************************** SIMU ********************************* -->
+  <section name="SIMU">
+    <define name="YAW_RESPONSE_FACTOR" value="0.5"/> <!-- a to low of a value gives bad simulation results -->
+  </section>
+
+</airframe>
+

conf/airframes/mm/rotor/qmk1.xml

@@ -203,6 +203,7 @@
     <subsystem name="gps"           type="ublox">
       <configure name="GPS_PORT"     value="UART3"/>
     </subsystem>
+    <subsystem name="stabilization"   type="euler"/>
     <subsystem name="ahrs"          type="cmpl"/>
   </firmware>
 

conf/autopilot/fixedwing.xml

@@ -34,7 +34,7 @@
    <configure name="ADC_IR_TOP"     value="ADC_0"/><!-- this is already the default if not set, change if needed -->
    <configure name="ADC_IR_NB_SAMPLES" value="16"/><!-- this is already the default if not set, change if needed -->
  </subsystem>
- <subsystem name="gps" 		    types="ublox_lea4p|lea5h">
+ <subsystem name="gps" 		    types="ublox_lea4p|lea5h|nmea">
    <configure name="GPS_BAUD"       values="B9600|B38400|B57600|B115200"/>
    <configure name="GPS_LED"        values="1,2,3,4,none"/>
  </subsystem>

conf/autopilot/lisa_l_test_progs.makefile

@@ -631,7 +631,7 @@ test_actuators_mkk.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 test_actuators_mkk.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
 test_actuators_mkk.srcs += downlink.c pprz_transport.c
 
-test_actuators_mkk.srcs += $(SRC_BOOZ)/booz2_commands.c
+test_actuators_mkk.srcs += $(SRC_FIRMWARE)/commands.c
 test_actuators_mkk.srcs += $(SRC_FIRMWARE)/actuators/actuators_mkk.c
 test_actuators_mkk.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c1
 test_actuators_mkk.srcs += $(SRC_FIRMWARE)/actuators/supervision.c
@@ -663,7 +663,7 @@ test_actuators_asctecv1.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 test_actuators_asctecv1.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
 test_actuators_asctecv1.srcs += downlink.c pprz_transport.c
 
-test_actuators_asctecv1.srcs += $(SRC_BOOZ)/booz2_commands.c
+test_actuators_asctecv1.srcs += $(SRC_FIRMWARE)/commands.c
 test_actuators_asctecv1.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c1
 test_actuators_asctecv1.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c
 test_actuators_asctecv1.CFLAGS += -DUSE_I2C1
@@ -701,6 +701,52 @@ test_bmp085.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
 
 
 
+#
+# Test manual : a simple test with rc and servos - I want to fly lisa/M
+#
+test_manual.ARCHDIR = $(ARCH)
+test_manual.CFLAGS  = -I$(SRC_FIRMWARE) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
+test_manual.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+test_manual.srcs    = $(SRC_AIRBORNE)/mcu.c \
+                 $(SRC_ARCH)/mcu_arch.c \
+                 test/test_manual.c               \
+              $(SRC_ARCH)/stm32_exceptions.c   \
+              $(SRC_ARCH)/stm32_vector_table.c
+test_manual.CFLAGS += -DUSE_LED
+test_manual.srcs   += $(SRC_ARCH)/led_hw.c
+test_manual.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED)
+test_manual.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
+test_manual.srcs   += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+
+test_manual.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
+test_manual.srcs   += $(SRC_ARCH)/mcu_periph/uart_arch.c
+
+test_manual.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
+test_manual.srcs   += downlink.c pprz_transport.c
+
+test_manual.srcs += $(SRC_FIRMWARE)/commands.c
+
+test_manual.CFLAGS += -I$(SRC_FIRMWARE)/actuators/arch/$(ARCH)
+#test_manual.srcs   += $(SRC_FIRMWARE)/actuators/actuators_pwm.c
+test_manual.srcs   += $(SRC_FIRMWARE)/actuators/arch/$(ARCH)/actuators_pwm_arch.c
+test_manual.srcs   += $(SRC_FIRMWARE)/actuators/actuators_heli.c
+
+
+test_manual.CFLAGS += -I$(SRC_BOOZ) -I$(SRC_BOOZ)/arch/$(ARCH)
+test_manual.CFLAGS += -DRADIO_CONTROL
+ifdef RADIO_CONTROL_LED
+test_manual.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED)
+endif
+test_manual.CFLAGS += -DRADIO_CONTROL_BIND_IMPL_FUNC=radio_control_spektrum_try_bind
+test_manual.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/spektrum.h\"
+test_manual.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_PRIMARY_PORT=$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT)
+test_manual.CFLAGS += -DOVERRIDE_$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT)_IRQ_HANDLER -DUSE_TIM6_IRQ
+test_manual.srcs   += $(SRC_SUBSYSTEMS)/radio_control.c                                 \
+                  subsystems/radio_control/spektrum.c          \
+                  $(SRC_ARCH)/subsystems/radio_control/spektrum_arch.c
+
+
+
 #
 # tunnel sw
 #

conf/autopilot/lisa_passthrough.makefile

@@ -61,7 +61,7 @@ stm_passthrough.srcs += $(SRC_LISA)/lisa_overo_link.c           \
 stm_passthrough.srcs += math/pprz_trig_int.c
 stm_passthrough.srcs += lisa/plug_sys.c
 
-stm_passthrough.srcs += $(SRC_BOOZ)/booz2_commands.c
+stm_passthrough.srcs += $(SRC_FIRMWARE)/commands.c
 
 # Radio control
 #

conf/autopilot/obsolete/booz_test_progs.makefile

@@ -197,7 +197,7 @@ test_actuators_mkk.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 test_actuators_mkk.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
 test_actuators_mkk.srcs += downlink.c pprz_transport.c
 
-test_actuators_mkk.srcs += $(SRC_BOOZ)/booz2_commands.c
+test_actuators_mkk.srcs += $(SRC_FIRMWARE)/commands.c
 test_actuators_mkk.srcs += $(SRC_FIRMWARE)/actuators/actuators_mkk.c
 test_actuators_mkk.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c0
 test_actuators_mkk.srcs += $(SRC_FIRMWARE)/actuators/supervision.c

conf/autopilot/obsolete/lisa_test_progs.makefile

@@ -607,7 +607,7 @@ test_actuators_mkk.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 test_actuators_mkk.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
 test_actuators_mkk.srcs += downlink.c pprz_transport.c
 
-test_actuators_mkk.srcs += $(SRC_BOOZ)/booz2_commands.c
+test_actuators_mkk.srcs += $(SRC_FIRMWARE)/commands.c
 test_actuators_mkk.srcs += $(SRC_FIRMWARE)/actuators/actuators_mkk.c
 test_actuators_mkk.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c1  -DUSE_TIM2_IRQ
 #test_actuators_mkk.CFLAGS += -DACTUATORS_ASCTEC_V2_PROTOCOL -DACTUATORS_ASCTEC_DEVICE=i2c1
@@ -640,7 +640,7 @@ test_actuators_asctec.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 test_actuators_asctec.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
 test_actuators_asctec.srcs += downlink.c pprz_transport.c
 
-test_actuators_asctec.srcs += $(SRC_BOOZ)/booz2_commands.c
+test_actuators_asctec.srcs += $(SRC_FIRMWARE)/commands.c
 test_actuators_asctec.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c
 test_actuators_asctec.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c1
 # -DBOOZ_START_DELAY=3
@@ -1100,7 +1100,7 @@ ptw.srcs += peripherals/max1168.c $(SRC_ARCH)/peripherals/max1168_arch.c
 ptw.srcs += peripherals/ms2100.c  $(SRC_ARCH)/peripherals/ms2100_arch.c
 ptw.srcs += math/pprz_trig_int.c
 
-ptw.srcs += $(SRC_BOOZ)/booz2_commands.c
+ptw.srcs += $(SRC_FIRMWARE)/commands.c
 
 # Radio control
 ptw.CFLAGS += -DRADIO_CONTROL

conf/autopilot/rotorcraft.makefile

@@ -26,8 +26,6 @@
 CFG_SHARED=$(PAPARAZZI_SRC)/conf/autopilot/subsystems/shared
 CFG_ROTORCRAFT=$(PAPARAZZI_SRC)/conf/autopilot/subsystems/rotorcraft
 
-SRC_BOOZ=booz
-SRC_BOOZ_ARCH=$(SRC_BOOZ)/arch/$(ARCH)
 SRC_BOOZ_TEST=$(SRC_BOOZ)/test
 SRC_BOOZ_PRIV=booz_priv
 
@@ -39,7 +37,7 @@ SRC_ARCH=arch/$(ARCH)
 
 CFG_BOOZ=$(PAPARAZZI_SRC)/conf/autopilot/
 
-ROTORCRAFT_INC = -I$(SRC_FIRMWARE) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -I$(SRC_BOARD)
+ROTORCRAFT_INC = -I$(SRC_FIRMWARE) -I$(SRC_BOARD)
 
 
 ap.ARCHDIR = $(ARCH)
@@ -51,6 +49,11 @@ ap.srcs    = $(SRC_FIRMWARE)/main.c
 ap.srcs   += mcu.c
 ap.srcs   += $(SRC_ARCH)/mcu_arch.c
 
+#
+# Math functions
+#
+$(TARGET).srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c math/pprz_trig_int.c
+
 ifeq ($(ARCH), stm32)
 ap.srcs += lisa/plug_sys.c
 endif
@@ -101,7 +104,7 @@ ap.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 ap.srcs += mcu_periph/i2c.c
 ap.srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.c
 
-ap.srcs += $(SRC_BOOZ)/booz2_commands.c
+ap.srcs += $(SRC_FIRMWARE)/commands.c
 
 #
 # Radio control choice
@@ -185,23 +188,14 @@ endif
 
 ap.srcs += $(SRC_FIRMWARE)/autopilot.c
 
-ap.srcs += math/pprz_trig_int.c
 ap.srcs += $(SRC_FIRMWARE)/stabilization.c
 ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_rate.c
 
-
-ap.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_INT
-ap.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/stabilization_attitude_int.h\"
-ap.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/stabilization_attitude_ref_euler_int.h\"
-ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_euler_int.c
-ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_euler_int.c
-
 ap.CFLAGS += -DUSE_NAVIGATION
 ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_h.c
 ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_v.c
 
 ap.srcs += $(SRC_SUBSYSTEMS)/ins.c
-ap.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c
 
 #
 # INS choice
@@ -216,7 +210,7 @@ ap.srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c
 ap.CFLAGS += -DUSE_VFF -DDT_VFILTER='(1./$(PERIODIC_FREQUENCY).)'
 
 ap.srcs += $(SRC_FIRMWARE)/navigation.c
-
+ap.srcs += subsystems/navigation/common_flight_plan.c
 
 #
 # FMS  choice

conf/autopilot/rotorcraft.xml

@@ -23,5 +23,6 @@
  <subsystem name="gps"       types="skytraq|ublox"/>
  <subsystem name="ahrs"      types="cmpl"/>
  <subsystem name="ins"       types="hff"/>
+ <subsystem name="stabilization"       types="euler|quaternion"/>
 
 </firmware>

conf/autopilot/subsystems/fixedwing/ahrs_ic.makefile

@@ -0,0 +1,42 @@
+#
+# AHRS_PROPAGATE_FREQUENCY
+# AHRS_H_X
+# AHRS_H_Y
+# AHRS_H_Z
+#
+
+AHRS_CFLAGS  = -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR
+ifdef AHRS_ALIGNER_LED
+AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
+endif
+AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl.h\"
+AHRS_SRCS   += subsystems/ahrs.c
+AHRS_SRCS   += subsystems/ahrs/ahrs_int_cmpl.c
+AHRS_SRCS   += subsystems/ahrs/ahrs_aligner.c
+
+ap.CFLAGS += $(AHRS_CFLAGS)
+ap.srcs += $(AHRS_SRCS)
+
+sim.CFLAGS += $(AHRS_CFLAGS)
+sim.srcs += $(AHRS_SRCS)
+
+
+# Extra stuff for fixedwings
+
+ifdef CPU_LED
+  ap.CFLAGS += -DAHRS_CPU_LED=$(CPU_LED)
+endif
+
+ifdef AHRS_PROPAGATE_FREQUENCY
+else
+  AHRS_PROPAGATE_FREQUENCY = 60
+endif
+
+ifdef AHRS_CORRECT_FREQUENCY
+else
+  AHRS_CORRECT_FREQUENCY = 60
+endif
+
+ap.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY)
+ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY)
+

conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile

@@ -10,7 +10,6 @@ ap.CFLAGS += -DUSE_AHRS
 ap.srcs   += $(SRC_SUBSYSTEMS)/ahrs.c
 ap.srcs   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
 ap.srcs   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm.c
-ap.srcs   += math/pprz_trig_int.c
 
 ifdef AHRS_ALIGNER_LED
   ap.CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)

conf/autopilot/subsystems/fixedwing/autopilot.makefile

@@ -81,6 +81,11 @@ $(TARGET).srcs += sys_time.c
 $(TARGET).CFLAGS 	+= -DINTER_MCU
 $(TARGET).srcs 		+= $(SRC_FIXEDWING)/inter_mcu.c
 
+#
+# Math functions
+#
+$(TARGET).srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c math/pprz_trig_int.c
+
 ######################################################################
 ##
 ## COMMON FOR ALL NON-SIMULATION TARGETS

conf/autopilot/subsystems/fixedwing/gps_nmea.makefile

@@ -0,0 +1,23 @@
+# NMEA GPS unit
+
+
+ap.CFLAGS += -DUSE_GPS -DNMEA -DGPS_USE_LATLONG
+ap.CFLAGS += -DGPS_LINK=$(GPS_PORT)
+ap.CFLAGS += -DUSE_$(GPS_PORT)
+ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
+
+ifneq ($(GPS_LED),none)
+  ap.CFLAGS += -DGPS_LED=$(GPS_LED)
+endif
+
+ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\"
+ap.srcs   += $(SRC_SUBSYSTEMS)/gps/gps_nmea.c
+
+$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
+
+sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
+sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
+sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
+
+jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
+jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c

conf/autopilot/subsystems/fixedwing/gps_ublox_lea4p.makefile

@@ -10,7 +10,13 @@ ifneq ($(GPS_LED),none)
   ap.CFLAGS += -DGPS_LED=$(GPS_LED)
 endif
 
-ap.srcs   += $(SRC_FIXEDWING)/gps_ubx.c
+ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\"
+ap.srcs   += $(SRC_SUBSYSTEMS)/gps/gps_ubx.c
 
-$(TARGET).srcs += $(SRC_FIXEDWING)/gps.c $(SRC_FIXEDWING)/latlong.c
+$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
 
+sim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
+sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
+
+jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
+jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c

conf/autopilot/subsystems/fixedwing/gps_ublox_lea5h.makefile

@@ -10,6 +10,14 @@ ifneq ($(GPS_LED),none)
   ap.CFLAGS += -DGPS_LED=$(GPS_LED)
 endif
 
-ap.srcs   += $(SRC_FIXEDWING)/gps_ubx.c
+ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\"
+ap.srcs   += $(SRC_SUBSYSTEMS)/gps/gps_ubx.c
 
-$(TARGET).srcs += $(SRC_FIXEDWING)/gps.c $(SRC_FIXEDWING)/latlong.c
+$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
+
+sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
+sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
+sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
+
+jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
+jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c

conf/autopilot/subsystems/fixedwing/gps_ublox_lea5h_hitl.makefile

@@ -2,5 +2,5 @@
 
 
 ap.CFLAGS += -DUSE_GPS -DUBX -DGPS_USE_LATLONG
-
+ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\"
-ap.srcs   += $(SRC_FIXEDWING)/gps_ubx.c $(SRC_FIXEDWING)/gps.c $(SRC_FIXEDWING)/latlong.c
+ap.srcs   +=  $(SRC_SUBSYSTEMS)/gps/gps_ubx.c $(SRC_SUBSYSTEMS)/gps.c

conf/autopilot/subsystems/fixedwing/gps_xsens.makefile

@@ -3,6 +3,5 @@
 
 # ap.CFLAGS += -DGPS
 
-ap.srcs   += $(SRC_FIXEDWING)/gps_xsens.c $(SRC_FIXEDWING)/gps.c $(SRC_FIXEDWING)/latlong.c
+$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
 
-sim.srcs += $(SRC_FIXEDWING)/gps.c $(SRC_FIXEDWING)/latlong.c

conf/autopilot/subsystems/fixedwing/imu_analog.makefile

@@ -55,7 +55,6 @@ imu_CFLAGS += -DADC_CHANNEL_ACCEL_X=$(ACCEL_X) -DADC_CHANNEL_ACCEL_Y=$(ACCEL_Y)
 
 imu_srcs += $(SRC_SUBSYSTEMS)/imu.c
 imu_srcs += $(SRC_SUBSYSTEMS)/imu/imu_analog.c
-imu_srcs += math/pprz_trig_int.c
 
 endif
 

conf/autopilot/subsystems/fixedwing/imu_booz.makefile

@@ -52,7 +52,6 @@ imu_srcs += $(SRC_ARCH)/peripherals/max1168_arch.c
 #ifeq ($(ARCH), lpc21)
 imu_CFLAGS += -DSSP_VIC_SLOT=9
 imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=8
-#FIXME ms2100 not used on this imu
 #else ifeq ($(ARCH), stm32)
 #imu_CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ
 #imu_CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT)
@@ -63,8 +62,6 @@ imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=8
 ap.srcs += $(imu_srcs)
 ap.CFLAGS += $(imu_CFLAGS)
 
-imu_srcs += math/pprz_trig_int.c
-
 #
 # Simulator
 #

conf/autopilot/subsystems/fixedwing/navigation.makefile

@@ -5,6 +5,7 @@
 
 $(TARGET).CFLAGS += -DNAV
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/nav.c
+$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/common_flight_plan.c
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/traffic_info.c
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/nav_survey_rectangle.c $(SRC_SUBSYSTEMS)/navigation/nav_line.c
 

conf/autopilot/subsystems/fixedwing/navigation_extra.makefile

@@ -5,6 +5,7 @@
 
 $(TARGET).CFLAGS += -DNAV
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/nav.c
+$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/common_flight_plan.c
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/traffic_info.c
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/nav_survey_rectangle.c $(SRC_SUBSYSTEMS)/navigation/nav_line.c