[autopilot] dig some old code to generate autopilots

Makefile.ac

@@ -45,6 +45,8 @@ SETTINGS_MODULES=$(ACINCLUDE)/settings_modules.xml
 SETTINGS_TELEMETRY=$(ACINCLUDE)/settings_telemetry.xml
 MAKEFILE_AC=$(ACINCLUDE)/Makefile.ac
 MODULES_H=$(AC_GENERATED)/modules.h
+MODULES_DIR=$(PAPARAZZI_HOME)/conf/modules/
+AUTOPILOT_H=$(AC_GENERATED)/autopilot.h
 AIRCRAFT_MD5=$(AIRCRAFT_CONF_DIR)/aircraft.md5
 
 # "make Q=''" to get full echo
@@ -70,7 +72,7 @@ print_version:
 	@echo "-----------------------------------------------------------------------"
 
 
-all_ac_h: $(AIRFRAME_H) $(MODULES_H) $(SETTINGS_H) $(MAKEFILE_AC) $(PERIODIC_H)
+all_ac_h: $(AIRFRAME_H) $(MODULES_H) $(SETTINGS_H) $(MAKEFILE_AC) $(PERIODIC_H) $(AUTOPILOT_H)
 	@echo "TARGET: " $(TARGET)"\n" > $(ACINCLUDE)/$(TARGET)_srcs.list
 	@echo "CFLAGS: " $($(TARGET).CFLAGS)"\n"  >> $(ACINCLUDE)/$(TARGET)_srcs.list
 	@echo "LDFLAGS: " $($(TARGET).LDFLAGS)"\n"  >> $(ACINCLUDE)/$(TARGET)_srcs.list
@@ -130,6 +132,12 @@ $(MODULES_H) : $(CONF)/$(AIRFRAME_XML) $(TOOLS)/gen_modules.out $(CONF)/modules/
 	$(Q)$(TOOLS)/gen_modules.out $(SETTINGS_MODULES) $< > $@
 	$(Q)chmod a+r $@
 
