Merge branch 'master' into quat_transformations

to get updates/fixes from master for testing
2026-06-05 15:30:08 +08:00 · 2013-09-28 00:21:03 +02:00
parent 9077861210 d3ac39c6fc
commit 110c20ea2e
73 changed files with 1453 additions and 501 deletions
@@ -40,6 +40,7 @@
 # /conf/
 /conf/conf.xml
 /conf/conf.xml.20*
 /conf/conf_personal.xml.20*
 /conf/control_panel.xml
 /conf/%gconf.xml
 /conf/maps_data/*
@@ -113,7 +113,7 @@ update_google_version:
 conf: conf/conf.xml conf/control_panel.xml conf/maps.xml
-conf/%.xml :conf/%.xml.example
+conf/%.xml :conf/%_example.xml
 	[ -L $@ ] || [ -f $@ ] || cp $< $@
@@ -294,7 +294,7 @@ ab_clean:
 replace_current_conf_xml:
 	test conf/conf.xml && mv conf/conf.xml conf/conf.xml.backup.$(BUILD_DATETIME)
-	cp conf/tests_conf.xml conf/conf.xml
+	cp conf/conf_tests.xml conf/conf.xml
 restore_conf_xml:
 	test conf/conf.xml.backup.$(BUILD_DATETIME) && mv conf/conf.xml.backup.$(BUILD_DATETIME) conf/conf.xml
@@ -81,6 +81,38 @@ load upload program: $(OBJDIR)/$(TARGET).elf
 	# Kill the application
 	-echo "killall -9 $(TARGET).elf" | telnet $(HOST)
 	# Make the target dir and edit the config
 	-{ 												\
 	  echo "mkdir -p $(TARGET_DIR)";				\
 	} | telnet $(HOST)
 	# Upload the drivers and new application
 	{																								\
 	  echo "binary";																				\
 	  echo "put $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/cdc-acm.ko /$(SUB_DIR)/cdc-acm.ko";		\
 	  echo "put $(OBJDIR)/$(TARGET).elf /$(SUB_DIR)/$(TARGET).elf";									\
 	  echo "quit";																					\
 	} | ftp -n $(HOST)
 	# Upload the modules and start the application
 	-{ 															\
 	  echo "insmod $(TARGET_DIR)/cdc-acm.ko";					\
 	  echo "chmod 777 $(TARGET_DIR)/$(TARGET).elf";				\
 	  echo "$(TARGET_DIR)/$(TARGET).elf > /dev/null 2>&1 &";	\
 	} | telnet $(HOST)
 ifeq ($(ARDRONE2_REBOOT),1)
 	-{ 										\
 	  echo "reboot";						\
 	} | telnet $(HOST)
 endif
 # Program the device and start it.
 load2 upload2 program2: $(OBJDIR)/$(TARGET).elf
 	# Kill the application
 	-echo "killall -9 $(TARGET).elf" | telnet $(HOST)
 	# Make the target dir and edit the config
 	-{ 												\
 	  echo "mkdir -p $(TARGET_DIR)";				\
@@ -119,6 +151,8 @@ ifeq ($(ARDRONE2_REBOOT),1)
 endif
 # Link: create ELF output file from object files.
 .SECONDARY : $(OBJDIR)/$(TARGET).elf
 .PRECIOUS :   $(OBJ_C_OMAP) $(OBJ_CPP_OMAP)
@@ -140,6 +174,7 @@ $(OBJDIR)/%.o : %.cpp $(OBJDIR)/../Makefile.ac
 	$(Q)test -d $(dir $@) || mkdir -p $(dir $@)
 	$(Q)$(CXX) -c $(CXXFLAGS) $< -o $@
 # Listing of phony targets.
 .PHONY : all build elf clean clean_list
@@ -2,13 +2,15 @@
 <airframe>
  <section name="IMU" prefix="IMU_">
-    <!-- Magneto calibration -->
+
-    <define name="MAG_X_NEUTRAL" value="57"/>
+    <!-- Magneto calibration (done) -->
-    <define name="MAG_Y_NEUTRAL" value="-13"/>
+    <define name="MAG_X_NEUTRAL" value="8"/>
-    <define name="MAG_Z_NEUTRAL" value="-150"/>
+    <define name="MAG_Y_NEUTRAL" value="22"/>
-    <define name="MAG_X_SENS" value="18.4329549733" integer="16"/>
+    <define name="MAG_Z_NEUTRAL" value="-182"/>
-    <define name="MAG_Y_SENS" value="16.877985337" integer="16"/>
+    <define name="MAG_X_SENS" value="15.6093106965" integer="16"/>
-    <define name="MAG_Z_SENS" value="20.2550157453" integer="16"/>
+    <define name="MAG_Y_SENS" value="13.7479252753" integer="16"/>
    <define name="MAG_Z_SENS" value="16.7383397999" integer="16"/>
    <!-- Magneto current calibration -->
    <define name="MAG_X_CURRENT_COEF" value="0.00175105205138"/>
@@ -2,13 +2,18 @@
 <airframe>
  <section name="IMU" prefix="IMU_">
-    <!-- Magneto calibration -->
+    <!-- Accelero (done) -->
-    <define name="MAG_X_NEUTRAL" value="16"/>
+    <define name="ACCEL_X_NEUTRAL" value="2048"/>
-    <define name="MAG_Y_NEUTRAL" value="-11"/>
+    <define name="ACCEL_Y_NEUTRAL" value="2076"/>
-    <define name="MAG_Z_NEUTRAL" value="-131"/>
+    <define name="ACCEL_Z_NEUTRAL" value="2048"/>
-    <define name="MAG_X_SENS" value="17.206234987" integer="16"/>
+
-    <define name="MAG_Y_SENS" value="16.4124010821" integer="16"/>
+    <!-- Magneto calibration (done) -->
-    <define name="MAG_Z_SENS" value="13.6629556861" integer="16"/>
+    <define name="MAG_X_NEUTRAL" value="8"/>
 	<define name="MAG_Y_NEUTRAL" value="-7"/>
 	<define name="MAG_Z_NEUTRAL" value="-227"/>
 	<define name="MAG_X_SENS" value="15.5234215153" integer="16"/>
 	<define name="MAG_Y_SENS" value="14.2647577704" integer="16"/>
 	<define name="MAG_Z_SENS" value="14.9418031715" integer="16"/>
    <!-- Magneto current calibration -->
    <define name= "MAG_X_CURRENT_COEF" value="0.00202964254187"/>
@@ -2,13 +2,18 @@
 <airframe>
  <section name="IMU" prefix="IMU_">
-    <!-- Magneto calibration -->
+    <!-- Accelero (done) -->
-    <define name="MAG_X_NEUTRAL" value="33"/>
+    <define name="ACCEL_X_NEUTRAL" value="2068"/>
-    <define name="MAG_Y_NEUTRAL" value="44"/>
+    <define name="ACCEL_Y_NEUTRAL" value="2068"/>
-    <define name="MAG_Z_NEUTRAL" value="-264"/>
+    <define name="ACCEL_Z_NEUTRAL" value="2048"/>
-    <define name="MAG_X_SENS" value="19.1984157982" integer="16"/>
+
-    <define name="MAG_Y_SENS" value="18.7031225235" integer="16"/>
+    <!-- Magneto calibration (done) -->
-    <define name="MAG_Z_SENS" value="19.0380688485" integer="16"/>
+    <define name="MAG_X_NEUTRAL" value="71"/>
 	<define name="MAG_Y_NEUTRAL" value="-1"/>
 	<define name="MAG_Z_NEUTRAL" value="-166"/>
 	<define name="MAG_X_SENS" value="13.9587052153" integer="16"/>
 	<define name="MAG_Y_SENS" value="17.2130452079" integer="16"/>
 	<define name="MAG_Z_SENS" value="16.2785989428" integer="16"/>
    <!-- Magneto current calibration -->
    <define name="MAG_X_CURRENT_COEF" value="0.00175105205138"/>
@@ -2,13 +2,18 @@
 <airframe>
  <section name="IMU" prefix="IMU_">
-    <!-- Magneto calibration -->
+    <!-- Accelero (done) -->
-    <define name="MAG_X_NEUTRAL" value="33"/>
+    <define name="ACCEL_X_NEUTRAL" value="2048"/>
-    <define name="MAG_Y_NEUTRAL" value="44"/>
+    <define name="ACCEL_Y_NEUTRAL" value="2056"/>
-    <define name="MAG_Z_NEUTRAL" value="-264"/>
+    <define name="ACCEL_Z_NEUTRAL" value="2048"/>
-    <define name="MAG_X_SENS" value="19.1984157982" integer="16"/>
+
-    <define name="MAG_Y_SENS" value="18.7031225235" integer="16"/>
+    <!-- Magneto calibration (done) -->
-    <define name="MAG_Z_SENS" value="19.0380688485" integer="16"/>
+    <define name="MAG_X_NEUTRAL" value="-59"/>
    <define name="MAG_Y_NEUTRAL" value="-11"/>
    <define name="MAG_Z_NEUTRAL" value="-246"/>
    <define name="MAG_X_SENS" value="15.6071828304" integer="16"/>
    <define name="MAG_Y_SENS" value="14.9363172663" integer="16"/>
    <define name="MAG_Z_SENS" value="15.9964364143" integer="16"/>
    <!-- Magneto current calibration -->
    <define name="MAG_X_CURRENT_COEF" value="0.00175105205138"/>
@@ -1,14 +1,14 @@
-<!-- ARDrone calibration file -->
+<!-- ARDrone calibration file (Finished calibration) -->
 <airframe>
  <section name="IMU" prefix="IMU_">
    <!-- Magneto calibration -->
-    <define name="MAG_X_NEUTRAL" value="33"/>
+    <define name="MAG_X_NEUTRAL" value="18"/>
-    <define name="MAG_Y_NEUTRAL" value="44"/>
+	<define name="MAG_Y_NEUTRAL" value="7"/>
-    <define name="MAG_Z_NEUTRAL" value="-264"/>
+	<define name="MAG_Z_NEUTRAL" value="-190"/>
-    <define name="MAG_X_SENS" value="19.1984157982" integer="16"/>
+	<define name="MAG_X_SENS" value="17.5832018436" integer="16"/>
-    <define name="MAG_Y_SENS" value="18.7031225235" integer="16"/>
+	<define name="MAG_Y_SENS" value="16.9906378261" integer="16"/>
-    <define name="MAG_Z_SENS" value="19.0380688485" integer="16"/>
+	<define name="MAG_Z_SENS" value="18.4485786615" integer="16"/>
    <!-- Magneto current calibration -->
    <define name="MAG_X_CURRENT_COEF" value="0.00175105205138"/>
@@ -1,14 +1,14 @@
-<!-- ARDrone calibration file -->
+<!-- ARDrone calibration file (done) -->
 <airframe>
  <section name="IMU" prefix="IMU_">
    <!-- Magneto calibration -->
-    <define name="MAG_X_NEUTRAL" value="33"/>
+    <define name="MAG_X_NEUTRAL" value="0"/>
-    <define name="MAG_Y_NEUTRAL" value="44"/>
+	<define name="MAG_Y_NEUTRAL" value="0"/>
-    <define name="MAG_Z_NEUTRAL" value="-264"/>
+	<define name="MAG_Z_NEUTRAL" value="-190"/>
-    <define name="MAG_X_SENS" value="19.1984157982" integer="16"/>
+	<define name="MAG_X_SENS" value="16.0204597092" integer="16"/>
-    <define name="MAG_Y_SENS" value="18.7031225235" integer="16"/>
+	<define name="MAG_Y_SENS" value="15.5973173468" integer="16"/>
-    <define name="MAG_Z_SENS" value="19.0380688485" integer="16"/>
+	<define name="MAG_Z_SENS" value="17.1460428729" integer="16"/>
    <!-- Magneto current calibration -->
    <define name="MAG_X_CURRENT_COEF" value="0.00175105205138"/>
@@ -2,6 +2,11 @@
 <airframe>
  <section name="IMU" prefix="IMU_">
    <!-- Accelero (done) -->
    <define name="ACCEL_X_NEUTRAL" value="2056"/>
    <define name="ACCEL_Y_NEUTRAL" value="2052"/>
    <define name="ACCEL_Z_NEUTRAL" value="2048"/>
    <!-- Magneto calibration -->
    <define name="MAG_X_NEUTRAL" value="33"/>
    <define name="MAG_Y_NEUTRAL" value="44"/>
@@ -2,13 +2,18 @@
 <airframe>
  <section name="IMU" prefix="IMU_">
-    <!-- Magneto calibration -->
+    <!-- Accelero (done) -->
-    <define name="MAG_X_NEUTRAL" value="33"/>
+    <define name="ACCEL_X_NEUTRAL" value="2052"/>
-    <define name="MAG_Y_NEUTRAL" value="44"/>
+    <define name="ACCEL_Y_NEUTRAL" value="2052"/>
-    <define name="MAG_Z_NEUTRAL" value="-264"/>
+    <define name="ACCEL_Z_NEUTRAL" value="2048"/>
-    <define name="MAG_X_SENS" value="19.1984157982" integer="16"/>
+
-    <define name="MAG_Y_SENS" value="18.7031225235" integer="16"/>
+    <!-- Magneto calibration (done) -->
-    <define name="MAG_Z_SENS" value="19.0380688485" integer="16"/>
+<define name="MAG_X_NEUTRAL" value="17"/>
 <define name="MAG_Y_NEUTRAL" value="-31"/>
 <define name="MAG_Z_NEUTRAL" value="-195"/>
 <define name="MAG_X_SENS" value="15.2127658227" integer="16"/>
 <define name="MAG_Y_SENS" value="16.1444639915" integer="16"/>
 <define name="MAG_Z_SENS" value="16.5058009114" integer="16"/>
    <!-- Magneto current calibration -->
    <define name="MAG_X_CURRENT_COEF" value="0.00175105205138"/>
@@ -1,14 +1,20 @@
-<!-- ARDrone calibration file -->
+	<!-- ARDrone calibration file -->
 <airframe>
  <section name="IMU" prefix="IMU_">
    <!-- Accelero (done) -->
    <define name="ACCEL_X_NEUTRAL" value="2040"/>
    <define name="ACCEL_Y_NEUTRAL" value="2044"/>
    <define name="ACCEL_Z_NEUTRAL" value="2048"/>
    <!-- Magneto calibration -->
-    <define name="MAG_X_NEUTRAL" value="33"/>
+    <define name="MAG_X_NEUTRAL" value="-14"/>
-    <define name="MAG_Y_NEUTRAL" value="44"/>
+    <define name="MAG_Y_NEUTRAL" value="-15"/>
-    <define name="MAG_Z_NEUTRAL" value="-264"/>
+    <define name="MAG_Z_NEUTRAL" value="-202"/>
-    <define name="MAG_X_SENS" value="19.1984157982" integer="16"/>
+    <define name="MAG_X_SENS" value="15.71361627" integer="16"/>
-    <define name="MAG_Y_SENS" value="18.7031225235" integer="16"/>
+    <define name="MAG_Y_SENS" value="15.7480197906" integer="16"/>
-    <define name="MAG_Z_SENS" value="19.0380688485" integer="16"/>
+    <define name="MAG_Z_SENS" value="16.1966392929" integer="16"/>
    <!-- Magneto current calibration -->
    <define name="MAG_X_CURRENT_COEF" value="0.00175105205138"/>
@@ -2,13 +2,18 @@
 <airframe>
  <section name="IMU" prefix="IMU_">
-    <!-- Magneto calibration -->
+    <!-- Accelero (done) -->
-    <define name="MAG_X_NEUTRAL" value="33"/>
+    <define name="ACCEL_X_NEUTRAL" value="2048"/>
-    <define name="MAG_Y_NEUTRAL" value="44"/>
+    <define name="ACCEL_Y_NEUTRAL" value="2052"/>
-    <define name="MAG_Z_NEUTRAL" value="-264"/>
+    <define name="ACCEL_Z_NEUTRAL" value="2048"/>
-    <define name="MAG_X_SENS" value="19.1984157982" integer="16"/>
+
-    <define name="MAG_Y_SENS" value="18.7031225235" integer="16"/>
+    <!-- Magneto calibration (done) -->
-    <define name="MAG_Z_SENS" value="19.0380688485" integer="16"/>
+    <define name="MAG_X_NEUTRAL" value="5"/>
 	<define name="MAG_Y_NEUTRAL" value="-13"/>
 	<define name="MAG_Z_NEUTRAL" value="-161"/>
 	<define name="MAG_X_SENS" value="15.4856734848" integer="16"/>
 	<define name="MAG_Y_SENS" value="15.6311803268" integer="16"/>
 	<define name="MAG_Z_SENS" value="17.1897525095" integer="16"/>
    <!-- Magneto current calibration -->
    <define name="MAG_X_CURRENT_COEF" value="0.00175105205138"/>
@@ -13,13 +13,13 @@
      <define name="AUTOPILOT_DISABLE_AHRS_KILL"/>
      <define name="LINK_HOST" value="\\\"192.168.2.255\\\""/>
-      <!--  configure name="HOST" value="192.168.2.$(AC_ID)"/ -->
+      <configure name="HOST" value="192.168.2.$(AC_ID)"/>
      <configure name="ARDRONE2_START_PAPARAZZI" value="1"/>
      <configure name="ARDRONE2_WIFI_MODE" value="2"/>
      <configure name="ARDRONE2_SSID" value="tudelft_swarmhub"/>
      <configure name="ARDRONE2_IP_ADDRESS_BASE" value="192.168.2."/>
      <configure name="ARDRONE2_IP_ADDRESS_PROBE" value="$(AC_ID)"/>
-      <configure name="ARDRONE2_REBOOT" value="1"/>
+
    </target>
 	<!-- Subsystem section -->
@@ -36,6 +36,8 @@
  <modules main_freq="512">
    <load name="gps_ubx_ucenter.xml"/>
    <load name="flight_time.xml"/>
    <load name="TUDelft/imav2013.xml"/>
  </modules>
  <commands>
@@ -76,10 +78,6 @@
  <include href="conf/airframes/TUDelft/IMAV2013/ARDrone/$AC_ID_calib.xml"/>
  <section name="IMU" prefix="IMU_">
    <define name="ACCEL_X_NEUTRAL" value="2056"/>
    <define name="ACCEL_Y_NEUTRAL" value="2060"/>
    <define name="ACCEL_Z_NEUTRAL" value="2052"/>
    <!-- Body to IMU is no difference -->
    <define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
@@ -89,9 +87,9 @@
  <!-- local magnetic field, calculated for: 52°3'56", 4°31'24" -->
  <!-- http://paparazzi.enac.fr/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
  <section name="AHRS" prefix="AHRS_">
-    <define name="H_X" value="0.3892503"/>
+    <define name="H_X" value="0.513081"/>
-    <define name="H_Y" value="0.0017972"/>
+    <define name="H_Y" value="-0.00242783"/>
-    <define name="H_Z" value="0.9211303"/>
+    <define name="H_Z" value="0.858336"/>
  </section>
  <section name="INS" prefix="INS_">
@@ -181,10 +179,10 @@
  </section>
  <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
-    <define name="MAX_BANK" value="20" unit="deg"/>
+    <define name="MAX_BANK" value="30" unit="deg"/>
    <define name="PGAIN" value="79"/>
    <define name="DGAIN" value="100"/>
-    <define name="IGAIN" value="26"/>
+    <define name="IGAIN" value="60"/>
  </section>
  <section name="AUTOPILOT">
@@ -196,9 +194,9 @@
  <section name="BAT">
    <define name="MILLIAMP_AT_FULL_THROTTLE" value="8700"/>
-    <define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
+    <define name="CATASTROPHIC_BAT_LEVEL" value="9.1" unit="V"/>
-    <define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
+    <define name="CRITIC_BAT_LEVEL" value="9.35" unit="V"/>
-    <define name="LOW_BAT_LEVEL" value="9.7" unit="V"/>
+    <define name="LOW_BAT_LEVEL" value="9.45" unit="V"/>
    <define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
  </section>
 </airframe>
@@ -55,12 +55,12 @@
    <define name="ACCEL_Z_NEUTRAL" value="0"/>
    <!-- MAGNETO CALIBRATION DELFT -->
-    <define name="MAG_X_NEUTRAL" value="302"/>
+    <define name="MAG_X_NEUTRAL" value="-157"/>
-    <define name="MAG_Y_NEUTRAL" value="-69"/>
+    <define name="MAG_Y_NEUTRAL" value="-379"/>
-    <define name="MAG_Z_NEUTRAL" value="100"/>
+    <define name="MAG_Z_NEUTRAL" value="130"/>
-    <define name="MAG_X_SENS" value="4.12281780645" integer="16"/>
+    <define name="MAG_X_SENS" value="3.65789837026" integer="16"/>
-    <define name="MAG_Y_SENS" value="4.2491445745" integer="16"/>
+    <define name="MAG_Y_SENS" value="3.85373116155" integer="16"/>
-    <define name="MAG_Z_SENS" value="4.44132606171" integer="16"/>
+    <define name="MAG_Z_SENS" value="4.28038049955" integer="16"/>
    <!-- MAGNETO CURRENT CALIBRATION -->
    <define name="MAG_X_CURRENT_COEF" value="0.0103422023767"/>
@@ -71,12 +71,12 @@
 <section name="AUTOPILOT">
   <define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
   <define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_DIRECT"/>
-   <define name="MODE_AUTO2" value="AP_MODE_ATTITUDE_DIRECT"/>
+   <define name="MODE_AUTO2" value="AP_MODE_ATTITUDE_Z_HOLD"/>
 </section>
 <section name="BAT">
   <define name="MILLIAMP_AT_FULL_THROTTLE" value="14000"/>
-   <define name="CATASTROPHIC_BAT_LEVEL" value="4.3" unit="V"/>
+   <define name="CATASTROPHIC_BAT_LEVEL" value="3.0" unit="V"/>
 </section>
@@ -106,29 +106,29 @@
    <define name="DEADBAND_A" value="250"/>
    <!-- reference -->
-    <define name="REF_OMEGA_P" value="800" unit="deg/s"/>
+   <define name="REF_OMEGA_P" value="1500" unit="deg/s"/>
    <define name="REF_ZETA_P" value="0.85"/>
    <define name="REF_MAX_P" value="400." unit="deg/s"/>
    <define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
-    <define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
+    <define name="REF_OMEGA_Q" value="1500" unit="deg/s"/>
    <define name="REF_ZETA_Q" value="0.85"/>
    <define name="REF_MAX_Q" value="400." unit="deg/s"/>
    <define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
    <define name="REF_OMEGA_R" value="500" unit="deg/s"/>
    <define name="REF_ZETA_R" value="0.85"/>
-    <define name="REF_MAX_R" value="90." unit="deg/s"/>
+    <define name="REF_MAX_R" value="180." unit="deg/s"/>
-    <define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
+    <define name="REF_MAX_RDOT" value="RadOfDeg(1000.)"/>
    <!-- feedback -->
-    <define name="PHI_PGAIN" value="150"/>
+    <define name="PHI_PGAIN" value="180"/>
-    <define name="PHI_DGAIN" value="100"/>
+    <define name="PHI_DGAIN" value="140"/>
-    <define name="PHI_IGAIN" value="20"/>
+    <define name="PHI_IGAIN" value="80"/>
-    <define name="THETA_PGAIN" value="150"/>
+    <define name="THETA_PGAIN" value="180"/>
-    <define name="THETA_DGAIN" value="100"/>
+    <define name="THETA_DGAIN" value="140"/>
-    <define name="THETA_IGAIN" value="20"/>
+    <define name="THETA_IGAIN" value="80"/>
    <define name="PSI_PGAIN" value="500"/>
    <define name="PSI_DGAIN" value="200"/>
@@ -154,10 +154,10 @@
   <define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
   <define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
   <define name="MAX_SUM_ERR" value="2000000"/>
-   <define name="HOVER_KP" value="400"/>
+   <define name="HOVER_KP" value="531"/>
-   <define name="HOVER_KD" value="350"/>
+   <define name="HOVER_KD" value="356"/>
-   <define name="HOVER_KI" value="144"/>
+   <define name="HOVER_KI" value="0"/>
-   <define name="GUIDANCE_V_NOMINAL_HOVER_THROTTLE" value="0.9"/>
+   <define name="NOMINAL_HOVER_THROTTLE" value="0.669"/>
  </section>
  <section name="AHRS" prefix="AHRS_">
@@ -1,22 +1,12 @@
 <conf>
  <aircraft
   name="Ardrone2_raw_180"
   ac_id="180"
   airframe="airframes/TUDelft/IMAV2013/ardrone2_raw.xml"
   radio="radios/cockpitSX.xml"
   telemetry="telemetry/default_ardrone.xml"
   flight_plan="flight_plans/rotorcraft_basic.xml"
   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml"
   gui_color="red"
  />
  <aircraft
   name="Ardrone2_raw_181"
   ac_id="181"
   airframe="airframes/TUDelft/IMAV2013/ardrone2_raw.xml"
   radio="radios/cockpitSX.xml"
   telemetry="telemetry/default_ardrone.xml"
-   flight_plan="flight_plans/rotorcraft_basic.xml"
+   flight_plan="flight_plans/TUDelft/IMAV2013_rotorcraft.xml"
-   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml"
+   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml settings/modules/flight_time.xml"
   gui_color="red"
  />
  <aircraft
@@ -25,8 +15,8 @@
   airframe="airframes/TUDelft/IMAV2013/ardrone2_raw.xml"
   radio="radios/cockpitSX.xml"
   telemetry="telemetry/default_ardrone.xml"
-   flight_plan="flight_plans/rotorcraft_basic.xml"
+   flight_plan="flight_plans/TUDelft/IMAV2013_rotorcraft.xml"
-   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml"
+   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml settings/modules/flight_time.xml"
   gui_color="red"
  />
  <aircraft
@@ -35,8 +25,8 @@
   airframe="airframes/TUDelft/IMAV2013/ardrone2_raw.xml"
   radio="radios/cockpitSX.xml"
   telemetry="telemetry/default_ardrone.xml"
-   flight_plan="flight_plans/rotorcraft_basic.xml"
+   flight_plan="flight_plans/TUDelft/IMAV2013_rotorcraft.xml"
-   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml"
+   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml settings/modules/flight_time.xml"
   gui_color="red"
  />
  <aircraft
@@ -45,8 +35,8 @@
   airframe="airframes/TUDelft/IMAV2013/ardrone2_raw.xml"
   radio="radios/cockpitSX.xml"
   telemetry="telemetry/default_ardrone.xml"
-   flight_plan="flight_plans/rotorcraft_basic.xml"
+   flight_plan="flight_plans/TUDelft/IMAV2013_rotorcraft.xml"
-   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml"
+   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml settings/modules/flight_time.xml"
   gui_color="red"
  />
  <aircraft
@@ -55,8 +45,8 @@
   airframe="airframes/TUDelft/IMAV2013/ardrone2_raw.xml"
   radio="radios/cockpitSX.xml"
   telemetry="telemetry/default_ardrone.xml"
-   flight_plan="flight_plans/rotorcraft_basic.xml"
+   flight_plan="flight_plans/TUDelft/IMAV2013_rotorcraft.xml"
-   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml"
+   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml settings/modules/flight_time.xml"
   gui_color="red"
  />
  <aircraft
@@ -65,8 +55,8 @@
   airframe="airframes/TUDelft/IMAV2013/ardrone2_raw.xml"
   radio="radios/cockpitSX.xml"
   telemetry="telemetry/default_ardrone.xml"
-   flight_plan="flight_plans/rotorcraft_basic.xml"
+   flight_plan="flight_plans/TUDelft/IMAV2013_rotorcraft.xml"
-   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml"
+   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml settings/modules/flight_time.xml"
   gui_color="red"
  />
  <aircraft
@@ -75,8 +65,8 @@
   airframe="airframes/TUDelft/IMAV2013/ardrone2_raw.xml"
   radio="radios/cockpitSX.xml"
   telemetry="telemetry/default_ardrone.xml"
-   flight_plan="flight_plans/rotorcraft_basic.xml"
+   flight_plan="flight_plans/TUDelft/IMAV2013_rotorcraft.xml"
-   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml"
+   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml settings/modules/flight_time.xml"
   gui_color="red"
  />
  <aircraft
@@ -85,8 +75,8 @@
   airframe="airframes/TUDelft/IMAV2013/ardrone2_raw.xml"
   radio="radios/cockpitSX.xml"
   telemetry="telemetry/default_ardrone.xml"
-   flight_plan="flight_plans/rotorcraft_basic.xml"
+   flight_plan="flight_plans/TUDelft/IMAV2013_rotorcraft.xml"
-   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml"
+   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml settings/modules/flight_time.xml"
   gui_color="red"
  />
  <aircraft
@@ -95,8 +85,8 @@
   airframe="airframes/TUDelft/IMAV2013/ardrone2_raw.xml"
   radio="radios/cockpitSX.xml"
   telemetry="telemetry/default_ardrone.xml"
-   flight_plan="flight_plans/rotorcraft_basic.xml"
+   flight_plan="flight_plans/TUDelft/IMAV2013_rotorcraft.xml"
-   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml"
+   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml settings/modules/flight_time.xml"
   gui_color="red"
  />
  <aircraft
@@ -105,8 +95,8 @@
   airframe="airframes/TUDelft/IMAV2013/ardrone2_raw.xml"
   radio="radios/cockpitSX.xml"
   telemetry="telemetry/default_ardrone.xml"
-   flight_plan="flight_plans/rotorcraft_basic.xml"
+   flight_plan="flight_plans/TUDelft/IMAV2013_rotorcraft.xml"
-   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml"
+   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml settings/modules/flight_time.xml"
   gui_color="red"
  />
  <aircraft
@@ -115,8 +105,8 @@
   airframe="airframes/TUDelft/IMAV2013/ardrone2_raw.xml"
   radio="radios/cockpitSX.xml"
   telemetry="telemetry/default_ardrone.xml"
-   flight_plan="flight_plans/rotorcraft_basic.xml"
+   flight_plan="flight_plans/TUDelft/IMAV2013_rotorcraft.xml"
-   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml"
+   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml settings/modules/flight_time.xml"
   gui_color="red"
  />
  <aircraft
@@ -125,8 +115,8 @@
   airframe="airframes/TUDelft/IMAV2013/ardrone2_raw.xml"
   radio="radios/cockpitSX.xml"
   telemetry="telemetry/default_ardrone.xml"
-   flight_plan="flight_plans/rotorcraft_basic.xml"
+   flight_plan="flight_plans/TUDelft/IMAV2013_rotorcraft.xml"
-   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml"
+   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml settings/modules/flight_time.xml"
   gui_color="red"
  />
  <aircraft
@@ -135,8 +125,8 @@
   airframe="airframes/TUDelft/IMAV2013/ardrone2_raw.xml"
   radio="radios/cockpitSX.xml"
   telemetry="telemetry/default_ardrone.xml"
-   flight_plan="flight_plans/rotorcraft_basic.xml"
+   flight_plan="flight_plans/TUDelft/IMAV2013_rotorcraft.xml"
-   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml"
+   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml settings/modules/flight_time.xml"
   gui_color="red"
  />
  <aircraft
@@ -145,8 +135,8 @@
   airframe="airframes/TUDelft/IMAV2013/ardrone2_raw.xml"
   radio="radios/cockpitSX.xml"
   telemetry="telemetry/default_ardrone.xml"
-   flight_plan="flight_plans/rotorcraft_basic.xml"
+   flight_plan="flight_plans/TUDelft/IMAV2013_rotorcraft.xml"
-   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml"
+   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml settings/modules/flight_time.xml"
   gui_color="red"
  />
  <aircraft
@@ -155,8 +145,8 @@
   airframe="airframes/TUDelft/IMAV2013/ardrone2_raw.xml"
   radio="radios/cockpitSX.xml"
   telemetry="telemetry/default_ardrone.xml"
-   flight_plan="flight_plans/rotorcraft_basic.xml"
+   flight_plan="flight_plans/TUDelft/IMAV2013_rotorcraft_vision.xml"
-   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml"
+   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml settings/modules/flight_time.xml"
   gui_color="red"
  />
  <aircraft
@@ -165,8 +155,8 @@
   airframe="airframes/TUDelft/IMAV2013/ardrone2_raw.xml"
   radio="radios/cockpitSX.xml"
   telemetry="telemetry/default_ardrone.xml"
-   flight_plan="flight_plans/rotorcraft_basic.xml"
+   flight_plan="flight_plans/TUDelft/IMAV2013_rotorcraft.xml"
-   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml"
+   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml settings/modules/flight_time.xml"
   gui_color="red"
  />
  <aircraft
@@ -189,6 +179,16 @@
   settings=" settings/fixedwing_basic.xml settings/control/ctl_basic.xml settings/estimation/ins_neutrals.xml"
   gui_color="white"
  />
  <aircraft
   name="MicroQuadTurnigy"
   ac_id="136"
   airframe="airframes/TUDelft/quadrotor_lisa_m_2_pwm_hbk_ppm.xml"
   radio="radios/R6107SP_7ch.xml"
   telemetry="telemetry/default_rotorcraft.xml"
   flight_plan="flight_plans/TUDelft/IMAV2013_rotorcraft.xml"
   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/modules/imu_quality_assessment.xml"
   gui_color="blue"
  />
  <aircraft
   name="Quadrotor_LisaS"
   ac_id="132"
@@ -219,4 +219,14 @@
   settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml"
   gui_color="green"
  />
  <aircraft
   name="MicroQuadTurnigy"
   ac_id="136"
   airframe="airframes/TUDelft/quadrotor_lisa_m_2_pwm_hbk_ppm.xml"
   radio="radios/R6107SP_7ch.xml"
   telemetry="telemetry/default_rotorcraft.xml"
   flight_plan="flight_plans/TUDelft/rotorcraft_basic_delft.xml"
   settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/modules/imu_quality_assessment.xml"
   gui_color="blue"
  />
 </conf>
@@ -28,11 +28,14 @@
    <!-- Actuators are automatically chosen according to board-->
    <subsystem name="imu"     type="lisa_s_v0.1"/>
    <subsystem name="ahrs"    type="float_dcm"/>
      <define name="LISA_S_UPSIDE_DOWN"/>
    <subsystem name="control"/>
    <subsystem name="navigation"/>
    <!-- Sensors -->
    <subsystem name="gps" type="ublox"/>
    <subsystem name="ins" type="alt_float"/>
    <subsystem name="actuators" type="pwm_double"/>
  </firmware>
 <modules>
@@ -40,10 +43,12 @@
 </modules>
 <!-- commands section -->
-  <servos>
+  <servos driver="Pwm">
-    <servo name="MOTOR" no="1" min="1040" neutral="1040" max="2000"/>
+    <servo name="MOTOR" no="2" min="1040" neutral="1040" max="2000"/>
  </servos>
  <servos driver="Pwm_double">
    <servo name="AILEVON_LEFT" no="0" min="1200" neutral="1500" max="1800"/>
-    <servo name="AILEVON_RIGHT" no="2" min="1800" neutral="1500" max="1200"/>
+    <servo name="AILEVON_RIGHT" no="1" min="1800" neutral="1500" max="1200"/>
  </servos>
  <commands>
@@ -77,7 +82,7 @@
  </section>
  <section name="IMU" prefix="IMU_">
-    <define name="BODY_TO_IMU_PHI" value="190." unit="deg"/>
+    <define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
    <define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
@@ -50,12 +50,12 @@
    <define name="ACCEL_Z_NEUTRAL" value="0"/>
    <!-- From delft MAVLab (TODO) -->
-    <define name="MAG_X_NEUTRAL" value="396"/>
+    <define name="MAG_X_NEUTRAL" value="-638"/>
-    <define name="MAG_Y_NEUTRAL" value="-260"/>
+    <define name="MAG_Y_NEUTRAL" value="-101"/>
-    <define name="MAG_Z_NEUTRAL" value="-91"/>
+    <define name="MAG_Z_NEUTRAL" value="-307"/>
-    <define name="MAG_X_SENS" value="3.71635905504" integer="16"/>
+    <define name="MAG_X_SENS" value="3.61215021824" integer="16"/>
-    <define name="MAG_Y_SENS" value="3.5526454063" integer="16"/>
+    <define name="MAG_Y_SENS" value="4.23407732206" integer="16"/>
-    <define name="MAG_Z_SENS" value="3.52339566014" integer="16"/>
+    <define name="MAG_Z_SENS" value="4.28237277804" integer="16"/>
  </section>
 <section name="AUTOPILOT">
@@ -85,45 +85,43 @@
    <!-- setpoints -->
    <define name="SP_MAX_PHI" value="45." unit="deg"/>
    <define name="SP_MAX_THETA" value="45." unit="deg"/>
    <define name="SP_MAX_PSI" value="45." unit="deg"/>
    <define name="SP_MAX_R" value="90." unit="deg/s"/>
    <define name="SP_MAX_P" value="90." unit="deg/s"/>
    <define name="DEADBAND_R" value="250"/>
    <define name="DEADBAND_A" value="250"/>
    <!-- reference -->
-    <define name="REF_OMEGA_P" value="800" unit="deg/s"/>
+    <define name="REF_OMEGA_P" value="3000" unit="deg/s"/>
    <define name="REF_ZETA_P" value="0.85"/>
-    <define name="REF_MAX_P" value="400." unit="deg/s"/>
+    <define name="REF_MAX_P" value="300." unit="deg/s"/>
-    <define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
+    <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
-    <define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
+    <define name="REF_OMEGA_Q" value="3000" unit="deg/s"/>
    <define name="REF_ZETA_Q" value="0.85"/>
-    <define name="REF_MAX_Q" value="400." unit="deg/s"/>
+    <define name="REF_MAX_Q" value="300." unit="deg/s"/>
-    <define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
+    <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
-    <define name="REF_OMEGA_R" value="500" unit="deg/s"/>
+    <define name="REF_OMEGA_R" value="3000" unit="deg/s"/>
    <define name="REF_ZETA_R" value="0.85"/>
-    <define name="REF_MAX_R" value="90." unit="deg/s"/>
+    <define name="REF_MAX_R" value="300." unit="deg/s"/>
-    <define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
+    <define name="REF_MAX_RDOT" value="RadOfDeg(7000.)"/>
    <!-- feedback -->
    <define name="PHI_PGAIN" value="3052"/>
    <define name="PHI_DGAIN" value="108"/>
-    <define name="PHI_IGAIN" value="0"/>
+    <define name="PHI_IGAIN" value="14"/>
    <define name="THETA_PGAIN" value="3052"/>
    <define name="THETA_DGAIN" value="108"/>
-    <define name="THETA_IGAIN" value="0"/>
+    <define name="THETA_IGAIN" value="14"/>
-    <define name="PSI_PGAIN" value="944"/>
+    <define name="PSI_PGAIN" value="1099"/>
-    <define name="PSI_DGAIN" value="300"/>
+    <define name="PSI_DGAIN" value="201"/>
-    <define name="PSI_IGAIN" value="10"/>
+    <define name="PSI_IGAIN" value="19"/>
    <!-- feedforward -->
    <define name="PHI_DDGAIN" value="0"/>
    <define name="THETA_DDGAIN" value="0"/>
-    <define name="PSI_DDGAIN" value=" 300"/>
+    <define name="PSI_DDGAIN" value="0"/>
    <define name="PHI_AGAIN" value="0"/>
    <define name="THETA_AGAIN" value="0"/>
@@ -153,9 +151,9 @@
  </section>
 <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
-   <define name="PGAIN" value="50"/>
+   <define name="PGAIN" value="16"/>
-   <define name="DGAIN" value="50"/>
+   <define name="DGAIN" value="35"/>
-   <define name="IGAIN" value="0"/>
+   <define name="IGAIN" value="6"/>
 </section>
 <section name="MISC">
@@ -5,7 +5,6 @@
  <firmware name="rotorcraft">
    <target name="ap" board="ardrone2_raw">
      <define name="USE_INS_NAV_INIT"/>
      <!--configure name="USE_NEW_I2C_DRIVER" value="1"/ -->
      <configure name="AHRS_PROPAGATE_FREQUENCY" value="200"/>
      <define name="AHRS_PROPAGATE_FREQUENCY" value="200"/>
      <define name="INS_PROPAGATE_FREQUENCY" value="200"/>
@@ -20,15 +19,14 @@
      <subsystem name="radio_control" type="ppm"/>
    </target>
 	<!-- Subsystem section -->
    <subsystem name="motor_mixing"/>
    <subsystem name="actuators" type="ardrone2"/>
    <subsystem name="imu" type="ardrone2"/>
    <subsystem name="gps" type="ublox"/>
    <subsystem name="stabilization" type="int_quat"/>
    <subsystem name="ahrs" type="int_cmpl_quat"/>
    <subsystem name="ins" type="extended"/>
  </firmware>
  <modules main_freq="512">
@@ -39,7 +37,7 @@
    <axis name="PITCH" failsafe_value="0"/>
    <axis name="ROLL" failsafe_value="0"/>
    <axis name="YAW" failsafe_value="0"/>
-    <axis name="THRUST" failsafe_value="0"/>
+    <axis name="THRUST" failsafe_value="3000"/>
  </commands>
  <servos driver="Default">
@@ -55,11 +53,12 @@
    <define name="TRIM_YAW" value="0"/>
    <define name="NB_MOTOR" value="4"/>
    <define name="SCALE" value="255"/>
    <!-- Time cross layout (X), as documented in paparazzi -->
-    <define name="ROLL_COEF" value="{ 256, -256,  -256,  256 }"/>
+    <define name="ROLL_COEF" value="{   256, -256, -256,  256  }"/>
-    <define name="PITCH_COEF" value="{  256, 256, -256,  -256 }"/>
+    <define name="PITCH_COEF" value="{  256,  256, -256,  -256 }"/>
-    <define name="YAW_COEF" value="{ -256,  256, -256,  256 }"/>
+    <define name="YAW_COEF" value="{   -256,  256, -256,  256  }"/>
-    <define name="THRUST_COEF" value="{  256,  256,  256,  256 }"/>
+    <define name="THRUST_COEF" value="{ 256,  256,  256,  256  }"/>
  </section>
  <command_laws>
@@ -71,36 +70,42 @@
  </command_laws>
  <section name="IMU" prefix="IMU_">
-    <define name="ACCEL_X_NEUTRAL" value="2056"/>
+    <!-- Accelero -->
-    <define name="ACCEL_Y_NEUTRAL" value="2060"/>
+    <define name="ACCEL_X_NEUTRAL" value="2048"/>
-    <define name="ACCEL_Z_NEUTRAL" value="2052"/>
+    <define name="ACCEL_Y_NEUTRAL" value="2048"/>
    <define name="ACCEL_Z_NEUTRAL" value="2048"/>
-    <define name="MAG_X_NEUTRAL" value="99"/>
+    <!-- Magneto calibration -->
-    <define name="MAG_Y_NEUTRAL" value="59"/>
+    <define name="MAG_X_NEUTRAL" value="0"/>
-    <define name="MAG_Z_NEUTRAL" value="-65"/>
+    <define name="MAG_Y_NEUTRAL" value="0"/>
-    <define name="MAG_X_SENS" value="14.8494072406" integer="16"/>
+    <define name="MAG_Z_NEUTRAL" value="-180"/>
-    <define name="MAG_Y_SENS" value="14.4187929352" integer="16"/>
+    <define name="MAG_X_SENS" value="16." integer="16"/>
-    <define name="MAG_Z_SENS" value="15.4390790318" integer="16"/>
+    <define name="MAG_Y_SENS" value="16." integer="16"/>
-    <define name="MAG_X_CURRENT_COEF" value="0.00175105205138"/>
+    <define name="MAG_Z_SENS" value="16." integer="16"/>
    <define name="MAG_Y_CURRENT_COEF" value="0.000172518663485"/>
    <define name="MAG_Z_CURRENT_COEF" value="0.00022802329524"/>
    <!-- Magneto current calibration -->
    <define name="MAG_X_CURRENT_COEF" value="0.0"/>
    <define name="MAG_Y_CURRENT_COEF" value="0.0"/>
    <define name="MAG_Z_CURRENT_COEF" value="0.0"/>
  </section>
-
+  <section name="IMU" prefix="IMU_">
    <!-- roll -->
    <define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
    <!-- pitch -->
    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
    <!-- yaw -->
    <define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
  </section>
-  <!-- local magnetic field, calculated for: 52°3'56", 4°31'24" -->
+  <!-- local magnetic field -->
  <!-- http://paparazzi.enac.fr/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
  <section name="AHRS" prefix="AHRS_">
-    <define name="H_X" value="0.3892503"/>
+    <!-- Toulouse -->
    <define name="H_X" value="0.513081"/>
    <define name="H_Y" value="-0.00242783"/>
    <define name="H_Z" value="0.858336"/>
    <!-- Delft -->
    <!--define name="H_X" value="0.3892503"/>
    <define name="H_Y" value="0.0017972"/>
-    <define name="H_Z" value="0.9211303"/>
+    <define name="H_Z" value="0.9211303"/ -->
  </section>
  <section name="INS" prefix="INS_">
@@ -132,7 +137,6 @@
    <define name="DDGAIN_R" value="300"/>
  </section>
  <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
    <!-- setpoints -->
    <define name="SP_MAX_PHI" value="45" unit="deg"/>
@@ -191,10 +195,13 @@
  </section>
  <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
    <!-- Good weather -->
    <define name="MAX_BANK" value="20" unit="deg"/>
    <!-- Bad weather -->
    <!-- define name="MAX_BANK" value="32" unit="deg"/ -->
    <define name="PGAIN" value="79"/>
    <define name="DGAIN" value="100"/>
-    <define name="IGAIN" value="26"/>
+    <define name="IGAIN" value="60"/>
  </section>
  <section name="SIMULATOR" prefix="NPS_">
@@ -217,5 +224,4 @@
    <define name="LOW_BAT_LEVEL" value="9.7" unit="V"/>
    <define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
  </section>
 </airframe>
@@ -63,15 +63,16 @@
    <define name="MAG_Z_SENS" value="4.54518768636" integer="16"/>
    <!-- MAGNETO CURRENT CALIBRATION -->
-    <define name= "MAG_X_CURRENT_COEF" value="0.0103422023767"/>
+    <define name="MAG_X_CURRENT_COEF" value="0.0103422023767"/>
-    <define name= "MAG_Y_CURRENT_COEF" value="0.0084568317783"/>
+    <define name="MAG_Y_CURRENT_COEF" value="0.0084568317783"/>
-    <define name= "MAG_Z_CURRENT_COEF" value="-0.01935617335"/>
+    <define name="MAG_Z_CURRENT_COEF" value="-0.01935617335"/>
  </section>
 <section name="AUTOPILOT">
   <define name="MODE_STARTUP" value="AP_MODE_NAV"/>
   <define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
   <define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_DIRECT"/>
-   <define name="MODE_AUTO2" value="AP_MODE_ATTITUDE_DIRECT"/>
+   <define name="MODE_AUTO2" value="AP_MODE_NAV"/>
 </section>
 <section name="BAT">
@@ -117,10 +118,10 @@
    <define name="REF_MAX_Q" value="300." unit="deg/s"/>
    <define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
-    <define name="REF_OMEGA_R" value="1000" unit="deg/s"/>
+    <define name="REF_OMEGA_R" value="500" unit="deg/s"/>
    <define name="REF_ZETA_R" value="0.85"/>
    <define name="REF_MAX_R" value="180." unit="deg/s"/>
-    <define name="REF_MAX_RDOT" value="RadOfDeg(2000.)"/>
+    <define name="REF_MAX_RDOT" value="RadOfDeg(1000.)"/>
    <!-- feedback -->
    <define name="PHI_PGAIN" value="399"/>
@@ -155,9 +156,9 @@
   <define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
   <define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
   <define name="MAX_SUM_ERR" value="2000000"/>
-   <define name="HOVER_KP" value="400"/>
+   <define name="HOVER_KP" value="276"/>
-   <define name="HOVER_KD" value="350"/>
+   <define name="HOVER_KD" value="455"/>
-   <define name="HOVER_KI" value="144"/>
+   <define name="HOVER_KI" value="100"/>
   <define name="GUIDANCE_V_NOMINAL_HOVER_THROTTLE" value="0.9"/>
  </section>
@@ -168,9 +169,9 @@
  </section>
 <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
-   <define name="PGAIN" value="50"/>
+   <define name="PGAIN" value="39"/>
   <define name="DGAIN" value="50"/>
-   <define name="IGAIN" value="0"/>
+   <define name="IGAIN" value="39"/>
 </section>
 <section name="MISC">
@@ -197,6 +198,7 @@
      <configure name="AHRS_PROPAGATE_FREQUENCY" value="500"/>
      <configure name="LISA_S_BARO" value="BARO_MS5611_SPI"/>
        <define name="AUTOPILOT_DISABLE_AHRS_KILL"/>
    </target>
    <target name="nps" board="pc">
@@ -53,6 +53,7 @@ nps.srcs += $(NPSDIR)/nps_main.c                 \
       $(NPSDIR)/nps_sensor_baro.c               \
       $(NPSDIR)/nps_sensor_gps.c                \
       $(NPSDIR)/nps_electrical.c                \
       $(NPSDIR)/nps_atmosphere.c                \
       $(NPSDIR)/nps_radio_control.c             \
       $(NPSDIR)/nps_radio_control_joystick.c    \
       $(NPSDIR)/nps_radio_control_spektrum.c    \
@@ -49,6 +49,7 @@ nps.srcs += $(NPSDIR)/nps_main.c                 \
       $(NPSDIR)/nps_sensor_baro.c               \
       $(NPSDIR)/nps_sensor_gps.c                \
       $(NPSDIR)/nps_electrical.c                \
       $(NPSDIR)/nps_atmosphere.c                \
       $(NPSDIR)/nps_radio_control.c             \
       $(NPSDIR)/nps_radio_control_joystick.c    \
       $(NPSDIR)/nps_radio_control_spektrum.c    \
@@ -47,7 +47,7 @@ include $(CFG_SHARED)/imu_aspirin_v2_common.makefile
 # so that it will be unselected at init (baro CS line high)
 #
 ifeq ($(ARCH), lpc21)
-IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE1
+#IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE1
 else ifeq ($(ARCH), stm32)
 # SLAVE3 is on PC13, which is the baro CS
 IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE3
@@ -0,0 +1,113 @@
 <!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
 <flight_plan alt="152" ground_alt="147" lat0="43 33 50.83" lon0="1 28 52.61" max_dist_from_home="400" name="Booz Test Enac" security_height="2">
  <header>
 #include "autopilot.h"
  </header>
  <waypoints>
    <waypoint name="HOME" x="0.0" y="0.0"/>
    <waypoint name="CLIMB" x="0.0" y="5.0"/>
    <waypoint name="STDBY" x="-2.0" y="-5.0"/>
    <waypoint name="p1" x="3.6" y="-13.9"/>
    <waypoint name="p2" x="27.5" y="-48.2"/>
    <waypoint name="p3" x="16.7" y="-19.6"/>
    <waypoint name="p4" x="13.7" y="-40.7"/>
    <waypoint name="FA1" x="200" y="200"/>
    <waypoint name="FA2" x="200" y="-200"/>
    <waypoint name="FA3" x="-200" y="-200"/>
    <waypoint name="FA4" x="-200" y="200"/>
    <waypoint name="KILL1" x="280" y="280"/>
    <waypoint name="KILL2" x="280" y="-280"/>
    <waypoint name="KILL3" x="-280" y="-280"/>
    <waypoint name="KILL4" x="-280" y="280"/>
    <waypoint name="CAM" x="-20" y="-50" height="2."/>
    <waypoint name="TD" x="5.6" y="-10.9"/>
  </waypoints>
  <sectors>
    <sector color="red" name="Flight_Area">
      <corner name="FA1"/>
      <corner name="FA2"/>
      <corner name="FA3"/>
      <corner name="FA4"/>
    </sector>
    <sector color="red" name="Kill">
      <corner name="KILL1"/>
      <corner name="KILL2"/>
      <corner name="KILL3"/>
      <corner name="KILL4"/>
    </sector>
  </sectors>
  <exceptions>
    <!-- Check inside small flight area, then goto Center(Standby) -->
    <exception cond="Or(! InsideFlight_Area(GetPosX(), GetPosY()), GetPosAlt() > ground_alt + 50) && !(nav_block == BLOCK_Init) && !(nav_block == BLOCK_Geo_init) && !(nav_block == BLOCK_Landed)" deroute="Standby"/>
    <!-- Check if battery is empty, then Land Here -->
    <exception cond="electrical.bat_low && !(nav_block == BLOCK_Land) && !(nav_block == BLOCK_Flare) && !(nav_block == BLOCK_Landed)" deroute="Land_Here"/>
    <!-- Check if time is up(10 seconds range), then Land Here -->
    <exception cond="10 > time_until_land && !(nav_block == BLOCK_Land) && !(nav_block == BLOCK_Flare) && !(nav_block == BLOCK_Landed)" deroute="Land_Here"/>
  </exceptions>
  <blocks>
    <block name="Init">
      <call fun="NavKillThrottle()"/>
      <while cond="!GpsFixValid() || gps.pacc > 1500 || !(ahrs.status == AHRS_RUNNING)"/>
    </block>
    <block name="Geo init">
      <while cond="LessThan(NavBlockTime(), 10)"/>
      <call fun="NavSetGroundReferenceHere()"/>
      <!--<call fun="NavSetAltitudeReferenceHere()"/>-->
    </block>
    <block name="Holding point">
      <call fun="NavKillThrottle()"/>
      <attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
    </block>
    <block name="Start Engine">
      <call fun="NavResurrect()"/>
      <attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
    </block>
    <block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
      <call fun="NavSetWaypointHere(WP_CLIMB)"/>
      <exception cond="stateGetPositionEnu_f()->z > 5.0" deroute="Standby"/>
      <attitude pitch="0" roll="0" throttle="0.5" vmode="throttle"/>
    </block>
    <block name="Standby" strip_button="Standby" strip_icon="home.png">
      <go wp="STDBY"/>
      <deroute block="line_p1_p2"/>
    </block>
    <block name="stay_p1">
      <stay wp="p1"/>
    </block>
    <block name="go_p2">
      <go wp="p2"/>
      <deroute block="stay_p1"/>
    </block>
    <block name="line_p1_p2">
      <go from="p1" hmode="route" wp="p2"/>
      <stay wp="p2" until="stage_time>10"/>
      <go from="p2" hmode="route" wp="p1"/>
      <deroute block="stay_p1"/>
    </block>
    <block name="route">
      <go from="p1" hmode="route" wp="p3"/>
      <go from="p3" hmode="route" wp="p4"/>
      <go from="p4" hmode="route" wp="p1"/>
      <deroute block="stay_p1"/>
    </block>
    <block name="circle">
      <circle radius="nav_radius" wp="p1"/>
    </block>
    <block name="land here" strip_button="Land Here" strip_icon="land-right.png">
      <call fun="NavSetWaypointHere(WP_TD)"/>
    </block>
    <block name="land">
      <go wp="TD"/>
    </block>
    <block name="flare">
      <exception cond="NavDetectGround()" deroute="Holding point"/>
      <exception cond="!nav_is_in_flight()" deroute="landed"/>
      <call fun="NavStartDetectGround()"/>
      <stay climb="-0.8" vmode="climb" wp="TD"/>
    </block>
    <block name="landed">
      <attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
    </block>
  </blocks>
 </flight_plan>
@@ -48,6 +48,7 @@
     <field name="my" type="int16" />
     <field name="mz" type="int16" />
     <field name="chksum" type="uint16" />
     <field name="checksum_errors" type="uint32" />
   </message>
   <message name="ATTITUDE" id="6">
@@ -626,7 +627,11 @@
    <field name="GX3_chksm"	type="uint16"/>
  </message>
- <!-- 74 is free -->
+  <message name="EXPLAIN" id="74">
    <field name="type" type="uint8" values="NAME|SETTING|WAYPOINT|BLOCK|IMAV2013"/>
    <field name="id" type="uint8"/>
    <field name="string" type="uint8[]"/>
  </message>
  <message name="VIDEO_TELEMETRY" id="75">
    <field name="blob_x1" 	type="int32" 	unit="pixels"/>
@@ -1860,7 +1865,13 @@
    <field name="rc_status"       type="uint8" values="OK|LOST|REALLY_LOST"/>
  </message>
- <!--213 is free -->
+  <message name="TUNE_VERT" id="213">
    <field name="z_sp"      type="int32" alt_unit="m"    alt_unit_coef="0.0039063"/>
    <field name="est_z"     type="int32" alt_unit="m"    alt_unit_coef="0.0039063"/>
    <field name="ref_z"     type="int32" alt_unit="m"    alt_unit_coef="0.0039063"/>
    <field name="ref_zd"    type="int32" alt_unit="m/s"  alt_unit_coef="0.0000019"/>
  </message>
 <!--214 is free -->
 <!--215 is free -->
 <!--216 is free -->
@@ -0,0 +1,18 @@
 <!DOCTYPE module SYSTEM "module.dtd">
 <module name="flight_time" dir="time">
  <doc>
    <description>
      Flight time calculator
      Allows to check how much time is left before the end of the competition.
    </description>
  </doc>
  <header>
    <file name="flight_time.h"/>
  </header>
  <init fun="flight_time_init()"/>
  <periodic fun="flight_time_periodic()" freq="1"/> <!--(0.1171875) 1Hz only valid in case: the modules run a 512Hz but the module generator thinks it runs at 60Hz therefore the scaling factor should be 60/512-->
  <makefile>
    <file name="flight_time.c"/>
  </makefile>
 </module>
@@ -0,0 +1,9 @@
 <!DOCTYPE settings SYSTEM "../settings.dtd">
 <settings>
  <dl_settings>
    <dl_settings name="IMAV 2013">
      <dl_setting var="time_until_land" module="modules/time/flight_time" min="0" step="1" max="1800" param="FLIGHT_TIME_LEFT"/>
    </dl_settings>
  </dl_settings>
 </settings>
@@ -6,7 +6,11 @@
    <dl_settings NAME="Sim">
       <dl_setting var="nps_bypass_ahrs" min="0" step="1" max="1" module="nps/nps_autopilot_rotorcraft" shortname="bypass_ahrs" values="No|Yes"/>
       <dl_setting var="nps_bypass_ins" min="0" step="1" max="1" module="nps/nps_autopilot_rotorcraft" shortname="bypass_ins" values="No|Yes"/>
       <dl_setting var="gps_has_fix" min="0" step="1" max="1" module="subsystems/gps/gps_sim_nps" shortname="gps_fix" values="No|Yes"/>
       <dl_setting var="nps_electrical.supply_voltage" min="0" step="0.1" max="24" module="nps/nps_electrical" shortname="bat_voltage" unit="V"/>
       <dl_setting var="nps_atmosphere.wind_speed" min="0" step="0.1" max="25" module="nps/nps_atmosphere" shortname="wind_speed" unit="m/s"/>
       <dl_setting var="nps_atmosphere.wind_dir" min="0" step="1" max="360" module="nps/nps_atmosphere" shortname="wind_dir" unit="rad" alt_unit="deg"/>
       <dl_setting var="nps_atmosphere.turbulence_severity" min="0" step="1" max="7" module="nps/nps_atmosphere" shortname="turbulence"/>
    </dl_settings>
  </dl_settings>
@@ -21,7 +21,7 @@ $(DATADIR):
 $(DATADIR)/maps.google.com: $(DATADIR) FORCE
 	@echo "-----------------------------------------------"
 	@echo "DOWNLOAD: google maps version code";
-	$(Q)wget -q -t 2 -T 20 -O $(@) http://maps.google.com/ || \
+	$(Q)wget -q -t 1 -T 10 -O $(@) http://maps.google.com/ || \
 		(rm -f $(@) && \
 		echo "Could not download google maps version code" && \
 		echo "-----------------------------------------------" && \
@@ -0,0 +1,262 @@
 #!/usr/bin/env python
 from __future__ import print_function
 import pygtk
 import gtk
 pygtk.require('2.0')
 import os
 import shutil
 import datetime
 from fnmatch import fnmatch
 import subprocess
 class ConfChooser:
    # General Functions
    def update_combo(self, combo, list):
        combo.set_sensitive(False)
        combo.get_model().clear()
        current_index = 0
        for (i, text) in enumerate(list):
            combo.append_text(text)
            if os.path.join(self.conf_dir, text) == os.path.realpath(self.conf_xml):
                current_index = i
        combo.set_active(current_index)
        combo.set_sensitive(True)
    def update_label(self):
        desc = "Current conf: "
        if not os.path.lexists(self.conf_xml):
                desc += "does not exist"
        else:
            if os.path.islink(self.conf_xml):
                if os.path.exists(self.conf_xml):
                    desc += "symlink to "
                else:
                    desc += "broken symlink to "
            real_conf_path = os.path.realpath(self.conf_xml)
            desc += os.path.relpath(real_conf_path, self.conf_dir)
        self.explain.set_text(desc)
    # CallBack Functions
    def find_conf_files(self):
        conf_files = []
        pattern = "conf[._-]*xml*"
        backup_pattern = "conf[._-]*xml.20[0-9][0-9]-[01][0-9]-[0-3][0-9]_*"
        excludes = ["%gconf.xml"]
        for path, subdirs, files in os.walk(self.conf_dir):
            for name in files:
                if self.exclude_backups and fnmatch(name, backup_pattern):
                    continue
                if fnmatch(name, pattern):
                    filepath = os.path.join(path, name)
                    entry = os.path.relpath(filepath, self.conf_dir)
                    if not os.path.islink(filepath) and entry not in excludes:
                        conf_files.append(entry)
        conf_files.sort()
        self.update_combo(self.conf_file_combo, conf_files)
    def about(self):
        gui_dialogs.about(paparazzi.home_dir)
    def set_backups(self, widget):
        self.exclude_backups = not widget.get_active()
        self.find_conf_files()
    def launch(self, widget):
        os.system("./paparazzi &");
        gtk.main_quit()
    def backupconf(self, use_personal=False):
        timestr = datetime.datetime.now().strftime("%Y-%m-%d_%H:%M")
        if os.path.islink(self.conf_xml):
            if self.verbose:
                print("Symlink does not need backup");
        else:
            newname = "conf.xml." + timestr
            backup_file = os.path.join(self.conf_dir, newname)
            shutil.copyfile(self.conf_xml, backup_file)
            print("Made a backup: " + newname)
        if use_personal:
            backup_name = self.conf_personal_name + "." + timestr
            conf_personal_backup = os.path.join(self.conf_dir, backup_name)
            if os.path.exists(self.conf_personal):
                print("Backup conf.xml.personal to " + backup_name)
                shutil.copyfile(self.conf_personal, conf_personal_backup)
    def delete(self, widget):
        filename = os.path.join(self.conf_dir, self.conf_file_combo.get_active_text())
        # TODO: dialog: are you certain?
        os.remove(filename)
        self.update_label()
        self.find_conf_files()
    def accept(self, widget):
        selected = self.conf_file_combo.get_active_text()
        if selected == "conf.xml":
            print("conf.xml is not a symlink, maybe you want to copy it to your personal file first?")
        else:
            self.backupconf()
            link_source = self.conf_file_combo.get_active_text()
            os.remove(self.conf_xml)
            os.symlink(selected, self.conf_xml)
            self.update_label()
            self.find_conf_files()
    def personal(self, widget):
        if os.path.exists(self.conf_personal):
            print("Your personal conf file already exists!")
        else:
            self.backupconf(True)
            template_file = os.path.join(self.conf_dir, self.conf_file_combo.get_active_text())
            shutil.copyfile(template_file, self.conf_personal)
            os.remove(self.conf_xml)
            os.symlink(self.conf_personal_name, self.conf_xml)
            self.update_label()
            self.find_conf_files()
    # Constructor Functions
    def destroy(self, widget, data=None):
        gtk.main_quit()
    def __init__(self):
        self.window = gtk.Window(gtk.WINDOW_TOPLEVEL)
        self.window.set_title("Paparazzi Configuration Chooser")
        self.my_vbox = gtk.VBox()
        # if PAPARAZZI_HOME not set, then assume the tree containing this
        # file is a reasonable substitute
        self.paparazzi_home = os.getenv("PAPARAZZI_HOME", os.path.dirname(os.path.abspath(__file__)))
        self.conf_dir = os.path.join(self.paparazzi_home, "conf")
        self.conf_xml = os.path.join(self.conf_dir, "conf.xml")
        self.conf_personal_name = "conf_personal.xml"
        self.conf_personal = os.path.join(self.conf_dir, self.conf_personal_name)
        self.exclude_backups = True
        self.verbose = False
        # MenuBar
        mb = gtk.MenuBar()
        # File
        filemenu = gtk.Menu()
        # File Title
        filem = gtk.MenuItem("File")
        filem.set_submenu(filemenu)
        exitm = gtk.MenuItem("Exit")
        exitm.connect("activate", gtk.main_quit)
        filemenu.append(exitm)
        mb.append(filem)
        # Help
        helpmenu = gtk.Menu()
        # Help Title
        helpm = gtk.MenuItem("Help")
        helpm.set_submenu(helpmenu)
        aboutm = gtk.MenuItem("About")
        aboutm.connect("activate", self.about)
        helpmenu.append(aboutm)
        mb.append(helpm)
        self.my_vbox.pack_start(mb,False)
        # Combo Bar
        self.conf_label = gtk.Label("Conf:")
        self.conf_file_combo = gtk.combo_box_new_text()
        self.find_conf_files()
 #        self.firmwares_combo.connect("changed", self.parse_list_of_airframes)
        self.conf_file_combo.set_size_request(600,30)
        self.confbar = gtk.HBox()
        self.confbar.pack_start(self.conf_label)
        self.confbar.pack_start(self.conf_file_combo)
        self.my_vbox.pack_start(self.confbar, False)
        ### show backups button
        self.btnBackups = gtk.CheckButton("show backups")
        self.btnBackups.connect("toggled", self.set_backups)
        self.my_vbox.pack_start(self.btnBackups, False)
        ##### Explain current config
        self.explain = gtk.Label("");
        self.update_label()
        self.exbar = gtk.HBox()
        self.exbar.pack_start(self.explain)
        self.my_vbox.pack_start(self.exbar, False)
        ##### Buttons
        self.btnAccept = gtk.Button("Set Selected Conf As Active")
        self.btnAccept.connect("clicked", self.accept)
        self.btnAccept.set_tooltip_text("Set Conf as Active")
        self.btnPersonal = gtk.Button("Create Personal Conf Based on Selected")
        self.btnPersonal.connect("clicked", self.personal)
        self.btnPersonal.set_tooltip_text("Create Personal Conf Based on Selected and Activate")
        self.btnDelete = gtk.Button("Delete Selected")
        self.btnDelete.connect("clicked", self.delete)
        self.btnDelete.set_tooltip_text("Permanently Delete")
        self.btnLaunch = gtk.Button("Launch Paparazzi")
        self.btnLaunch.connect("clicked", self.launch)
        self.btnLaunch.set_tooltip_text("Launch Paparazzi with current conf.xml")
        self.btnExit = gtk.Button("Exit")
        self.btnExit.connect("clicked", self.destroy)
        self.btnExit.set_tooltip_text("Close application")
        self.toolbar = gtk.HBox()
        self.toolbar.pack_start(self.btnAccept)
        self.toolbar.pack_start(self.btnPersonal)
        self.toolbar.pack_start(self.btnDelete)
        self.toolbar.pack_start(self.btnLaunch)
        self.toolbar.pack_start(self.btnExit)
        self.my_vbox.pack_start(self.toolbar, False)
        ##### Bottom
        self.window.add(self.my_vbox)
        self.window.show_all()
        self.window.set_position(gtk.WIN_POS_CENTER_ALWAYS)
        self.window.connect("destroy", self.destroy)
    def main(self):
        gtk.main()
 if __name__ == "__main__":
    import sys
    if (len(sys.argv) > 1):
        airframe_file = sys.argv[1]
    gui = ConfChooser()
    gui.main()
@@ -92,6 +92,9 @@
 #endif
 /** @todo: these should go into libopencm3 */
 #define TIM9				TIM9_BASE
 #define TIM12				TIM12_BASE
 int32_t actuators_pwm_values[ACTUATORS_PWM_NB];
