diff --git a/conf/airframes/ENAC/hoops_gen_ap.xml b/conf/airframes/ENAC/hoops_gen_ap.xml
index 2fbfa32a1d..bbbbf4ce98 100644
--- a/conf/airframes/ENAC/hoops_gen_ap.xml
+++ b/conf/airframes/ENAC/hoops_gen_ap.xml
@@ -10,10 +10,8 @@
 
 <airframe name="Hooper Gen AP">
 
-  <autopilot name="rotorcraft_autopilot.xml"/>
-
   <firmware name="rotorcraft">
-    <configure name="USE_GENERATED_AUTOPILOT" value="TRUE"/>
+    <autopilot name="rotorcraft_autopilot.xml"/>
 
     <configure name="AHRS_ALIGNER_LED" value="2"/>
     <define name="IMU_MPU9250_READ_MAG" value="0"/>
diff --git a/conf/airframes/airframe.dtd b/conf/airframes/airframe.dtd
index 0749957b89..70551bac10 100644
--- a/conf/airframes/airframe.dtd
+++ b/conf/airframes/airframe.dtd
@@ -24,8 +24,8 @@
 <!ELEMENT modules (load|module)*>
 <!ELEMENT load (configure|define)*>
 <!ELEMENT configure EMPTY>
-<!ELEMENT firmware (target|subsystem|module|configure|define|comment)*>
-<!ELEMENT target (subsystem|module|configure|define|comment)*>
+<!ELEMENT firmware (target|subsystem|module|autopilot|configure|define|comment)*>
+<!ELEMENT target (subsystem|module|autopilot|configure|define|comment)*>
 <!ELEMENT subsystem (configure|define|comment)*>
 <!ELEMENT module (configure|define|comment)*>
 <!ELEMENT autopilot EMPTY>
diff --git a/conf/autopilot/fixedwing_autopilot.xml b/conf/autopilot/fixedwing_autopilot.xml
index 94e6869e7a..fd5467949d 100644
--- a/conf/autopilot/fixedwing_autopilot.xml
+++ b/conf/autopilot/fixedwing_autopilot.xml
@@ -9,78 +9,115 @@
     </modules-->
 
     <includes>
-      <!-- <include href=""/> -->
+      <include name="generated/airframe.h"/>
+      <include name="autopilot.h"/>
+      <include name="autopilot_rc_helpers.h"/>
+      <include name="inter_mcu.h"/>
+      <include name="nav.h"/>
+      <include name="guidance/guidance_common.h"/>
+      <include name="guidance/guidance_h.h"/>
+      <include name="stabilization/stabilization_attitude.h"/>
+      <include name="subsystems/gps.h"/>
     </includes>
 
     <control_block name="actuators_ap">
-      <call fun="SetCommandsFromAP(commands)"/>
-      <call fun="SetActuatorsFromCommands(commands)"/>
+      <call fun="PPRZ_MUTEX_LOCK(ap_state_mtx)"/>
+      <call fun="AP_COMMAND_SET_THROTTLE(v_ctl_throttle_slewed)"/>
+      <call fun="AP_COMMAND_SET_ROLL(-h_ctl_aileron_setpoint)"/>
+      <call fun="AP_COMMAND_SET_PITCH(h_ctl_elevator_setpoint)"/>
+      <call fun="AP_COMMAND_SET_YAW(h_ctl_rudder_setpoint)"/>
+      <call fun="AP_COMMAND_SET_CL(h_ctl_flaps_setpoint)"/>
+      <call fun="PPRZ_MUTEX_UNLOCK(ap_state_mtx)"/>
+    </control_block>
+
+    <control_block name="set_attitude_from_rc">
+      <call fun="PPRZ_MUTEX_LOCK(fbw_state_mtx)"/>
+      <call fun="AP_SETPOINT_ROLL(h_ctl_roll_setpoint, AUTO1_MAX_ROLL)"/>
+      <call fun="AP_SETPOINT_PITCH(h_ctl_pitch_setpoint, AUTO1_MAX_PITCH)"/>
+      <call fun="AP_SETPOINT_YAW_RATE(h_ctl_yaw_rate_setpoint, AUTO1_MAX_YAW_RATE)"/>
+      <call fun="AP_SETPOINT_THROTTLE(v_ctl_throttle_setpoint)"/>
+      <call fun="PPRZ_MUTEX_UNLOCK(fbw_state_mtx)"/>
     </control_block>
 
     <control_block name="attitude">
       <call fun="h_ctl_attitude_loop()"/>
-      <call fun="h_ctl_throttle_slew()"/>
+      <call fun="v_ctl_throttle_slew()"/>
+    </control_block>
+
+    <control_block name="guidance">
+      <call fun="h_ctl_guidance_loop()"/>
+      <call fun="v_ctl_guidance_loop()"/>
     </control_block>
 
     <exceptions>
-      <exception cond="too_far_from_home" deroute="HOME"/>
+      <exception cond="too_far_from_home && autopilot_in_flight()" deroute="HOME"/>
     </exceptions>
 
     <mode name="MANUAL" gcs_name="MANU">
       <select cond="RCMode0()"/>
-      <control>
-        <call fun="SetCommandsFromRC(commands, rc_values)"/>
-        <call fun="SetActuatorsFromCommands(commands)"/>
+      <control freq="4"> <!-- only for display -->
+        <call fun="common_nav_periodic_task_4Hz()"/>
+        <call fun="nav_periodic_task()"/>
       </control>
-      <exception cond="RCLost()" deroute="HOME"/>
+      <control> <!-- only for display -->
+        <call fun="v_ctl_throttle_slewed = imcu_get_radio(RADIO_THROTTLE)"/>
+      </control>
+      <exception cond="RCLost() && autopilot_in_flight()" deroute="HOME"/>
     </mode>
 
     <mode name="AUTO1">
       <select cond="RCMode1()"/>
+      <control freq="4"> <!-- only for display -->
+        <call fun="common_nav_periodic_task_4Hz()"/>
+        <call fun="nav_periodic_task()"/>
+      </control>
       <control>
-        <call fun="SetAttitudeThrottleFromRC(rc_values)"/>
+        <call_block name="set_attitude_from_rc"/>
         <call_block name="attitude"/>
         <call_block name="actuators_ap"/>
       </control>
-      <exception cond="RCLost()" deroute="HOME"/>
+      <exception cond="RCLost() && autopilot_in_flight()" deroute="HOME"/>
     </mode>
 
     <mode name="AUTO2">
       <select cond="$DEFAULT_MODE"/>
       <select cond="RCMode2()" exception="HOME"/>
-      <select cond="RCMode2() && DLMode2()"/>
       <control freq="4">
-      <call fun="navigation_task()"/>
+        <call fun="common_nav_periodic_task_4Hz()"/>
+        <call fun="nav_periodic_task()"/>
       </control>
       <control>
-        <call fun="SetApOnlyCommands(commands)"/>
-        <call fun="SetAutoCommandsFromRC(commands, rc_values)" cond="!RCLost()"/>
+        <call_block name="guidance"/>
         <call_block name="attitude"/>
         <call_block name="actuators_ap"/>
       </control>
-      <exception cond="GPSLost()" deroute="GPS_LOST"/>
+      <exception cond="GpsIsLost() && autopilot_in_flight()" deroute="GPS_LOST"/>
     </mode>
 
     <mode name="HOME" settings="hide">
       <control freq="4">
+        <call fun="common_nav_periodic_task_4Hz()"/>
         <call fun="nav_home()"/>
       </control>
       <control>
+        <call_block name="guidance"/>
         <call_block name="attitude"/>
         <call_block name="actuators_ap"/>
       </control>
-      <exception cond="GPSLost()" deroute="GPS_LOST"/>
+      <exception cond="GpsIsLost() && autopilot_in_flight()" deroute="GPS_LOST"/>
     </mode>
 
     <mode name="GPS_LOST" gcs_name="NOGPS" settings="hide">
       <control freq="4">
-        <call fun="nav_gps_lost()"/>
+        <call fun="common_nav_periodic_task_4Hz()"/>
+        <call fun="nav_without_gps()"/>
       </control>
       <control>
+        <call_block name="guidance"/>
         <call_block name="attitude"/>
         <call_block name="actuators_ap"/>
       </control>
-      <exception cond="!GPSLost()" deroute="$LAST_MODE"/>
+      <exception cond="!GpsIsLost()" deroute="$LAST_MODE"/>
     </mode>
 
   </state_machine>
diff --git a/conf/autopilot/rotorcraft_autopilot.xml b/conf/autopilot/rotorcraft_autopilot.xml
index 135835de6c..4e50837a61 100644
--- a/conf/autopilot/rotorcraft_autopilot.xml
+++ b/conf/autopilot/rotorcraft_autopilot.xml
@@ -29,13 +29,13 @@
     </settings>
 
     <control_block name="set_actuators">