+$(AUTOPILOT_H) : $(CONF)/$(AIRFRAME_XML) $(TOOLS)/gen_autopilot.out $(CONF)/autopilot/*.xml
+	$(Q)test -d $(AC_GENERATED) || mkdir -p $(AC_GENERATED)
+	@echo BUILD $@
+	$(Q)$(TOOLS)/gen_autopilot.out $(CONF)/$(AIRFRAME_XML) $@
+	$(Q)chmod a+r $@
+
 $(SETTINGS_MODULES) : $(MODULES_H)
 $(SETTINGS_TELEMETRY) : $(PERIODIC_H)
 

conf/autopilot/autopilot.dtd

@@ -0,0 +1,61 @@
+<!-- Paparazzi Autopilot DTD -->
+
+<!ELEMENT autopilot (modules*,control_block*,exceptions?,mode*)>
+<!ELEMENT control_block (call*)>
+<!ELEMENT exceptions (exception*)>
+<!ELEMENT mode (select*,control*,exception*)>
+<!ELEMENT select EMPTY>
+<!ELEMENT control (call|call_block)*>
+<!ELEMENT exception EMPTY>
+<!ELEMENT call EMPTY>
+<!ELEMENT call_block EMPTY>
+<!ELEMENT modules (load)*>
+<!ELEMENT load (configure|define)*>
+<!ELEMENT define EMPTY>
+<!ELEMENT configure EMPTY>
+
+<!ATTLIST autopilot
+name CDATA #IMPLIED>
+
+<!ATTLIST control_block
+name CDATA #REQUIRED>
+
+<!ATTLIST mode
+name CDATA #REQUIRED
+start CDATA #IMPLIED
+stop CDATA #IMPLIED>
+
+<!ATTLIST exceptions>
+
+<!ATTLIST select
+cond CDATA #REQUIRED
+exception CDATA #IMPLIED>
+
+<!ATTLIST control
+freq CDATA #REQUIRED>
+
+<!ATTLIST exception
+cond CDATA #REQUIRED
+deroute CDATA #REQUIRED>
+
+<!ATTLIST call
+fun CDATA #REQUIRED
+cond CDATA #IMPLIED>
+
+<!ATTLIST call_block
+name CDATA #REQUIRED>
+
+<!ATTLIST modules>
+
+<!ATTLIST load
+name CDATA #REQUIRED
+target CDATA #IMPLIED>
+
+<!ATTLIST define
+name CDATA #REQUIRED
+value CDATA #IMPLIED>
+
+<!ATTLIST configure
+name CDATA #REQUIRED
+value CDATA #REQUIRED>
+

conf/autopilot/booz_autopilot.xml

@@ -0,0 +1,88 @@
+<!DOCTYPE autopilot SYSTEM "autopilot.dtd">
+
+<autopilot name="Booz Quadrotor Autopilot (Basic version)">
+
+  <control_block name="attitude_loop">
+    <call fun="SetAttitudeFromRC(rc_values)"/>
+    <call fun="AddAttitudeFromFMS()" cond="fms.enabled"/>
+    <call fun="booz_stabilization_attitude_run(booz2_autopilot_in_flight)"/>
+  </control_block>
+
+  <control_block name="altitude_loop">
+    <call fun="SetAltitudeFromFMS()" cond="fms.enabled"/>
+    <call fun="b2_gv_update_ref_from_z_sp(booz2_guidance_v_z_sp)"/>
+    <call fun="run_hover_loop(booz2_autopilot_in_flight)"/>
+    <call fun="SaturateThrottle(rc_values)"/>
+  </control_block>
+
+  <exceptions>
+    <exception cond="too_far_from_home" deroute="HOME"/>
+  </exceptions>
+
+  <mode name="ATTITUDE" start="booz_stabilization_attitude_enter()">
+    <select cond="$DEFAULT_MODE"/>
+    <select cond="RCMode0()"/>
+    <control freq="512">
+      <call_block name="attitude_loop"/>
+      <call fun="SetThrottleFromRC(rc_values)"/>
+      <call fun="actuators_set(booz2_autopilot_motors_on)"/>
+    </control>
+    <exception cond="RCLost()" deroute="FAILSAFE"/>
+  </mode>
+
+  <mode name="VERTICAL" start="booz_stabilization_attitude_enter()|booz_guidance_v_enter()">
+    <select cond="RCMode1()"/>
+    <control freq="512">
+      <call_block name="attitude_loop"/>
+      <call_block name="altitude_loop"/>
+      <call fun="actuators_set(booz2_autopilot_motors_on)"/>
+    </control>
+    <exception cond="RCLost()" deroute="FAILSAFE"/>
+  </mode>
+
+  <mode name="NAV" start="booz_guidance_h_nav_enter()|booz_guidance_v_enter()">
+    <select cond="RCMode2()" exception="HOME"/>
+    <select cond="RCMode2() && DLModeNav()"/>
+    <control freq="32">
+      <call fun="nav_periodic_task()"/>
+    </control>
+    <control freq="512">
+      <call fun="SetCommandFromAP()"/>
+      <call fun="GuidanceNavHorizontal()"/>
+      <call fun="GuidanceNavVertical()"/>
+      <call fun="AddAttitudeFromRC(rc_values)" cond="!RCLost()"/>
+      <call fun="booz_stabilization_attitude_run(booz2_autopilot_in_flight)"/>
+      <call fun="SaturateThrottle(rc_values)" cond="!RCLost()"/>
+      <call fun="actuators_set(booz2_autopilot_motors_on)"/>
+      <call_block name="actuators_ap"/>
+    </control>
+    <exception cond="GPSLost()" deroute="FAILSAFE"/>
+  </mode>
+
+  <mode name="HOME" start="booz_guidance_h_nav_enter()|booz_guidance_v_enter()">
+    <control freq="32">
+      <call fun="nav_home()"/>
+    </control>
+    <control freq="512">
+      <call fun="SetCommandFromAP()"/>
+      <call fun="GuidanceNavHorizontal()"/>
+      <call fun="GuidanceNavVertical()"/>
+      <call fun="booz_stabilization_attitude_run(booz2_autopilot_in_flight)"/>
+      <call fun="actuators_set(booz2_autopilot_motors_on)"/>
+    </control>
+    <exception cond="GPSLost()" deroute="FAILSAFE"/>
+  </mode>
+
+  <!-- Safe landing -->
+  <mode name="FAILSAFE" start="failsafe_enter()|booz_stabilization_attitude_enter()|booz_guidance_v_enter()" stop="failsafe_exit()">
+    <control freq="512">
+      <call fun="SetFailsafeCommand()"/>
+      <call fun="booz_stabilization_attitude_run(booz2_autopilot_in_flight)"/>
+      <call fun="b2_gv_update_ref_from_zd_sp(booz2_guidance_v_zd_sp)"/>
+      <call fun="run_hover_loop(booz2_autopilot_in_flight)"/>
+      <call fun="actuators_set(booz2_autopilot_motors_on)"/>
+    </control>
+    <exception cond="!GPSLost()" deroute="$LAST_MODE"/>
+  </mode>
+
+</autopilot>

conf/autopilot/fw_autopilot.xml

@@ -0,0 +1,80 @@
+<!DOCTYPE autopilot SYSTEM "autopilot.dtd">
+
+<autopilot name="Fixed Wing Autopilot">
+
+  <!--modules>
+    <load name=""/>
+  </modules-->
+
+  <control_block name="actuators_ap">
+    <call fun="SetCommandsFromAP(commands)"/>
+    <call fun="SetActuatorsFromCommands(commands)"/>
+  </control_block>
+
+  <control_block name="attitude">
+    <call fun="h_ctl_attitude_loop()"/>
+    <call fun="h_ctl_throttle_slew()"/>
+  </control_block>
+
+  <exceptions>
+    <exception cond="too_far_from_home" deroute="HOME"/>
+  </exceptions>
+
+  <mode name="MANUAL">
+    <select cond="RCMode0()"/>
+    <control freq="60">
+      <call fun="SetCommandsFromRC(commands, rc_values)"/>
+      <call fun="SetActuatorsFromCommands(commands)"/>
+    </control>
+    <exception cond="RCLost()" deroute="HOME"/>
+  </mode>
+
+  <mode name="AUTO1">
+    <select cond="RCMode1()"/>
+    <control freq="60">
+      <call fun="SetAttitudeThrottleFromRC(rc_values)"/>
+      <call_block name="attitude"/>
+      <call_block name="actuators_ap"/>
+    </control>
+    <exception cond="RCLost()" 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()"/>
+    </control>
+    <control freq="60">
+      <call fun="SetApOnlyCommands(commands)"/>
+      <call fun="SetAutoCommandsFromRC(commands, rc_values)" cond="!RCLost()"/>
+      <call_block name="attitude"/>
+      <call_block name="actuators_ap"/>
+    </control>
+    <exception cond="GPSLost()" deroute="GPS_LOST"/>
+  </mode>
+
+  <mode name="HOME">
+    <control freq="4">
+      <call fun="nav_home()"/>
+    </control>
+    <control freq="60">
+      <call_block name="attitude"/>
+      <call_block name="actuators_ap"/>
+    </control>
+    <exception cond="GPSLost()" deroute="GPS_LOST"/>
+  </mode>
+
+  <mode name="GPS_LOST">
+    <control freq="4">
+      <call fun="nav_gps_lost()"/>
+    </control>
+    <control freq="60">
+      <call_block name="attitude"/>
+      <call_block name="actuators_ap"/>
+    </control>
+    <exception cond="!GPSLost()" deroute="$LAST_MODE"/>
+  </mode>
+
+</autopilot>

sw/lib/ocaml/xml2h.ml

@@ -47,6 +47,15 @@ let sprint_float_array = fun l ->
     | x::xs -> x ^","^ loop xs in
   "{" ^ loop l
 
+let start_and_begin_out = fun xml_file h_name out ->
+  let xml = Xml.parse_file xml_file in
+
+  fprintf out "/* This file has been generated from %s */\n" xml_file;
+  fprintf out "/* Please DO NOT EDIT */\n\n";
+
+  fprintf out "#ifndef %s\n" h_name;
+  fprintf out "#define %s\n\n" h_name;
+  xml
 
 let start_and_begin = fun xml_file h_name ->
   let xml = Xml.parse_file xml_file in