@@ -102,10 +105,8 @@ static inline void actuators_pwm_arch_channel_init(uint32_t timer_peripheral,
                                                   enum tim_oc_id oc_id) {
  timer_disable_oc_output(timer_peripheral, oc_id);
 #if STM32F4
  //There is no such register in TIM9 and 12.
  if (timer_peripheral != TIM9 && timer_peripheral != TIM12)
 #endif
    timer_disable_oc_clear(timer_peripheral, oc_id);
  timer_enable_oc_preload(timer_peripheral, oc_id);
  timer_set_oc_slow_mode(timer_peripheral, oc_id);
@@ -142,21 +143,15 @@ static inline void set_servo_timer(uint32_t timer, uint32_t period, uint8_t chan
   * - Alignement edge.
   * - Direction up.
   */
 #if STM32F4
  if ((timer == TIM9) || (timer == TIM12))
    //There are no EDGE and DIR settings in TIM9 and TIM12
    timer_set_mode(timer, TIM_CR1_CKD_CK_INT, 0, 0);
  else
 #endif
    timer_set_mode(timer, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP);
  // TIM1, 8 and 9 use APB2 clock, all others APB1
 #if STM32F4
  if (timer != TIM1 && timer != TIM8 && timer != TIM9) {
 #else
  if (timer != TIM1 && timer != TIM8) {
 #endif
    timer_set_prescaler(timer, (TIMER_APB1_CLK / ONE_MHZ_CLK) - 1); // 1uS
  } else {
    // TIM9, 1 and 8 use APB2 clock
@@ -69,7 +69,8 @@ int fd;
 void electrical_init(void) {
  // First we try to kill the program.elf if it is running (done here because initializes first)
-  system("killall -9 program.elf");
+  int ret = system("killall -9 program.elf");
  (void) ret;
  // Initialize 12c device for power
  fd = open( "/dev/i2c-1", O_RDWR );
@@ -36,26 +36,26 @@
 #include <math.h>
 #include <errno.h>
 #include <assert.h>
 #include "std.h"
 #include "navdata.h"
 #include "subsystems/ins.h"
 #define NAVDATA_PACKET_SIZE 60
 #define NAVDATA_BUFFER_SIZE 80
 #define NAVDATA_START_BYTE 0x3a
-typedef struct {
+static inline bool_t acquire_baro_calibration(void);
-	uint8_t isInitialized;
+static void navdata_cropbuffer(int cropsize);
 	uint8_t isOpen;
 	uint16_t bytesRead;
 	uint32_t totalBytesRead;
 	uint32_t packetsRead;
 	uint8_t buffer[NAVDATA_BUFFER_SIZE];
 } navdata_port;
 static navdata_port port;
 static int nav_fd;
 navdata_port nav_port;
 static int nav_fd = 0;
 static int16_t previousUltrasoundHeight;
 measures_t navdata;
 #include "subsystems/sonar.h"
 uint16_t sonar_meas = 0;
 // FIXME(ben): there must be a better home for these
 ssize_t full_write(int fd, const uint8_t *buf, size_t count)
 {
@@ -100,15 +100,15 @@ static void navdata_write(const uint8_t *buf, size_t count)
    perror("navdata_write: Write failed");
 }
-int navdata_init()
+bool_t navdata_init()
 {
-  nav_fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY | O_NONBLOCK);
+  if (nav_fd <= 0) {
-  if (nav_fd == -1)
+    nav_fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY | O_NONBLOCK);
-  {
+
-    perror("navdata_init: Unable to open /dev/ttyO1 - ");
+    if (nav_fd == -1) {
-    return 1;
+      perror("navdata_init: Unable to open /dev/ttyO1 - ");
-  } else {
+      return FALSE;
-    port.isOpen = 1;
+    }
  }
  fcntl(nav_fd, F_SETFL, 0); //read calls are non blocking
@@ -132,31 +132,40 @@ int navdata_init()
  uint8_t cmd=0x02;
  navdata_write(&cmd, 1);
-  baro_calibrated = 0;
+  // read some potential dirt (retry alot of times)
-  acquire_baro_calibration();
+  char tmp[100];
  for(int i = 0; i < 100; i++) {
    uint16_t dirt = read(nav_fd, tmp, sizeof tmp);
    (void) dirt;
    cmd=0x02;
    navdata_write(&cmd, 1);
    usleep(200);
  }
  baro_calibrated = FALSE;
  if(!acquire_baro_calibration())
    return FALSE;
  // start acquisition
-  cmd=0x01;
+  cmd = 0x01;
  navdata_write(&cmd, 1);
-  navdata_imu_available = 0;
+  navdata_imu_available = FALSE;
-  navdata_baro_available = 0;
+  navdata_baro_available = FALSE;
  port.bytesRead = 0;
  port.totalBytesRead = 0;
  port.packetsRead = 0;
  port.isInitialized = 1;
  previousUltrasoundHeight = 0;
  nav_port.checksum_errors = 0;
  nav_port.bytesRead = 0;
  nav_port.totalBytesRead = 0;
  nav_port.packetsRead = 0;
  nav_port.isInitialized = TRUE;
-  return 0;
+  return TRUE;
 }
-void acquire_baro_calibration()
+static inline bool_t acquire_baro_calibration(void)
 {
  char tmp[100];
  read(nav_fd, tmp, sizeof tmp); // read some potential dirt
  // start baro calibration acquisition
  uint8_t cmd=0x17; // send cmd 23
  navdata_write(&cmd, 1);
@@ -166,7 +175,7 @@ void acquire_baro_calibration()
  if (full_read(nav_fd, calibBuffer, sizeof calibBuffer) < 0)
  {
    perror("acquire_baro_calibration: read failed");
-    return;
+    return FALSE;
  }
  baro_calibration.ac1 = calibBuffer[0]  << 8 | calibBuffer[1];
@@ -195,32 +204,19 @@ void acquire_baro_calibration()
  printf("Calibration MC: %d\n", baro_calibration.mc);
  printf("Calibration MD: %d\n", baro_calibration.md);
-  baro_calibrated = 1;
+  baro_calibrated = TRUE;
-}
+  return TRUE;
 void navdata_close()
 {
  port.isOpen = 0;
  close(nav_fd);
 }
 void navdata_read()
 {
-  int newbytes = 0;
+  int newbytes = read(nav_fd, nav_port.buffer+nav_port.bytesRead, NAVDATA_BUFFER_SIZE-nav_port.bytesRead);
  if (port.isInitialized != 1)
    navdata_init();
  if (port.isOpen != 1)
    return;
  newbytes = read(nav_fd, port.buffer+port.bytesRead, NAVDATA_BUFFER_SIZE-port.bytesRead);
  // because non-blocking read returns -1 when no bytes available
  if (newbytes > 0)
  {
-    port.bytesRead += newbytes;
+    nav_port.bytesRead += newbytes;
-    port.totalBytesRead += newbytes;
+    nav_port.totalBytesRead += newbytes;
  }
 }
@@ -235,7 +231,7 @@ static void baro_update_logic(void)
  if (temp_or_press_was_updated_last == 0)  // Last update was press so we are now waiting for temp
  {
    // temp was updated
-    temp_or_press_was_updated_last = 1;
+    temp_or_press_was_updated_last = TRUE;
    // This means that press must remain constant
    if (lastpressval != 0)
@@ -244,16 +240,16 @@ static void baro_update_logic(void)
      if (lastpressval != navdata.pressure)
      {
        // wait for temp again
-        temp_or_press_was_updated_last = 0;
+        temp_or_press_was_updated_last = FALSE;
        sync_errors++;
-        navdata_baro_available = 1;
+        navdata_baro_available = TRUE;
      }
    }
  }
  else
  {
    // press was updated
-    temp_or_press_was_updated_last = 0;
+    temp_or_press_was_updated_last = FALSE;
    // This means that temp must remain constant
    if (lasttempval != 0)
@@ -262,37 +258,52 @@ static void baro_update_logic(void)
      if (lasttempval != navdata.temperature_pressure)
      {
        // wait for press again
-        temp_or_press_was_updated_last = 1;
+        temp_or_press_was_updated_last = TRUE;
        sync_errors++;
      }
    }
-    navdata_baro_available = 1;
+    navdata_baro_available = TRUE;
  }
  lastpressval = navdata.pressure;
  lasttempval = navdata.temperature_pressure;
  // debug
  // navdata->temperature_pressure = sync_errors;
 }
 void navdata_update()
 {
  static bool_t last_checksum_wrong = FALSE;
  // Check if initialized
  if (!nav_port.isInitialized) {
    navdata_init();
    return;
  }
  // Start reading
  navdata_read();
  // while there is something interesting to do...
-  while (port.bytesRead >= NAVDATA_PACKET_SIZE)
+  while (nav_port.bytesRead >= NAVDATA_PACKET_SIZE)
  {
-    if (port.buffer[0] == NAVDATA_START_BYTE)
+    if (nav_port.buffer[0] == NAVDATA_START_BYTE)
    {
-      // if checksum is OK
+      assert(sizeof navdata == NAVDATA_PACKET_SIZE);
-      if ( 1 ) // we dont know how to calculate the checksum
+      memcpy(&navdata, nav_port.buffer, NAVDATA_PACKET_SIZE);
 //      if ( navdata_checksum() == 0 )
      {
 	assert(sizeof navdata == NAVDATA_PACKET_SIZE);
        memcpy(&navdata, port.buffer, NAVDATA_PACKET_SIZE);
      // Calculating the checksum
      uint16_t checksum = 0;
      for(int i = 2; i < NAVDATA_PACKET_SIZE-2; i += 2) {
        checksum += nav_port.buffer[i] + (nav_port.buffer[i+1] << 8);
      }
      // When checksum is incorrect
      if(navdata.chksum != checksum) {
        printf("Checksum error [calculated: %d] [packet: %d] [diff: %d]\n",checksum , navdata.chksum, checksum-navdata.chksum);
        nav_port.checksum_errors++;
      }
      // When we already dropped a packet or checksum is correct
      if(last_checksum_wrong || navdata.chksum == checksum) {
        // Invert byte order so that TELEMETRY works better
        uint8_t tmp;
        uint8_t* p = (uint8_t*) &(navdata.pressure);
@@ -306,87 +317,59 @@ void navdata_update()
        baro_update_logic();
-        navdata_imu_available = 1;
+#ifdef USE_SONAR
        if (navdata.ultrasound < 10000)
        {
            sonar_meas = navdata.ultrasound;
            ins_update_sonar();
-        port.packetsRead++;
+        }
-//        printf("CCRC=%d, GCRC=%d, error=%d\n", crc, navdata->chksum, abs(crc-navdata->chksum));
+#endif
-        //navdata_getHeight();
+
        navdata_imu_available = TRUE;
        last_checksum_wrong = FALSE;
        nav_port.packetsRead++;
      }
-      navdata_CropBuffer(NAVDATA_PACKET_SIZE);
+
      // Crop the buffer
      navdata_cropbuffer(NAVDATA_PACKET_SIZE);
    }
    else
    {
      // find start byte, copy all data from startbyte to buffer origin, update bytesread
      uint8_t * pint;
-      pint = memchr(port.buffer, NAVDATA_START_BYTE, port.bytesRead);
+      pint = memchr(nav_port.buffer, NAVDATA_START_BYTE, nav_port.bytesRead);
      if (pint != NULL) {
-        navdata_CropBuffer(pint - port.buffer);
+        navdata_cropbuffer(pint - nav_port.buffer);
      } else {
        // if the start byte was not found, it means there is junk in the buffer
-        port.bytesRead = 0;
+        nav_port.bytesRead = 0;
      }
    }
  }
 }
-void navdata_CropBuffer(int cropsize)
+int16_t navdata_height(void) {
 {
  if (port.bytesRead - cropsize < 0) {
    // TODO think about why the amount of bytes read minus the cropsize gets below zero
    printf("BytesRead(=%d) - Cropsize(=%d) may not be below zero. port->buffer=%p\n", port.bytesRead, cropsize, port.buffer);
    return;
  }
  memmove(port.buffer, port.buffer+cropsize, NAVDATA_BUFFER_SIZE-cropsize);
  port.bytesRead -= cropsize;
 }
 int16_t navdata_getHeight() {
  if (navdata.ultrasound > 10000) {
    return previousUltrasoundHeight;
  }
  int16_t ultrasoundHeight = 0;
  ultrasoundHeight = (navdata.ultrasound - 880) / 26.553;
  previousUltrasoundHeight = ultrasoundHeight;
  return ultrasoundHeight;
 }
-// The checksum should be calculated here: we don't know the algorithm
+static void navdata_cropbuffer(int cropsize)
-uint16_t navdata_checksum() {
+{
-  navdata_cks = 0;
+  if (nav_port.bytesRead - cropsize < 0) {
-  navdata_cks += navdata.nu_trame;
+    // TODO think about why the amount of bytes read minus the cropsize gets below zero
-  navdata_cks += navdata.ax;
+    printf("BytesRead(=%d) - Cropsize(=%d) may not be below zero. port->buffer=%p\n", nav_port.bytesRead, cropsize, nav_port.buffer);
-  navdata_cks += navdata.ay;
+    return;
-  navdata_cks += navdata.az;
+  }
  navdata_cks += navdata.vx;
  navdata_cks += navdata.vy;
  navdata_cks += navdata.vz;
  navdata_cks += navdata.temperature_acc;
  navdata_cks += navdata.temperature_gyro;
  navdata_cks += navdata.ultrasound;
  navdata_cks += navdata.us_debut_echo;
  navdata_cks += navdata.us_fin_echo;
  navdata_cks += navdata.us_association_echo;
  navdata_cks += navdata.us_distance_echo;
  navdata_cks += navdata.us_curve_time;
  navdata_cks += navdata.us_curve_value;
  navdata_cks += navdata.us_curve_ref;
  navdata_cks += navdata.nb_echo;
  navdata_cks += navdata.sum_echo;
  navdata_cks += navdata.gradient;
  navdata_cks += navdata.flag_echo_ini;
  navdata_cks += navdata.pressure;
  navdata_cks += navdata.temperature_pressure;
  navdata_cks += navdata.mx;
  navdata_cks += navdata.my;
  navdata_cks += navdata.mz;
 //  navdata_cks += navdata->chksum;
-  return 0; // we dont know how to calculate the checksum
+  memmove(nav_port.buffer, nav_port.buffer+cropsize, NAVDATA_BUFFER_SIZE-cropsize);
  nav_port.bytesRead -= cropsize;
 }
@@ -95,25 +95,29 @@ struct bmp180_baro_calibration
  int32_t b5;
 };
 #define NAVDATA_BUFFER_SIZE 80
 typedef struct {
    uint8_t isInitialized;
    uint16_t bytesRead;
    uint32_t totalBytesRead;
    uint32_t packetsRead;
    uint32_t checksum_errors;
    uint8_t buffer[NAVDATA_BUFFER_SIZE];
 } navdata_port;
 extern measures_t navdata;
 extern navdata_port nav_port;
 struct bmp180_baro_calibration baro_calibration;
 navdata_port* port;
 uint16_t navdata_cks;
 uint8_t navdata_imu_available;
 uint8_t navdata_baro_available;
 int16_t previousUltrasoundHeight;
 uint8_t baro_calibrated;
-int navdata_init(void);
+bool_t navdata_init(void);
 void navdata_close(void);
 void navdata_read(void);
 void navdata_update(void);
-void navdata_CropBuffer(int cropsize);
+int16_t navdata_height(void);
 uint16_t navdata_checksum(void);
 int16_t navdata_getHeight(void);
 void acquire_baro_calibration(void);
 ssize_t full_write(int fd, const uint8_t *buf, size_t count);
 ssize_t full_read(int fd, uint8_t *buf, size_t count);
@@ -129,21 +129,34 @@ void autopilot_init(void) {
 void autopilot_periodic(void) {
  RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
 #if FAILSAFE_GROUND_DETECT
 INFO("Using FAILSAFE_GROUND_DETECT")
  if (autopilot_mode == AP_MODE_FAILSAFE && autopilot_ground_detected) {
    autopilot_set_mode(AP_MODE_KILL);
    autopilot_ground_detected = FALSE;
  }
 #endif
-  /* set failsafe commands, if in FAILSAFE or KILL mode */
+
-#if !FAILSAFE_GROUND_DETECT
+  /* If in FAILSAFE mode and either already not in_flight anymore
-  if (autopilot_mode == AP_MODE_KILL ||
+   * or just "detected" ground, go to KILL mode.
-      autopilot_mode == AP_MODE_FAILSAFE) {
+   */
-#else
+  if (autopilot_mode == AP_MODE_FAILSAFE) {
-  if (autopilot_mode == AP_MODE_KILL) {
+    if (!autopilot_in_flight)
      autopilot_set_mode(AP_MODE_KILL);
 #if FAILSAFE_GROUND_DETECT
 INFO("Using FAILSAFE_GROUND_DETECT: KILL")
    if (autopilot_ground_detected)
      autopilot_set_mode(AP_MODE_KILL);
 #endif
  }
  /* Reset ground detection _after_ running flight plan
   */
  if (!autopilot_in_flight || autopilot_ground_detected) {
    autopilot_ground_detected = FALSE;
    autopilot_detect_ground_once = FALSE;
  }
  /* Set fixed "failsafe" commands from airframe file if in KILL mode.
   * If in FAILSAFE mode, run normal loops with failsafe attitude and
   * downwards velocity setpoints.
   */
  if (autopilot_mode == AP_MODE_KILL) {
    SetCommands(commands_failsafe);
  }
  else {
@@ -171,7 +184,6 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {
        break;
 #endif
      case AP_MODE_KILL:
        autopilot_set_motors_on(FALSE);
        autopilot_in_flight = FALSE;
        autopilot_in_flight_counter = 0;
        guidance_h_mode_changed(GUIDANCE_H_MODE_KILL);
@@ -215,6 +227,8 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {
        break;
 #endif
      case AP_MODE_KILL:
        autopilot_set_motors_on(FALSE);
        stabilization_cmd[COMMAND_THRUST] = 0;
        guidance_v_mode_changed(GUIDANCE_V_MODE_KILL);
        break;
      case AP_MODE_RC_DIRECT:
@@ -646,6 +646,14 @@
                  &guidance_v_delta_t);		\
  }
 #define PERIODIC_SEND_TUNE_VERT(_trans, _dev) {     \
    DOWNLINK_SEND_TUNE_VERT(_trans, _dev,           \
                            &guidance_v_z_sp,		\
                            &ins_ltp_pos.z,			\
                            &guidance_v_z_ref,		\
                            &guidance_v_zd_ref);   \
  }
 #define PERIODIC_SEND_HOVER_LOOP(_trans, _dev) {				\
    DOWNLINK_SEND_HOVER_LOOP(_trans, _dev,				\
                   &guidance_h_pos_sp.x,		\
@@ -983,7 +991,8 @@
 	&navdata.mx, \
 	&navdata.my, \
 	&navdata.mz, \
-	&navdata.chksum \
+	&navdata.chksum, \
    &nav_port.checksum_errors \
 	)
 #else
 #define PERIODIC_SEND_ARDRONE_NAVDATA(_trans, _dev) {}
@@ -998,10 +1007,13 @@
 #ifdef USE_UART1
 #define PERIODIC_SEND_UART1_ERRORS(_trans, _dev) {   \
    const uint8_t _bus1 = 1;                         \
    uint16_t ore = uart1.ore;                        \
    uint16_t ne_err = uart1.ne_err;                  \
    uint16_t fe_err = uart1.fe_err;                  \
    DOWNLINK_SEND_UART_ERRORS(_trans, _dev,          \
-                             &uart1.ore,             \
+                             &ore,                   \
-                             &uart1.ne_err,          \
+                             &ne_err,                \
-                             &uart1.fe_err,          \
+                             &fe_err,                \
                             &_bus1);                \
  }
 #else
@@ -1011,10 +1023,13 @@
 #ifdef USE_UART2
 #define PERIODIC_SEND_UART2_ERRORS(_trans, _dev) {   \
    const uint8_t _bus2 = 2;                         \
    uint16_t ore = uart2.ore;                        \
    uint16_t ne_err = uart2.ne_err;                  \
    uint16_t fe_err = uart2.fe_err;                  \
    DOWNLINK_SEND_UART_ERRORS(_trans, _dev,          \
-                             &uart2.ore,             \
+                             &ore,                   \
-                             &uart2.ne_err,          \
+                             &ne_err,                \
-                             &uart2.fe_err,          \
+                             &fe_err,                \
                             &_bus2);                \
  }
 #else
@@ -1024,10 +1039,13 @@
 #ifdef USE_UART3
 #define PERIODIC_SEND_UART3_ERRORS(_trans, _dev) {   \
    const uint8_t _bus3 = 3;                         \
    uint16_t ore = uart3.ore;                        \
    uint16_t ne_err = uart3.ne_err;                  \
    uint16_t fe_err = uart3.fe_err;                  \
    DOWNLINK_SEND_UART_ERRORS(_trans, _dev,          \
-                             &uart3.ore,             \
+                             &ore,                   \
-                             &uart3.ne_err,          \
+                             &ne_err,                \
-                             &uart3.fe_err,          \
+                             &fe_err,                \
                             &_bus3);                \
  }
 #else
@@ -1037,10 +1055,13 @@
 #ifdef USE_UART5
 #define PERIODIC_SEND_UART5_ERRORS(_trans, _dev) {   \
    const uint8_t _bus5 = 5;                         \
    uint16_t ore = uart5.ore;                        \
    uint16_t ne_err = uart5.ne_err;                  \
    uint16_t fe_err = uart5.fe_err;                  \
    DOWNLINK_SEND_UART_ERRORS(_trans, _dev,          \
-                             &uart5.ore,             \
+                             &ore,                   \
-                             &uart5.ne_err,          \
+                             &ne_err,                \
-                             &uart5.fe_err,          \
+                             &fe_err,                \
                             &_bus5);                \
  }
 #else
@@ -48,8 +48,6 @@ struct EcefCoor_d {
 /**
 * @brief vector in Latitude, Longitude and Altitude
 * @details Units lat,lon: radians
 * Unit alt: meters above MSL
 */
 struct LlaCoor_d {
  double lon; ///< in radians
@@ -48,8 +48,6 @@ struct EcefCoor_f {
 /**
 * @brief vector in Latitude, Longitude and Altitude
 * @details Units lat,lon: radians
 * Unit alt: meters above MSL
 */
 struct LlaCoor_f {
  float lon; ///< in radians
@@ -92,11 +92,40 @@ void enu_of_ecef_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct Ece
 void ned_of_ecef_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
  struct EnuCoor_i enu;
  enu_of_ecef_point_i(&enu, def, ecef);
  ENU_OF_TO_NED(*ned, enu);
 }
 /** Convert a ECEF position to local ENU.
 * @param[out] enu  ENU position in meter << #INT32_POS_FRAC
 * @param[in]  def  local coordinate system definition
 * @param[in]  ecef ECEF position in cm
 */
 void enu_of_ecef_pos_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
  struct EnuCoor_i enu_cm;
  enu_of_ecef_point_i(&enu_cm, def, ecef);
  /* enu = (enu_cm / 100) << INT32_POS_FRAC
   * to loose less range:
   * enu_cm = enu << (INT32_POS_FRAC-2) / 25
   * which puts max enu output Q23.8 range to 8388km / 25 = 335km
   */
  INT32_VECT3_LSHIFT(*enu, enu_cm, INT32_POS_FRAC-2);
  VECT3_SDIV(*enu, *enu, 25);
 }
 /** Convert a ECEF position to local NED.
 * @param[out] ned  NED position in meter << #INT32_POS_FRAC
 * @param[in]  def  local coordinate system definition
 * @param[in]  ecef ECEF position in cm
 */
 void ned_of_ecef_pos_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
  struct EnuCoor_i enu;
  enu_of_ecef_pos_i(&enu, def, ecef);
  ENU_OF_TO_NED(*ned, enu);
 }
 void enu_of_ecef_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
@@ -149,11 +178,23 @@ void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct Ne
  ecef_of_enu_vect_i(ecef, def, &enu);
 }
 /** Convert a point in local ENU to ECEF.
 * @param[out] ecef ECEF point in cm
 * @param[in]  def  local coordinate system definition
 * @param[in]  enu  ENU point in cm
 */
 void ecef_of_enu_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) {
  ecef_of_enu_vect_i(ecef, def, enu);
  INT32_VECT3_ADD(*ecef, def->ecef);
 }
 /** Convert a point in local NED to ECEF.
 * @param[out] ecef ECEF point in cm
 * @param[in]  def  local coordinate system definition
 * @param[in]  ned  NED point in cm
 */
 void ecef_of_ned_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) {
  struct EnuCoor_i enu;
  ENU_OF_TO_NED(enu, *ned);
@@ -161,6 +202,37 @@ void ecef_of_ned_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct N
 }
 /** Convert a local ENU position to ECEF.
 * @param[out] ecef ECEF position in cm
 * @param[in]  def  local coordinate system definition
 * @param[in]  enu  ENU position in meter << #INT32_POS_FRAC
 */
 void ecef_of_enu_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) {
  /* enu_cm = (enu * 100) >> INT32_POS_FRAC
   * to loose less range:
   * enu_cm = (enu * 25) >> (INT32_POS_FRAC-2)
   * which puts max enu input Q23.8 range to 8388km / 25 = 335km
   */
  struct EnuCoor_i enu_cm;
  VECT3_SMUL(enu_cm, *enu, 25);
  INT32_VECT3_RSHIFT(enu_cm, enu_cm, INT32_POS_FRAC-2);
  ecef_of_enu_vect_i(ecef, def, &enu_cm);
  INT32_VECT3_ADD(*ecef, def->ecef);
 }
 /** Convert a local NED position to ECEF.
 * @param[out] ecef ECEF position in cm
 * @param[in]  def  local coordinate system definition
 * @param[in]  ned  NED position in meter << #INT32_POS_FRAC
 */
 void ecef_of_ned_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) {
  struct EnuCoor_i enu;
  ENU_OF_TO_NED(enu, *ned);
  ecef_of_enu_pos_i(ecef, def, &enu);
 }
 void enu_of_lla_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla) {
  struct EcefCoor_i ecef;
  ecef_of_lla_i(&ecef,lla);
@@ -50,8 +50,6 @@ struct EcefCoor_i {
 /**
 * @brief vector in Latitude, Longitude and Altitude
 * @details Units lat,lon: radians*1e7
 * Unit alt: centimeters above MSL
 */
 struct LlaCoor_i {
  int32_t lon; ///< in radians*1e7
@@ -106,6 +104,8 @@ extern void lla_of_ecef_i(struct LlaCoor_i* out, struct EcefCoor_i* in);
 extern void ecef_of_lla_i(struct EcefCoor_i* out, struct LlaCoor_i* in);
 extern void enu_of_ecef_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef);
 extern void ned_of_ecef_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef);
 extern void enu_of_ecef_pos_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef);
 extern void ned_of_ecef_pos_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef);
 extern void enu_of_ecef_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef);
 extern void ned_of_ecef_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef);
 extern void enu_of_lla_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla);