-      <call fun="SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight, autopilot_motors_on)"/>
+      <call fun="SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight(), autopilot_get_motors_on())"/>
       <call fun="SetActuatorsFromCommands(commands, autopilot_mode)"/>
     </control_block>
 
     <control_block name="attitude_loop">
-      <call fun="stabilization_attitude_read_rc(autopilot_in_flight, FALSE, FALSE)"/>
-      <call fun="stabilization_attitude_run(autopilot_in_flight)"/>
+      <call fun="stabilization_attitude_read_rc(autopilot_in_flight(), FALSE, FALSE)"/>
+      <call fun="stabilization_attitude_run(autopilot_in_flight())"/>
     </control_block>
 
     <control_block name="throttle_direct">
@@ -46,7 +46,7 @@
 
     <control_block name="altitude_loop">
       <call fun="gv_update_ref_from_z_sp(guidance_v_z_sp)"/>
-      <call fun="run_hover_loop(autopilot_in_flight)"/>
+      <call fun="run_hover_loop(autopilot_in_flight())"/>
       <!--call fun="SaturateThrottle(rc_values)"/-->
     </control_block>
 
@@ -99,12 +99,13 @@
         <call fun="nav_periodic_task()"/>
       </control>
       <control>
-        <call fun="guidance_v_thrust_adapt(autopilot_in_flight)"/>
-        <call fun="guidance_v_from_nav(autopilot_in_flight)"/>
-        <call fun="guidance_h_from_nav(autopilot_in_flight)"/>
+        <call fun="guidance_v_read_rc()"/>
+        <call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
+        <call fun="guidance_v_from_nav(autopilot_in_flight())"/>
+        <call fun="guidance_h_from_nav(autopilot_in_flight())"/>
         <call_block name="set_actuators"/>
       </control>
-      <exception cond="GpsIsLost() && autopilot_in_flight" deroute="FAILSAFE"/>
+      <exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
     </mode>
 
     <mode name="GUIDED">
@@ -119,12 +120,13 @@
         <call fun="nav_periodic_task()"/>
       </control>
       <control>
-        <call fun="guidance_v_thrust_adapt(autopilot_in_flight)"/>
-        <call fun="guidance_v_guided_run(autopilot_in_flight)"/>
-        <call fun="guidance_h_guided_run(autopilot_in_flight)"/>
+        <call fun="guidance_v_read_rc()"/>
+        <call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
+        <call fun="guidance_v_guided_run(autopilot_in_flight())"/>
+        <call fun="guidance_h_guided_run(autopilot_in_flight())"/>
         <call_block name="set_actuators"/>
       </control>
-      <exception cond="GpsIsLost() && autopilot_in_flight" deroute="FAILSAFE"/>
+      <exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
     </mode>
 
     <mode name="HOME">
@@ -137,9 +139,10 @@
         <call fun="nav_home()"/>
       </control>
       <control>
-        <call fun="guidance_v_thrust_adapt(autopilot_in_flight)"/>
-        <call fun="guidance_v_from_nav(autopilot_in_flight)"/>
-        <call fun="guidance_h_from_nav(autopilot_in_flight)"/>
+        <call fun="guidance_v_read_rc()"/>
+        <call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
+        <call fun="guidance_v_from_nav(autopilot_in_flight())"/>
+        <call fun="guidance_h_from_nav(autopilot_in_flight())"/>
         <call_block name="set_actuators"/>
       </control>
       <exception cond="GpsIsLost()" deroute="FAILSAFE"/>
@@ -154,9 +157,9 @@
       </on_enter>
       <control>
         <call fun="stabilization_attitude_set_failsafe_setpoint()"/>
-        <call fun="stabilization_attitude_run(autopilot_in_flight)"/>
+        <call fun="stabilization_attitude_run(autopilot_in_flight())"/>
         <call fun="gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z)"/>
-        <call fun="run_hover_loop(autopilot_in_flight)"/>
+        <call fun="run_hover_loop(autopilot_in_flight())"/>
         <call_block name="set_actuators"/>
       </control>
       <exception cond="!GpsIsLost()" deroute="$LAST_MODE"/>
@@ -167,8 +170,7 @@
       <select cond="$DEFAULT_MODE"/>
       <select cond="kill_switch_is_on()"/>
       <on_enter>
-        <call fun="autopilot_in_flight = false"/>
-        <call fun="autopilot_in_flight_counter = 0"/>
+        <call fun="autopilot_set_in_flight(false)"/>
         <call fun="autopilot_set_motors_on(false)"/>
         <call fun="stabilization_cmd[COMMAND_THRUST] = 0"/>
       </on_enter>
diff --git a/conf/modules/guidance_basic_fw.xml b/conf/modules/guidance_basic_fw.xml
index 7e2d91e221..a10b9c5ae4 100644
--- a/conf/modules/guidance_basic_fw.xml
+++ b/conf/modules/guidance_basic_fw.xml
@@ -47,10 +47,12 @@
   </settings>
   <header>
     <file name="guidance_v.h"/>
+    <file name="guidance_h.h"/>
   </header>
   <init fun="v_ctl_init()"/>
   <makefile target="ap|sim|nps" firmware="fixedwing">
     <file name="guidance_v.c" dir="$(SRC_FIRMWARE)/guidance"/>
+    <file name="guidance_h.c" dir="$(SRC_FIRMWARE)/guidance"/>
     <define name="CTRL_TYPE_H" value="firmwares/fixedwing/guidance/guidance_v.h" type="string"/>
   </makefile>
 </module>
diff --git a/conf/modules/guidance_full_pid_fw.xml b/conf/modules/guidance_full_pid_fw.xml
index 8e03b24235..c19b23e585 100644
--- a/conf/modules/guidance_full_pid_fw.xml
+++ b/conf/modules/guidance_full_pid_fw.xml
@@ -75,10 +75,12 @@
   </settings>
   <header>
     <file name="guidance_v_n.h"/>
+    <file name="guidance_h.h"/>
   </header>
   <init fun="v_ctl_init()"/>
   <makefile target="ap|sim|nps" firmware="fixedwing">
     <file name="guidance_v_n.c" dir="$(SRC_FIRMWARE)/guidance"/>
+    <file name="guidance_h.c" dir="$(SRC_FIRMWARE)/guidance"/>
     <define name="CTRL_TYPE_H" value="firmwares/fixedwing/guidance/guidance_v_n.h" type="string"/>
   </makefile>
 </module>