sw/lib/ocaml/xml2h.mli

@@ -28,6 +28,7 @@ val define : string -> string -> unit
 val define_string : string -> string -> unit
 val xml_error : string -> 'a
 val sprint_float_array : string list -> string
+val start_and_begin_out : string -> string -> out_channel -> Xml.xml
 val start_and_begin : string -> string -> Xml.xml
 val start_and_begin_c : string -> string -> Xml.xml
 val begin_c_out : string -> string -> out_channel -> unit

sw/tools/Makefile

@@ -31,7 +31,7 @@ OCAMLNETINCLUDES=$(shell ocamlfind query -r -i-format netstring) $(shell ocamlfi
 OCAMLNETCMA=$(shell ocamlfind query -r -a-format -predicates byte netstring) $(shell ocamlfind query -r -a-format -predicates byte netclient)
 LIBPPRZCMA=$(LIBPPRZDIR)/lib-pprz.cma
 
-all: gen_common.cmo gen_aircraft.out gen_airframe.out gen_messages2.out gen_messages.out gen_ubx.out gen_mtk.out gen_flight_plan.out gen_radio.out gen_periodic.out gen_settings.out gen_xsens.out gen_modules.out gen_abi.out find_free_msg_id.out gen_srtm.out mergelogs
+all: gen_common.cmo gen_aircraft.out gen_airframe.out gen_messages2.out gen_messages.out gen_ubx.out gen_mtk.out gen_flight_plan.out gen_radio.out gen_periodic.out gen_settings.out gen_xsens.out gen_modules.out gen_autopilot.out gen_abi.out find_free_msg_id.out gen_srtm.out mergelogs
 
 FP_CMO = fp_proc.cmo gen_flight_plan.cmo
 ABS_FP = $(FP_CMO:%=$$PAPARAZZI_SRC/sw/tools/%)

sw/tools/gen_autopilot.ml

@@ -0,0 +1,342 @@
+(*
+ * $Id: gen_modules.ml 4538 2010-02-04 09:45:08Z gautier $
+ *
+ * XML preprocessing for core autopilot
+ *  
+ * Copyright (C) 2010 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA. 
+ *
+ *)
+
+open Printf
+open Xml2h
+
+let (//) = Filename.concat
+
+let paparazzi_conf = Env.paparazzi_home // "conf"
+let autopilot_dir = paparazzi_conf // "autopilot"
+
+(** Formatting with a margin *)
+let margin = ref 0
+let step = 2
+
+let right () = margin := !margin + step
+let left () = margin := !margin - step
+
+let lprintf = fun out f ->
+  fprintf out "%s" (String.make !margin ' ');
+  fprintf out f
+
+let get_ap_modes = fun ap ->
+  List.filter (fun m -> (Xml.tag m) = "mode") (Xml.children ap)
+
+let get_ap_control_block = fun ap ->
+  List.filter (fun m -> (Xml.tag m) = "control_block") (Xml.children ap)
+
+let get_ap_exceptions = fun ap ->
+  try ExtXml.child ap "exceptions"
+  with _ -> Xml.Element ("exceptions", [], [])
+
+let get_mode_exceptions = fun mode ->
+  List.filter (fun m -> (Xml.tag m) = "exception") (Xml.children mode)
+
+let print_mode_name = fun name ->
+  String.concat "" ["AUTOPILOT_MODE_"; (String.uppercase name)]
+
+(** Define modes *)
+let print_modes = fun modes out_h ->
+  let mode_idx = ref 0 in
+  List.iter (fun m ->
+    try
+      let name = Xml.attrib m "name" in
+      lprintf out_h "#define %s %d\n" (print_mode_name name) !mode_idx;
+      mode_idx := !mode_idx + 1;
+    with _ -> ()
+    )
+  modes
+
+(** Init function: set last_mode to initial ap_mode *) (* TODO really needed ? *)
+let print_ap_init = fun modes out_h ->
+
+  let find_default_mode = fun _ ->
+    let default = List.find_all (fun m ->
+      List.exists (fun s -> if Xml.tag s = "select" then Xml.attrib s "cond" = "$DEFAULT_MODE" else false) (Xml.children m)
+    ) modes in
+    match List.length default with
+      0 -> List.hd modes
+    | 1 -> List.hd default
+    | _ -> failwith "Autopilot Core Error: only one default mode can be set"
+  in
+
+  let default = find_default_mode () in
+  lprintf out_h "\nstatic inline void autopilot_core_init(void) {\n";
+  right ();
+  lprintf out_h "autopilot_mode = %s;\n" (print_mode_name (Xml.attrib default "name"));
+  lprintf out_h "private_autopilot_mode = autopilot_mode;\n";
+  lprintf out_h "last_autopilot_mode = autopilot_mode;\n";
+  left ();
+  lprintf out_h "}\n"
+
+(** Function to test if a mode is selected *)
+let print_test_select = fun modes out_h ->
+  lprintf out_h "\nstatic inline uint8_t autopilot_core_mode_select(void) {\n";
+  right ();
+  List.iter (fun m -> (* Test select for all modes *)
+    let select = List.filter (fun s -> Xml.tag s = "select") (Xml.children m) in
+    List.iter (fun s -> (* In each mode, build condition and exceptions' list *)
+      let cond = Xml.attrib s "cond" in
+      let except = try String.concat " || "
+        (List.map (fun e -> Printf.sprintf "private_autopilot_mode != %s" (print_mode_name e))
+          (Str.split (Str.regexp "|") (Xml.attrib s "exception"))
+        ) with _ -> "" in
+      match (cond, String.length except) with
+        ("$DEFAULT_MODE", _) -> ()
+      | (_, 0) -> lprintf out_h "if (%s) { return %s; }\n" cond (print_mode_name (Xml.attrib m "name"))
+      | (_, _) -> lprintf out_h "if ((%s) && (%s)) { return %s; }\n" cond except (print_mode_name (Xml.attrib m "name"))
+    ) select;
+  ) modes;
+  lprintf out_h "return private_autopilot_mode;\n";
+  left ();
+  lprintf out_h "}\n"
+
+
+(** Function to test exceptions on modes
+ *  The generated function returns the new mode if an exception is true
+ *)
+let print_test_exception = fun modes out_h ->
+  (** Test condition and deroute to given mode or last mode *)
+  let print_exception = fun ex ->
+    let name = Xml.attrib ex "deroute"
+    and cond = Xml.attrib ex "cond" in
+    match name with
+      "$LAST_MODE" -> lprintf out_h "if (%s) { return last_autopilot_mode; }\n" cond
+    | _ -> lprintf out_h "if (%s) { return %s; }\n" cond (print_mode_name name)
+  in
+
+  lprintf out_h "\nstatic inline uint8_t autopilot_core_mode_exceptions(uint8_t mode) {\n";
+  right ();
+  lprintf out_h "switch ( mode ) { \n";
+  right ();
+  List.iter (fun m -> (* Test exceptions for all modes *)
+    lprintf out_h "case %s :\n" (print_mode_name (Xml.attrib m "name"));
+    right ();
+    lprintf out_h "{\n";
+    right ();
+    let exceptions = get_mode_exceptions m in
+    List.iter (fun e ->
+      print_exception e
+    ) exceptions;
+    left ();
+    lprintf out_h "}\n";
+    lprintf out_h "break;\n";
+    left ()
+  ) modes;
+  left ();
+  lprintf out_h "}\n";
+  lprintf out_h "return mode;\n";
+  left ();
+  lprintf out_h "}\n"
+
+(** Function to test global exceptions
+ *  The generated function returns the mode of the last true exception in the list
+ *)
+let print_global_exceptions = fun exceptions out_h ->
+  lprintf out_h "\nstatic inline uint8_t autopilot_core_global_exceptions(uint8_t mode) {\n";
+  right ();
+  List.iter (fun ex -> lprintf out_h "if (%s) { mode = %s; }\n" (Xml.attrib ex "cond") (print_mode_name (Xml.attrib ex "deroute")))
+    (Xml.children exceptions);
+  lprintf out_h "return mode;\n";
+  left ();
+  lprintf out_h "}\n"
+
+(** Set mode by calling start and stop functions if needed *)
+let print_set_mode = fun modes out_h ->
+
+  let print_case = fun mode f ->
+    lprintf out_h "case %s :\n" (print_mode_name (Xml.attrib mode "name"));
+    right ();
+    List.iter (fun x -> lprintf out_h "%s;\n" x) (Str.split (Str.regexp "|") f);
+    lprintf out_h "break;\n";
+    left ();
+  in
+  let print_switch = fun var modes t ->
+    lprintf out_h "switch ( %s ) { \n" var;
+    right ();
+    List.iter (fun m ->
+      try
+        let stop = Xml.attrib m t in
+        print_case m stop
+      with _ -> ()
+    ) modes;
+    left ();
+    lprintf out_h "}\n"
+  in
+
+  lprintf out_h "\nstatic inline void autopilot_core_set_mode(uint8_t new_mode) {\n\n";
+  right ();
+  lprintf out_h "if (new_mode == private_autopilot_mode) return;\n\n"; (* set mode if different from current mode *)
+  (* Print stop functions for each modes *)
+  print_switch "private_autopilot_mode" modes "stop";
+  lprintf out_h "\n";
+  (* Print start functions for each modes *)
+  print_switch "new_mode" modes "start";
+  lprintf out_h "\n";
+  lprintf out_h "last_autopilot_mode = private_autopilot_mode;\n";
+  lprintf out_h "private_autopilot_mode = new_mode;\n";
+  lprintf out_h "autopilot_mode = new_mode;\n";
+  left ();
+  lprintf out_h "}\n"
+
+
+(** Peridiodic function: calls control loops according to the ap_mode *)
+let print_ap_periodic = fun modes ctrl_block main_freq out_h ->
+
+ (** Print function *)
+  let print_call = fun call ->
+    try
+      let f = Xml.attrib call "fun" in
+      let cond = try String.concat "" ["if ("; (Xml.attrib call "cond"); ") { "; f; "; }\n"]
+           with _ -> String.concat "" [f; ";\n"] in
+      lprintf out_h "%s" cond
+    with _ -> ()
+  in
+  (** Print a control block *)
+  let print_ctrl = fun ctrl ->
+    List.iter (fun c ->
+      match (Xml.tag c) with
+        "call" -> print_call c
+      | "call_block" -> List.iter print_call (Xml.children (List.find (fun n -> (Xml.attrib c "name") = (Xml.attrib n "name")) ctrl_block))
+      | _ -> ()
+    ) (Xml.children ctrl)
+  in
+  (** Equivalent to the RunOnceEvery macro *)
+  let print_prescaler = fun pre ctrl ->
+    lprintf out_h "{ \n";
+    right ();
+    lprintf out_h "static uint16_t prescaler = 0;\n";
+    lprintf out_h "prescaler++;\n";
+    lprintf out_h "if (prescaler >= %d) {\n" pre;
+    right ();
+    lprintf out_h "prescaler = 0;\n";
+    print_ctrl ctrl;
+    left ();
+    lprintf out_h "}\n";
+    left ();
+    lprintf out_h "}\n"
+  in
+  let get_control = fun mode ->
+    List.filter (fun m -> (Xml.tag m) = "control") (Xml.children mode)
+  in
+
+  (** Start printing the main periodic task *)
+  lprintf out_h "\nstatic inline void autopilot_core_periodic_task(void) {\n\n";
+  right ();
+  lprintf out_h "uint8_t mode = autopilot_core_mode_select();\n"; (* get selected mode *)
+  lprintf out_h "mode = autopilot_core_mode_exceptions(mode);\n"; (* change mode according to exceptions *)
+  lprintf out_h "mode = autopilot_core_global_exceptions(mode);\n"; (* change mode according to global exceptions *)
+  lprintf out_h "autopilot_core_set_mode(mode);\n\n"; (* set new mode and call start/stop functions *)
+  lprintf out_h "switch ( private_autopilot_mode ) { \n";
+  right ();
+  List.iter (fun m -> (* Print control loops for each modes *)
+    lprintf out_h "case %s :\n" (print_mode_name (Xml.attrib m "name"));
+    right ();
+    lprintf out_h "{\n";
+    right ();
+    List.iter (fun c -> (* Look for control loops *)
+      let ctrl_freq = try int_of_string (Xml.attrib c "freq") with _ -> main_freq in
+      let prescaler = main_freq / ctrl_freq in
+      match prescaler with
+        0 -> failwith "Autopilot Core Error: control freq higher than main freq"
+      | 1 -> print_ctrl c (* no prescaler if running at main_freq *)
+      | _ -> print_prescaler prescaler c
+    ) (get_control m);
+    left ();
+    lprintf out_h "}\n";
+    lprintf out_h "break;\n\n";
+    left ();
+  ) modes;
+  left ();
+  lprintf out_h "}\n";
+  left ();
+  lprintf out_h "}\n"
+
+(** Parse config file *)
+let parse_modes ap freq out_h =
+  let modes = get_ap_modes ap in
+  let ctrl_block = get_ap_control_block ap in
+  print_modes modes out_h;
+  fprintf out_h "\nEXTERN_AP uint8_t autopilot_mode;\n";
+  fprintf out_h "\n#ifdef AUTOPILOT_CORE_C\n";
+  fprintf out_h "\n#include \"modules.h\"\n";
+  fprintf out_h "uint8_t private_autopilot_mode;\n";
+  fprintf out_h "uint8_t last_autopilot_mode;\n\n";
+  print_ap_init modes out_h;
+  print_test_select modes out_h;
+  print_test_exception modes out_h;
+  print_global_exceptions (get_ap_exceptions ap) out_h;
+  print_set_mode modes out_h;
+  print_ap_periodic modes ctrl_block freq out_h;
+  fprintf out_h "\n#endif // AUTOPILOT_CORE_C\n"
+
+let h_name = "AUTOPILOT_CORE_H"
+
+(** Main generation function
+  * Usage: main_freq xml_file_input h_file_output
+  *)
+let gen_autopilot main_freq xml_file h_file =
+  let out_h = open_out h_file in
+  try
+    let ap_xml = start_and_begin_out xml_file h_name out_h in
+    let _ = try
+      let ap_name = Xml.attrib ap_xml "name" in
+      fprintf out_h "/*** %s ***/\n\n" ap_name;
+    with _ -> () in
+    fprintf out_h "#ifdef AUTOPILOT_CORE_C\n";
+    fprintf out_h "#define EXTERN_AP\n";
+    fprintf out_h "#else\n";
+    fprintf out_h "#define EXTERN_AP extern\n";
+    fprintf out_h "#endif\n";
+    parse_modes ap_xml main_freq out_h;
+    fprintf out_h "\n#endif // %s\n" h_name;
+    close_out out_h
+  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
+  | Dtd.Check_error e -> fprintf stderr "%s: DTD error:%s\n%!" xml_file (Dtd.check_error e); exit 1
+  | Dtd.Parse_error e -> fprintf stderr "%s: DTD error:%s\n%!" xml_file (Dtd.parse_error e); exit 1
+
+(* Main call *)
+let () =
+  if Array.length Sys.argv <> 3 then
+    failwith (Printf.sprintf "Usage: %s airframe_xml_file out_h_file" Sys.argv.(0));
+  let xml_file = Sys.argv.(1)
+  and h_file = Sys.argv.(2) in
+  try
+    let xml = Xml.parse_file xml_file in
+    let (autopilot, ap_freq) = Gen_common.get_autopilot_of_airframe xml in
+    gen_autopilot ap_freq autopilot h_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
+  | Dtd.Check_error e -> fprintf stderr "%s: DTD error:%s\n%!" xml_file (Dtd.check_error e); exit 1
+  | Dtd.Parse_error e -> fprintf stderr "%s: DTD error:%s\n%!" xml_file (Dtd.parse_error e); exit 1
+  | Not_found -> let out_h = open_out h_file in close_out out_h; exit 0
+
+

sw/tools/gen_common.ml

@@ -30,8 +30,10 @@ let (//) = Filename.concat
 
 let paparazzi_conf = Env.paparazzi_home // "conf"
 let modules_dir = paparazzi_conf // "modules"
+let autopilot_dir = paparazzi_conf // "autopilot"
 
 let default_module_targets = "ap|sim"
+let default_freq = 60
 
 (** remove all duplicated elements of a list *)
 let singletonize = fun l ->
@@ -65,7 +67,7 @@ let targets_of_field = fun field default ->
 
 (** [get_modules_of_airframe xml]
  * Returns a list of module configuration from airframe file *)
-let get_modules_of_airframe = fun xml ->
+let rec get_modules_of_airframe = fun xml ->
   (* extract all "modules" sections *)
   let section = List.filter (fun s -> compare (Xml.tag s) "modules" = 0) (Xml.children xml) in
   (* Raise error if more than one modules section *)
@@ -73,12 +75,20 @@ let get_modules_of_airframe = fun xml ->
     [modules] ->
       (* if only one section, returns a list of configuration *)
       let t_global = targets_of_field modules "" in
-      List.map (fun m ->
-        if compare (Xml.tag m) "load" <> 0 then Xml2h.xml_error "load";
+      let get_module = fun m t ->
         let file = modules_dir // ExtXml.attrib m "name" in
-        let targets = singletonize (t_global @ targets_of_field m "") in
+        let targets = singletonize (t @ targets_of_field m "") in
         { xml = ExtXml.parse_file file; file = file; param = Xml.children m; extra_targets = targets }
-      ) (Xml.children modules)
+      in
+      List.flatten (List.map (fun m ->
+        if compare (Xml.tag m) "load" <> 0 then Xml2h.xml_error "load";
+        let airframe_module = [get_module m t_global] in
+        let ap_module = try
+          let ap_file = autopilot_dir // ExtXml.attrib m "autopilot" in
+          get_modules_of_airframe (ExtXml.parse_file ap_file)
+        with _ -> [] in
+        List.flatten [airframe_module @ ap_module]
+      ) (Xml.children modules))
   | [] -> []
   | _ -> failwith "Error: you have more than one 'modules' section in your airframe file"
 
@@ -126,3 +136,17 @@ let get_modules_dir = fun modules ->
   let dir = List.map (fun m -> try Xml.attrib m.xml "dir" with _ -> ExtXml.attrib m.xml "name") modules in
   singletonize (List.sort compare dir)
 
+(** [get_autopilot_of_airframe xml]
+ * Returns (autopilot xml, main freq) from airframe xml file *)
+let get_autopilot_of_airframe = fun xml ->
+  (* extract all "modules" sections *)
+  let section = List.filter (fun s -> compare (Xml.tag s) "modules" = 0) (Xml.children xml) in
+  (* Raise error if more than one modules section *)
+  match section with
+    [modules] ->
+      let main_freq = try int_of_string (Xml.attrib modules "main_freq") with _ -> default_freq in
+      let ap = try Xml.attrib modules "autopilot" with _ -> raise Not_found in
+      (autopilot_dir // ap, main_freq)
+  | [] -> raise Not_found
+  | _ -> failwith "Error: you have more than one 'modules' section in your airframe file"
+

sw/tools/gen_common.mli

@@ -35,6 +35,9 @@ val modules_dir : string
 (* Default targets for modules *)
 val default_module_targets : string
 
+(* Default AP freq *)
+val default_freq : int
+
 (** remove all duplicated elements of a list *)
 val singletonize : 'a list -> 'a list
 
@@ -63,3 +66,9 @@ val get_modules_name : Xml.xml -> string list
  * Returns the list of modules directories *)
 val get_modules_dir : module_conf list -> string list
 
+(** [get_autopilot_of_airframe 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 * int)
+