@@ -114,6 +114,8 @@ extern void enu_of_lla_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struc
 extern void ned_of_lla_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct LlaCoor_i* lla);
 extern void ecef_of_enu_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu);
 extern void ecef_of_ned_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned);
 extern void ecef_of_enu_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu);
 extern void ecef_of_ned_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned);
 extern void ecef_of_enu_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu);
 extern void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned);
@@ -0,0 +1,46 @@
 /*
 * Copyright (C) 2013  Elisabeth van der Sman, 2013 Freek van Tienen
 *
 * This file is part of paparazzi.
 *
 * paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with paparazzi; see the file COPYING.  If not, write to
 * the Free Software Foundation, 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 *
 */
 /**
 * @file modules/time/flight_time.c
 *
 * Flight time counter that can be set from the gcs
 */
 #include "flight_time.h"
 #include "generated/airframe.h"
 uint16_t time_until_land;
 #ifndef FLIGHT_TIME_LEFT
 #define FLIGHT_TIME_LEFT 10000
 #endif
 void flight_time_init(void) {
  time_until_land = FLIGHT_TIME_LEFT;
 }
 void flight_time_periodic( void ) {
  // Count downwards
  if(time_until_land > 0)
    time_until_land--;
 }
@@ -0,0 +1,39 @@
 /*
 * Copyright (C) 2013  Elisabeth van der Sman, 2013 Freek van Tienen
 *
 * This file is part of paparazzi.
 *
 * paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with paparazzi; see the file COPYING.  If not, write to
 * the Free Software Foundation, 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 *
 */
 /**
 * @file modules/time/flight_time.h
 *
 * Flight time counter that can be set from the gcs
 */
 #ifndef FLIGHT_TIME_H
 #define FLIGHT_TIME_H
 #include "std.h"
 extern uint16_t time_until_land;
 void flight_time_init(void);
 void flight_time_periodic(void);
 #endif /* FLIGHT_TIME_H */