diff --git a/sw/airborne/arch/sim/sim_ap.c b/sw/airborne/arch/sim/sim_ap.c
index 460a78e792..a86fec963c 100644
--- a/sw/airborne/arch/sim/sim_ap.c
+++ b/sw/airborne/arch/sim/sim_ap.c
@@ -62,7 +62,11 @@ value sim_sys_time_task(value unit)
 value sim_periodic_task(value unit)
 {
   sensors_task();
+#if USE_GENERATED_AUTOPILOT
+  autopilot_periodic();
+#else
   attitude_loop();
+#endif
   reporting_task();
   modules_periodic_task();
   periodic_task_fbw();
@@ -80,7 +84,9 @@ value sim_monitor_task(value unit)
 
 value sim_nav_task(value unit)
 {
+#if !USE_GENERATED_AUTOPILOT
   navigation_task();
+#endif
   return unit;
 }
 
diff --git a/sw/airborne/firmwares/fixedwing/autopilot_firmware.h b/sw/airborne/firmwares/fixedwing/autopilot_firmware.h
index 58a04d25bb..c5ec6df54f 100644
--- a/sw/airborne/firmwares/fixedwing/autopilot_firmware.h
+++ b/sw/airborne/firmwares/fixedwing/autopilot_firmware.h
@@ -31,6 +31,14 @@
 #include "std.h"
 #include "autopilot.h"
 
+// FIXME, move to control
+#define LATERAL_MODE_MANUAL    0
+#define LATERAL_MODE_ROLL_RATE 1
+#define LATERAL_MODE_ROLL      2
+#define LATERAL_MODE_COURSE    3
+#define LATERAL_MODE_NB        4
+extern uint8_t lateral_mode;
+
 /** Supply voltage in deciVolt.
  * This the ap copy of the measurement from fbw
  * FIXME use electrical module ?
diff --git a/sw/airborne/firmwares/fixedwing/autopilot_generated.c b/sw/airborne/firmwares/fixedwing/autopilot_generated.c
index e4220b8945..19736c4bc1 100644
--- a/sw/airborne/firmwares/fixedwing/autopilot_generated.c
+++ b/sw/airborne/firmwares/fixedwing/autopilot_generated.c
@@ -55,6 +55,13 @@ void autopilot_generated_periodic(void)
   // copy generated mode to public mode (may be changed on internal exceptions)
   autopilot.mode = autopilot_mode_ap;
 
+#if defined MCU_SPI_LINK || defined MCU_UART_LINK || defined MCU_CAN_LINK
+  link_mcu_send();
+#elif defined INTER_MCU && defined SINGLE_MCU
+  /**Directly set the flag indicating to FBW that shared buffer is available*/
+  inter_mcu_received_ap = true;
+#endif
+
 }
 
 /** AP mode setting handler
@@ -75,7 +82,7 @@ void autopilot_generated_set_mode(uint8_t new_autopilot_mode)
 }
 
 
-void autopilot_generated_set_motors_on(bool motors_on)
+void autopilot_generated_set_motors_on(bool motors_on __attribute__((unused)))
 {
   // Do nothing on fixedwing ?
 }
@@ -96,6 +103,9 @@ static inline void copy_from_to_fbw(void)
 void autopilot_generated_on_rc_frame(void)
 {
 
+  // update electrical from FBW
+  imcu_get_electrical(&vsupply, &current, &energy);
+
   // FIXME what to do here ?
   copy_from_to_fbw();
 
diff --git a/sw/airborne/firmwares/fixedwing/autopilot_rc_helpers.h b/sw/airborne/firmwares/fixedwing/autopilot_rc_helpers.h
new file mode 100644
index 0000000000..83513be998
--- /dev/null
+++ b/sw/airborne/firmwares/fixedwing/autopilot_rc_helpers.h
@@ -0,0 +1,61 @@
+/*
+ * Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
+ *
+ * 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, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ * @file firmwares/fixedwing/autopilot_rc_helpers.h
+ *
+ * Some helper functions to check RC sticks.
+ */
+
+#ifndef AUTOPILOT_RC_HELPERS_H
+#define AUTOPILOT_RC_HELPERS_H
+
+#include "generated/airframe.h"
+#include "subsystems/radio_control.h"
+
+/** RC mode switch position helper
+ *  switch positions threshold are evenly spaced
+ *
+ *  @param[in] chan RC mode channel number
+ *  @param[in] pos switch position to be tested
+ *  @param[in] max maximum number of position of the switch
+ *  @return true if current position is the same as requested (and RC status is OK)
+ */
+static inline bool rc_mode_switch(uint8_t chan, uint8_t pos, uint8_t max)
+{
+  if (radio_control.status != RC_OK) return false;
+  if (pos >= max) return false;
+  int32_t v = (int32_t)radio_control.values[chan] - MIN_PPRZ;
+  // round final value
+  int32_t p = (((((int32_t)max - 1) * 10 * v) / (MAX_PPRZ - MIN_PPRZ)) + 5) / 10;
+  Bound(p, 0, max - 1); // just in case
+  return pos == (uint8_t)p;
+}
+
+/** Convenience macro for 3way switch
+ */
+#ifdef RADIO_MODE
+#define RCMode0() rc_mode_switch(RADIO_MODE, 0, 3)
+#define RCMode1() rc_mode_switch(RADIO_MODE, 1, 3)
+#define RCMode2() rc_mode_switch(RADIO_MODE, 2, 3)
+#endif
+
+
+#endif /* AUTOPILOT_RC_HELPERS_H */
diff --git a/sw/airborne/firmwares/fixedwing/autopilot_static.c b/sw/airborne/firmwares/fixedwing/autopilot_static.c
index be049b7bc1..922c2957e9 100644
--- a/sw/airborne/firmwares/fixedwing/autopilot_static.c
+++ b/sw/airborne/firmwares/fixedwing/autopilot_static.c
@@ -54,9 +54,9 @@ static inline uint8_t pprz_mode_update(void);
 static inline uint8_t mcu1_status_update(void);
 static inline void copy_from_to_fbw(void);
 
-/** mode to enter when RC is lost in PPRZ_MODE_MANUAL or PPRZ_MODE_AUTO1 */
+/** mode to enter when RC is lost in AP_MODE_MANUAL or AP_MODE_AUTO1 */
 #ifndef RC_LOST_MODE
-#define RC_LOST_MODE PPRZ_MODE_HOME
+#define RC_LOST_MODE AP_MODE_HOME
 #endif
 
 
