diff --git a/conf/messages.xml b/conf/messages.xml
index 8ed75686cb..d410a9f22e 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -322,6 +322,15 @@
       <field name="groundspeed_sp" type="float" unit="m/s"/>
   </message>
 
+  <message name="AIRSPEED" id="54">
+      <field name="adc" type="uint16"/>
+      <field name="airspeed" type="float" unit="m/s"/>
+      <field name="airspeed_sp" type="float" unit="m/s"/>
+      <field name="airspeed_cnt" type="float" unit="m/s"/>
+      <field name="groundspeed_sp" type="float" unit="m/s"/>
+  </message>
+
+
   <message name="BARO_WORDS" id="46">
       <field name="w1"  type="uint16"/>
       <field name="w2"  type="uint16"/>
@@ -366,6 +375,11 @@
     <field name="values" type="int16[]" alt_unit_coef="1e-3"/>
  </message>
 
+  <message name="VISUALTARGET" id="235">
+    <field name="x" type="uint16"></field>
+    <field name="y" type="uint16"></field>
+  </message>
+
  <message name="DEBUG_IR_I2C" id="53">
     <field name="top" type="int16" unit="adc"></field>
  </message>
@@ -526,6 +540,11 @@
     <field name="values" type="uint16[]" unit="ticks"/>
   </message>
 
+  <message name="OPTICFLOW" id="234">
+    <field name="flow" type="uint16"></field>
+    <field name="ref_alt" type="float"></field>
+  </message>
+
   <message name="RC" id="101">
     <field name="values" type="int16[]" unit="pprz" format="%d"/>
   </message>
@@ -550,12 +569,12 @@
     <field name="values" type="uint16[]" unit="none"/>
   </message>
 
-   <message name="BETH" id="106">
-     <field name="azimuth" type="uint16"/>
-     <field name="elevation" type="uint16"/>
-     <field name="tilt" type="uint16"/>
-     <field name="other" type="uint16"/>
-   </message>
+  <message name="BETH" id="106">
+    <field name="azimuth" type="uint16"/>
+    <field name="elevation" type="uint16"/>
+    <field name="tilt" type="uint16"/>
+    <field name="other" type="uint16"/>
+  </message>
 
   <message name="DC_SHOT" id="110">
      <field name="photo_nr" type="int16" unit=""></field>
@@ -597,6 +616,16 @@
     <field name="climb" type="float"/>
   </message>
 
+  <message name="VERTICAL_ENERGY" id="124">
+    <field name="Epot_err" type="float"/>
+    <field name="Ekin_err" type="float"/>
+    <field name="Etot_err" type="float"/>
+    <field name="Edis_err" type="float"/>
+    <field name="throttle" type="float"/>
+    <field name="nav_pitch" type="float"/>
+    <field name="speed_sp" type="float"/>
+  </message>
+
  <message name="BOOZ2_STAB_ATTITUDE_FLOAT" id="130">
     <field name="est_p"         type="float" alt_unit="degres/s" alt_unit_coef="57.29578"/>   
     <field name="est_q"         type="float" alt_unit="degres/s" alt_unit_coef="57.29578"/>   
diff --git a/conf/ubx.xml b/conf/ubx.xml
index eaa6053c5b..6c4dfe9243 100644
--- a/conf/ubx.xml
+++ b/conf/ubx.xml
@@ -205,4 +205,24 @@
 
  </class>
 
+ <class name="MAVPILOT" ID="100">
+  <message name="COMMAND" ID="0x05" length="12">
+    <field name="S1" format="U2"/>
+    <field name="S2" format="U2"/>
+    <field name="S3" format="U2"/>
+    <field name="S4" format="U2"/>
+    <field name="S5" format="U2"/>
+    <field name="S6" format="U2"/>
+  </message>
+
+  <message name="FBW" ID="0x06" length="6">
+    <field name="MOD" format="U1"/>
+    <field name="STAT" format="U1"/>
+    <field name="ERR" format="U1"/>
+    <field name="VOLT" format="U1"/>
+    <field name="CURRENT" format="U2" />
+  </message>
+ </class>
+
+
 </ubx>
diff --git a/sw/airborne/fw_v_ctl.c b/sw/airborne/fw_v_ctl.c
index 01333efb15..441ee141ca 100644
--- a/sw/airborne/fw_v_ctl.c
+++ b/sw/airborne/fw_v_ctl.c
@@ -336,6 +336,7 @@ inline static void v_ctl_climb_auto_throttle_loop(void) {
   controlled_throttle = (err_airspeed + v_ctl_auto_airspeed_sum_err * v_ctl_auto_airspeed_igain) * v_ctl_auto_airspeed_pgain;
 
   // Done, set outputs
+  Bound(controlled_throttle, 0, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE);
   f_throttle = controlled_throttle;
   nav_pitch = v_ctl_pitch_of_vz;
   v_ctl_throttle_setpoint = TRIM_UPPRZ(f_throttle * MAX_PPRZ);
diff --git a/sw/airborne/main_ap.c b/sw/airborne/main_ap.c
index 8599969ea2..76ffecfe51 100644
--- a/sw/airborne/main_ap.c
+++ b/sw/airborne/main_ap.c
@@ -69,7 +69,7 @@
 #include "adc_generic.h"
 #endif
 
-#ifdef USE_AIRSPEED
+#if defined USE_AIRSPEED || defined MEASURE_AIRSPEED
 #include "airspeed.h"
 #endif
 
@@ -147,9 +147,6 @@
 #include "osam_imu_ugear.h"
 #endif 
 
-#ifdef XSENS 
-#include "xsens_ins.h"
-#endif
 /*code added by Haiyang Chao ends*/
 
 #if ! defined CATASTROPHIC_BAT_LEVEL && defined LOW_BATTERY
@@ -659,7 +656,8 @@ void periodic_task_ap( void ) {
 #if defined GYRO
       gyro_update();
 #endif
-#if defined USE_AIRSPEED
+
+#if defined USE_AIRSPEED || defined MEASURE_AIRSPEED
       airspeed_update();
 #endif
 #ifdef INFRARED
@@ -711,7 +709,7 @@ void init_ap( void ) {
 #ifdef GYRO
   gyro_init();
 #endif
-#ifdef USE_AIRSPEED
+#if defined USE_AIRSPEED || defined MEASURE_AIRSPEED
   airspeed_init();
 #endif
 #ifdef GPS
@@ -913,6 +911,9 @@ void event_task_ap( void ) {
       gps_verbose_downlink = !launch;
       UseGpsPosNoSend(estimator_update_state_gps);
       gps_downlink();
+#ifdef GPS_TRIGGERED_FUNCTION
+	GPS_TRIGGERED_FUNCTION();
+#endif
       gps_pos_available = FALSE;
     }
   }