@@ -69,8 +69,7 @@ void stateCalcPositionEcef_i(void) {
    ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
  }
  else if (bit_is_set(state.pos_status, POS_NED_I)) {
-    /// @todo check if resolution is good enough
+    ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
    ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
  }
  else if (bit_is_set(state.pos_status, POS_NED_F)) {
    /* transform ned_f to ecef_f, set status bit, then convert to int */
@@ -114,7 +113,7 @@ void stateCalcPositionNed_i(void) {
      INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
    }
    else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
-      ned_of_ecef_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
+      ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
    }
    else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
      /* transform ecef_f -> ned_f, set status bit, then convert to int */
@@ -206,7 +205,7 @@ void stateCalcPositionEnu_i(void) {
      INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i);
    }
    else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
-      enu_of_ecef_point_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
+      enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
    }
    else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
      /* transform ecef_f -> enu_f, set status bit, then convert to int */
@@ -306,8 +305,7 @@ void stateCalcPositionLla_i(void) {
  }
  else if (bit_is_set(state.pos_status, POS_NED_I)) {
    /* transform ned_i -> ecef_i -> lla_i, set status bits */
-    /// @todo check if resolution is enough
+    ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
    ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
    SetBit(state.pos_status, POS_ECEF_I);
    lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); /* uses double version internally */
  }