@@ -89,7 +89,7 @@ void autopilot_event(void)
 
 void autopilot_static_init(void)
 {
-  autopilot.mode = PPRZ_MODE_AUTO2;
+  autopilot.mode = AP_MODE_AUTO2;
 
   lateral_mode = LATERAL_MODE_MANUAL;
   gps_lost = false;
@@ -116,11 +116,11 @@ void autopilot_static_on_rc_frame(void)
 
   /* really_lost is true if we lost RC in MANUAL or AUTO1 */
   uint8_t really_lost = bit_is_set(imcu_get_status(), STATUS_RADIO_REALLY_LOST) &&
-                        (autopilot_get_mode() == PPRZ_MODE_AUTO1 || autopilot_get_mode() == PPRZ_MODE_MANUAL);
+                        (autopilot_get_mode() == AP_MODE_AUTO1 || autopilot_get_mode() == AP_MODE_MANUAL);
 
-  if (autopilot_get_mode() != PPRZ_MODE_HOME && autopilot_get_mode() != PPRZ_MODE_GPS_OUT_OF_ORDER && autopilot.launch) {
+  if (autopilot_get_mode() != AP_MODE_HOME && autopilot_get_mode() != AP_MODE_GPS_OUT_OF_ORDER && autopilot.launch) {
     if (too_far_from_home || datalink_lost() || higher_than_max_altitude()) {
-      mode_changed = autopilot_set_mode(PPRZ_MODE_HOME);
+      mode_changed = autopilot_set_mode(AP_MODE_HOME);
     }
     if (really_lost) {
       mode_changed = autopilot_set_mode(RC_LOST_MODE);
@@ -144,7 +144,7 @@ void autopilot_static_on_rc_frame(void)
   /** In AUTO1 mode, compute roll setpoint and pitch setpoint from
    * \a RADIO_ROLL and \a RADIO_PITCH \n
    */
-  if (autopilot_get_mode() == PPRZ_MODE_AUTO1) {
+  if (autopilot_get_mode() == AP_MODE_AUTO1) {
     /** Roll is bounded between [-AUTO1_MAX_ROLL;AUTO1_MAX_ROLL] */
     h_ctl_roll_setpoint = FLOAT_OF_PPRZ(imcu_get_radio(RADIO_ROLL), 0., AUTO1_MAX_ROLL);
 
@@ -158,7 +158,7 @@ void autopilot_static_on_rc_frame(void)
 
   /** In AUTO1, throttle comes from RADIO_THROTTLE
       In MANUAL, the value is copied to get it in the telemetry */
-  if (autopilot_get_mode() == PPRZ_MODE_MANUAL || autopilot_get_mode() == PPRZ_MODE_AUTO1) {
+  if (autopilot_get_mode() == AP_MODE_MANUAL || autopilot_get_mode() == AP_MODE_AUTO1) {
     v_ctl_throttle_setpoint = imcu_get_radio(RADIO_THROTTLE);
   }
   /** else asynchronously set by v_ctl_climb_loop(); */
@@ -173,7 +173,7 @@ void autopilot_static_on_rc_frame(void)
   /* the SITL check is a hack to prevent "automatic" launch in NPS */
 #ifndef SITL
   if (!autopilot.flight_time) {
-    if (autopilot_get_mode() == PPRZ_MODE_AUTO2 && imcu_get_radio(RADIO_THROTTLE) > THROTTLE_THRESHOLD_TAKEOFF) {
+    if (autopilot_get_mode() == AP_MODE_AUTO2 && imcu_get_radio(RADIO_THROTTLE) > THROTTLE_THRESHOLD_TAKEOFF) {
       autopilot.launch = true;
     }
   }
@@ -212,12 +212,12 @@ void navigation_task(void)
   static uint8_t last_pprz_mode;
 
   /** If aircraft is launched and is in autonomus mode, go into
-      PPRZ_MODE_GPS_OUT_OF_ORDER mode (Failsafe) if we lost the GPS */
+      AP_MODE_GPS_OUT_OF_ORDER mode (Failsafe) if we lost the GPS */
   if (autopilot.launch) {
     if (GpsTimeoutError) {
-      if (autopilot_get_mode() == PPRZ_MODE_AUTO2 || autopilot_get_mode() == PPRZ_MODE_HOME) {
+      if (autopilot_get_mode() == AP_MODE_AUTO2 || autopilot_get_mode() == AP_MODE_HOME) {
         last_pprz_mode = autopilot_get_mode();
-        autopilot_set_mode(PPRZ_MODE_GPS_OUT_OF_ORDER);
+        autopilot_set_mode(AP_MODE_GPS_OUT_OF_ORDER);
         autopilot_send_mode();
         gps_lost = true;
       }
@@ -231,9 +231,9 @@ void navigation_task(void)
 #endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */
 
   common_nav_periodic_task_4Hz();
-  if (autopilot_get_mode() == PPRZ_MODE_HOME) {
+  if (autopilot_get_mode() == AP_MODE_HOME) {
     nav_home();
-  } else if (autopilot_get_mode() == PPRZ_MODE_GPS_OUT_OF_ORDER) {
+  } else if (autopilot_get_mode() == AP_MODE_GPS_OUT_OF_ORDER) {
     nav_without_gps();
   } else {
     nav_periodic_task();
@@ -254,8 +254,8 @@ void navigation_task(void)
     v_ctl_altitude_loop();
   }
 
-  if (autopilot_get_mode() == PPRZ_MODE_AUTO2 || autopilot_get_mode() == PPRZ_MODE_HOME
-      || autopilot_get_mode() == PPRZ_MODE_GPS_OUT_OF_ORDER) {
+  if (autopilot_get_mode() == AP_MODE_AUTO2 || autopilot_get_mode() == AP_MODE_HOME
+      || autopilot_get_mode() == AP_MODE_GPS_OUT_OF_ORDER) {
 #ifdef H_CTL_RATE_LOOP
     /* Be sure to be in attitude mode, not roll */
     h_ctl_auto1_rate = false;
@@ -272,7 +272,7 @@ void navigation_task(void)
 void attitude_loop(void)
 {
 
-  if (autopilot_get_mode() >= PPRZ_MODE_AUTO2) {
+  if (autopilot_get_mode() >= AP_MODE_AUTO2) {
 #if CTRL_VERTICAL_LANDING
     if (v_ctl_mode == V_CTL_MODE_LANDING) {
       v_ctl_landing_loop();
@@ -339,29 +339,29 @@ void attitude_loop(void)
 #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
 static inline uint8_t pprz_mode_update(void)
 {
-  if ((autopilot_get_mode() != PPRZ_MODE_HOME &&
-       autopilot_get_mode() != PPRZ_MODE_GPS_OUT_OF_ORDER)
+  if ((autopilot_get_mode() != AP_MODE_HOME &&
+       autopilot_get_mode() != AP_MODE_GPS_OUT_OF_ORDER)
 #ifdef UNLOCKED_HOME_MODE
       || true
 #endif
      ) {
 #ifndef RADIO_AUTO_MODE
-    return autopilot_set_mode(PPRZ_MODE_OF_PULSE(imcu_get_radio(RADIO_MODE)));
+    return autopilot_set_mode(AP_MODE_OF_PULSE(imcu_get_radio(RADIO_MODE)));
 #else
     INFO("Using RADIO_AUTO_MODE to switch between AUTO1 and AUTO2.")
     /* If RADIO_AUTO_MODE is enabled mode swithing will be seperated between two switches/channels
-     * RADIO_MODE will switch between PPRZ_MODE_MANUAL and any PPRZ_MODE_AUTO mode selected by RADIO_AUTO_MODE.
+     * RADIO_MODE will switch between AP_MODE_MANUAL and any AP_MODE_AUTO mode selected by RADIO_AUTO_MODE.
      *
      * This is mainly a cludge for entry level radios with no three-way switch but two available two-way switches which can be used.
      */
-    if (PPRZ_MODE_OF_PULSE(imcu_get_radio(RADIO_MODE)) == PPRZ_MODE_MANUAL) {
+    if (AP_MODE_OF_PULSE(imcu_get_radio(RADIO_MODE)) == AP_MODE_MANUAL) {
       /* RADIO_MODE in MANUAL position */
-      return autopilot_set_mode(PPRZ_MODE_MANUAL);
+      return autopilot_set_mode(AP_MODE_MANUAL);
     } else {
       /* RADIO_MODE not in MANUAL position.
        * Select AUTO mode bassed on RADIO_AUTO_MODE channel
        */
-      return autopilot_set_mode((imcu_get_radio(RADIO_AUTO_MODE) > THRESHOLD2) ? PPRZ_MODE_AUTO2 : PPRZ_MODE_AUTO1);
+      return autopilot_set_mode((imcu_get_radio(RADIO_AUTO_MODE) > THRESHOLD2) ? AP_MODE_AUTO2 : AP_MODE_AUTO1);
     }
 #endif // RADIO_AUTO_MODE
   } else {
diff --git a/sw/airborne/firmwares/fixedwing/autopilot_static.h b/sw/airborne/firmwares/fixedwing/autopilot_static.h
index e6eacc768e..717c1084be 100644
--- a/sw/airborne/firmwares/fixedwing/autopilot_static.h
+++ b/sw/airborne/firmwares/fixedwing/autopilot_static.h
@@ -33,12 +33,12 @@
 
 /** AP modes.
  */
-#define  PPRZ_MODE_MANUAL 0
-#define  PPRZ_MODE_AUTO1 1
-#define  PPRZ_MODE_AUTO2 2
-#define  PPRZ_MODE_HOME 3
-#define  PPRZ_MODE_GPS_OUT_OF_ORDER 4
-#define  PPRZ_MODE_NB 5
+#define  AP_MODE_MANUAL 0
+#define  AP_MODE_AUTO1 1
+#define  AP_MODE_AUTO2 2
+#define  AP_MODE_HOME 3
+#define  AP_MODE_GPS_OUT_OF_ORDER 4
+#define  AP_MODE_NB 5
 
 /** Static autopilot functions
  */
@@ -55,29 +55,6 @@ extern void autopilot_static_set_motors_on(bool motors_on);
 extern void navigation_task(void);
 extern void attitude_loop(void);
 
-/** Threshold for RC mode detection.
- */
-#define THRESHOLD_MANUAL_PPRZ (MIN_PPRZ / 2)
-#define THRESHOLD1 THRESHOLD_MANUAL_PPRZ
-#define THRESHOLD2 (MAX_PPRZ/2)
-
-#define PPRZ_MODE_OF_PULSE(pprz) \
-  (pprz > THRESHOLD2 ? PPRZ_MODE_AUTO2 : \
-   (pprz > THRESHOLD1 ? PPRZ_MODE_AUTO1 : PPRZ_MODE_MANUAL))
-
-
-// FIXME, move to control
-#define LATERAL_MODE_MANUAL    0
-#define LATERAL_MODE_ROLL_RATE 1
-#define LATERAL_MODE_ROLL      2
-#define LATERAL_MODE_COURSE    3
-#define LATERAL_MODE_NB        4
-extern uint8_t lateral_mode;
-
-#define STICK_PUSHED(pprz) (pprz < THRESHOLD1 || pprz > THRESHOLD2)
-#define FLOAT_OF_PPRZ(pprz, center, travel) ((float)pprz / (float)MAX_PPRZ * travel + center)
-
-#define THROTTLE_THRESHOLD_TAKEOFF (pprz_t)(MAX_PPRZ * 0.9)
 
 /* CONTROL_RATE will be removed in the next release
  * please use CONTROL_FREQUENCY instead
diff --git a/sw/airborne/firmwares/fixedwing/autopilot_utils.h b/sw/airborne/firmwares/fixedwing/autopilot_utils.h
index d63565522d..354972a4ba 100644
--- a/sw/airborne/firmwares/fixedwing/autopilot_utils.h
+++ b/sw/airborne/firmwares/fixedwing/autopilot_utils.h
@@ -30,6 +30,83 @@
 #define AUTOPILOT_UTILS_H
 
 #include "std.h"
+#include "inter_mcu.h"
+#include "generated/airframe.h"
+
+/** Get mode from pulse
+ */
+#define AP_MODE_OF_PULSE(pprz) \
+  (pprz > THRESHOLD2 ? AP_MODE_AUTO2 : \
+   (pprz > THRESHOLD1 ? AP_MODE_AUTO1 : AP_MODE_MANUAL))
+
+/** Stick pushed
+ */
+#define STICK_PUSHED(pprz) (pprz < THRESHOLD1 || pprz > THRESHOLD2)
+
+/** pprz_t to float with saturation
+ */
+#define FLOAT_OF_PPRZ(pprz, center, travel) ((float)pprz / (float)MAX_PPRZ * travel + center)
+
+/** Takeoff detection threshold from throttle
+ */
+#define THROTTLE_THRESHOLD_TAKEOFF (pprz_t)(MAX_PPRZ * 0.9)
+
+/** Threshold for RC mode detection.
+ */
+#define THRESHOLD_MANUAL_PPRZ (MIN_PPRZ / 2)
+#define THRESHOLD1 THRESHOLD_MANUAL_PPRZ
+#define THRESHOLD2 (MAX_PPRZ/2)
+
+/** AP command setter macros for usual commands
+ */
+
+// COMMAND_ROLL
+#define AP_COMMAND_SET_ROLL(_roll) { ap_state->commands[COMMAND_ROLL] = _roll; }
+
+// COMMAND_PITCH
+#define AP_COMMAND_SET_PITCH(_pitch) { ap_state->commands[COMMAND_PITCH] = _pitch; }
+
+// COMMAND_YAW
+#if H_CTL_YAW_LOOP && defined COMMAND_YAW
+#define AP_COMMAND_SET_YAW(_yaw) { ap_state->commands[COMMAND_YAW] = _yaw; }
+#else
+#define AP_COMMAND_SET_YAW(_yaw) {}
+#endif
+
+// COMMAND_THROTTLE
+#define AP_COMMAND_SET_THROTTLE(_throttle) { ap_state->commands[COMMAND_THROTTLE] = _throttle; }
+
+// COMMAND_CL
+#if H_CTL_CL_LOOP && defined COMMAND_CL
+#define AP_COMMAND_SET_CL(_cl) { ap_state->commands[COMMAND_CL] = cl; }
+#else
+#define AP_COMMAND_SET_CL(_cl) {}
+#endif
+
+// ROLL setpoint from RADIO
+#define AP_SETPOINT_ROLL(_roll, _max) { \
+  _roll = FLOAT_OF_PPRZ(fbw_state->channels[RADIO_ROLL], 0., _max); \
+}
+
+// PITCH setpoint from RADIO
+#define AP_SETPOINT_PITCH(_pitch, _max) { \
+  _pitch = FLOAT_OF_PPRZ(fbw_state->channels[RADIO_PITCH], 0., _max); \
+}
+
+// PITCH setpoint from RADIO
+#if H_CTL_YAW_LOOP && defined RADIO_YAW
+#define AP_SETPOINT_YAW_RATE(_yaw, _max) { \
+  _yaw = FLOAT_OF_PPRZ(fbw_state->channels[RADIO_YAW], 0., _max); \
+}
+#else
+#define AP_SETPOINT_YAW_RATE(_yaw, _max) {}
+#endif
+
+// THROTTLE setpoint from RADIO
+#define AP_SETPOINT_THROTTLE(_throttle) { \
+  _throttle = fbw_state->channels[RADIO_THROTTLE]; \
+}
+
 
 #endif // AUTOPILOT_UTILS_H
 
diff --git a/sw/airborne/firmwares/fixedwing/fixedwing_datalink.c b/sw/airborne/firmwares/fixedwing/fixedwing_datalink.c
index f7f8f47cdf..ec1bc94693 100644
--- a/sw/airborne/firmwares/fixedwing/fixedwing_datalink.c
+++ b/sw/airborne/firmwares/fixedwing/fixedwing_datalink.c
@@ -52,7 +52,7 @@
 #include "autopilot.h"
 uint8_t joystick_block;
 #define JoystickHandeDatalink(_roll_int8, _pitch_int8, _throttle_int8) { \
-    if (autopilot_get_mode() == PPRZ_MODE_AUTO2 && nav_block == joystick_block) {  \
+    if (autopilot_get_mode() == AP_MODE_AUTO2 && nav_block == joystick_block) {  \
       h_ctl_roll_setpoint = _roll_int8 * (AUTO1_MAX_ROLL / 0x7f);       \
       h_ctl_pitch_setpoint = _pitch_int8 * (AUTO1_MAX_PITCH / 0x7f);    \
       v_ctl_throttle_setpoint = (MAX_PPRZ/0x7f) * _throttle_int8;       \
diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_common.h b/sw/airborne/firmwares/fixedwing/guidance/guidance_common.h
index ea632ed2b9..44df276b2a 100644
--- a/sw/airborne/firmwares/fixedwing/guidance/guidance_common.h
+++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_common.h
@@ -72,6 +72,11 @@ extern void v_ctl_altitude_loop(void);
 extern void v_ctl_climb_loop(void);
 extern void v_ctl_landing_loop(void);
 
+/** General guidance logic
+ *  This will call the proper control loops according to the sub-modes
+ */
+extern void v_ctl_guidance_loop(void);
+
 /** Computes throttle_slewed from throttle_setpoint */
 extern void v_ctl_throttle_slew(void);
 
diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_h.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_h.c
new file mode 100644
index 0000000000..b4ee8171df
--- /dev/null
+++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_h.c
@@ -0,0 +1,41 @@
+/*
+ * Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
+ *
+ * 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, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ * @file firmwares/fixedwing/guidance/guidance_h.c
+ * Horizontal guidance logic for fixed wing vehicles.
+ *
+ * This is a hack for generated autopilot
+ */
+
+#include "firmwares/fixedwing/guidance/guidance_h.h"
+#include "firmwares/fixedwing/autopilot_firmware.h"
+#include "firmwares/fixedwing/guidance/guidance_common.h"
+#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
+
+void h_ctl_guidance_loop(void)
+{
+  if (lateral_mode >= LATERAL_MODE_COURSE) {
+    h_ctl_course_loop();  /* aka compute nav_desired_roll */
+  }
+  // Copy the pitch setpoint from the guidance to the stabilization control
+  h_ctl_pitch_setpoint = v_ctl_pitch_setpoint;
+}
+
diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_h.h b/sw/airborne/firmwares/fixedwing/guidance/guidance_h.h
new file mode 100644
index 0000000000..962b98c7ea
--- /dev/null
+++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_h.h
@@ -0,0 +1,40 @@
+/*
+ * Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
+ *
+ * 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, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ * @file firmwares/fixedwing/guidance/guidance_h.h
+ * Horizontal guidance logic for fixed wing vehicles.
+ *
+ */
+
+#ifndef FW_GUIDANCE_H
+#define FW_GUIDANCE_H
+
+#include "std.h"
+
+
+/** General guidance logic
+ *  This will call the proper control loops according to the sub-modes
+ */
+extern void h_ctl_guidance_loop(void);
+
+#endif
+
+
diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c
index d72af78f47..c9f054d05c 100644
--- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c
+++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c
@@ -262,6 +262,45 @@ void v_ctl_init(void)
 #endif
 }
 
+void v_ctl_guidance_loop(void)
+{
+  if (v_ctl_mode == V_CTL_MODE_AUTO_ALT) {
+    v_ctl_altitude_loop();
+  }
+#if CTRL_VERTICAL_LANDING
+  if (v_ctl_mode == V_CTL_MODE_LANDING) {
+    v_ctl_landing_loop();
+  } else {
+#endif
+    if (v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE) {
+      v_ctl_throttle_setpoint = nav_throttle_setpoint;
+      v_ctl_pitch_setpoint = nav_pitch;
+    } else {
+      if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB) {
+        v_ctl_climb_loop();
+      } /* v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB */
+    } /* v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE */
+#if CTRL_VERTICAL_LANDING
+  } /* v_ctl_mode == V_CTL_MODE_LANDING */
+#endif
+
+#if defined V_CTL_THROTTLE_IDLE
+  Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE * MAX_PPRZ), MAX_PPRZ);
+#endif
+
+#ifdef V_CTL_POWER_CTL_BAT_NOMINAL
+  if (vsupply > 0.) {
+    v_ctl_throttle_setpoint *= 10. * V_CTL_POWER_CTL_BAT_NOMINAL / (float)vsupply;
+    v_ctl_throttle_setpoint = TRIM_UPPRZ(v_ctl_throttle_setpoint);
+  }
+#endif
+
+  if (autopilot.kill_throttle || (!autopilot.flight_time && !autopilot.launch)) {
+    v_ctl_throttle_setpoint = 0;
+  }
+}
+
+
 /**
  * outer loop
  * \brief Computes v_ctl_climb_setpoint and sets v_ctl_auto_throttle_submode
diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
index b17d235954..850faf3688 100644
--- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
+++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
@@ -179,6 +179,44 @@ void v_ctl_init(void)
   v_ctl_throttle_setpoint = 0;
 }
 
+void v_ctl_guidance_loop(void)
+{
+  if (v_ctl_mode == V_CTL_MODE_AUTO_ALT) {
+    v_ctl_altitude_loop();
+  }
+#if CTRL_VERTICAL_LANDING
+  if (v_ctl_mode == V_CTL_MODE_LANDING) {
+    v_ctl_landing_loop();
+  } else {
+#endif
+    if (v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE) {
+      v_ctl_throttle_setpoint = nav_throttle_setpoint;
+      v_ctl_pitch_setpoint = nav_pitch;
+    } else {
+      if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB) {
+        v_ctl_climb_loop();
+      } /* v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB */
+    } /* v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE */
+#if CTRL_VERTICAL_LANDING
+  } /* v_ctl_mode == V_CTL_MODE_LANDING */
+#endif
+
+#if defined V_CTL_THROTTLE_IDLE
+  Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE * MAX_PPRZ), MAX_PPRZ);
+#endif
+
+#ifdef V_CTL_POWER_CTL_BAT_NOMINAL
+  if (vsupply > 0.) {
+    v_ctl_throttle_setpoint *= 10. * V_CTL_POWER_CTL_BAT_NOMINAL / (float)vsupply;
+    v_ctl_throttle_setpoint = TRIM_UPPRZ(v_ctl_throttle_setpoint);
+  }
+#endif
+
+  if (autopilot.kill_throttle || (!autopilot.flight_time && !autopilot.launch)) {
+    v_ctl_throttle_setpoint = 0;
+  }
+}
+
 /**
  * outer loop
  * \brief Computes v_ctl_climb_setpoint and sets v_ctl_auto_throttle_submode
@@ -216,7 +254,7 @@ static inline void v_ctl_set_pitch(void)
 {
   static float last_err = 0.;
 
-  if (autopilot_get_mode() == PPRZ_MODE_MANUAL || autopilot.launch == false) {
+  if (autopilot_get_mode() == AP_MODE_MANUAL || autopilot.launch == false) {
     v_ctl_auto_pitch_sum_err = 0;
   }
 
@@ -244,7 +282,7 @@ static inline void v_ctl_set_throttle(void)
 {
   static float last_err = 0.;
 
-  if (autopilot_get_mode() == PPRZ_MODE_MANUAL || autopilot.launch == false) {
+  if (autopilot_get_mode() == AP_MODE_MANUAL || autopilot.launch == false) {
     v_ctl_auto_throttle_sum_err = 0;
   }
 
@@ -307,7 +345,7 @@ static inline void v_ctl_set_airspeed(void)
 
 
   // Reset integrators in manual or before flight
-  if (autopilot_get_mode() == PPRZ_MODE_MANUAL || autopilot.launch == false) {
+  if (autopilot_get_mode() == AP_MODE_MANUAL || autopilot.launch == false) {
     v_ctl_auto_throttle_sum_err = 0.;
     v_ctl_auto_pitch_sum_err = 0.;
     v_ctl_auto_airspeed_throttle_sum_err = 0.;
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index ae9924d0fc..44ef0e1e68 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -100,7 +100,9 @@ PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BO
 
 /* if PRINT_CONFIG is defined, print some config options */
 PRINT_CONFIG_VAR(PERIODIC_FREQUENCY)
+#if !USE_GENERATED_AUTOPILOT
 PRINT_CONFIG_VAR(NAVIGATION_FREQUENCY)
+#endif
 PRINT_CONFIG_VAR(CONTROL_FREQUENCY)
 
 /* TELEMETRY_FREQUENCY is defined in generated/periodic_telemetry.h
@@ -138,7 +140,9 @@ tid_t modules_tid;     ///< id for modules_periodic_task() timer
 tid_t telemetry_tid;   ///< id for telemetry_periodic() timer
 tid_t sensors_tid;     ///< id for sensors_task() timer
 tid_t attitude_tid;    ///< id for attitude_loop() timer
+#if !USE_GENERATED_AUTOPILOT
 tid_t navigation_tid;  ///< id for navigation_task() timer
+#endif
 tid_t monitor_tid;     ///< id for monitor_task() timer
 #if USE_BARO_BOARD
 tid_t baro_tid;          ///< id for baro_periodic() timer
@@ -195,7 +199,9 @@ void init_ap(void)
 
   /**** start timers for periodic functions *****/
   sensors_tid = sys_time_register_timer(1. / PERIODIC_FREQUENCY, NULL);
+#if !USE_GENERATED_AUTOPILOT
   navigation_tid = sys_time_register_timer(1. / NAVIGATION_FREQUENCY, NULL);
+#endif
   attitude_tid = sys_time_register_timer(1. / CONTROL_FREQUENCY, NULL);
   modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL);
   telemetry_tid = sys_time_register_timer(1. / TELEMETRY_FREQUENCY, NULL);
diff --git a/sw/airborne/firmwares/fixedwing/nav.c b/sw/airborne/firmwares/fixedwing/nav.c
index 23fd7f0cb9..39d2b33941 100644
--- a/sw/airborne/firmwares/fixedwing/nav.c
+++ b/sw/airborne/firmwares/fixedwing/nav.c
@@ -170,7 +170,7 @@ void nav_glide(uint8_t start_wp, uint8_t wp)
 #define MAX_HEIGHT_CARROT 150.
 
 #define Goto3D(radius) {                                                \
-    if (autopilot_get_mode() == PPRZ_MODE_AUTO2) {                                 \
+    if (autopilot_get_mode() == AP_MODE_AUTO2) {                                 \
       int16_t yaw = imcu_get_radio(RADIO_YAW);                          \
       if (yaw > MIN_DX || yaw < -MIN_DX) {                              \
         carrot_x += FLOAT_OF_PPRZ(yaw, 0, -20.);                        \
diff --git a/sw/airborne/firmwares/fixedwing/nav.h b/sw/airborne/firmwares/fixedwing/nav.h
index 8f6e7d975b..eaae4da154 100644
--- a/sw/airborne/firmwares/fixedwing/nav.h
+++ b/sw/airborne/firmwares/fixedwing/nav.h
@@ -206,7 +206,7 @@ bool nav_approaching_xy(float x, float y, float from_x, float from_y, float appr
 
 #define NavAttitude(_roll) { \
     lateral_mode = LATERAL_MODE_ROLL; \
-    if(autopilot_get_mode() != PPRZ_MODE_AUTO1)  \
+    if(autopilot_get_mode() != AP_MODE_AUTO1)  \
     {h_ctl_roll_setpoint = _roll;} \
   }
 
diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c
index 59dd3bebfd..5cce790d43 100644
--- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c
+++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c
@@ -487,7 +487,7 @@ inline static void h_ctl_roll_loop(void)
   struct FloatRates *body_rate = stateGetBodyRates_f();
   float d_err = h_ctl_ref.roll_rate - body_rate->p;
 
-  if (autopilot_get_mode() == PPRZ_MODE_MANUAL || autopilot.launch == false) {
+  if (autopilot_get_mode() == AP_MODE_MANUAL || autopilot.launch == false) {
     h_ctl_roll_sum_err = 0.;
   } else {
     if (h_ctl_roll_igain > 0.) {
@@ -604,7 +604,7 @@ inline static void h_ctl_pitch_loop(void)
   last_err = err;
 #endif
 
-  if (autopilot_get_mode() == PPRZ_MODE_MANUAL || autopilot.launch == false) {
+  if (autopilot_get_mode() == AP_MODE_MANUAL || autopilot.launch == false) {
     h_ctl_pitch_sum_err = 0.;
   } else {
     if (h_ctl_pitch_igain > 0.) {
@@ -646,7 +646,7 @@ inline static void h_ctl_yaw_loop(void)
   float ny = 0.f;
 #endif
 
-  if (autopilot_get_mode() == PPRZ_MODE_MANUAL || autopilot.launch == false) {
+  if (autopilot_get_mode() == AP_MODE_MANUAL || autopilot.launch == false) {
     h_ctl_yaw_ny_sum_err = 0.;
   } else {
     if (h_ctl_yaw_ny_igain > 0.) {
@@ -729,7 +729,7 @@ inline static void h_ctl_cl_loop(void)
   }
 
   // no control in manual mode
-  if (autopilot_get_mode() == PPRZ_MODE_MANUAL) {
+  if (autopilot_get_mode() == AP_MODE_MANUAL) {
     cmd = 0.f;
   }
   // bound max flap angle
diff --git a/sw/airborne/modules/cam_control/cam.c b/sw/airborne/modules/cam_control/cam.c
index bfec0fd44f..adc3af0d24 100644
--- a/sw/airborne/modules/cam_control/cam.c
+++ b/sw/airborne/modules/cam_control/cam.c
@@ -130,7 +130,7 @@ void cam_periodic(void)
 {
 #if defined(CAM_FIXED_FOR_FPV_IN_AUTO1) && CAM_FIXED_FOR_FPV_IN_AUTO1 == 1
   //Position the camera for straight view.
-  if (autopilot_get_mode() == PPRZ_MODE_AUTO2) {
+  if (autopilot_get_mode() == AP_MODE_AUTO2) {
 #endif
     switch (cam_mode) {
       case CAM_MODE_OFF:
@@ -169,7 +169,7 @@ void cam_periodic(void)
         break;
     }
 #if defined(CAM_FIXED_FOR_FPV_IN_AUTO1) && CAM_FIXED_FOR_FPV_IN_AUTO1 == 1
-  } else if (autopilot_get_mode() == PPRZ_MODE_AUTO1) {
+  } else if (autopilot_get_mode() == AP_MODE_AUTO1) {
     //Position the camera for straight view.
 
 #if defined(CAM_TILT_POSITION_FOR_FPV)
diff --git a/sw/airborne/modules/cam_control/point.c b/sw/airborne/modules/cam_control/point.c
index b25acc1b77..1cb6e3a48f 100644
--- a/sw/airborne/modules/cam_control/point.c
+++ b/sw/airborne/modules/cam_control/point.c
@@ -345,8 +345,8 @@ void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude,
 
 #if defined(RADIO_CAM_LOCK)
       float radio_cam_lock = imcu_get_radio(RADIO_CAM_LOCK);
-      if ((radio_cam_lock > MAX_PPRZ / 2) && autopilot_get_mode() == PPRZ_MODE_AUTO2) { cam_lock = true; }
-      if ((radio_cam_lock < MIN_PPRZ / 2) && autopilot_get_mode() == PPRZ_MODE_AUTO2) { cam_lock = false; }
+      if ((radio_cam_lock > MAX_PPRZ / 2) && autopilot_get_mode() == AP_MODE_AUTO2) { cam_lock = true; }
+      if ((radio_cam_lock < MIN_PPRZ / 2) && autopilot_get_mode() == AP_MODE_AUTO2) { cam_lock = false; }
 #endif
     // When the variable "cam_lock" is set then the last calculated position is set as the target waypoint.
     if (cam_lock == FALSE) {
diff --git a/sw/airborne/modules/datalink/mavlink.c b/sw/airborne/modules/datalink/mavlink.c
index 87a441c8c7..5ea5948d9f 100644
--- a/sw/airborne/modules/datalink/mavlink.c
+++ b/sw/airborne/modules/datalink/mavlink.c
@@ -441,16 +441,16 @@ static void mavlink_send_heartbeat(struct transport_tx *trans, struct link_devic
 #ifdef AP
   uint8_t mav_type = MAV_TYPE_FIXED_WING;
   switch (autopilot_get_mode()) {
-    case PPRZ_MODE_MANUAL:
+    case AP_MODE_MANUAL:
       mav_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
       break;
-    case PPRZ_MODE_AUTO1:
+    case AP_MODE_AUTO1:
       mav_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
       break;
-    case PPRZ_MODE_AUTO2:
+    case AP_MODE_AUTO2:
       mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
       break;
-    case PPRZ_MODE_HOME:
+    case AP_MODE_HOME:
       mav_mode |= MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED;
       break;
     default:
diff --git a/sw/airborne/modules/nav/takeoff_detect.c b/sw/airborne/modules/nav/takeoff_detect.c
index 84f0279760..67c2257d9f 100644
--- a/sw/airborne/modules/nav/takeoff_detect.c
+++ b/sw/airborne/modules/nav/takeoff_detect.c
@@ -89,7 +89,7 @@ void takeoff_detect_periodic(void)
     case TO_DETECT_ARMED:
       // test for "nose up" + AP in AUTO2 (+ GPS OK ? FIXME)
       if (stateGetNedToBodyEulers_f()->theta > TAKEOFF_DETECT_LAUNCH_PITCH
-          && autopilot_get_mode() == PPRZ_MODE_AUTO2) {
+          && autopilot_get_mode() == AP_MODE_AUTO2) {
         takeoff_detect.timer++;
       } else {
         // else reset timer
@@ -105,7 +105,7 @@ void takeoff_detect_periodic(void)
     case TO_DETECT_LAUNCHING:
       // abort if pitch goes below threshold while launching
       if (stateGetNedToBodyEulers_f()->theta < TAKEOFF_DETECT_ABORT_PITCH
-          || autopilot_get_mode() != PPRZ_MODE_AUTO2) {
+          || autopilot_get_mode() != AP_MODE_AUTO2) {
         // back to ARMED state
         autopilot.launch = false;
         takeoff_detect.state = TO_DETECT_ARMED;
diff --git a/sw/airborne/modules/switching/auto1_commands.c b/sw/airborne/modules/switching/auto1_commands.c
index e9623c1370..80a95ce7b1 100644
--- a/sw/airborne/modules/switching/auto1_commands.c
+++ b/sw/airborne/modules/switching/auto1_commands.c
@@ -28,7 +28,7 @@
 void periodic_auto1_commands(void)
 {
   // Copy Radio commands in AUTO1
-  if (pprz_mode == PPRZ_MODE_AUTO1) {
+  if (autopilot_get_mode() == AP_MODE_AUTO1) {
 #ifdef COMMAND_HATCH
 #ifndef RADIO_HATCH
 #error auto1_commands COMMAND_HATCH needs RADIO_HATCH channel
diff --git a/sw/ground_segment/tmtc/fw_server.ml b/sw/ground_segment/tmtc/fw_server.ml
index cea9b93bb8..e6cea6c772 100644
--- a/sw/ground_segment/tmtc/fw_server.ml
+++ b/sw/ground_segment/tmtc/fw_server.ml
@@ -244,7 +244,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
           else "MANUAL";
       end ;
       if a.fbw.rc_mode = "FAILSAFE" then
-        a.ap_mode <- 5 (* Override and set FAIL(Safe) Mode *)
+        a.ap_mode <- -2 (* Override and set to FAIL (see server.ml) *)
       else
         a.ap_mode <- check_index (ivalue "ap_mode") (modes_of_aircraft a) "AP_MODE"
     | "CAM" ->
diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml
index 7c1332192e..d37604d286 100644
--- a/sw/ground_segment/tmtc/server.ml
+++ b/sw/ground_segment/tmtc/server.ml
@@ -49,8 +49,8 @@ let logs_path = Env.paparazzi_home // "var" // "logs"
 let conf_xml = ExtXml.parse_file (Env.paparazzi_home // "conf" // "conf.xml")
 let srtm_path = Env.paparazzi_home // "data" // "srtm"
 
-let get_indexed_value = fun t i ->
-  if i >= 0 then t.(i) else "UNK"
+let get_indexed_value = fun ?(text="UNK") t i ->
+  if i >= 0 then t.(i) else text
 
 (** The aircrafts store *)
 let aircrafts = Hashtbl.create 3
@@ -416,7 +416,7 @@ let send_aircraft_msg = fun ac ->
                   "energy", PprzLink.Int a.energy] in
     Ground_Pprz.message_send my_id "ENGINE_STATUS" values;
 
-    let ap_mode = get_indexed_value (modes_of_aircraft a) a.ap_mode in
+    let ap_mode = get_indexed_value ~text:(if a.ap_mode = -2 then "FAIL" else "UNK") (modes_of_aircraft a) a.ap_mode in
     let gaz_mode = get_indexed_value gaz_modes a.gaz_mode in
     let lat_mode = get_indexed_value lat_modes a.lateral_mode in
     let horiz_mode = get_indexed_value horiz_modes a.horizontal_mode in
diff --git a/sw/ground_segment/tmtc/server_globals.ml b/sw/ground_segment/tmtc/server_globals.ml
index f7600a1467..2b23c1b106 100644
--- a/sw/ground_segment/tmtc/server_globals.ml
+++ b/sw/ground_segment/tmtc/server_globals.ml
@@ -6,7 +6,7 @@ let hostname = ref "localhost"
 let port = ref 8889
 
 (** FIXME: Should be read from messages.xml *)
-let fixedwing_ap_modes = [|"MANUAL";"AUTO1";"AUTO2";"HOME";"NOGPS";"FAIL"|]
+let fixedwing_ap_modes = [|"MANUAL";"AUTO1";"AUTO2";"HOME";"NOGPS"|]
 let rotorcraft_ap_modes = [|"KILL";"SAFE";"HOME";"RATE";"ATT";"R_RCC";"A_RCC";"ATT_C";"R_ZH";"A_ZH";"HOVER";"HOV_C";"H_ZH";"NAV";"RC_D";"CF";"FWD";"MODULE";"FLIP";"GUIDED"|]
 let _AUTO2 = 2
 let gaz_modes = [|"MANUAL";"THROTTLE";"CLIMB";"ALT"|]
diff --git a/sw/lib/ocaml/gen_common.ml b/sw/lib/ocaml/gen_common.ml
index 44ef3594d8..d5fec3cda3 100644
--- a/sw/lib/ocaml/gen_common.ml
+++ b/sw/lib/ocaml/gen_common.ml
@@ -24,6 +24,7 @@
 
 open Printf
 
+exception Firmware_Found of Xml.xml
 
 (** simple boolean expressions *)
 type bool_expr =
@@ -99,17 +100,61 @@ let targets_of_field =
 
 (** [get_autopilot_of_airframe xml]
     * Returns (autopilot xml, main freq) from airframe xml file *)
-let get_autopilot_of_airframe = fun xml ->
-  (* extract all "autopilot" sections *)
-  let section = List.filter (fun s -> compare (Xml.tag s) "autopilot" = 0) (Xml.children xml) in
+let get_autopilot_of_airframe = fun ?target xml ->
+  (* first, find firmware related to the target *)
+  let firmware =
+    match target with
+    | None -> None
+    | Some t -> begin try
+        Xml.iter (fun x ->
+          if Xml.tag x = "firmware" then begin
+            Xml.iter (fun xt ->
+              if Xml.tag xt = "target" then begin
+                if Xml.attrib xt "name" = t then raise (Firmware_Found x)
+              end) x
+          end) xml;
+          None
+        with Firmware_Found f -> Some f | _ -> None
+    end
+  in
+  (* extract all autopilot node from xml tree for correct target *)
+  let rec iter = fun targets ap xml ->
+    match xml with
+    | Xml.PCData _ -> ap
+    | Xml.Element (tag, _attrs, children) when tag = "autopilot" ->
+        [Xml.Element (tag, _attrs, children)] @ ap (* found an autopilot *)
+    | Xml.Element (tag, _attrs, children) when tag = "firmware" ->
+        begin match firmware with
+        | Some f when String.compare (Xml.to_string f) (Xml.to_string xml) = 0 ->
+            List.fold_left (fun acc xml ->
+              iter targets acc xml) ap children
+        | None ->
+            List.fold_left (fun acc xml ->
+              iter targets acc xml) ap children
+        | _ -> ap end (* skip wrong firmware *)
+    | Xml.Element (tag, _attrs, children) when tag = "target" ->
+        let target_name = Xml.attrib xml "name" in
+        begin match target with
+        | None ->
+            List.fold_left
+              (fun acc xml -> iter targets acc xml) ap children
+        | Some t when t = target_name ->
+            List.fold_left
+              (fun acc xml -> iter targets acc xml) ap children
+        | _ -> ap end (* skip wrong target *)
+    | Xml.Element (tag, _attrs, children) ->
+        List.fold_left
+          (fun acc xml -> iter targets acc xml) ap children
+  in
+  let ap = iter target [] xml in
   (* Raise error if more than one modules section *)
-  match section with
+  match ap with
       [autopilot] ->
         let freq = try Some (Xml.attrib autopilot "freq") with _ -> None in
         let ap = try Xml.attrib autopilot "name" with _ -> raise Not_found in
         (autopilot_dir // ap, freq)
     | [] -> raise Not_found
-    | _ -> failwith "Error: you have more than one 'autopilot' section in your airframe file"
+    | _ -> failwith "Error: you have more than one 'autopilot' section per firmware/target in your airframe file"
 
 (** [get_targets_of_module xml]
  * Returns the boolean expression of targets of a module *)
@@ -198,7 +243,6 @@ let expand_includes = fun ac_id xml ->
         else x @ [c]
       ) [] children)
 
-exception Firmware_Found of Xml.xml
 (** [get_modules_of_airframe xml]
  * Returns a list of module configuration from airframe file *)
 let rec get_modules_of_airframe = fun ?target xml ->
@@ -265,7 +309,7 @@ let rec get_modules_of_airframe = fun ?target xml ->
   let modules = iter_modules (Var "") [] xml in
   let ap_modules =
     try
-      let ap_file = fst (get_autopilot_of_airframe xml) in
+      let ap_file = fst (get_autopilot_of_airframe ?target xml) in
       iter_modules (Var "") [] (ExtXml.parse_file ap_file)
     with _ -> [] in
   let modules = List.rev (ap_modules @ modules) in
diff --git a/sw/lib/ocaml/gen_common.mli b/sw/lib/ocaml/gen_common.mli
index 30f4f222f0..cf35671d60 100644
--- a/sw/lib/ocaml/gen_common.mli
+++ b/sw/lib/ocaml/gen_common.mli
@@ -88,11 +88,11 @@ val get_modules_name : string -> Xml.xml -> string list
  * Returns the list of modules directories *)
 val get_modules_dir : module_conf list -> string list
 
-(** [get_autopilot_of_airframe xml]
+(** [get_autopilot_of_airframe ?target xml]
  * Returns (autopilot file, main freq) from airframe xml file
  * Raise Not_found if no autopilot
  * Fail if more than one *)
-val get_autopilot_of_airframe : Xml.xml -> (string * string option)
+val get_autopilot_of_airframe : ?target:string -> Xml.xml -> (string * string option)
 
 (** [is_element_unselected target modules file]
  * Returns True if [target] is supported in the element [file] and, if it is
diff --git a/sw/misc/rctx/main_rctx.c b/sw/misc/rctx/main_rctx.c
index 175d174bfc..c1e3a69541 100644
--- a/sw/misc/rctx/main_rctx.c
+++ b/sw/misc/rctx/main_rctx.c
@@ -177,7 +177,7 @@ void event_task_rctx( void) {
 #ifdef USE_RCTX_MODE_SWITCH
     // TODO: set rxtx_mode from GPIO connected switch (e.g. I2C pins)
 #else
-    rctx_mode = PPRZ_MODE_OF_PULSE(rc_values[RADIO_MODE], 0) & 3;
+    rctx_mode = AP_MODE_OF_PULSE(rc_values[RADIO_MODE], 0) & 3;
 #endif
 
     rctx_mode |= rctx_under_voltage << 2;
diff --git a/sw/tools/generators/gen_aircraft.ml b/sw/tools/generators/gen_aircraft.ml
index 0848ab95ec..e2afd316ca 100644
--- a/sw/tools/generators/gen_aircraft.ml
+++ b/sw/tools/generators/gen_aircraft.ml
@@ -294,6 +294,12 @@ let parse_firmware = fun makefile_ac ac_id ac_xml firmware fp ->
         fprintf makefile_ac "BOARD_PROCESSOR = %s\n" proc
       with Xml.No_attribute _ -> ()
     end;
+    begin (* auto activation of generated autopilot if needed *)
+      try
+        let _ = Gen_common.get_autopilot_of_airframe ~target:target_name ac_xml in
+        fprintf makefile_ac "USE_GENERATED_AUTOPILOT = TRUE\n";
+      with Not_found -> ()
+    end;
     List.iter (configure_xml2mk makefile_ac) config;
     List.iter (configure_xml2mk makefile_ac) t_config;
     List.iter (subsystem_configure_xml2mk makefile_ac) subsystems;
diff --git a/sw/tools/generators/gen_autopilot.ml b/sw/tools/generators/gen_autopilot.ml
index 7d9253715d..f9b7f83bb9 100644
--- a/sw/tools/generators/gen_autopilot.ml
+++ b/sw/tools/generators/gen_autopilot.ml
@@ -473,7 +473,8 @@ let () =
   and h_dir = Sys.argv.(2)
   and out_set = open_out Sys.argv.(3) in
   let (autopilot, ap_freq) = try
-    Gen_common.get_autopilot_of_airframe (Xml.parse_file xml_file)
+    let target = try Some (Sys.getenv "TARGET") with _ -> None in
+    Gen_common.get_autopilot_of_airframe ?target (Xml.parse_file xml_file)
     with
       Xml.Error e -> fprintf stderr "%s: XML error:%s\n" xml_file (Xml.error e); exit 1
     | Dtd.Prove_error e -> fprintf stderr "%s: DTD error:%s\n%!" xml_file (Dtd.prove_error e); exit 1
diff --git a/sw/tools/generators/gen_settings.ml b/sw/tools/generators/gen_settings.ml
index 17ef682727..2e47ce2d21 100644
--- a/sw/tools/generators/gen_settings.ml
+++ b/sw/tools/generators/gen_settings.ml
@@ -268,7 +268,7 @@ let parse_rc_setting = fun xml ->
 
 
 let parse_rc_mode = fun xml ->
-  lprintf "if (pprz_mode == PPRZ_MODE_%s) { \\\n" (ExtXml.attrib xml "name");
+  lprintf "if (autopilot_get_mode() == AP_MODE_%s) { \\\n" (ExtXml.attrib xml "name");
   right ();
   List.iter parse_rc_setting (Xml.children xml);
   left (); lprintf "} \\\n"