[autopilot] compiling generated AP for fixedwing (#2055)

* [autopilot] compiling generated AP for fixedwing

* update some old variable names for autopilot mode

* prevent oscillation on the ground with real aircraft

* add guidance loop to 'new' control

all this guidance code should really be better factorized

* fix rotorcraft autopilot for new API

* read guidance RC if needed

* use generated autopilot as soon as an autopilot file is defined

* autopilot node at airframe, firmware or target level

The autopilot can be specific to one of the firmware, or one of the
target.
At the moment, it will fail at runtime if more than one autopilot in the
same airframe since server don't know which target is in use.

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"/>

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>

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>

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>

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>

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>

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;
 }
 

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 ?

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();
 

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 */

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 {

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

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
 

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;       \

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);
 

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;
+}
+

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
+
+

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

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.;

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);

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.);                        \

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;} \
   }
 

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

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)

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) {

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:

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;

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

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" ->

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

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"|]

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

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

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;

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;

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

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"