@@ -360,8 +358,7 @@ void stateCalcPositionEcef_f(void) {
  }
  else if (bit_is_set(state.pos_status, POS_NED_I)) {
    /* transform ned_i -> ecef_i -> ecef_f, set status bits */
-    /// @todo check if resolution is good enough
+    ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
    ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
    SetBit(state.pos_status, POS_ECEF_F);
    ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i);
  }
@@ -396,7 +393,7 @@ void stateCalcPositionNed_f(void) {
    }
    else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
      /* transform ecef_i -> ned_i -> ned_f, set status bits */
-      ned_of_ecef_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
+      ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
      SetBit(state.pos_status, POS_NED_I);
      NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
    }
@@ -407,7 +404,7 @@ void stateCalcPositionNed_f(void) {
      /* transform lla_i -> ecef_i -> ned_i -> ned_f, set status bits */
      ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */
      SetBit(state.pos_status, POS_ECEF_I);
-      ned_of_ecef_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
+      ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
      SetBit(state.pos_status, POS_NED_I);
      NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
    }
@@ -481,7 +478,7 @@ void stateCalcPositionEnu_f(void) {
    }
    else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
      /* transform ecef_i -> enu_i -> enu_f, set status bits */
-      enu_of_ecef_point_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
+      enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
      SetBit(state.pos_status, POS_ENU_I);
      ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
    }
