Merge pull request #1540 from paparazzi/flight_plan_modules

Load modules directly from a flight plan.

Mainly meant for specifying which nav modules to load directly in the flight plan.
This should make easier to set up the airframe file as it doesn't need to contain the modules needed only for some specific flight plans (where some nav routines are called that are provided by nav modules).

Makefile.ac

@@ -208,11 +208,11 @@ $(SETTINGS_H) : $(SETTINGS_XMLS_DEP) $(CONF_XML) $(SETTINGS_MODULES) $(SETTINGS_
 	$(Q)chmod a+r $@
 	$(Q)cp $(SETTINGS_XMLS_DEP) $(AIRCRAFT_CONF_DIR)/settings
 
-$(MODULES_H) : $(CONF)/$(AIRFRAME_XML) $(GENERATORS)/gen_modules.out $(CONF)/modules/*.xml
+$(MODULES_H) : $(CONF)/$(AIRFRAME_XML) $(FLIGHT_PLAN_XML) $(GENERATORS)/gen_modules.out $(CONF)/modules/*.xml
 	$(Q)test -d $(AC_GENERATED) || mkdir -p $(AC_GENERATED)
 	@echo GENERATE $@
 	$(eval $@_TMP := $(shell $(MKTEMP)))
-	$(Q)$(GENERATORS)/gen_modules.out $(SETTINGS_MODULES) $(DEFAULT_MODULES_FREQUENCY) $< > $($@_TMP)
+	$(Q)$(GENERATORS)/gen_modules.out $(SETTINGS_MODULES) $(DEFAULT_MODULES_FREQUENCY) $(FLIGHT_PLAN_XML) $< > $($@_TMP)
 	$(Q)mv $($@_TMP) $@
 	$(Q)chmod a+r $@
 

conf/airframes/examples/microjet_lisa_m.xml

@@ -51,19 +51,6 @@
       <define name="AIR_DATA_CALC_AMSL_BARO" value="TRUE"/>
     </load>
 
-    <!-- extra navigation routines -->
-    <load name="nav_bungee_takeoff.xml"/>
-    <load name="nav_line.xml"/>
-    <load name="nav_line_border.xml"/>
-    <load name="nav_line_osam.xml"/>
-    <load name="nav_flower.xml"/>
-    <load name="nav_smooth.xml"/>
-    <load name="nav_survey_polygon.xml"/>
-    <load name="nav_survey_poly_osam.xml"/>
-    <load name="nav_survey_zamboni.xml"/>
-    <load name="nav_vertical_raster.xml"/>
-    <load name="nav_spiral.xml"/>
-
     <load name="digital_cam_servo.xml">
       <define name="DC_SHUTTER_SERVO" value="COMMAND_SHUTTER" />
     </load>

conf/flight_plans/flight_plan.dtd

@@ -1,6 +1,6 @@
 <!-- Paparazzi flight plan DTD -->
 
-<!ELEMENT flight_plan (header?,waypoints,sectors?,variables?,includes?,exceptions?,blocks)>
+<!ELEMENT flight_plan (header?,waypoints,sectors?,variables?,modules?,includes?,exceptions?,blocks)>
 
 <!ELEMENT procedure (param*,header?,waypoints?,sectors?,exceptions?,blocks?)>
 
@@ -17,6 +17,11 @@
 <!ELEMENT variables (variable*)>
 <!ELEMENT variable EMPTY>
 
+<!ELEMENT modules (module*)>
+<!ELEMENT module  (configure|define)*>
+<!ELEMENT configure EMPTY>
+<!ELEMENT define EMPTY>
+
 <!ELEMENT includes (include*)>
 
 <!ELEMENT exceptions (exception*)>
@@ -104,6 +109,19 @@ alt_unit CDATA #IMPLIED
 alt_unit_coef CDATA #IMPLIED
 values CDATA #IMPLIED>
 
+<!ATTLIST modules>
+
+<!ATTLIST module
+name CDATA #REQUIRED
+type CDATA #IMPLIED>
+
+<!ATTLIST define
+name CDATA #REQUIRED
+value CDATA #IMPLIED>
+
+<!ATTLIST configure
+name CDATA #REQUIRED
+value CDATA #REQUIRED>
 
 <!ATTLIST blocks>
 

conf/flight_plans/nav_modules.xml

@@ -33,6 +33,20 @@
       <corner name="S5"/>
     </sector>
   </sectors>
+  <modules>
+    <!-- extra navigation routines -->
+    <module name="nav" type="bungee_takeoff"/>
+    <module name="nav" type="line"/>
+    <module name="nav" type="line_border"/>
+    <module name="nav" type="line_osam"/>
+    <module name="nav" type="flower"/>
+    <module name="nav" type="smooth"/>
+    <module name="nav" type="survey_polygon"/>
+    <module name="nav" type="survey_poly_osam"/>
+    <module name="nav" type="survey_zamboni"/>
+    <module name="nav" type="vertical_raster"/>
+    <module name="nav" type="spiral"/>
+  </modules>
   <exceptions/>
   <blocks>
     <block name="Wait GPS">

sw/lib/ocaml/gen_common.ml

@@ -179,6 +179,64 @@ let rec get_modules_of_airframe = fun ?target xml ->
   | None -> modules
   | Some t -> List.filter (fun m -> test_targets t m.targets) modules
 
+
+(** [get_modules_of_flight_plan xml]
+ * Returns a list of module configuration from flight plan file *)
+let get_modules_of_flight_plan = fun xml ->
+  let rec iter_modules = fun targets modules xml ->
+    match xml with
+    | Xml.PCData _ -> modules
+    | Xml.Element (tag, _attrs, children) when tag = "module" ->
+        begin try
+          let m = get_module xml targets in
+          List.fold_left
+            (fun acc xml -> iter_modules targets acc xml)
+            (m :: modules) children
+        with _ -> modules end
+    | Xml.Element (tag, _attrs, children) ->
+        List.fold_left
+          (fun acc xml -> iter_modules targets acc xml) modules children in
+  List.rev (iter_modules [] [] xml)
+
+(** [singletonize_modules xml]
+ * Returns a list of singletonized modules were options are merged
+ *)
+let singletonize_modules = fun ?(verbose=false) ?target xml ->
+  let rec loop = fun l ->
+    match l with
+    | [] | [_] -> l
+    | x::xs ->
+        let (duplicates, rest) = List.partition (fun m -> m.file = x.file) xs in
+        if List.length duplicates > 0 && verbose then begin
+          (* print info message on stderr *)
+          let t = match target with None -> "" | Some t -> Printf.sprintf " for target %s" t in
+          Printf.eprintf "Info: module '%s' has been loaded several times%s, merging options\n" x.filename t;
+          List.iter (fun opt ->
+            let name = Xml.attrib opt "name" in
+            List.iter (fun d ->
+              List.iter (fun d_opt ->
+                if Xml.attrib d_opt "name" = name then
+                  Printf.eprintf "Warning: - option '%s' is defined multiple times, this may cause unwanted behavior or compilation errors\n" name
+              ) d.param;
+            ) duplicates;
+          ) x.param;
+        end;
+        let m = { name = x.name; xml = x.xml; file = x.file; filename = x.filename;
+        vpath = x.vpath; param = List.flatten (List.map (fun m -> m.param) ([x] @ duplicates));
+        targets = singletonize (List.flatten (List.map (fun m -> m.targets) ([x] @ duplicates))) } in
+        m::loop rest
+  in
+  loop xml
+
+(** [get_modules_of_config ?target flight_plan airframe]
+ * Returns a list of pair (modules ("load" node), targets) from airframe file and flight plan.
+ * The modules are singletonized and options are merged *)
+let get_modules_of_config = fun ?target ?verbose af_xml fp_xml ->
+  let af_modules = get_modules_of_airframe ?target af_xml
+  and fp_modules = get_modules_of_flight_plan fp_xml in
+  (* singletonize modules list *)
+  singletonize_modules ?verbose ?target (af_modules @ fp_modules)
+
 (** [get_modules_name xml]
     * Returns a list of loaded modules' name *)
 let get_modules_name = fun xml ->

sw/lib/ocaml/gen_common.mli

@@ -51,6 +51,15 @@ val get_module : Xml.xml -> string list -> module_conf
  * Returns a list of pair (modules ("load" node), targets) from airframe file *)
 val get_modules_of_airframe : ?target: string -> Xml.xml -> module_conf list
 
+(** [get_modules_of_flight_plan xml]
+ * Returns a list of module configuration from flight plan file *)
+val get_modules_of_flight_plan : Xml.xml -> module_conf list
+
+(** [get_modules_of_config ?target flight_plan airframe]
+ * Returns a list of pair (modules ("load" node), targets) from airframe file and flight plan.
+ * The modules are singletonized and options are merged *)
+val get_modules_of_config : ?target:string -> ?verbose:bool -> Xml.xml -> Xml.xml -> module_conf list
+
 (** [test_targets target targets]
  * Test if [target] is allowed [targets]
  * Return true if target is allowed, false if target is not in list or rejected (prefixed by !) *)

sw/supervision/pc_aircraft.ml

@@ -165,9 +165,9 @@ let save_callback = fun ?user_save gui ac_combo tree tree_modules () ->
 type selected_t = Selected | Unselected | Unknown
 
 (* Get the settings (string list) with current modules *)
-let get_settings_modules = fun ac_xml settings_modules ->
+let get_settings_modules = fun ac_xml fp_xml settings_modules ->
   (* get modules *)
-  let modules = Gen_common.get_modules_of_airframe ac_xml in
+  let modules = Gen_common.get_modules_of_config ac_xml fp_xml in
   let modules = List.map (fun m -> m.Gen_common.xml, m.Gen_common.file ) modules in
   (* get list of settings files *)
   let settings = List.fold_left (fun l (m, f) ->
@@ -316,8 +316,10 @@ let ac_combo_handler = fun gui (ac_combo:Gtk_tools.combo) target_combo flash_com
           log (sprintf "Error airframe file not found: %s\n" x);
           Xml.Element ("airframe", [], []);
       in
+      let fp_file = (Env.paparazzi_home // "conf" // (Xml.attrib aircraft "flight_plan")) in
+      let fp_xml = Xml.parse_file fp_file in
       let settings_modules = try
-        get_settings_modules af_xml (ExtXml.attrib_or_default aircraft "settings_modules" "")
+        get_settings_modules af_xml fp_xml (ExtXml.attrib_or_default aircraft "settings_modules" "")
       with
       | Failure x -> prerr_endline x; []
       | _ -> []

sw/tools/generators/gen_aircraft.ml

@@ -145,8 +145,8 @@ let module_xml2mk = fun f target firmware m ->
         ) section
     ) m.xml
 
-let modules_xml2mk = fun f target xml ->
-  let modules = Gen_common.get_modules_of_airframe ~target xml in
+let modules_xml2mk = fun f target xml fp ->
+  let modules = Gen_common.get_modules_of_config ~target ~verbose:true xml fp in
   (* print modules directories and includes for all targets *)
   fprintf f "\n# include modules directory for all targets\n";
   (* get dir list *)
@@ -205,7 +205,7 @@ let fallback_subsys_xml2mk = fun f global_targets firmware target xml ->
     ignore(Gen_common.get_module xml global_targets)
   with Gen_common.Subsystem _file -> subsystem_xml2mk f firmware xml
 
-let parse_firmware = fun makefile_ac ac_xml firmware ->
+let parse_firmware = fun makefile_ac ac_xml firmware fp ->
   let firmware_name = Xml.attrib firmware "name" in
   (* get the configures, targets, subsystems and defines for this firmware *)
   let config, rest = ExtXml.partition_tag "configure" (Xml.children firmware) in
@@ -225,7 +225,7 @@ let parse_firmware = fun makefile_ac ac_xml firmware ->
     fprintf makefile_ac "\n###########\n# -target: '%s'\n" target_name;
     fprintf makefile_ac "ifeq ($(TARGET), %s)\n" target_name;
     let target_name = Xml.attrib target "name" in
-    let modules = modules_xml2mk makefile_ac target_name ac_xml in
+    let modules = modules_xml2mk makefile_ac target_name ac_xml fp in
     begin (* Check for "processor" attribute *)
       try
         let proc = Xml.attrib target "processor" in
@@ -252,21 +252,22 @@ let parse_firmware = fun makefile_ac ac_xml firmware ->
 
 
 (** Search and dump the firmware section *)
-let dump_firmware = fun f ac_xml firmware ->
+let dump_firmware = fun f ac_xml firmware fp ->
   try
     fprintf f "\n####################################################\n";
     fprintf f   "# makefile firmware '%s'\n" (Xml.attrib firmware "name");
     fprintf f   "####################################################\n";
-    parse_firmware f ac_xml firmware
+    parse_firmware f ac_xml firmware fp
   with Xml.No_attribute _ -> failwith "Warning: firmware name is undeclared"
 
-let dump_firmware_sections = fun makefile_ac xml ->
+let dump_firmware_sections = fun makefile_ac fp xml ->
   ExtXml.iter_tag "firmware"
-    (fun tag -> dump_firmware makefile_ac xml tag) xml
+    (fun tag -> dump_firmware makefile_ac xml tag fp) xml
 
 (** Extracts the makefile sections of an airframe file *)
-let extract_makefile = fun ac_id airframe_file makefile_ac ->
+let extract_makefile = fun ac_id airframe_file flight_plan_file makefile_ac ->
   let xml = Xml.parse_file airframe_file in
+  let fp = Xml.parse_file flight_plan_file in
   let f = open_out makefile_ac in
   fprintf f "# This file has been generated by gen_aircraft from %s by %s\n"
     airframe_file Sys.executable_name;
@@ -277,7 +278,7 @@ let extract_makefile = fun ac_id airframe_file makefile_ac ->
   (** Search and dump makefile sections that have a "location" attribute set to "before" or no attribute *)
   dump_makefile_section xml f airframe_file "before";
   (** Search and dump the firmware sections *)
-  dump_firmware_sections f xml;
+  dump_firmware_sections f fp xml;
   (** Search and dump makefile sections that have a "location" attribute set to "after" *)
   dump_makefile_section xml f airframe_file "after";
   close_out f
@@ -318,6 +319,9 @@ let () =
     let airframe_file = value "airframe" in
     let abs_airframe_file = paparazzi_conf // airframe_file in
 
+    let flight_plan_file = value "flight_plan" in
+    let abs_flight_plan_file = paparazzi_conf // flight_plan_file in
+
     mkdir (Env.paparazzi_home // "var");
     mkdir (Env.paparazzi_home // "var" // "aircrafts");
     mkdir aircraft_dir;
@@ -332,7 +336,7 @@ let () =
     mkdir (aircraft_conf_dir // "telemetry");
 
     let target = try Sys.getenv "TARGET" with _ -> "" in
-    let modules = Gen_common.get_modules_of_airframe ~target (Xml.parse_file abs_airframe_file) in
+    let modules = Gen_common.get_modules_of_config ~target (Xml.parse_file abs_airframe_file) (Xml.parse_file abs_flight_plan_file) in
     (* normal settings *)
     let settings = try Env.filter_settings (value "settings") with _ -> "" in
     (* remove settings if not supported for the current target *)
@@ -410,7 +414,7 @@ let () =
 
     let temp_makefile_ac = Filename.temp_file "Makefile.ac" "tmp" in
 
-    let () = extract_makefile (value "ac_id") abs_airframe_file temp_makefile_ac in
+    let () = extract_makefile (value "ac_id") abs_airframe_file abs_flight_plan_file temp_makefile_ac in
 
     (* Create Makefile.ac only if needed *)
     let makefile_ac = aircraft_dir // "Makefile.ac" in

sw/tools/generators/gen_modules.ml

@@ -378,9 +378,10 @@ let write_settings = fun xml_file out_set modules ->
 let h_name = "MODULES_H"
 
 let () =
-  if Array.length Sys.argv <> 4 then
-    failwith (Printf.sprintf "Usage: %s out_settings_file default_freq xml_file" Sys.argv.(0));
-  let xml_file = Sys.argv.(3)
+  if Array.length Sys.argv <> 5 then
+    failwith (Printf.sprintf "Usage: %s out_settings_file default_freq fp_file xml_file" Sys.argv.(0));
+  let xml_file = Sys.argv.(4)
+  and fp_file = Sys.argv.(3)
   and default_freq = int_of_string(Sys.argv.(2))
   and out_set = open_out Sys.argv.(1) in
   try
@@ -406,14 +407,14 @@ let () =
     let modules =
       try
         let target = Sys.getenv "TARGET" in
-        GC.get_modules_of_airframe ~target xml
+        GC.get_modules_of_config ~target xml (Xml.parse_file fp_file)
       with
       | Not_found -> failwith "TARTGET env needs to be specified to generate modules files"
     in
     (* Extract modules names (file name and module name) *)
     let modules_name =
       (List.map (fun m -> try Xml.attrib m.GC.xml "name" with _ -> "") modules) @
-        (List.map (fun m -> m.GC.filename) modules) in
+      (List.map (fun m -> m.GC.filename) modules) in
     (* Extract xml modules nodes *)
     let modules_list = List.map (fun m -> m.GC.xml) modules in
     check_dependencies modules_list modules_name;