@@ -492,7 +489,7 @@ void stateCalcPositionEnu_f(void) {
      /* transform lla_i -> ecef_i -> enu_i -> enu_f, set status bits */
      ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */
      SetBit(state.pos_status, POS_ECEF_I);
-      enu_of_ecef_point_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
+      enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
      SetBit(state.pos_status, POS_ENU_I);
      ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
    }
@@ -148,7 +148,7 @@ struct State {
  /**
   * Position in Latitude, Longitude and Altitude.
   * Units lat,lon: radians*1e7
-   * Units alt: centimeters above MSL
+   * Units alt: milimeters above reference ellipsoid
   */
  struct LlaCoor_i lla_pos_i;
@@ -196,7 +196,7 @@ struct State {
  /**
   * Position in Latitude, Longitude and Altitude.
   * Units lat,lon: radians
-   * Units alt: meters above MSL
+   * Units alt: meters above reference ellipsoid
   */
  struct LlaCoor_f lla_pos_f;
@@ -29,6 +29,7 @@
 #endif
 bool_t gps_available;
 bool_t gps_has_fix;
 void  gps_feed_value() {
  gps.ecef_pos.x = sensors.gps.ecef_pos.x * 100.;
@@ -75,10 +76,14 @@ void  gps_feed_value() {
  gps.utm_pos.zone = nav_utm_zone0;
 #endif
-  gps.fix = GPS_FIX_3D;
+  if (gps_has_fix)
    gps.fix = GPS_FIX_3D;
  else
    gps.fix = GPS_FIX_NONE;
  gps_available = TRUE;
 }
 void gps_impl_init() {
  gps_available = FALSE;
  gps_has_fix = TRUE;
 }
--- a/Show More
+++ b/Show More