From 09c0c8ccb914a5711f3b643f34f4c8aacb3d0e76 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Sun, 5 Jul 2020 21:38:09 +0200 Subject: [PATCH] Aircraft generator (#2545) * [ocaml] massive update of the build process convert individual code generator to a single process, parsing everything once and calling the required generators - remove all subsystems makefiles - fix module name - example of radio file without ctl attribute * [build] start with flight plan to make semaphore happy * [ocaml] move the buffer outside the lazy block for some reason, it seems to make problems with compilation server Semaphore it really makes no sense, but nevermind... --- Makefile.ac | 113 +- conf/airframes/airframe.dtd | 37 +- conf/autopilot/autopilot.dtd | 7 +- .../fixedwing/navigation_extra.makefile | 7 - .../rotorcraft/gps_datalink.makefile | 1 - .../rotorcraft/gps_sim_hitl.makefile | 1 - .../subsystems/rotorcraft/gps_sirf.makefile | 1 - .../subsystems/rotorcraft/gps_udp.makefile | 1 - .../subsystems/shared/gps_furuno.makefile | 1 - .../shared/gps_mediatek_diy.makefile | 1 - .../subsystems/shared/gps_nmea.makefile | 1 - .../subsystems/shared/gps_piksi.makefile | 1 - .../subsystems/shared/gps_skytraq.makefile | 1 - .../subsystems/shared/gps_ublox.makefile | 1 - .../subsystems/shared/gps_ublox_utm.makefile | 1 - .../subsystems/shared/sdlog.makefile | 1 - conf/modules/nav_line.xml | 2 +- conf/radios/T14SG_SBUS.xml | 26 +- conf/radios/cockpitMM.xml | 14 +- conf/radios/radio.dtd | 5 +- conf/settings/settings.dtd | 22 +- conf/telemetry/telemetry.dtd | 3 +- .../subsystems/radio_control/spektrum_arch.c | 1 + .../subsystems/radio_control/spektrum_arch.h | 2 - sw/ground_segment/cockpit/live.ml | 2 +- sw/ground_segment/tmtc/Makefile | 8 +- .../tmtc/{aircraft.ml => aircraft_server.ml} | 0 .../{aircraft.mli => aircraft_server.mli} | 0 sw/ground_segment/tmtc/airprox.ml | 2 +- sw/ground_segment/tmtc/airprox.mli | 2 +- sw/ground_segment/tmtc/kml.ml | 2 +- sw/ground_segment/tmtc/kml.mli | 8 +- sw/ground_segment/tmtc/parse_messages_v1.ml | 18 +- sw/ground_segment/tmtc/parse_messages_v1.mli | 2 +- sw/ground_segment/tmtc/server.ml | 23 +- .../tmtc/{settings.ml => settings_gui.ml} | 0 sw/lib/ocaml/Makefile | 3 +- sw/lib/ocaml/aircraft.ml | 379 +++++++ sw/lib/ocaml/airframe.ml | 160 +++ sw/lib/ocaml/autopilot.ml | 109 ++ sw/lib/ocaml/egm96.ml | 4 +- sw/lib/ocaml/env.ml | 128 +-- sw/lib/ocaml/env.mli | 10 +- sw/lib/ocaml/extXml.ml | 26 +- sw/lib/ocaml/extXml.mli | 15 +- sw/lib/ocaml/flight_plan.ml | 91 ++ sw/lib/ocaml/gen_common.ml | 384 +------ sw/lib/ocaml/gen_common.mli | 65 +- sw/lib/ocaml/module.ml | 365 +++++++ sw/lib/ocaml/ocaml_tools.ml | 18 + sw/lib/ocaml/ocaml_tools.mli | 26 +- sw/lib/ocaml/radio.ml | 85 ++ sw/lib/ocaml/settings.ml | 136 +++ sw/lib/ocaml/telemetry.ml | 145 +++ sw/lib/ocaml/xml2h.ml | 23 +- sw/lib/ocaml/xml2h.mli | 9 +- sw/logalizer/sd2log.ml | 4 +- sw/simulator/data.ml | 2 +- sw/supervision/pc_aircraft.ml | 22 +- sw/tools/generators/Makefile | 11 +- sw/tools/generators/dump_flight_plan.ml | 35 + sw/tools/generators/gen_aircraft.ml | 652 +++++------- sw/tools/generators/gen_airframe.ml | 274 +++-- sw/tools/generators/gen_autopilot.ml | 108 +- sw/tools/generators/gen_flight_plan.ml | 997 +++++++++--------- sw/tools/generators/gen_makefile.ml | 167 +++ sw/tools/generators/gen_modules.ml | 340 +++--- sw/tools/generators/gen_periodic.ml | 102 +- sw/tools/generators/gen_radio.ml | 175 ++- sw/tools/generators/gen_settings.ml | 334 ++---- 70 files changed, 3168 insertions(+), 2554 deletions(-) delete mode 100644 conf/firmwares/subsystems/fixedwing/navigation_extra.makefile delete mode 100644 conf/firmwares/subsystems/rotorcraft/gps_datalink.makefile delete mode 100644 conf/firmwares/subsystems/rotorcraft/gps_sim_hitl.makefile delete mode 100644 conf/firmwares/subsystems/rotorcraft/gps_sirf.makefile delete mode 100644 conf/firmwares/subsystems/rotorcraft/gps_udp.makefile delete mode 100644 conf/firmwares/subsystems/shared/gps_furuno.makefile delete mode 100644 conf/firmwares/subsystems/shared/gps_mediatek_diy.makefile delete mode 100644 conf/firmwares/subsystems/shared/gps_nmea.makefile delete mode 100644 conf/firmwares/subsystems/shared/gps_piksi.makefile delete mode 100644 conf/firmwares/subsystems/shared/gps_skytraq.makefile delete mode 100644 conf/firmwares/subsystems/shared/gps_ublox.makefile delete mode 100644 conf/firmwares/subsystems/shared/gps_ublox_utm.makefile delete mode 100644 conf/firmwares/subsystems/shared/sdlog.makefile rename sw/ground_segment/tmtc/{aircraft.ml => aircraft_server.ml} (100%) rename sw/ground_segment/tmtc/{aircraft.mli => aircraft_server.mli} (100%) rename sw/ground_segment/tmtc/{settings.ml => settings_gui.ml} (100%) create mode 100644 sw/lib/ocaml/aircraft.ml create mode 100644 sw/lib/ocaml/airframe.ml create mode 100644 sw/lib/ocaml/autopilot.ml create mode 100644 sw/lib/ocaml/flight_plan.ml create mode 100644 sw/lib/ocaml/module.ml create mode 100644 sw/lib/ocaml/radio.ml create mode 100644 sw/lib/ocaml/settings.ml create mode 100644 sw/lib/ocaml/telemetry.ml create mode 100644 sw/tools/generators/dump_flight_plan.ml create mode 100644 sw/tools/generators/gen_makefile.ml diff --git a/Makefile.ac b/Makefile.ac index c5116ac7ab..6ff732697c 100644 --- a/Makefile.ac +++ b/Makefile.ac @@ -31,7 +31,6 @@ CONF=$(PAPARAZZI_HOME)/conf VAR=$(PAPARAZZI_HOME)/var CONF_XML ?= $(CONF)/conf.xml AIRBORNE=sw/airborne -MESSAGES_XML = $(VAR)/messages.xml # make sure the TARGET variable is set if needed for current make target ifneq (,$(findstring $(MAKECMDGOALS),all_ac_h radio_ac_h flight_plan_ac_h)) @@ -50,35 +49,10 @@ endif AIRCRAFT_CONF_DIR = $(AIRCRAFT_BUILD_DIR)/conf AC_GENERATED = $(AIRCRAFT_BUILD_DIR)/$(TARGET)/generated -AIRFRAME_H=$(AC_GENERATED)/airframe.h -PERIODIC_H=$(AC_GENERATED)/periodic_telemetry.h -RADIO_H=$(AC_GENERATED)/radio.h -FLIGHT_PLAN_H=$(AC_GENERATED)/flight_plan.h -FLIGHT_PLAN_XML=$(AIRCRAFT_BUILD_DIR)/flight_plan.xml SRCS_LIST=$(AIRCRAFT_BUILD_DIR)/$(TARGET)_srcs.list TMP_LIST=$(AIRCRAFT_BUILD_DIR)/$(TARGET)_tmp.list -SETTINGS_H=$(AC_GENERATED)/settings.h -SETTINGS_XMLS=$(patsubst %,$(CONF)/%,$(SETTINGS)) -SETTINGS_XMLS_DEP=$(filter-out %~,$(SETTINGS_XMLS)) -SETTINGS_XML=$(AIRCRAFT_BUILD_DIR)/settings.xml -SETTINGS_MODULES=$(AIRCRAFT_BUILD_DIR)/settings_modules.xml -SETTINGS_TELEMETRY=$(AIRCRAFT_BUILD_DIR)/settings_telemetry.xml -SETTINGS_AUTOPILOT=$(AIRCRAFT_BUILD_DIR)/settings_autopilot.xml -SETTINGS_FLIGHTPLAN=$(AIRCRAFT_BUILD_DIR)/settings_flightplan.xml -MAKEFILE_AC=$(AIRCRAFT_BUILD_DIR)/Makefile.ac -MODULES_H=$(AC_GENERATED)/modules.h -MODULES_DIR=$(PAPARAZZI_HOME)/conf/modules/ -AUTOPILOT_DIR=$(AC_GENERATED)/ -AIRCRAFT_MD5=$(AIRCRAFT_CONF_DIR)/aircraft.md5 GENERATE_KEYS ?= 0 -UNAME = $(shell uname -s) -ifeq ("$(UNAME)","Darwin") - MKTEMP = gmktemp -else - MKTEMP = mktemp -endif - # By default, detect number of processors for parallel compilation # same as passing J=AUTO from toplevel make. # Number of processes can also be explicitly set with e.g. J=4 @@ -155,7 +129,7 @@ print_version: all_ac_h: $(SRCS_LIST) qt_project generate_keys build_rust_modules -$(SRCS_LIST) : $(CONF_XML) $(AIRFRAME_H) $(MODULES_H) $(SETTINGS_H) $(MAKEFILE_AC) $(PERIODIC_H) +$(SRCS_LIST): @echo "TARGET: " $(TARGET) > $(SRCS_LIST) @echo "CFLAGS: " $(CFLAGS) $(IINCDIR) $(TOPT) >> $(SRCS_LIST) @echo "LDFLAGS: " $($(TARGET).LDFLAGS) >> $(SRCS_LIST) @@ -223,105 +197,26 @@ ifneq ($(PAPARAZZI_QT_GEN),) $(Q)./sw/tools/qt_project.py $(AIRCRAFT) $(CONF_XML) $(SRCS_LIST) endif -radio_ac_h : $(RADIO_H) - -flight_plan_ac_h : $(FLIGHT_PLAN_H) $(FLIGHT_PLAN_XML) - -makefile_ac: $(MAKEFILE_AC) - -$(AIRFRAME_H) : $(CONF)/$(AIRFRAME_XML) $(CONF_XML) $(AIRCRAFT_MD5) $(GENERATORS)/gen_airframe.out $(GENERATORS)/gen_autopilot.out $(CONF)/autopilot/*.xml - $(Q)test -d $(AC_GENERATED) || mkdir -p $(AC_GENERATED) - @echo GENERATE $@ from $(AIRFRAME_XML) - $(eval $@_TMP := $(shell $(MKTEMP))) - $(Q)$(GENERATORS)/gen_airframe.out $(AC_ID) $(AIRCRAFT) $(MD5SUM) $< > $($@_TMP) - $(Q)mv $($@_TMP) $@ - $(Q)chmod a+r $@ - $(Q)cp $(CONF)/airframes/airframe.dtd $(AIRCRAFT_CONF_DIR)/airframes - @echo GENERATE autopilots in $(AUTOPILOT_DIR) - $(Q)$(GENERATORS)/gen_autopilot.out $(CONF)/$(AIRFRAME_XML) $(AUTOPILOT_DIR) $(SETTINGS_AUTOPILOT) - -$(RADIO_H) : $(CONF)/$(RADIO) $(CONF_XML) $(GENERATORS)/gen_radio.out - $(Q)test -d $(AC_GENERATED) || mkdir -p $(AC_GENERATED) - @echo GENERATE $@ from $(RADIO) - $(eval $@_TMP := $(shell $(MKTEMP))) - $(Q)$(GENERATORS)/gen_radio.out $< > $($@_TMP) - $(Q)mv $($@_TMP) $@ - $(Q)chmod a+r $@ - $(Q)cp $< $(AIRCRAFT_CONF_DIR)/radios - -$(PERIODIC_H) : $(MESSAGES_XML) $(CONF_XML) $(CONF)/$(TELEMETRY) $(MAKEFILE_AC) - $(Q)test -d $(AC_GENERATED) || mkdir -p $(AC_GENERATED) - @echo GENERATE $@ from $(TELEMETRY) - $(eval $@_TMP := $(shell $(MKTEMP))) - $(Q)$(GENERATORS)/gen_periodic.out $(MESSAGES_XML) $(CONF)/$(TELEMETRY) $(TELEMETRY_FREQUENCY) $(SETTINGS_TELEMETRY) > $($@_TMP) - $(Q)mv $($@_TMP) $@ - $(Q)chmod a+r $@ - $(Q)cp $(CONF)/$(TELEMETRY) $(AIRCRAFT_CONF_DIR)/telemetry - -$(FLIGHT_PLAN_H) : $(CONF)/$(FLIGHT_PLAN) $(CONF_XML) $(GENERATORS)/gen_flight_plan.out - $(Q)test -d $(AC_GENERATED) || mkdir -p $(AC_GENERATED) - @echo GENERATE $@ from $(FLIGHT_PLAN) - $(eval $@_TMP := $(shell $(MKTEMP))) - $(Q)$(GENERATORS)/gen_flight_plan.out -settings $(SETTINGS_FLIGHTPLAN) $< > $($@_TMP) - $(Q)mv $($@_TMP) $@ - $(Q)chmod a+r $@ - $(Q)cp $< $(AIRCRAFT_CONF_DIR)/flight_plans - -$(FLIGHT_PLAN_XML) : $(CONF)/$(FLIGHT_PLAN) $(CONF_XML) $(GENERATORS)/gen_flight_plan.out - @echo GENERATE $@ from $(FLIGHT_PLAN) - $(eval $@_TMP := $(shell $(MKTEMP))) - $(Q)$(GENERATORS)/gen_flight_plan.out -dump $< > $($@_TMP) - $(Q)mv $($@_TMP) $@ - $(Q)chmod a+r $@ - -# split system settings (generated) from user settings with a '--' -$(SETTINGS_H) : $(SETTINGS_XMLS_DEP) $(CONF_XML) $(SETTINGS_MODULES) $(SETTINGS_TELEMETRY) $(SETTINGS_AUTOPILOT) $(GENERATORS)/gen_settings.out - $(Q)test -d $(AC_GENERATED) || mkdir -p $(AC_GENERATED) - @echo GENERATE $@ - $(eval $@_TMP := $(shell $(MKTEMP))) - $(Q)$(GENERATORS)/gen_settings.out $(SETTINGS_XML) $(SETTINGS_AUTOPILOT) $(SETTINGS_TELEMETRY) $(SETTINGS_FLIGHTPLAN) $(SETTINGS_MODULES) -- $(SETTINGS_XMLS) > $($@_TMP) - $(Q)mv $($@_TMP) $@ - $(Q)chmod a+r $@ - $(Q)cp $(SETTINGS_XMLS_DEP) $(AIRCRAFT_CONF_DIR)/settings - -$(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 $(AC_ID) $(SETTINGS_MODULES) $(DEFAULT_MODULES_FREQUENCY) $(FLIGHT_PLAN_XML) $< > $($@_TMP) - $(Q)mv $($@_TMP) $@ - $(Q)chmod a+r $@ - -$(SETTINGS_MODULES) : $(MODULES_H) -$(SETTINGS_TELEMETRY) : $(PERIODIC_H) -$(SETTINGS_FLIGHTPLAN) : $(FLIGHT_PLAN_H) - %.ac_h : $(GENERATORS)/gen_aircraft.out $(Q)if (expr "$(AIRCRAFT)") > /dev/null; then : ; else echo "AIRCRAFT undefined: type 'make AIRCRAFT=AircraftName ...'"; exit 1; fi @echo "#######################################" @echo "# BUILD AIRCRAFT=$(AIRCRAFT), TARGET $*" @echo "#######################################" - $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) PAPARAZZI_QT_GEN=$(PAPARAZZI_QT_GEN) TARGET=$* Q=$(Q) $(GENERATORS)/gen_aircraft.out $(AIRCRAFT) $(CONF_XML) + $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) PAPARAZZI_QT_GEN=$(PAPARAZZI_QT_GEN) TARGET=$* Q=$(Q) $(GENERATORS)/gen_aircraft.out -all -name $(AIRCRAFT) -target $* -conf $(CONF_XML) %.qt: %.ac_h @echo "GENERATED Qt project" %.compile: %.ac_h | print_version + $(MAKE) TARGET=$* -f Makefile.ac all_ac_h cd $(AIRBORNE); $(MAKE) -j$(NPROCS) TARGET=$* all %.upload: %.compile cd $(AIRBORNE); $(MAKE) TARGET=$* upload -sim sim.compile: sim.ac_h | print_version - cd $(AIRBORNE); $(MAKE) -j$(NPROCS) TARGET=sim ARCHI=sim ARCH=sim all - -# Rules for backward compatibility (old guys are used to !) -fbw : fbw.compile -ap: ap.compile - clean_ac : $(Q)if (expr "$(AIRCRAFT)") > /dev/null; then : ; else echo "AIRCRAFT undefined: type 'make AIRCRAFT=AircraftName ...'"; exit 1; fi @echo "CLEANING $(AIRCRAFT)" $(Q)rm -fr $(AIRCRAFT_BUILD_DIR) -.PHONY: all_ac_h radio_ac_h flight_plan_ac_h makefile_ac clean_ac print_version generate_keys +.PHONY: all_ac_h clean_ac print_version generate_keys diff --git a/conf/airframes/airframe.dtd b/conf/airframes/airframe.dtd index 1cd899ed92..894b5d886d 100644 --- a/conf/airframes/airframe.dtd +++ b/conf/airframes/airframe.dtd @@ -1,6 +1,6 @@ - + @@ -9,7 +9,7 @@ - + @@ -19,14 +19,10 @@ - - - - + - - - + + @@ -44,12 +40,7 @@ freq CDATA #IMPLIED> - - +board CDATA #REQUIRED> @@ -119,26 +110,10 @@ name CDATA #REQUIRED value CDATA #REQUIRED description CDATA #IMPLIED> - - - - - - - - + + @@ -72,8 +72,9 @@ name CDATA #REQUIRED> - with ) - - - diff --git a/conf/firmwares/subsystems/rotorcraft/gps_datalink.makefile b/conf/firmwares/subsystems/rotorcraft/gps_datalink.makefile deleted file mode 100644 index dfa5343924..0000000000 --- a/conf/firmwares/subsystems/rotorcraft/gps_datalink.makefile +++ /dev/null @@ -1 +0,0 @@ -$(error Error: The gps datalink subsystem has been converted to a module, replace by ) diff --git a/conf/firmwares/subsystems/rotorcraft/gps_sim_hitl.makefile b/conf/firmwares/subsystems/rotorcraft/gps_sim_hitl.makefile deleted file mode 100644 index f0146d2901..0000000000 --- a/conf/firmwares/subsystems/rotorcraft/gps_sim_hitl.makefile +++ /dev/null @@ -1 +0,0 @@ -$(error Error: replace by ) diff --git a/conf/firmwares/subsystems/rotorcraft/gps_sirf.makefile b/conf/firmwares/subsystems/rotorcraft/gps_sirf.makefile deleted file mode 100644 index b48a3d6eec..0000000000 --- a/conf/firmwares/subsystems/rotorcraft/gps_sirf.makefile +++ /dev/null @@ -1 +0,0 @@ -$(error Error: The gps sirf subsystem has been converted to a module, replace by ) diff --git a/conf/firmwares/subsystems/rotorcraft/gps_udp.makefile b/conf/firmwares/subsystems/rotorcraft/gps_udp.makefile deleted file mode 100644 index be8db05458..0000000000 --- a/conf/firmwares/subsystems/rotorcraft/gps_udp.makefile +++ /dev/null @@ -1 +0,0 @@ -$(error Error: The gps udp subsystem has been converted to a module, replace by ) diff --git a/conf/firmwares/subsystems/shared/gps_furuno.makefile b/conf/firmwares/subsystems/shared/gps_furuno.makefile deleted file mode 100644 index b161586dd0..0000000000 --- a/conf/firmwares/subsystems/shared/gps_furuno.makefile +++ /dev/null @@ -1 +0,0 @@ -$(error Error: The gps furuno subsystem has been converted to a module, replace by ) diff --git a/conf/firmwares/subsystems/shared/gps_mediatek_diy.makefile b/conf/firmwares/subsystems/shared/gps_mediatek_diy.makefile deleted file mode 100644 index 161c8a363c..0000000000 --- a/conf/firmwares/subsystems/shared/gps_mediatek_diy.makefile +++ /dev/null @@ -1 +0,0 @@ -$(error Error: The gps mediatek subsystem has been converted to a module, replace by ) diff --git a/conf/firmwares/subsystems/shared/gps_nmea.makefile b/conf/firmwares/subsystems/shared/gps_nmea.makefile deleted file mode 100644 index e7744d5c32..0000000000 --- a/conf/firmwares/subsystems/shared/gps_nmea.makefile +++ /dev/null @@ -1 +0,0 @@ -$(error Error: The gps nmea subsystem has been converted to a module, replace with ) diff --git a/conf/firmwares/subsystems/shared/gps_piksi.makefile b/conf/firmwares/subsystems/shared/gps_piksi.makefile deleted file mode 100644 index 4d1356025d..0000000000 --- a/conf/firmwares/subsystems/shared/gps_piksi.makefile +++ /dev/null @@ -1 +0,0 @@ -$(error Error: The gps piksi subsystem has been converted to a module, replace by ) diff --git a/conf/firmwares/subsystems/shared/gps_skytraq.makefile b/conf/firmwares/subsystems/shared/gps_skytraq.makefile deleted file mode 100644 index 8cc0c8abd6..0000000000 --- a/conf/firmwares/subsystems/shared/gps_skytraq.makefile +++ /dev/null @@ -1 +0,0 @@ -$(error Error: The gps skytraq subsystem has been converted to a module, replace by ) diff --git a/conf/firmwares/subsystems/shared/gps_ublox.makefile b/conf/firmwares/subsystems/shared/gps_ublox.makefile deleted file mode 100644 index 8fa89a8d2a..0000000000 --- a/conf/firmwares/subsystems/shared/gps_ublox.makefile +++ /dev/null @@ -1 +0,0 @@ -$(error Error: The gps ublox subsystem has been converted to a module, replace by ) diff --git a/conf/firmwares/subsystems/shared/gps_ublox_utm.makefile b/conf/firmwares/subsystems/shared/gps_ublox_utm.makefile deleted file mode 100644 index 9942dfdb66..0000000000 --- a/conf/firmwares/subsystems/shared/gps_ublox_utm.makefile +++ /dev/null @@ -1 +0,0 @@ -$(error Please replace with ) diff --git a/conf/firmwares/subsystems/shared/sdlog.makefile b/conf/firmwares/subsystems/shared/sdlog.makefile deleted file mode 100644 index 66029e9158..0000000000 --- a/conf/firmwares/subsystems/shared/sdlog.makefile +++ /dev/null @@ -1 +0,0 @@ -$(error Error: The sdlog subsystem has been replaced by a module and renamed pprzlog, replace by ) diff --git a/conf/modules/nav_line.xml b/conf/modules/nav_line.xml index 0cc685a070..dcc9211d15 100644 --- a/conf/modules/nav_line.xml +++ b/conf/modules/nav_line.xml @@ -1,6 +1,6 @@ - + Fixedwing navigation along a line with nice U-turns. diff --git a/conf/radios/T14SG_SBUS.xml b/conf/radios/T14SG_SBUS.xml index 59b8278a0c..ea5b6637ef 100644 --- a/conf/radios/T14SG_SBUS.xml +++ b/conf/radios/T14SG_SBUS.xml @@ -44,17 +44,17 @@ --> - - - - - - - - - - - - - + + + + + + + + + + + + + diff --git a/conf/radios/cockpitMM.xml b/conf/radios/cockpitMM.xml index ff9c4e1671..dc53fa7e6f 100644 --- a/conf/radios/cockpitMM.xml +++ b/conf/radios/cockpitMM.xml @@ -44,11 +44,11 @@ - - - - - - - + + + + + + + diff --git a/conf/radios/radio.dtd b/conf/radios/radio.dtd index 4d6232b76d..55d01062e5 100644 --- a/conf/radios/radio.dtd +++ b/conf/radios/radio.dtd @@ -12,9 +12,10 @@ pulse_type CDATA #REQUIRED> + average CDATA #IMPLIED + reverse CDATA #IMPLIED> diff --git a/conf/settings/settings.dtd b/conf/settings/settings.dtd index 701c57c7cf..8edd75b269 100644 --- a/conf/settings/settings.dtd +++ b/conf/settings/settings.dtd @@ -1,20 +1,9 @@ - - - - - - - - - - - - - - + + + @@ -26,6 +15,10 @@ target CDATA #IMPLIED name CDATA #IMPLIED > + + diff --git a/sw/airborne/arch/sim/subsystems/radio_control/spektrum_arch.c b/sw/airborne/arch/sim/subsystems/radio_control/spektrum_arch.c index 182a7d0df8..829c7d2e33 100644 --- a/sw/airborne/arch/sim/subsystems/radio_control/spektrum_arch.c +++ b/sw/airborne/arch/sim/subsystems/radio_control/spektrum_arch.c @@ -28,6 +28,7 @@ #include "subsystems/radio_control.h" #include "subsystems/radio_control/spektrum_arch.h" +#include "subsystems/radio_control/spektrum.h" #include "std.h" #include diff --git a/sw/airborne/arch/sim/subsystems/radio_control/spektrum_arch.h b/sw/airborne/arch/sim/subsystems/radio_control/spektrum_arch.h index ffdc9f964a..41f027f0a2 100644 --- a/sw/airborne/arch/sim/subsystems/radio_control/spektrum_arch.h +++ b/sw/airborne/arch/sim/subsystems/radio_control/spektrum_arch.h @@ -25,9 +25,7 @@ #include "subsystems/radio_control/spektrum_radio.h" -extern void spektrum_event(void (*_received_frame_handler)(void)); #define RadioControlEventImp spektrum_event -extern void spektrum_try_bind(void); #if USE_NPS extern void radio_control_feed(void); diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml index 46cc393ec7..24b60a1461 100644 --- a/sw/ground_segment/cockpit/live.ml +++ b/sw/ground_segment/cockpit/live.ml @@ -470,7 +470,7 @@ let create_ac = fun ?(confirm_kill=true) alert (geomap:G.widget) (acs_notebook:G (* do not check dtd if it is a http url *) let via_http = Str.string_match (Str.regexp "http") af_url 0 in let af_xml = ExtXml.parse_file ~noprovedtd:via_http af_file in - let af_xml = try Gen_common.expand_includes ac_id af_xml with _ -> af_xml in + let af_xml = try Airframe.expand_includes ac_id af_xml with _ -> af_xml in (** Get an alternate speech name if available *) let speech_name = get_speech_name af_xml name in diff --git a/sw/ground_segment/tmtc/Makefile b/sw/ground_segment/tmtc/Makefile index 60f59677b9..49c477cc99 100644 --- a/sw/ground_segment/tmtc/Makefile +++ b/sw/ground_segment/tmtc/Makefile @@ -34,7 +34,7 @@ LINKPKG = $(PKG) -linkpkg -dllpath-pkg pprz,pprzlink XPKG = -package pprz.xlib XLINKPKG = $(XPKG) -linkpkg -dllpath-pkg pprz.xlib,pprzlink -SERVERCMO = server_globals.cmo aircraft.cmo wind.cmo airprox.cmo kml.cmo parse_messages_v1.ml intruder.cmo server.cmo +SERVERCMO = server_globals.cmo aircraft_server.cmo wind.cmo airprox.cmo kml.cmo parse_messages_v1.ml intruder.cmo server.cmo SERVERCMX = $(SERVERCMO:.cmo=.cmx) @@ -49,7 +49,7 @@ messages : messages.cmo $(LIBPPRZCMA) $(LIBPPRZLINKCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) gtkInit.cmo $< -settings : settings.cmo ../cockpit/page_settings.cmo $(LIBPPRZCMA) $(LIBPPRZLINKCMA) +settings : settings_gui.cmo ../cockpit/page_settings.cmo $(LIBPPRZCMA) $(LIBPPRZLINKCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(XLINKPKG) gtkInit.cmo -I ../cockpit gtk_save_settings.cmo saveSettings.cmo page_settings.cmo $< @@ -94,8 +94,8 @@ ivy2serial : ivy2serial.cmo $(LIBPPRZCMA) $(LIBPPRZLINKCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) gtkInit.cmo $< -settings.cmo : INCLUDES += -I ../cockpit -settings.cmo : ../cockpit/page_settings.cmi +settings_gui.cmo : INCLUDES += -I ../cockpit +settings_gui.cmo : ../cockpit/page_settings.cmi %.cmo : %.ml @echo OC $< diff --git a/sw/ground_segment/tmtc/aircraft.ml b/sw/ground_segment/tmtc/aircraft_server.ml similarity index 100% rename from sw/ground_segment/tmtc/aircraft.ml rename to sw/ground_segment/tmtc/aircraft_server.ml diff --git a/sw/ground_segment/tmtc/aircraft.mli b/sw/ground_segment/tmtc/aircraft_server.mli similarity index 100% rename from sw/ground_segment/tmtc/aircraft.mli rename to sw/ground_segment/tmtc/aircraft_server.mli diff --git a/sw/ground_segment/tmtc/airprox.ml b/sw/ground_segment/tmtc/airprox.ml index 9782eb5fc1..73cdfa11da 100644 --- a/sw/ground_segment/tmtc/airprox.ml +++ b/sw/ground_segment/tmtc/airprox.ml @@ -22,7 +22,7 @@ * *) -open Aircraft +open Aircraft_server open Latlong module Alerts_Pprz = PprzLink.Messages(struct let name = "alert" end) diff --git a/sw/ground_segment/tmtc/airprox.mli b/sw/ground_segment/tmtc/airprox.mli index a7990ba429..1674ccdc30 100644 --- a/sw/ground_segment/tmtc/airprox.mli +++ b/sw/ground_segment/tmtc/airprox.mli @@ -23,5 +23,5 @@ *) type alert_level = string option -val check_airprox : Aircraft.aircraft -> Aircraft.aircraft -> alert_level +val check_airprox : Aircraft_server.aircraft -> Aircraft_server.aircraft -> alert_level (** [check_airprox ac1 ac2] Returns airprox level *) diff --git a/sw/ground_segment/tmtc/kml.ml b/sw/ground_segment/tmtc/kml.ml index f1d1b07f6d..34d655f243 100644 --- a/sw/ground_segment/tmtc/kml.ml +++ b/sw/ground_segment/tmtc/kml.ml @@ -23,7 +23,7 @@ *) -open Aircraft +open Aircraft_server open Latlong open Printf open Server_globals diff --git a/sw/ground_segment/tmtc/kml.mli b/sw/ground_segment/tmtc/kml.mli index 11b6b1c185..b19a52a7b1 100644 --- a/sw/ground_segment/tmtc/kml.mli +++ b/sw/ground_segment/tmtc/kml.mli @@ -24,8 +24,8 @@ val enabled : bool ref val no_http : bool ref -val build_files : Aircraft.aircraft -> unit -val update_waypoints : Aircraft.aircraft -> unit -val update_horiz_mode : Aircraft.aircraft -> unit -val update_ac : Aircraft.aircraft -> unit +val build_files : Aircraft_server.aircraft -> unit +val update_waypoints : Aircraft_server.aircraft -> unit +val update_horiz_mode : Aircraft_server.aircraft -> unit +val update_ac : Aircraft_server.aircraft -> unit diff --git a/sw/ground_segment/tmtc/parse_messages_v1.ml b/sw/ground_segment/tmtc/parse_messages_v1.ml index af6a2eb5a3..b77d663088 100644 --- a/sw/ground_segment/tmtc/parse_messages_v1.ml +++ b/sw/ground_segment/tmtc/parse_messages_v1.ml @@ -24,7 +24,7 @@ open Printf open Server_globals -open Aircraft +open Aircraft_server open Latlong module LL = Latlong module U = Unix @@ -132,7 +132,7 @@ let hmsl_of_ref = fun nav_ref d_hmsl -> let heading_from_course = ref false -let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> +let log_and_parse = fun ac_name (a:Aircraft_server.aircraft) msg values -> let value = fun x -> try PprzLink.assoc x values with Not_found -> failwith (sprintf "Error: field '%s' not found\n" x) in let fvalue = fun x -> @@ -207,7 +207,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> let desired_east = foi32value "carrot_east" /. pos_frac and desired_north = foi32value "carrot_north" /. pos_frac and desired_alt = foi32value "carrot_up" /. pos_frac in - a.desired_pos <- Aircraft.add_pos_to_nav_ref nav_ref ~z:desired_alt (desired_east, desired_north); + a.desired_pos <- Aircraft_server.add_pos_to_nav_ref nav_ref ~z:desired_alt (desired_east, desired_north); a.desired_altitude <- desired_alt +. (hmsl_of_ref nav_ref a.d_hmsl); a.desired_course <- foi32value "carrot_psi" /. angle_frac (* a.desired_climb <- ?? *) @@ -246,7 +246,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> Some nav_ref -> let x = (try fvalue "x" with _ -> fvalue "desired_x") and y = (try fvalue "y" with _ -> fvalue "desired_y") in - a.desired_pos <- Aircraft.add_pos_to_nav_ref nav_ref (x, y); + a.desired_pos <- Aircraft_server.add_pos_to_nav_ref nav_ref (x, y); | None -> () end; a.desired_altitude <- (try fvalue "altitude" with _ -> fvalue "desired_altitude"); @@ -401,7 +401,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> begin match a.nav_ref, a.horizontal_mode with Some nav_ref, 2 -> (** FIXME *) - a.horiz_mode <- Circle (Aircraft.add_pos_to_nav_ref nav_ref (fvalue "center_east", fvalue "center_north"), truncate (fvalue "radius")); + a.horiz_mode <- Circle (Aircraft_server.add_pos_to_nav_ref nav_ref (fvalue "center_east", fvalue "center_north"), truncate (fvalue "radius")); if !Kml.enabled then Kml.update_horiz_mode a | _ -> () end @@ -409,8 +409,8 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> begin match a.nav_ref, a.horizontal_mode with Some nav_ref, 1 -> (** FIXME *) - let p1 = Aircraft.add_pos_to_nav_ref nav_ref (fvalue "segment_east_1", fvalue "segment_north_1") - and p2 = Aircraft.add_pos_to_nav_ref nav_ref (fvalue "segment_east_2", fvalue "segment_north_2") in + let p1 = Aircraft_server.add_pos_to_nav_ref nav_ref (fvalue "segment_east_1", fvalue "segment_north_1") + and p2 = Aircraft_server.add_pos_to_nav_ref nav_ref (fvalue "segment_east_2", fvalue "segment_north_2") in a.horiz_mode <- Segment (p1, p2); if !Kml.enabled then Kml.update_horiz_mode a | _ -> () @@ -423,8 +423,8 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> a.time_since_last_survey_msg <- 0.; match a.nav_ref with Some nav_ref -> - let p1 = Aircraft.add_pos_to_nav_ref nav_ref (fvalue "west", fvalue "south") - and p2 = Aircraft.add_pos_to_nav_ref nav_ref (fvalue "east", fvalue "north") in + let p1 = Aircraft_server.add_pos_to_nav_ref nav_ref (fvalue "west", fvalue "south") + and p2 = Aircraft_server.add_pos_to_nav_ref nav_ref (fvalue "east", fvalue "north") in a.survey <- Some (p1, p2) | None -> () end diff --git a/sw/ground_segment/tmtc/parse_messages_v1.mli b/sw/ground_segment/tmtc/parse_messages_v1.mli index a17026859b..796394dca2 100644 --- a/sw/ground_segment/tmtc/parse_messages_v1.mli +++ b/sw/ground_segment/tmtc/parse_messages_v1.mli @@ -23,6 +23,6 @@ *) val log_and_parse : - string -> Aircraft.aircraft -> PprzLink.message -> PprzLink.values -> unit + string -> Aircraft_server.aircraft -> PprzLink.message -> PprzLink.values -> unit (** [log_and_parse ac_id ac msg vs] *) diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml index 78ea9445cb..0e357badce 100644 --- a/sw/ground_segment/tmtc/server.ml +++ b/sw/ground_segment/tmtc/server.ml @@ -31,7 +31,7 @@ let replay_old_log = ref false open Printf open Latlong open Server_globals -open Aircraft +open Aircraft_server open Quaternion (*open Intruder*) module U = Unix @@ -69,7 +69,9 @@ let send_aircrafts_msg = fun _asker _values -> let expand_aicraft x = let ac_name = ExtXml.attrib x "name" in try - Env.expand_ac_xml x + let ac = Aircraft.parse_aircraft ~parse_all:true "" x in + if List.length ac.Aircraft.xml > 0 then Xml.Element (Xml.tag x, Xml.attribs x, ac.Aircraft.xml) + else failwith "Nothing to parse" with Failure msg -> begin prerr_endline ("A failure occurred while processing aircraft '"^ac_name^"'"); @@ -193,7 +195,7 @@ let send_cam_status = fun a -> let utmx = dx *. cos angles.y -. dy *. sin angles.y and utmy = dx *. sin angles.y +. dy *. cos angles.y in - Aircraft.add_pos_to_nav_ref (Geo a.pos) (utmx, utmy) in + Aircraft_server.add_pos_to_nav_ref (Geo a.pos) (utmx, utmy) in let geo_1 = find_point_on_ground tr_rotated and geo_2 = find_point_on_ground tl_rotated @@ -203,7 +205,7 @@ let send_cam_status = fun a -> let lats = sprintf "%f,%f,%f,%f," ((Rad>>Deg)geo_1.posn_lat) ((Rad>>Deg)geo_2.posn_lat) ((Rad>>Deg)geo_3.posn_lat) ((Rad>>Deg)geo_4.posn_lat) in let longs = sprintf "%f,%f,%f,%f," ((Rad>>Deg)geo_1.posn_long) ((Rad>>Deg)geo_2.posn_long) ((Rad>>Deg)geo_3.posn_long) ((Rad>>Deg)geo_4.posn_long) in - let twgs84 = Aircraft.add_pos_to_nav_ref nav_ref a.cam.target in + let twgs84 = Aircraft_server.add_pos_to_nav_ref nav_ref a.cam.target in let values = ["ac_id", PprzLink.String a.id; "lats", PprzLink.String lats; "longs", PprzLink.String longs; @@ -327,7 +329,7 @@ let send_telemetry_status = fun a -> (* if no link send anyway for rx_lost_time with special link id *) if Hashtbl.length a.link_status = 0 then begin - let vs = tl_payload "no_id" a.datalink_status (Aircraft.link_status_init ()) in + let vs = tl_payload "no_id" a.datalink_status (Aircraft_server.link_status_init ()) in Ground_Pprz.message_send my_id "TELEMETRY_STATUS" vs end else @@ -555,15 +557,18 @@ let new_aircraft = fun get_alive_md5sum real_id -> if not is_replayed then check_md5sum real_id (get_alive_md5sum ()) aircraft_conf_dir; - let ac = Aircraft.new_aircraft real_id ac_name xml_fp airframe_xml in + let ac = Aircraft_server.new_aircraft real_id ac_name xml_fp airframe_xml in let update = fun () -> for i = 0 to Array.length ac.svinfo - 1 do ac.svinfo.(i).age <- ac.svinfo.(i).age + 1; done in ignore (ac.ap_modes <- try - let (ap_file, _) = Gen_common.get_autopilot_of_airframe airframe_xml in - Some (modes_from_autopilot (ExtXml.parse_file ap_file)) + let ac = Aircraft.parse_aircraft "" airframe_xml in + match ac.Aircraft.autopilots with + | None -> None + | Some [(_, ap)] -> Some (modes_from_autopilot ap.Autopilot.xml) + | _ -> None (* more than one *) with _ -> None); ignore (Glib.Timeout.add 1000 (fun _ -> update (); true)); @@ -846,7 +851,7 @@ let link_report = fun logging _sender vs -> try let ac = Hashtbl.find aircrafts ac_id in let link_status = { - Aircraft.rx_lost_time = PprzLink.int_assoc "rx_lost_time" vs; + Aircraft_server.rx_lost_time = PprzLink.int_assoc "rx_lost_time" vs; rx_bytes = PprzLink.int_assoc "rx_bytes" vs; rx_msgs = PprzLink.int_assoc "rx_msgs" vs; rx_bytes_rate = PprzLink.float_assoc "rx_bytes_rate" vs; diff --git a/sw/ground_segment/tmtc/settings.ml b/sw/ground_segment/tmtc/settings_gui.ml similarity index 100% rename from sw/ground_segment/tmtc/settings.ml rename to sw/ground_segment/tmtc/settings_gui.ml diff --git a/sw/lib/ocaml/Makefile b/sw/lib/ocaml/Makefile index d7163ba359..c4d725b592 100644 --- a/sw/lib/ocaml/Makefile +++ b/sw/lib/ocaml/Makefile @@ -68,7 +68,8 @@ PKGCOMMON=pprzlink,xml-light,netclient,nettls-gnutls,glibivy,lablgtk2 XINCLUDES= XPKGCOMMON=pprzlink,xml-light,glibivy,$(LABLGTK2GNOMECANVAS),lablgtk2.glade -SRC = compat.ml fig.ml debug.ml base64.ml serial.ml ocaml_tools.ml expr_syntax.ml expr_parser.ml expr_lexer.ml extXml.ml env.ml xml2h.ml latlong.ml egm96.ml srtm.ml http.ml maps_support.ml gm.ml iGN.ml geometry_2d.ml cserial.o ubx.ml xmlCom.ml os_calls.ml editAirframe.ml defivybus.ml fp_proc.ml gen_common.ml quaternion.ml +SRC = compat.ml fig.ml debug.ml base64.ml serial.ml ocaml_tools.ml expr_syntax.ml expr_parser.ml expr_lexer.ml extXml.ml env.ml xml2h.ml latlong.ml egm96.ml srtm.ml http.ml maps_support.ml gm.ml iGN.ml geometry_2d.ml cserial.o ubx.ml xmlCom.ml os_calls.ml editAirframe.ml defivybus.ml fp_proc.ml quaternion.ml +SRC += gen_common.ml radio.ml settings.ml module.ml flight_plan.ml autopilot.ml airframe.ml telemetry.ml aircraft.ml CMO = $(SRC:.ml=.cmo) CMX = $(SRC:.ml=.cmx) diff --git a/sw/lib/ocaml/aircraft.ml b/sw/lib/ocaml/aircraft.ml new file mode 100644 index 0000000000..b5e094bd8f --- /dev/null +++ b/sw/lib/ocaml/aircraft.ml @@ -0,0 +1,379 @@ +(* + * Copyright (C) 2020 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, see + * . + *) + +(** + * Aircraft module for parsing XML config files + * Extract the global configuration of an aircraft + *) +module Af = Airframe +module AfT = Airframe.Target +module AfF = Airframe.Firmware + +let (//) = Filename.concat +let get_string_opt = fun x -> match x with Some s -> s | None -> "" + +(* type of loading (user, auto) *) +type load_type = UserLoad | AutoLoad | Unloaded + +(* configuration sorted by target *) +type target_conf = { + configures: Module.configure list; (* configure variables *) + configures_default: Module.configure list; (* default configure options *) + defines: Module.define list; (* define flags *) + firmware_name: string; + board_type: string; + modules: (load_type * Module.t) list; (* list of modules *) + autopilot: bool; (* autopilot if any *) +} + +(* init target conf structure *) +let init_target_conf = fun firmware_name board_type -> + { configures = []; configures_default = []; defines = []; + firmware_name; board_type; modules = []; autopilot = false } + +(* global aircraft structure *) +type t = { + name: string; + config_by_target: (string, target_conf) Hashtbl.t; (* configuration sorted by target name: (string, target_conf) *) + all_modules: Module.t list; (* list of all modules *) + airframe: Airframe.t option; + autopilots: (string option * Autopilot.t) list option; + flight_plan: Flight_plan.t option; + radio: Radio.t option; + telemetry: Telemetry.t option; + settings: Settings.t list option; + xml: Xml.xml list (* config as a list of Xml node *) +} + +let init_aircraft_conf = fun name -> + { name; config_by_target = Hashtbl.create 5; all_modules = []; + airframe = None; autopilots = None; flight_plan = None; + radio = None; telemetry = None; settings = None; + xml = [] + } + +(* add a module if compatible with target and firmware + * and its autoloaded modules to a conf, return final conf *) +let rec target_conf_add_module = fun conf target firmware name mtype load_type -> + let m = Module.from_module_name name mtype in + (* add autoloaded modules *) + let conf = List.fold_left (fun c autoload -> + target_conf_add_module c target firmware autoload.Module.aname autoload.Module.atype AutoLoad + ) conf m.Module.autoloads in + (* check compatibility with target *) + if Module.check_loading target firmware m then + (* check is the module itself is already loaded, merging options in all case *) + let add_module = if List.exists (fun (_, lm) -> m.Module.name = lm.Module.name) conf.modules + then [] else [(load_type, m)] in + (* add configures and defines to conf if needed *) + { conf with + configures = List.fold_left (fun cm mk -> + if Module.check_mk target firmware mk then + List.fold_left (fun cmk c -> + if not (c.Module.cvalue = None) then cmk @ [c] + else cmk + ) cm mk.Module.configures + else + cm) conf.configures m.Module.makefiles; + configures_default = List.fold_left (fun cm mk -> + if Module.check_mk target firmware mk then + List.fold_left (fun cmk c -> + if not (c.Module.default = None && c.Module.case = None) then cmk @ [c] + else cmk + ) cm mk.Module.configures + else + cm) conf.configures_default m.Module.makefiles; + modules = conf.modules @ add_module } + else begin + (* add "unloaded" module for reference *) + { conf with modules = conf.modules @ [(Unloaded, m)] } end + + +(* sort element of an airframe type by target *) +let sort_airframe_by_target = fun config_by_target airframe -> + match airframe with + | None -> () + | Some a -> + (* build a list of pairs (target, firmware) *) + let l = List.fold_left (fun lf f -> + List.fold_left (fun lt t -> + lt @ [(t, f)]) lf f.AfF.targets + ) [] a.Af.firmwares in + (* iterate on each target *) + List.iter (fun (t, f) -> + let name = t.AfT.name in (* target name *) + if Hashtbl.mem config_by_target name then + failwith "[Error] Gen_airframe: two targets with the same name"; + (* init and add configure/define from airframe *) + let conf = init_target_conf f.AfF.name t.AfT.board in + let conf = { conf with + configures = t.AfT.configures @ f.AfF.configures; + defines = t.AfT.defines @ f.AfF.defines } in + (* iter on modules in target *) + let conf = List.fold_left (fun c m_af -> + let c = { c with + configures = c.configures @ m_af.Module.configures; + defines = c.defines @ m_af.Module.defines } in + target_conf_add_module c name f.AfF.name m_af.Module.name m_af.Module.mtype UserLoad + ) conf t.AfT.modules in + (* iter on modules in firmwares *) + let conf = List.fold_left (fun c m_af -> + let c = { c with + configures = c.configures @ m_af.Module.configures; + defines = c.defines @ m_af.Module.defines } in + target_conf_add_module c name f.AfF.name m_af.Module.name m_af.Module.mtype UserLoad + ) conf f.AfF.modules in + (* iter on deprecated 'modules' node *) + let conf = List.fold_left (fun c m -> + List.fold_left (fun c m_af -> + let c = { c with + configures = c.configures @ m_af.Module.configures; + defines = c.defines @ m_af.Module.defines } in + target_conf_add_module c name f.AfF.name m_af.Module.name m_af.Module.mtype UserLoad + ) c m.Airframe.OldModules.modules + ) conf a.Airframe.modules in + Hashtbl.add config_by_target name conf + ) l + +(** Extract a configuration element from aircraft config, + * returns a tuple with absolute file path and element object + * + * [bool -> Xml.xml -> string -> (Xml.xml -> a') -> (string * a' option)] + *) +let get_config_element = fun flag ac_xml elt f -> + if not flag then None + else + try + let file = Xml.attrib ac_xml elt in + let abs_file = Env.paparazzi_conf // file in + Some (f abs_file) + with Xml.No_attribute _ -> None (* no attribute elt in conf file *) + +let get_element_relative_path = fun flag ac_xml elt -> + if not flag then None + else + try + Some (Xml.attrib ac_xml elt) + with Xml.No_attribute _ -> None (* no attribute elt in conf file *) + + +(** Extract loaded modules from hashtbl config + *) +let get_loaded_modules = fun config_by_target target -> + try + let config = Hashtbl.find config_by_target target in + let modules = config.modules in + (List.fold_left (fun l (t, m) -> if t <> Unloaded then l @ [m] else l) [] modules) + with Not_found -> [] (* nothing for this target *) + +(** Extract all modules + * if a modules is not in any target, it will not appear in the list + * returns an alphabetically sorted list + *) +let get_all_modules = fun config_by_target -> + let modules = ref [] in + Hashtbl.iter (fun _ conf -> + List.iter (fun (_, m) -> + if not (List.exists (fun n -> m.Module.name = n.Module.name) !modules) then + modules := m :: !modules (* add module to list *) + ) conf.modules + ) config_by_target; + List.sort (fun m1 m2 -> compare m1.Module.name m2.Module.name) !modules + + + +let parse_aircraft = fun ?(parse_af=false) ?(parse_ap=false) ?(parse_fp=false) ?(parse_rc=false) ?(parse_tl=false) ?(parse_set=false) ?(parse_all=false) ?(verbose=false) target aircraft_xml -> + + let name = Xml.attrib aircraft_xml "name" in + let conf_aircraft = [] in (* accumulate aircraft XML config *) + let config_by_target = Hashtbl.create 5 in + + + if verbose then + Printf.printf "Parsing airframe%!"; + let airframe = get_config_element (parse_af || parse_all) aircraft_xml "airframe" Airframe.from_file in + if verbose then + Printf.printf " '%s'%!" (match airframe with None -> "None" | Some a -> a.Airframe.filename); + let conf_aircraft = conf_aircraft @ (match airframe with None -> [] | Some x -> [x.Airframe.xml]) in + if verbose then + Printf.printf ", sorting by target%!"; + sort_airframe_by_target config_by_target airframe; + if verbose then + Printf.printf ", extracting and parsing autopilot...%!"; + let autopilots = if parse_ap || parse_all then + begin + match airframe with + | None -> None + | Some af -> + (* extract autopilots *) + let autopilots = List.fold_left (fun lf f -> + let ap_f = match f.Airframe.Firmware.autopilot with None -> [] | Some a -> [a] in + let ap_t = List.fold_left (fun lt t -> + if t.Airframe.Target.name = target then + match t.Airframe.Target.autopilot with None -> lt | Some a -> a :: lt + else lt + ) ap_f f.Airframe.Firmware.targets in + ap_t @ lf + ) af.Airframe.autopilots af.Airframe.firmwares in + if List.length autopilots = 0 then None + else + let autopilots = List.map (fun af_ap -> + let filename = af_ap.Airframe.Autopilot.name in + let filename = if Filename.check_suffix filename ".xml" + then filename + else filename^".xml" in + let filename = Env.paparazzi_conf // "autopilot" // filename in + let ap = Autopilot.from_file filename in + (* extract modules from autopilot *) + Hashtbl.iter (fun target conf -> + let conf = List.fold_left (fun c m -> + let c = { c with + configures = c.configures @ m.Module.configures; + defines = c.defines @ m.Module.defines } in + target_conf_add_module c target "" m.Module.name m.Module.mtype UserLoad + ) conf ap.Autopilot.modules in + Hashtbl.replace config_by_target target conf + ) config_by_target; + (af_ap.Airframe.Autopilot.freq, ap) + ) autopilots in + let c = Hashtbl.find config_by_target target in + Hashtbl.replace config_by_target target { c with autopilot = true }; + Some autopilots + end + else None in + let conf_aircraft = conf_aircraft @ (match autopilots with None -> [] | Some lx -> List.map (fun (_, x) -> x.Autopilot.xml) lx) in + if verbose then + Printf.printf " done.\n%!"; + + if verbose then + Printf.printf "Parsing flight plan%!"; + let flight_plan = get_config_element (parse_fp || parse_all) aircraft_xml "flight_plan" Flight_plan.from_file in + if verbose then begin + Printf.printf " '%s'%!" (match flight_plan with None -> "None" | Some fp -> fp.Flight_plan.filename); + Printf.printf ", extracting modules...%!" + end; + begin match flight_plan with + | None -> () + | Some fp -> + Hashtbl.iter (fun target conf -> + let conf = List.fold_left (fun c m -> + let c = { c with + configures = c.configures @ m.Module.configures; + defines = c.defines @ m.Module.defines } in + target_conf_add_module c target "" m.Module.name m.Module.mtype UserLoad + ) conf fp.Flight_plan.modules in + Hashtbl.replace config_by_target target conf + ) config_by_target + end; + let conf_aircraft = conf_aircraft @ (match flight_plan with None -> [] | Some x -> [x.Flight_plan.xml]) in + if verbose then + Printf.printf " done\n%!"; + + if verbose then + Printf.printf "Parsing radio%!"; + let radio = get_config_element (parse_rc || parse_all) aircraft_xml "radio" Radio.from_file in + if verbose then + Printf.printf " '%s'...%!" (match radio with None -> "None" | Some rc -> rc.Radio.filename); + let conf_aircraft = conf_aircraft @ (match radio with None -> [] | Some x -> [x.Radio.xml]) in + if verbose then + Printf.printf " done\n%!"; + + if verbose then + Printf.printf "Parsing telemetry%!"; + let telemetry = get_config_element (parse_tl || parse_all) aircraft_xml "telemetry" Telemetry.from_file in + if verbose then + Printf.printf " '%s'...%!" (match telemetry with None -> "None" | Some tl -> tl.Telemetry.filename); + let conf_aircraft = conf_aircraft @ (match telemetry with None -> [] | Some x -> [x.Telemetry.xml]) in + if verbose then + Printf.printf " done\n%!"; + + (* TODO resolve modules dep *) + let loaded_modules = get_loaded_modules config_by_target target in + let all_modules = get_all_modules config_by_target in + + if verbose then + Printf.printf "Parsing settings...%!"; + let settings = + if parse_set || parse_all then begin + (* normal settings *) + let settings = try Env.filter_settings (ExtXml.attrib aircraft_xml "settings") with _ -> "" in + let settings_files = Str.split (Str.regexp " ") settings in + let settings = List.map + (fun f -> Settings.from_file (Env.paparazzi_conf // f)) settings_files in + (* modules settings *) + let settings_modules = + try Env.filter_settings (ExtXml.attrib aircraft_xml "settings_modules") + with _ -> "" in + let settings_modules_files = Str.split (Str.regexp " ") settings_modules in + let settings_modules = List.fold_left + (fun acc m -> + if List.exists (fun name -> + m.Module.xml_filename = (if Filename.is_relative name + then (Env.paparazzi_conf // name) else name)) settings_modules_files + then acc @ m.Module.settings else acc + ) [] loaded_modules in + (* system settings *) + let sys_tl_settings = Telemetry.get_sys_telemetry_settings telemetry in + let sys_mod_settings = Module.get_sys_modules_settings loaded_modules in + let sys_fp_settings = Flight_plan.get_sys_fp_settings flight_plan in + let sys_ap_settings = Autopilot.get_sys_ap_settings autopilots in + (* filter system settings *) + let system_settings = List.fold_left + (fun l s -> match s with None -> l | Some x -> x::l) [] + [sys_tl_settings; sys_fp_settings; sys_mod_settings; sys_ap_settings] + in + (* group into a common System dl_settings *) + let sys_dl_settings = List.fold_left (fun l s -> s.Settings.dl_settings @ l) [] system_settings in + let sys_dl_settings = { + Settings.Dl_settings.name = Some "System"; + dl_settings = sys_dl_settings; dl_setting = []; headers = []; + xml = Xml.Element ("dl_settings", [("name","System")], (List.map (fun s -> s.Settings.Dl_settings.xml) sys_dl_settings)) + } in + let system_settings = { + Settings.filename = ""; name = None; target = None; + dl_settings = [sys_dl_settings]; + xml = Xml.Element ("settings", [], [Xml.Element ("dl_settings", [], (List.map (fun s -> s.Settings.Dl_settings.xml) [sys_dl_settings]))]) + } in + (* join all settings in correct order *) + let settings = [system_settings] @ settings @ settings_modules in + (* filter on targets *) + let settings = List.fold_left (fun l s -> + if Gen_common.test_targets target (Gen_common.targets_of_string s.Settings.target) then s :: l + else l + ) [] settings in + Some (List.rev settings) + end + else None + in + let conf_aircraft = conf_aircraft @ (match settings with None -> [] | Some x -> [Settings.get_settings_xml x]) in + if verbose then + Printf.printf " done\n%!"; + + if verbose then begin + Printf.printf "Loading modules:\n"; + List.iter (fun m -> + Printf.printf " - %s (%s) [%s]\n" m.Module.name (get_string_opt m.Module.dir) m.Module.xml_filename) loaded_modules + end; + + (* return aircraft conf *) + { name; config_by_target; all_modules; + airframe; autopilots; flight_plan; radio; telemetry; settings; + xml = conf_aircraft } + diff --git a/sw/lib/ocaml/airframe.ml b/sw/lib/ocaml/airframe.ml new file mode 100644 index 0000000000..e10bea7099 --- /dev/null +++ b/sw/lib/ocaml/airframe.ml @@ -0,0 +1,160 @@ +(* + * Copyright (C) 2017 Gautier Hattenberger + * Cyril Allignol + * + * 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 + * . + *) + +(** + * Airframe module for parsing XML config files + *) + +module OT = Ocaml_tools + +module Autopilot = struct + + type t = { name: string; freq: string option; xml: Xml.xml } + + let from_xml = function + | Xml.Element ("autopilot", attrs, []) as xml -> + { name = Xml.attrib xml "name"; + freq = ExtXml.attrib_opt xml "freq"; + xml } + | _ -> failwith "Airframe.Autopilot.from_xml: unreachable" + +end + +module Include = struct + + type t = { href: string; xml: Xml.xml } + + let from_xml = function + | Xml.Element ("include", _, []) as xml -> + { href = Xml.attrib xml "href"; xml } + | _ -> failwith "Airframe.Include.from_xml: unreachable" + +end + +module Target = struct + + type t = { name: string; + board: string; + modules: Module.config list; + autopilot: Autopilot.t option; + configures: Module.configure list; + defines: Module.define list; + xml: Xml.xml } + + let from_xml = function + | Xml.Element ("target", _, children) as xml -> + { name = Xml.attrib xml "name"; + board = Xml.attrib xml "board"; + modules = ExtXml.parse_children "module" Module.config_from_xml children; + autopilot = begin + try Some (Autopilot.from_xml (ExtXml.child xml "autopilot")) + with _ -> None end; + configures = ExtXml.parse_children "configure" Module.parse_configure children; + defines = ExtXml.parse_children "define" Module.parse_define children; + xml } + | _ -> failwith "Airframe.Autopilot.from_xml: unreachable" + +end + +module Firmware = struct + + type t = { name: string; + targets: Target.t list; + modules: Module.config list; + autopilot: Autopilot.t option; + configures: Module.configure list; + defines: Module.define list; + xml: Xml.xml } + + let from_xml = function + | Xml.Element ("firmware", _, children) as xml -> + { name = Xml.attrib xml "name"; + targets = ExtXml.parse_children "target" Target.from_xml children; + modules = ExtXml.parse_children "module" Module.config_from_xml children; + autopilot = begin + try Some (Autopilot.from_xml (ExtXml.child xml "autopilot")) + with _ -> None end; + configures = ExtXml.parse_children "configure" Module.parse_configure children; + defines = ExtXml.parse_children "define" Module.parse_define children; + xml } + | _ -> failwith "Airframe.Firmware.from_xml: unreachable" + +end + +module OldModules = struct + + type t = { modules: Module.config list; + xml: Xml.xml } + + let from_xml = function + | Xml.Element ("modules", _, children) as xml -> + { modules = ExtXml.parse_children "module" Module.config_from_xml children; + xml } + | _ -> failwith "Airframe.Modules.from_xml: unreachable" + +end + +type t = { + filename: string; + name: string; + includes: Include.t list; + modules: OldModules.t list; (* NOTE this is a deprecated format, should be removed *) + firmwares: Firmware.t list; + autopilots: Autopilot.t list; + xml: Xml.xml +} + +let from_xml = function + | Xml.Element ("airframe", _, children) as xml -> + if List.exists (fun c -> Xml.tag c = "modules") children then + Printf.eprintf "\nWarning: 'modules' node is deprecated, please move modules to 'firmware' section\n%!"; + { filename = ""; name = Xml.attrib xml "name"; + includes = ExtXml.parse_children "include" Include.from_xml children; + modules = ExtXml.parse_children "modules" OldModules.from_xml children; + firmwares = ExtXml.parse_children "firmware" Firmware.from_xml children; + autopilots = ExtXml.parse_children "autopilot" Autopilot.from_xml children; + xml } + | _ -> failwith "Airframe.from_xml: unreachable" + +let from_file = fun filename -> + let af = from_xml (Xml.parse_file filename) in + { af with filename } + + +(** [expand_includes ac_id xml] + * Get expanded xml airframe if it contains 'include' nodes + *) +let expand_includes = fun ac_id xml -> + match xml with + | Xml.PCData d -> Xml.PCData d + | Xml.Element (tag, attrs, children) -> + Xml.Element (tag, attrs, + List.fold_left (fun x c -> + if Xml.tag c = "include" then begin + let filename = Str.global_replace (Str.regexp "\\$AC_ID") ac_id (ExtXml.attrib c "href") in + let filename = + if Filename.is_relative filename then Filename.concat Env.paparazzi_home filename + else filename in + let subxml = ExtXml.parse_file filename in + x @ (Xml.children subxml) + end + else x @ [c] + ) [] children) diff --git a/sw/lib/ocaml/autopilot.ml b/sw/lib/ocaml/autopilot.ml new file mode 100644 index 0000000000..9b377e3f74 --- /dev/null +++ b/sw/lib/ocaml/autopilot.ml @@ -0,0 +1,109 @@ +(* + * Copyright (C) 2017 Gautier Hattenberger + * Cyril Allignol + * + * 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 + * . + *) + +(** + * Autopilot module for parsing XML config files + *) + +module OT = Ocaml_tools + +type t = { + filename: string; + modules: Module.config list; + xml: Xml.xml; +} + +let from_xml = function + | Xml.Element ("autopilot", _, children) as xml -> + let modules = List.fold_left (fun m el -> + if Xml.tag el = "modules" then + m @ List.map Module.config_from_xml (Xml.children el) + else + m + ) [] children in + { filename = ""; modules; xml } + | _ -> failwith "Autopilot.from_xml: unreachable" + +let from_file = fun filename -> + let ap = from_xml (Xml.parse_file filename) in + { ap with filename } + + +(* return a Settings object from flight plan *) +let get_sys_ap_settings = fun autopilots -> + match autopilots with + | None -> None + | Some autopilots -> + let dl_settings = List.fold_left (fun sl (_, autopilot) -> + (* Filter state machines that need to be displayed *) + let sm_filtered = List.filter (fun sm -> + try (Compat.lowercase_ascii (Xml.attrib sm "settings_mode")) = "true" with _ -> false + ) (Xml.children autopilot.xml) in + if List.length sm_filtered = 0 then sl + else + (* Create node if there is at least one to display *) + let dl_set = List.fold_left (fun l sm -> + let modes = List.filter (fun m -> (Xml.tag m) = "mode") (Xml.children sm) in + let name = Xml.attrib sm "name" in + (* Iter on modes and store min, max and values *) + let (_, min, max, values) = List.fold_left (fun (current, min, max, values) m -> + let print = try Compat.lowercase_ascii (Xml.attrib m "settings") <> "hide" with _ -> true in + let name = Xml.attrib m "name" in + if print then begin + let min = match min with + | None -> Some current + | Some x -> Some x + in + let max = Some current in + let values = values @ [name] in + (current + 1, min, max, values) + end + else begin + let n = match min with None -> [] | _ -> [name] in + (current + 1, min, max, values @ n) + end + ) (0, None, None, []) modes in + (* check handler *) + let handler = try + let sh = Xml.attrib sm "settings_handler" in + let s = Str.split (Str.regexp "|") sh in + match s with + | [header; handler] -> [("header",header); ("handler",handler)] + | _ -> failwith "Gen_autopilot: invalid handler format" + with _ -> [("header","autopilot_core_"^name)] in + begin match min, max with + | Some min_idx, Some max_idx -> + Xml.Element ("dl_setting", [ + ("min", string_of_int min_idx); + ("max", string_of_int max_idx); + ("step", "1"); + ("var", "autopilot_mode_"^name); + ("shortname", name); + ("values", (String.concat "|" values))] + @ handler, []) :: l + | _, _ -> l + end + ) [] sm_filtered in + dl_set @ sl + ) [] autopilots in + let xml = Xml.Element ("dl_settings", [("name","Autopilot")], dl_settings) in + Some (Settings.from_xml (Xml.Element("settings",[],[xml]))) + diff --git a/sw/lib/ocaml/egm96.ml b/sw/lib/ocaml/egm96.ml index beb54e50c5..6e118c8c5a 100644 --- a/sw/lib/ocaml/egm96.ml +++ b/sw/lib/ocaml/egm96.ml @@ -29,12 +29,12 @@ let (//) = Filename.concat let ncols = 1440 let nrows = 721 +let n = ncols * nrows * 2 +let buf = Bytes.create n let data = lazy ( let path = [Env.paparazzi_home // "data" // "srtm"] in let f = Ocaml_tools.open_compress (Ocaml_tools.find_file path "WW15MGH.DAC") in - let n = ncols * nrows * 2 in - let buf = Bytes.create n in really_input f buf 0 n; buf) diff --git a/sw/lib/ocaml/env.ml b/sw/lib/ocaml/env.ml index 27bbda3d1b..c32b1ced50 100644 --- a/sw/lib/ocaml/env.ml +++ b/sw/lib/ocaml/env.ml @@ -41,11 +41,51 @@ let paparazzi_home = with _ -> Filename.concat (Sys.getenv "HOME") "paparazzi" +let paparazzi_conf = paparazzi_home // "conf" -let flight_plans_path = paparazzi_home // "conf" // "flight_plans" - +let flight_plans_path = paparazzi_conf // "flight_plans" let flight_plan_dtd = flight_plans_path // "flight_plan.dtd" +(** Returns the list of directories where to look for modules + * Default PAPARAZZI_HOME/conf/modules is always returned + * Extra directories can be added with PAPARAZZI_MODULES_PATH + * where where items are ':' separated and modules are in subfolders + * of a 'modules' folder + * ex: + * PAPARAZZI_MODULES_PATH=/home/me/pprz_modules + * - pprz_modules/modules + * -- module1 + * --- module1.xml + * --- module1.c + * --- module1.h + * -- module2 + * --- module2.xml + * --- module2.c + * --- module2.h + *) +let modules_paths = + let default_path = paparazzi_conf // "modules" in + try + let path = Sys.getenv "PAPARAZZI_MODULES_PATH" in + let dirs = Str.split (Str.regexp ":") path in + let paths = List.fold_left (fun dl dir -> + let sub_dirs = List.fold_left (fun sdl sdir -> + let d = dir // "modules" // sdir in + if Sys.is_directory d then d :: sdl else sdl + ) [] (Array.to_list (Sys.readdir (dir // "modules"))) in + dl @ sub_dirs) [] dirs + in + paths @ [default_path] + with + | Sys_error _ | Not_found -> [default_path] + +(** Returns the list of directories in PAPARAZZI_MODULES_PATH *) +let modules_ext_paths = + try + let path = Sys.getenv "PAPARAZZI_MODULES_PATH" in + Str.split (Str.regexp ":") path + with _ -> [] + let icon_file = paparazzi_home // "data/pictures/penguin_icon.png" let icon_gcs_file = paparazzi_home // "data/pictures/penguin_icon_gcs.png" let icon_mes_file = paparazzi_home // "data/pictures/penguin_icon_msg.png" @@ -70,7 +110,7 @@ let get_gcs_icon_path = fun theme icon -> (* or raise not found *) raise Not_found -let dump_fp = paparazzi_src // "sw" // "tools" // "generators" // "gen_flight_plan.out -dump" +let dump_fp = paparazzi_src // "sw" // "tools" // "generators" // "dump_flight_plan.out" let default_module_targets = "ap|sim|nps|hitl" @@ -84,88 +124,6 @@ let filter_settings = fun settings -> let sl = List.filter (fun s -> not (s.[0] = '[' && s.[String.length s - 1] = ']')) sl in String.concat " " sl -(* filter on modules based on target *) -let filter_modules_target = fun module_file -> - (* get TARGET env *) - let target = try Sys.getenv "TARGET" with _ -> "" in - (* look for a specific name after settings file (in case of modules) *) - let split = Str.split (Str.regexp "~") module_file in - let xml_file, name = match split with - | [f; n] -> f, n - | _ -> module_file, "" - in - let module_xml = ExtXml.parse_file xml_file in - if Xml.tag module_xml = "module" - then - begin - (* test if the module is loaded or not - * and if a specific sub-settings is selected *) - if List.exists (fun n -> - let local_target = ExtXml.attrib_or_default n "target" default_module_targets - and tag = Xml.tag n in - if tag = "makefile" then - Str.string_match (Str.regexp (".*"^target^".*")) local_target 0 - else false - ) (Xml.children module_xml) - then Xml.Element ("settings", [], - List.filter (fun t -> - Xml.tag t = "settings" && ExtXml.attrib_or_default t "name" "" = name) - (Xml.children module_xml)) - else Xml.Element ("settings",[],[]) - end - else module_xml - - -let expand_ac_xml = fun ?(raise_exception = true) ac_conf -> - let prefix = fun s -> sprintf "%s/conf/%s" paparazzi_home s in - let parse_file = fun ?(parse_filter=(fun x -> ExtXml.parse_file x)) a file -> - try - parse_filter file - with - Failure msg -> - if raise_exception then - failwith msg - else begin - prerr_endline msg; - make_element "parse error" ["file",a; "msg", msg] [] - end in - - let parse = fun ?(pre_filter=(fun x -> x)) ?(parse_filter=(fun x -> ExtXml.parse_file x)) a -> - List.map - (fun filename -> parse_file ~parse_filter a (prefix filename)) - (Str.split space_regexp (pre_filter (ExtXml.attrib ac_conf a))) in - - let parse_opt = fun ?(pre_filter=(fun x -> x)) ?(parse_filter=(fun x -> ExtXml.parse_file x)) a -> - try parse ~pre_filter ~parse_filter a with ExtXml.Error _ -> [] in - - (* dump expanded version of flight plan before parsing *) - let parse_fp = fun a -> - try - (* get full path file name *) - let fp = prefix (ExtXml.attrib ac_conf a) in - if Sys.is_directory fp then raise Not_found; - (* create a temporary dump file *) - let dump = Filename.temp_file "fp_dump" ".xml" in - (* set command then call it *) - let c = sprintf "%s %s > %s" dump_fp fp dump in - if Sys.command c <> 0 then - begin - Sys.remove dump; - failwith c - end; - (* parse temp fp file and then remove it *) - let fp_xml = parse_file a dump in - Sys.remove dump; - (* return Xml list *) - [fp_xml] - with _ -> [] - in - - let pervasives = parse "airframe" @ parse "telemetry" @ parse ~pre_filter:filter_settings "settings" in - let optionals = parse_opt "radio" @ parse_fp "flight_plan" @ parse_opt ~pre_filter:filter_settings ~parse_filter:filter_modules_target "settings_modules" @ pervasives in - - let children = Xml.children ac_conf@optionals in - make_element (Xml.tag ac_conf) (Xml.attribs ac_conf) children (* Run a command and return its results as a string. *) let read_process command = diff --git a/sw/lib/ocaml/env.mli b/sw/lib/ocaml/env.mli index 58a2518eb8..bf307bb8dd 100644 --- a/sw/lib/ocaml/env.mli +++ b/sw/lib/ocaml/env.mli @@ -28,9 +28,14 @@ val paparazzi_home : string val paparazzi_src : string (** Installation's files directory *) +val paparazzi_conf : string + val flight_plans_path : string val flight_plan_dtd : string +val modules_paths : string list +val modules_ext_paths : string list + val icon_file : string (** PNG paparazzi logo icon (48 x 48, 8-bit/color RGBA, non-interlaced) *) @@ -74,11 +79,6 @@ val filter_settings : string -> string * and keep the ones without brackets * (return a string of filtered name separate by white spaces) *) -val expand_ac_xml : ?raise_exception:bool -> Xml.xml -> Xml.xml -(** Expands a conf.xml aircraft entry, adding the XML configuration files -(listed as attributes) as children. Returns an element containing the error - message if raise_exception is false (default to true) *) - val get_paparazzi_version : unit -> string (** read the current paparazzi_version *) diff --git a/sw/lib/ocaml/extXml.ml b/sw/lib/ocaml/extXml.ml index c86010651e..936473a78f 100644 --- a/sw/lib/ocaml/extXml.ml +++ b/sw/lib/ocaml/extXml.ml @@ -71,10 +71,17 @@ let attrib = fun xml attr -> attr sprint_fields (Xml.attribs xml) in raise (Error msg) -let attrib_option = fun xml attr -> +let attrib_opt = fun xml attr -> try Some (Xml.attrib xml attr) with Xml.No_attribute _ -> None +let attrib_opt_map = fun xml attr f -> + try Some (f (Xml.attrib xml attr)) + with Xml.No_attribute _ -> None + +let attrib_opt_int = fun xml attr -> attrib_opt_map xml attr int_of_string +let attrib_opt_float = fun xml attr -> attrib_opt_map xml attr float_of_string + let tag_is = fun x v -> Compat.lowercase_ascii (Xml.tag x) = Compat.lowercase_ascii v let attrib_or_default = fun x a default -> @@ -145,16 +152,14 @@ let my_to_string_fmt = fun tab_attribs x -> Buffer.reset tmp; s - - let to_string_fmt = fun ?(tab_attribs = false) xml -> let l = Compat.lowercase_ascii in let rec lower = function | Xml.PCData _ as x -> x | Xml.Element (t, ats, cs) -> - Xml.Element(l t, - List.map (fun (a,v) -> (l a, v)) ats, - List.map lower cs) in + Xml.Element(l t, + List.map (fun (a, v) -> (l a, v)) ats, + List.map lower cs) in my_to_string_fmt tab_attribs (lower xml) @@ -204,6 +209,15 @@ let remove_child = fun ?(select= fun _ -> true) t xml -> List.fold_right (fun xml rest -> if tag_is xml t && select xml then rest else xml::rest) children []) | Xml.PCData _ -> xml +let parse_children = fun tag f children -> + List.fold_left (fun l x -> if Xml.tag x = tag then f x :: l else l) + [] children + +let parse_children_attribs = fun tag f children -> + List.fold_left + (fun l x -> if Xml.tag x = tag then f (Xml.attribs x) :: l else l) + [] children + let float_attrib = fun xml a -> let v = attrib xml a in diff --git a/sw/lib/ocaml/extXml.mli b/sw/lib/ocaml/extXml.mli index ed21acb6ce..897a027041 100644 --- a/sw/lib/ocaml/extXml.mli +++ b/sw/lib/ocaml/extXml.mli @@ -35,6 +35,9 @@ separator is [.]). May raise [Not_found]. *) val get_attrib : Xml.xml -> string -> string -> string (** [get_attrib xml path attrib_name] *) +val sprint_fields : unit -> (string * string) list -> string +(** [sprint_fields attribs] pretty print attribs *) + val attrib : Xml.xml -> string -> string val int_attrib : Xml.xml -> string -> int val float_attrib : Xml.xml -> string -> float @@ -43,7 +46,10 @@ val float_attrib : Xml.xml -> string -> float val tag_is : Xml.xml -> string -> bool (** [tag_is xml s] Case safe test *) -val attrib_option : Xml.xml -> string -> string option +val attrib_opt : Xml.xml -> string -> string option +val attrib_opt_map : Xml.xml -> string -> (string -> 'a) -> 'a option +val attrib_opt_int : Xml.xml -> string -> int option +val attrib_opt_float : Xml.xml -> string -> float option val attrib_or_default : Xml.xml -> string -> string -> string (** [get xml attribute_name default_value] *) @@ -67,6 +73,13 @@ val remove_child : ?select:(Xml.xml -> bool) -> string -> Xml.xml -> Xml.xml (** [delete_child ?select child_tag xml] Returns [xml] if not found *) +val parse_children : string -> (Xml.xml -> 'a) -> Xml.xml list -> 'a list +(** [parse_children tag f children] *) + +val parse_children_attribs : + string -> ((string * string) list -> 'a) -> Xml.xml list -> 'a list +(** [parse_children_attribs tag f children] *) + val iter_tag : string -> (Xml.xml -> unit) -> Xml.xml -> unit (** [iter_tag f tag xml] applies function [f] to every child of [xml] with tag [tag] *) diff --git a/sw/lib/ocaml/flight_plan.ml b/sw/lib/ocaml/flight_plan.ml new file mode 100644 index 0000000000..ba3ad43477 --- /dev/null +++ b/sw/lib/ocaml/flight_plan.ml @@ -0,0 +1,91 @@ +(* + * Copyright (C) 2017 Gautier Hattenberger + * Cyril Allignol + * + * 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 + * . + *) + +(** + * Flight_plan module for parsing XML config files + *) + +module OT = Ocaml_tools + +type t = { + filename: string; + settings: Settings.Dl_setting.t list; + modules: Module.config list; + xml: Xml.xml; +} + +let from_xml = function + | Xml.Element ("flight_plan", _, children) as xml -> + let settings = List.fold_left (fun s el -> + if Xml.tag el = "variables" then + s @ List.fold_left (fun s e -> + if Xml.tag e = "variable" then + let test = fun attrib -> + match ExtXml.attrib_opt e attrib with + | Some _ -> true | None -> false in + let get_opt = fun attrib -> ExtXml.attrib_opt e attrib in + if test "min" && test "max" && test "step" then + [{ Settings.Dl_setting.var = Xml.attrib e "var"; + shortname = get_opt "shortname"; + handler = None; + header = None; + xml = Xml.Element ("dl_setting", Xml.attribs e, []); + }] @ s + else s + else s + ) [] (Xml.children el) + else + s + ) [] children in + let modules = List.fold_left (fun m el -> + if Xml.tag el = "modules" then + m @ List.map Module.config_from_xml (Xml.children el) + else m + ) [] children in + { filename = ""; settings; modules; xml } + | _ -> failwith "Flight_plan.from_xml: unreachable" + +let from_file = fun filename -> + let fp = from_xml (Xml.parse_file filename) in + { fp with filename } + + +(* return a Settings object from flight plan *) +let get_sys_fp_settings = fun flight_plan -> + match flight_plan with + | None -> None + | Some fp -> + let dl_settings = fp.settings in + if List.length dl_settings = 0 then None + else + let dl_settings_xml = Xml.Element ("dl_settings", [ ("name", "Flight Plan") ], + List.map (fun x -> x.Settings.Dl_setting.xml) dl_settings) + in + let dl_settings = + { Settings.Dl_settings.name = Some "Flight Plan"; + dl_settings = []; dl_setting = dl_settings; + headers = ["generated/flight_plan"]; xml = dl_settings_xml } + in + let dl_settings_xml = Xml.Element ("dl_settings", [], [dl_settings_xml]) in + let xml = Xml.Element ("settings", [], [dl_settings_xml]) in + Some { Settings.filename = ""; name = None; target = None; dl_settings = [dl_settings]; xml } + + diff --git a/sw/lib/ocaml/gen_common.ml b/sw/lib/ocaml/gen_common.ml index 7eb65581a6..afe930f1b1 100644 --- a/sw/lib/ocaml/gen_common.ml +++ b/sw/lib/ocaml/gen_common.ml @@ -24,10 +24,9 @@ open Printf -exception Firmware_Found of Xml.xml - (** simple boolean expressions *) type bool_expr = + | Any | Var of string | Not of bool_expr | And of bool_expr * bool_expr @@ -35,6 +34,7 @@ type bool_expr = (** evaluate a boolean expression for a given value *) let rec eval_bool v = function + | Any -> true | Var x -> v = x | Not e -> not (eval_bool v e) | And (e1, e2) -> eval_bool v e1 && eval_bool v e2 @@ -43,6 +43,7 @@ let rec eval_bool v = function (** pretty print boolean expression *) let print_bool = fun v e -> let rec print_b v = function + | Any -> eprintf "Any " | Var x -> eprintf "Var ( %s =? %s ) " x v | Not e -> eprintf "Not ( "; (print_b v e); eprintf ") " | And (e1, e2) -> eprintf "And ( "; print_b v e1; print_b v e2; eprintf ") " @@ -50,22 +51,25 @@ let print_bool = fun v e -> in print_b v e; eprintf "\n" +(** pretty print boolean expression *) +let sprint_bool = fun v e -> + let rec print_b s v = function + | Any -> sprintf "%sAny " s + | Var x -> sprintf "%sVar ( %s =? %s ) " s x v + | Not e -> let s = sprintf "%sNot ( " s in + let s = print_b s v e in + sprintf "%s) " s + | And (e1, e2) -> let s = sprintf "%sAnd ( " s in + let s = print_b s v e1 in + let s = print_b s v e2 in + sprintf "%s) " s + | Or (e1, e2) -> let s = sprintf "%sOr ( " s in + let s = print_b s v e1 in + let s = print_b s v e2 in + sprintf "%s) " s + in + print_b "" v e -type module_conf = { - name: string; - xml: Xml.xml; - file: string; - filename: string; - vpath: string option;(* this field should be removed after transition phase *) - param: Xml.xml list; - targets: bool_expr - } - -let (//) = Filename.concat - -let paparazzi_conf = Env.paparazzi_home // "conf" -let modules_dir = paparazzi_conf // "modules" -let autopilot_dir = paparazzi_conf // "autopilot" (** remove all duplicated elements of a list *) let singletonize = fun ?(compare = compare) l -> @@ -81,141 +85,25 @@ let union = fun l1 l2 -> singletonize (l1 @ l2) (** union of a list of list *) let union_of_lists = fun l -> singletonize (List.flatten l) -(** [targets_of_field] - * Returns the targets expression of a makefile node in modules - * Default "ap|sim" *) -let targets_of_field = +(** [targets_of_string] + * Returns the targets expression of a string + *) +let targets_of_string = let rec expr_of_targets op = function - | [] -> Var "" + | [] -> Any | [e] -> Var e | l::ls -> op (Var l) (expr_of_targets op ls) in let pipe = Str.regexp "|" in - fun field default -> - let f = ExtXml.attrib_or_default field "target" default in - if String.length f > 0 && String.get f 0 = '!' then - Not (expr_of_targets (fun x y -> Or(x,y)) (Str.split pipe (String.sub f 1 ((String.length f) - 1)))) - else - expr_of_targets (fun x y -> Or(x,y)) (Str.split pipe f) + fun targets -> + match targets with + | None -> Any + | Some t -> + if String.length t > 0 && String.get t 0 = '!' then + Not (expr_of_targets (fun x y -> Or(x,y)) (Str.split pipe (String.sub t 1 ((String.length t) - 1)))) + else + expr_of_targets (fun x y -> Or(x,y)) (Str.split pipe t) -(** [get_autopilot_of_airframe xml] - * Returns (autopilot xml, main freq) from airframe xml file *) -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 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 per firmware/target in your airframe file" - -(** [get_targets_of_module xml] - * Returns the boolean expression of targets of a module *) -let get_targets_of_module = fun xml -> - Xml.fold (fun a x -> - match Compat.lowercase_ascii (Xml.tag x) with - | "makefile" when a = Var "" -> targets_of_field x Env.default_module_targets - | "makefile" -> Or (a, targets_of_field x Env.default_module_targets) - | _ -> a - ) (Var "") xml - -let module_name = fun xml -> - let name = ExtXml.attrib xml "name" in - try if Filename.check_suffix name ".xml" then Filename.chop_extension name else name with _ -> name - -exception Subsystem of string -let get_module = fun m global_targets -> - match Xml.tag m with - | "module" | "autoload" -> - let name = module_name m in - let filename = - let modtype = ExtXml.attrib_or_default m "type" "" in - name ^ (if modtype = "" then "" else "_") ^ modtype ^ ".xml" in - let file = modules_dir // filename in - if not (Sys.file_exists file) then raise (Subsystem file) else - let xml = ExtXml.parse_file file in - let targets = get_targets_of_module xml in - let targets = Or (global_targets, targets) in - { name = name; xml = xml; file = file; filename = filename; vpath = None; - param = Xml.children m; targets = targets } - | "load" -> (* this case should be removed after transition phase *) - let dir, vpath = - try - let dir = ExtXml.attrib m "dir" in - let dir = - if Filename.is_relative dir then Env.paparazzi_home // dir - else dir in - (dir, Some dir) - with _ -> modules_dir, None in - let filename = ExtXml.attrib m "name" in - let name = Filename.chop_extension filename in - let file = dir // filename in - let xml = ExtXml.parse_file file in - let targets = get_targets_of_module xml in - let extra_targets = Or (global_targets, targets_of_field m "") in - let targets = Or (extra_targets, targets) in - { name = name; xml = xml; file = file; filename = filename; vpath = vpath; - param = Xml.children m; targets = targets } - | _ -> Xml2h.xml_error "module, autoload or load" - -(** [get_autoloaded_modules module] - * Return a list of modules to be automaticaly added - * Only works with actual modules (no subsystems) *) -let rec get_autoloaded_modules = fun m -> - let m = get_module m (Var "") in - List.fold_left (fun l t -> - if ExtXml.tag_is t "autoload" then - let am = get_module t (Var "") in - (am :: ((try get_autoloaded_modules am.xml with _ -> []) @ l)) - else l - ) [] (Xml.children m.xml) (** [test_targets target targets] * Test if [target] is allowed [targets] @@ -223,207 +111,3 @@ let rec get_autoloaded_modules = fun m -> let test_targets = fun target targets -> eval_bool target targets -(** [expand_includes ac_id xml] - * Expand xml airframe file if it contains 'include' nodes - *) -let expand_includes = fun ac_id xml -> - match xml with - | Xml.PCData d -> Xml.PCData d - | Xml.Element (tag, attrs, children) -> - Xml.Element (tag, attrs, - List.fold_left (fun x c -> - if Xml.tag c = "include" then begin - let filename = Str.global_replace (Str.regexp "\\$AC_ID") ac_id (ExtXml.attrib c "href") in - let filename = - if Filename.is_relative filename then Env.paparazzi_home // filename - else filename in - let subxml = ExtXml.parse_file filename in - x @ (Xml.children subxml) - end - else x @ [c] - ) [] children) - -(** [get_modules_of_airframe xml] - * Returns a list of module configuration from airframe file *) -let rec get_modules_of_airframe = fun ?target xml -> - let is_module = fun tag -> List.mem tag [ "module"; "load" ] in - (* 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 modules from xml tree *) - let rec iter_modules = fun ?(subsystem_fallback=true) targets modules xml -> - match xml with - | Xml.PCData _ -> modules - | Xml.Element (tag, _attrs, children) when is_module tag -> - begin try - let m = get_module xml targets in - let al = get_autoloaded_modules xml in - List.fold_left - (fun acc xml -> iter_modules targets acc xml) - (m :: (al @ modules)) children - with Subsystem file -> - if subsystem_fallback then modules - else failwith ("Unkown module " ^ file) - end - | 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_modules targets acc xml) modules children - | None -> - List.fold_left (fun acc xml -> - iter_modules targets acc xml) modules children - | _ -> modules 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_modules targets acc xml) modules children - | Some t when t = target_name -> - List.fold_left - (fun acc xml -> iter_modules targets acc xml) modules children - | _ -> modules end - | Xml.Element (tag, _attrs, _children) when tag = "include" -> - let filename = ExtXml.attrib xml "href" in - let subxml = ExtXml.parse_file filename in - iter_modules targets modules subxml - | Xml.Element (tag, _attrs, children) -> - let (targets, use_fallback) = - if tag = "modules" then (targets_of_field xml "", false) else (targets, true) in - List.fold_left - (fun acc xml -> iter_modules ~subsystem_fallback:use_fallback targets acc xml) modules children in - let modules = iter_modules (Var "") [] xml in - let ap_modules = - try - 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 - match target with - | 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 (Var "") [] 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 = List.fold_left (fun a x -> - match a with - | Var "" -> x.targets - | _ -> Or (a, x.targets) - ) (Var "") ([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 ac_id af_xml fp_xml -> - let af_modules = get_modules_of_airframe ?target (expand_includes ac_id af_xml) - and fp_modules = get_modules_of_flight_plan fp_xml in - (* singletonize modules list and reverse list to have it in the correct order *) - List.rev (singletonize_modules ?verbose ?target (af_modules @ fp_modules)) - -(** [get_modules_name xml] - * Returns a list of loaded modules' name *) -let get_modules_name = fun ac_id xml -> - let target = try Sys.getenv "TARGET" with _ -> "" in - (* extract all modules sections for a given target *) - let modules = get_modules_of_airframe ~target (expand_includes ac_id xml) in - (* return a list of modules name *) - List.map (fun m -> ExtXml.attrib m.xml "name") modules - -(** [get_modules_dir xml] - * Returns the list of modules directories *) -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 dir - -(** [is_element_unselected target modules file] - * Returns True if [target] is supported in the element [file] and, if it is - * a module, that it is loaded, - * [file] being the file name of an Xml file (module or setting) *) -let is_element_unselected = fun ?(verbose=false) target modules name -> - try - let name = (Env.paparazzi_home // "conf" // name) in - let xml = ExtXml.parse_file name in - match Xml.tag xml with - | "settings" -> - let target_list = targets_of_field xml "" in - let unselected = not (test_targets target target_list) in - if unselected && not (target_list = Var ("")) && verbose then - begin Printf.printf "Info: settings '%s' unloaded for target '%s'\n" name target; flush stdout end; - unselected && not (target_list = Var ("")) - | "module" -> - let unselected = List.for_all (fun m -> m.file <> name) modules in - if unselected && verbose then - begin Printf.printf "Info: module '%s' unloaded for target '%s'\n" name target; flush stdout end - else begin - if verbose then - (* display possible unloading of settings when the module itself is loaded *) - List.iter (fun n -> - let tag = Xml.tag n in - let target_list = targets_of_field n "" in - let valid = test_targets target target_list in - if tag = "settings" && not (ExtXml.attrib_or_default n "target" "" = "") && not valid then - begin Printf.printf "Info: settings of module '%s' unloaded for target '%s'\n" name target; flush stdout end; - ) (Xml.children xml) - end; - unselected - | _ -> false - with _ -> false diff --git a/sw/lib/ocaml/gen_common.mli b/sw/lib/ocaml/gen_common.mli index cf35671d60..70fd477be1 100644 --- a/sw/lib/ocaml/gen_common.mli +++ b/sw/lib/ocaml/gen_common.mli @@ -24,79 +24,26 @@ (* simple boolean expressions *) type bool_expr = + | Any | Var of string | Not of bool_expr | And of bool_expr * bool_expr | Or of bool_expr * bool_expr -(* Module configuration: - * Xml node - * file (with path) - * file name only - * optional vpath - * parameters - * extrat targets - *) -type module_conf = { name : string; xml : Xml.xml; file : string; filename : string; vpath : string option; param : Xml.xml list; targets : bool_expr; } - -(* Modules directory *) -val modules_dir : string +val print_bool : string -> bool_expr -> unit +val sprint_bool : string -> bool_expr -> string (** remove all duplicated elements of a list *) val singletonize : ?compare: ('a -> 'a -> int) -> 'a list -> 'a list -(** [targets_of_field] Xml node, default - * Returns the targets expression of a makefile node in modules - * Default "ap|sim" *) -val targets_of_field : Xml.xml -> string -> bool_expr - -exception Subsystem of string -val module_name : Xml.xml -> string -val get_module : Xml.xml -> bool_expr -> module_conf - -(** [expand_includes ac_id xml] - * Expand xml airframe file if it contains 'include' nodes +(** [targets_of_string] targets + * Returns the targets expression of a string *) -val expand_includes : string -> Xml.xml -> Xml.xml - -(** [get_modules_of_airframe xml] - * 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 ac_id 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 -> string -> Xml.xml -> Xml.xml -> module_conf list +val targets_of_string : string option -> bool_expr (** [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 !) *) val test_targets : string -> bool_expr -> bool -(** [get_targets_of_module xml] Returns the boolean expression of targets of a module *) -val get_targets_of_module : Xml.xml -> bool_expr - -(** [get_modules_name ac_id xml] - * Returns a list of loaded modules' name *) -val get_modules_name : string -> Xml.xml -> string list - -(** [get_modules_dir xml] - * Returns the list of modules directories *) -val get_modules_dir : module_conf list -> string list - -(** [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 : ?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 - * a module, that it is loaded, - * [file] being the file name of an Xml file (module or setting) *) -val is_element_unselected : ?verbose:bool -> string -> module_conf list -> string -> bool diff --git a/sw/lib/ocaml/module.ml b/sw/lib/ocaml/module.ml new file mode 100644 index 0000000000..7359c3b90f --- /dev/null +++ b/sw/lib/ocaml/module.ml @@ -0,0 +1,365 @@ +(* + * Copyright (C) 2017 Gautier Hattenberger + * Cyril Allignol + * + * 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 + * . + *) + +(** + * 'Module' module for parsing XML config files + *) + +module OT = Ocaml_tools +module GC = Gen_common + +let find_name = fun xml -> + try + let name = Xml.attrib xml "name" in + if Filename.check_suffix name ".xml" then Filename.chop_extension name + else name + with + | Not_found -> + let msg = Printf.sprintf "Error: Attribute 'name' expected in %a" + ExtXml.sprint_fields (Xml.attribs xml) in + raise (ExtXml.Error msg) + +type file = { filename: string; directory: string option; filecond: string option } +type file_arch = file + +let parse_file = fun xml -> + match xml with + | Xml.Element ("file", _, []) | Xml.Element ("file_arch", _, []) -> + { filename = find_name xml; + directory = ExtXml.attrib_opt xml "dir"; + filecond = ExtXml.attrib_opt xml "cond" } + | _ -> failwith "Module.parse_file: unreachable" + +type configure = { + cname: string; + cvalue: string option; + case: string option; + default: string option; + cdescription: string option + } + +let parse_configure = fun xml -> + let get = fun x -> ExtXml.attrib_opt xml x in + { cname = find_name xml; cvalue = get "value"; case = get "case"; + default = get "default"; cdescription = get "description" } + +type define = { + dname: string; + dvalue: string option; + integer: int option; + dunit: string option; + dtype: string option; + ddescription: string option; + cond: string option + } + +let parse_define = fun xml -> + let get = fun x -> ExtXml.attrib_opt xml x in + { dname = find_name xml; dvalue = get "value"; + integer = begin match get "integer" with + | None -> None | Some i -> Some (int_of_string i) end; + dunit = get "unit"; dtype = get "type"; + ddescription = get "description"; cond = get "cond" } + +type incl = { element: string; condition: string option } + +type flag = { flag: string; value: string; fcond: string option } + +type raw = string + +type makefile = { + targets: string option; + firmware: string option; + condition: string option; + configures: configure list; + defines: define list; + inclusions: incl list; + flags: flag list; + files: file list; + files_arch: file list; + raws: raw list + } + +let empty_makefile = + { targets = None; firmware = None; condition = None; + configures = []; defines = []; + inclusions = []; flags = []; files = []; files_arch = []; raws = [] } + +let rec parse_makefile mkf = function + | Xml.Element ("makefile", _, children) as xml -> + let targets = ExtXml.attrib_opt xml "target" + and firmware = ExtXml.attrib_opt xml "firmware" + and condition = ExtXml.attrib_opt xml "cond" in + List.fold_left parse_makefile + { mkf with targets; firmware; condition } children + | Xml.Element ("configure", _, []) as xml -> + { mkf with configures = parse_configure xml :: mkf.configures } + | Xml.Element ("define", _, []) as xml -> + { mkf with defines = parse_define xml :: mkf.defines } + | Xml.Element ("include", _, []) as xml -> + { mkf with inclusions = + { element = find_name xml; + condition = ExtXml.attrib_opt xml "cond" } + :: mkf.inclusions } + | Xml.Element ("flag", _, []) as xml -> + let flag = Xml.attrib xml "name" and value = Xml.attrib xml "value" in + { mkf with flags = { flag; value; fcond = ExtXml.attrib_opt xml "cond" } + :: mkf.flags } + | Xml.Element ("file", _, []) as xml -> + { mkf with files = parse_file xml :: mkf.files } + | Xml.Element ("file_arch", _, []) as xml -> + { mkf with files_arch = parse_file xml :: mkf.files_arch } + | Xml.Element ("raw", [], [Xml.PCData raw]) -> + {mkf with raws = raw :: mkf.raws} + | _ -> failwith "Module.parse_makefile: unreachable" + +type autorun = True | False | Lock + +type period_freq = Unset | Set of float * float | Freq of string | Period of string + +type periodic = { + call: string; + fname: string; + period_freq: period_freq; + delay: int option; + start: string option; + stop: string option; + autorun: autorun + } + +let parse_periodic = fun xml -> + let get = fun x -> ExtXml.attrib_opt xml x in + let geti = fun x -> ExtXml.attrib_opt_int xml x in + let call = snd (List.find (fun (a, _) -> Compat.lowercase_ascii a = "fun") + (Xml.attribs xml)) in + let call_regexp = Str.regexp "\\([a-zA-Z_][a-zA-Z0-9_]*\\)\\(.*\\)" in + let fname = + if Str.string_match call_regexp call 0 then + let fname = Str.matched_group 1 call and args = Str.matched_group 2 call in + if args = "" || Str.string_match (Str.regexp "(.*)") args 0 then fname + else failwith ("Module.parse_periodic: invalid function call: " ^ call) + else failwith ("Module.parse_periodic: invalid function name: " ^ call) in + let period_freq = match get "period", get "freq" with + | None, None -> Unset + | None, Some f -> begin + try let f = float_of_string f in Set (1. /. f, f) + with _ -> Freq f + end + | Some p, None -> begin + try let p = float_of_string p in Set (p, 1. /. p) + with _ -> Period p + end + | Some p, Some _ -> begin + Printf.eprintf "Warning: both period and freq are defined "; + Printf.eprintf "but only period is used for function %s\n%!" fname; + try let p = float_of_string p in Set (p, 1. /. p) + with _ -> Period p + end + in + { call; fname; period_freq; delay = geti "delay"; + start = get "start"; stop = get "stop"; + autorun = match get "autorun" with + | None -> Lock + | Some "TRUE" | Some "true" -> True + | Some "FALSE" | Some "false" -> False + | Some "LOCK" | Some "lock" -> Lock + | Some _ -> failwith "Module.parse_periodic: unreachable" } + +type event = { ev: string; handlers: string list } + +let make_event = fun f handlers -> + { ev = f; + handlers = List.map + (function + | Xml.Element ("handler", _, []) as xml -> Xml.attrib xml "fun" + | _ -> failwith "Module.make_event: unreachable" + ) handlers } + +let fprint_event = fun ch e -> Printf.fprintf ch "%s;\n" e.ev + +type datalink = { message: string; func: string } + +let fprint_datalink = fun ch d -> + Printf.fprintf ch "(msg_id == DL_%s) { %s; }\n" d.message d.func + +type autoload = { + aname: string; + atype: string option + } + +type config = { name: string; + mtype: string option; + dir: string option; + configures: configure list; + defines: define list; + xml: Xml.xml } + +let config_from_xml = function + | Xml.Element ("module", _, children) as xml -> + { name = Xml.attrib xml "name"; + mtype = ExtXml.attrib_opt xml "type"; + dir = ExtXml.attrib_opt xml "dir"; + configures = ExtXml.parse_children "configure" parse_configure children; + defines = ExtXml.parse_children "define" parse_define children; + xml } + | _ -> failwith "Module.config_from_xml: unreachable" + +type t = { + xml_filename: string; + name: string; + dir: string option; + task: string option; + path: string; + doc: Xml.xml; + requires: string list; + conflicts: string list; + provides: string list; + autoloads: autoload list; + settings: Settings.t list; + headers: file list; + inits: string list; + periodics: periodic list; + events: event list; + datalinks: datalink list; + makefiles: makefile list; + xml: Xml.xml +} + +let empty = + { xml_filename = ""; name = ""; dir = None; + task = None; path = ""; doc = Xml.Element ("doc", [], []); + requires = []; conflicts = []; provides = []; autoloads = []; settings = []; + headers = []; inits = []; periodics = []; events = []; datalinks = []; + makefiles = []; xml = Xml.Element ("module", [], []) } + +let parse_module_list = Str.split (Str.regexp "[ \t]*,[ \t]*") + +let rec parse_xml m = function + | Xml.Element ("module", _, children) as xml -> + let name = find_name xml + and dir = ExtXml.attrib_opt xml "dir" + and task = ExtXml.attrib_opt xml "task" in + List.fold_left parse_xml { m with name; dir; task; xml } children + | Xml.Element ("doc", _, _) as xml -> { m with doc = xml } + (*| Xml.Element ("settings_file", [("name", name)], files) -> m (* TODO : remove unused *)*) + | Xml.Element ("settings", _, _) as xml -> + { m with settings = Settings.from_xml xml :: m.settings } + | Xml.Element ("depends", _, [Xml.PCData depends]) -> + { m with requires = parse_module_list depends } + | Xml.Element ("conflicts", _, [Xml.PCData conflicts]) -> + { m with conflicts = parse_module_list conflicts } + | Xml.Element ("provides", _, [Xml.PCData provides]) -> + { m with provides = parse_module_list provides } + | Xml.Element ("autoload", _, []) as xml -> + let aname = find_name xml + and atype = ExtXml.attrib_opt xml "type" in + { m with autoloads = { aname; atype } :: m.autoloads } + | Xml.Element ("header", [], files) -> + { m with headers = + List.fold_left (fun acc f -> parse_file f :: acc) m.headers files + } + | Xml.Element ("init", _, []) as xml -> + { m with inits = Xml.attrib xml "fun" :: m.inits } + | Xml.Element ("periodic", _, []) as xml -> + { m with periodics = parse_periodic xml :: m.periodics } + | Xml.Element ("event", _, handlers) as xml -> + let f = Xml.attrib xml "fun" in + { m with events = make_event f handlers :: m.events } + | Xml.Element ("datalink", _, []) as xml -> + let message = Xml.attrib xml "message" + and func = Xml.attrib xml "fun" in + { m with datalinks = { message; func } :: m.datalinks } + | Xml.Element ("makefile", _, _) as xml -> + { m with makefiles = parse_makefile empty_makefile xml :: m.makefiles } + | _ -> failwith "Module.parse_xml: unreachable" + +let from_xml = fun xml -> + let m = parse_xml empty xml in + { m with + settings = List.rev m.settings; + headers = List.rev m.headers; + inits = List.rev m.inits; + makefiles = List.rev m.makefiles + } + +let from_file = fun filename -> from_xml (Xml.parse_file filename) + +(** search and parse a module xml file and return a Module.t *) +(* FIXME search folder path: /*/.xml *) +exception Module_not_found of string +let from_module_name = fun name mtype -> + (* concat module type if needed *) + let name = match mtype with Some t -> name ^ "_" ^ t | None -> name in + (* determine if name already have an extension *) + let name = if Filename.check_suffix name ".xml" then name else name ^ ".xml" in + (* determine if name is implicit + * if not, search for absolute name in search path + * may raise Module_not_found if no file found *) + let name = + if Filename.is_implicit name then + let rec find_abs = function + | [] -> raise (Module_not_found name) + | b :: bl -> + let full_name = Filename.concat b name in + if Sys.file_exists full_name then full_name else find_abs bl + in find_abs Env.modules_paths + else if Sys.file_exists name then name + else raise (Module_not_found name) + in + let m = from_xml (ExtXml.parse_file name) in + let settings = List.map (fun s -> { s with Settings.filename = name }) m.settings in + { m with xml_filename = name; settings } + +(** check if a makefile node is compatible with a target and a firmware + * TODO add 'board' type filter ? *) +let check_mk = fun target firmware mk -> + (mk.firmware = (Some firmware) || mk.firmware = None) && GC.test_targets target (GC.targets_of_string mk.targets) + +(** check if a module is compatible with a target and a firmware *) +let check_loading = fun target firmware m -> + List.exists (check_mk target firmware) m.makefiles + +(* TODO merge *) +let status_name = fun mod_name p -> mod_name ^ "_" ^ p.fname ^ "_status" + +(* return a Settings object from modules *) +let get_sys_modules_settings = fun modules -> + (* build a XML node corresponding to the settings *) + let mod_settings = List.fold_left (fun lm m -> + let periodic_settings = List.fold_left (fun lp p -> + if not (p.autorun = Lock) then + lp @ [Xml.Element("dl_setting", + [("min","2"); + ("max","3"); + ("step","1"); + ("var", status_name m.name p); + ("shortname", p.fname); + ("values","START|STOP")],[])] + else lp + ) [] m.periodics in + lm @ periodic_settings + ) [] modules in + let xml = Xml.Element("dl_settings",[("name","Modules")],mod_settings) in + if List.length mod_settings > 0 then + Some (Settings.from_xml (Xml.Element("settings",[],[xml]))) + else + None + diff --git a/sw/lib/ocaml/ocaml_tools.ml b/sw/lib/ocaml/ocaml_tools.ml index 375b787e94..71c69c11b2 100644 --- a/sw/lib/ocaml/ocaml_tools.ml +++ b/sw/lib/ocaml/ocaml_tools.ml @@ -32,6 +32,9 @@ let open_compress file = Unix.open_process_in ("bunzip2 -c "^file) else open_in file +let compress file = + assert (Sys.command ("gzip "^file) = 0) + let extensions = ["";".gz";".Z";".bz2";".zip";".ZIP"] let find_file = fun path file -> @@ -92,3 +95,18 @@ let shifter = fun n default -> a.(!i) <- new_value; i := (!i + 1) mod n; old_value + + +let assoc_opt = fun k l -> + try Some (List.assoc k l) with Not_found -> None + +let assoc_opt_map = fun k l f -> + try Some (f (List.assoc k l)) with Not_found -> None + +let assoc_opt_int = fun k l -> + try Some (int_of_string (List.assoc k l)) with Not_found -> None + +let assoc_default = fun k l def -> + try List.assoc k l with Not_found -> def + + diff --git a/sw/lib/ocaml/ocaml_tools.mli b/sw/lib/ocaml/ocaml_tools.mli index e5ca8868ac..96fc00253a 100644 --- a/sw/lib/ocaml/ocaml_tools.mli +++ b/sw/lib/ocaml/ocaml_tools.mli @@ -2,6 +2,7 @@ * Utilities * * Copyright (C) 2004-2009 CENA/ENAC, Yann Le Fablec, Pascal Brisset + * Copyright (C) 2017 Gautier Hattenberger * * This file is part of paparazzi. * @@ -16,9 +17,8 @@ * 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. + * along with paparazzi; see the file COPYING. If not, see + * . * *) @@ -26,6 +26,9 @@ val open_compress : string -> in_channel (** [open_compress file] Opens [file] compressed or non-compressed. [.gz], [.Z], [.bz2], [.zip] and [.ZIP] recognized *) +val compress : string -> unit +(** [compress file] Compress [file] with gzip *) + val find_file : string list -> string -> string (** [find_file path file] Search for [file] or a compressed extension of it in [path]. Returns the first occurence found. Checked extensions are @@ -48,3 +51,20 @@ val make_1st_order_noise_generator : val shifter : int -> 'a -> ('a -> 'a) (* [shifter size init] Returns a shift register containing [size] values initialized to [init] *) + +val assoc_opt : 'a -> ('a * 'b) list -> 'b option +(* [assoc_opt a l] Returns the result of [List.assoc a l] + or None if [a] not in [l] *) + +val assoc_opt_map : 'a -> ('a * 'b) list -> ('b -> 'c) -> 'c option +(* [assoc_opt_map a l f] Returns the result of [f (List.assoc a l)] + or None if [a] not in [l] *) + +val assoc_opt_int : 'a -> ('a * string) list -> int option +(* [assoc_opt_map a l f] Returns the result of [int_of_string (List.assoc a l)] + or None if [a] not in [l] *) + +val assoc_default : 'a -> ('a * 'b) list -> 'b -> 'b +(* [assoc_default a l d] Returns the result of [List.assoc a l] + or [d] if [a] not in [l] *) + diff --git a/sw/lib/ocaml/radio.ml b/sw/lib/ocaml/radio.ml new file mode 100644 index 0000000000..c91c249c10 --- /dev/null +++ b/sw/lib/ocaml/radio.ml @@ -0,0 +1,85 @@ +(* + * Copyright (C) 2017 Gautier Hattenberger + * Cyril Allignol + * + * 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 + * . + *) + +(** + * Radio control module for parsing XML config files + *) + +type channel = { + cname: string; (* a.k.a. function in DTD *) + min: int; + max: int; + neutral: int; + average: bool; (* average data *) + reverse: bool; (* reverse min/max parameters *) +} + +let parse_channel = function + | Xml.Element ("channel", attribs, []) as xml -> + let bget = fun attrib -> + try Xml.attrib xml attrib <> "0" + with Xml.No_attribute _ -> false in + { cname = Xml.attrib xml "function"; + min = ExtXml.int_attrib xml "min"; + max = ExtXml.int_attrib xml "max"; + neutral = ExtXml.int_attrib xml "neutral"; + average = bget "average"; + reverse = bget "reverse"; + } + | _ -> failwith "Radio.parse_channel: unreachable" + + +type pulse = PositivePulse | NegativePulse + +type t = { + filename: string; + name: string; + data_min: int; + data_max: int; + sync_min: int; + sync_max: int; + pulse_type: pulse; + channels: channel list; + xml: Xml.xml; +} + +let from_xml = function + | Xml.Element ("radio", attribs, channels) as xml -> + let name = Xml.attrib xml "name" in + { filename = ""; + name; + data_min = ExtXml.int_attrib xml "data_min"; + data_max = ExtXml.int_attrib xml "data_max"; + sync_min = ExtXml.int_attrib xml "sync_min"; + sync_max = ExtXml.int_attrib xml "sync_max"; + pulse_type = begin match Xml.attrib xml "pulse_type" with + | "POSITIVE" -> PositivePulse + | "NEGATIVE" -> NegativePulse + | _ -> failwith "Radio.from_xml: unknown pulse type" + end; + channels = List.map parse_channel channels; + xml; + } + | _ -> failwith "Radio.from_xml: unreachable" + +let from_file = fun filename -> + let r = from_xml (Xml.parse_file filename) in + { r with filename } diff --git a/sw/lib/ocaml/settings.ml b/sw/lib/ocaml/settings.ml new file mode 100644 index 0000000000..cfdfa9e765 --- /dev/null +++ b/sw/lib/ocaml/settings.ml @@ -0,0 +1,136 @@ +(* + * Copyright (C) 2017 Gautier Hattenberger + * Cyril Allignol + * + * 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 + * . + *) + +(** + * Settings module for parsing XML config files + * + * FIXME order is not preserved if dl_settings and dl_setting are in separated lists + *) + +module Dl_setting = struct + + type t = { + var: string; + shortname: string option; + handler: string option; + header: string option; + xml: Xml.xml } + + let from_xml = function + | Xml.Element ("dl_setting", attribs, _) as xml -> + { var = Xml.attrib xml "var"; + shortname = ExtXml.attrib_opt xml "shortname"; + handler = ExtXml.attrib_opt xml "handler"; + header = begin + match ExtXml.attrib_opt xml "module", + ExtXml.attrib_opt xml "header" with + | Some m, _ -> + (*Printf.eprintf "Warning: please rename 'module' attribute in settings with 'header'\n"; FIXME *) + Some m + | None, Some h -> Some h + | None, None -> None + end; + xml; + } + | _ -> failwith "Settings.Dl_setting.from_xml: unreachable" + +end + +module Dl_settings = struct + + type t = { + name: string option; + dl_settings: t list; + dl_setting: Dl_setting.t list; + headers: string list; + xml: Xml.xml; } + + let rec iter_xml s = function + | Xml.Element ("dl_settings", attribs, children) as xml -> + { name = ExtXml.attrib_opt xml "name"; + dl_settings = List.map (iter_xml s) children; + dl_setting = []; + headers = []; + xml } + | Xml.Element ("dl_setting", attribs, _) as xml -> + { s with dl_setting = Dl_setting.from_xml xml :: s.dl_setting } + | Xml.Element ("include", [("header", name)], _) -> + { s with headers = name :: s.headers } + | _ -> failwith "Settings.Dl_settings.iter_xml: unreachable" + + let from_xml = iter_xml { name = None; dl_settings = []; dl_setting = []; headers = []; xml = Xml.Element ("dl_settings", [], []) } + +end + +type t = { + filename: string; + name: string option; (* for modules' settings *) + target: string option; + dl_settings: Dl_settings.t list; + xml: Xml.xml +} + +let from_xml = function + | Xml.Element ("settings", attribs, children) as xml -> + { filename = ""; + name = ExtXml.attrib_opt xml "name"; + target = ExtXml.attrib_opt xml "target"; + dl_settings = List.map Dl_settings.from_xml children; + xml } + | _ -> failwith "Settings.from_xml: unreachable" + +let from_file = fun filename -> + let s = from_xml (Xml.parse_file filename) in + { s with filename } + +(** + * Get singletonized list of headers from a Settings.t + *) +let get_headers = fun settings -> + let rec iter = fun headers s -> + let headers = List.fold_left (fun hl dl_s -> + match dl_s.Dl_setting.header with + | Some x -> if List.mem x hl then hl else x :: hl + | None -> hl + ) headers s.Dl_settings.dl_setting in + let headers = List.fold_left (fun hl h -> + if List.mem h hl then hl else h :: hl + ) headers s.Dl_settings.headers in + let headers = List.fold_left (fun hl dl_ss -> + iter hl dl_ss + ) headers s.Dl_settings.dl_settings in + headers + in + List.fold_left iter [] settings.dl_settings + + +(* Get settings as a single XML node *) +let get_settings_xml = fun settings -> + let settings_xml = List.fold_left (fun l s -> + if List.length s.dl_settings > 0 + then (Xml.children (ExtXml.child s.xml "dl_settings")) @ l + else l + ) [] settings + in + let settings_xml = List.rev settings_xml in (* list in correct order *) + let dl_settings = Xml.Element("dl_settings", [], settings_xml) in + Xml.Element ("settings", [], [dl_settings]) + diff --git a/sw/lib/ocaml/telemetry.ml b/sw/lib/ocaml/telemetry.ml new file mode 100644 index 0000000000..1f6b5600b4 --- /dev/null +++ b/sw/lib/ocaml/telemetry.ml @@ -0,0 +1,145 @@ +(* + * Copyright (C) 2017 Gautier Hattenberger + * Cyril Allignol + * + * 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 + * . + *) + +(** + * Periodic telemetry module for parsing XML config files + *) + +type msg_period = MsgPeriod of float | MsgFreq of float + +module Message = struct + + type t = { + name: string; + period: msg_period; + phase: int option; + xml: Xml.xml } + + let from_xml = function + | Xml.Element ("message", _, []) as xml -> + { name = Xml.attrib xml "name"; + period = begin + match ExtXml.attrib_opt_float xml "period", + ExtXml.attrib_opt_float xml "freq" with + | Some t, None -> MsgPeriod t + | None, Some f -> MsgFreq f + | Some _, Some _ -> failwith "Telemetry.Message.from_xml: either specify 'period' or 'freq' attribute, not both" + | None, None -> failwith "Telemetry.Message.from_xml: specify 'period' or 'freq' attribute" + end; + phase = ExtXml.attrib_opt_int xml "phase"; + xml + } + | _ -> failwith "Telemetry.Message.from_xml: unreachable" + +end + +module Mode = struct + + type t = { + name: string; + key_press: string option; + messages: Message.t list; + xml: Xml.xml } + + let from_xml = function + | Xml.Element ("mode", _, messages) as xml -> + { name = Xml.attrib xml "name"; + key_press = ExtXml.attrib_opt xml "key_press"; + messages = List.map Message.from_xml messages; + xml } + | _ -> failwith "Telemetry.Mode.from_xml: unreachable" + +end + +module Process = struct + + type t = { + name: string; + proc_type: string option; + modes: Mode.t list; + xml: Xml.xml } + + let from_xml = function + | Xml.Element ("process", attribs, modes) as xml -> + { name = Xml.attrib xml "name"; + proc_type = ExtXml.attrib_opt xml "type"; + modes = List.map Mode.from_xml modes; + xml } + | _ -> failwith "Telemetry.Process.from_xml: unreachable" + +end + +type t = { + filename: string; + processes: Process.t list; + xml: Xml.xml +} + +let from_xml = function + | Xml.Element ("telemetry", [], processes) as xml -> + { filename = ""; + processes = List.map Process.from_xml processes; + xml + } + | _ -> failwith "Telemetry.from_xml: unreachable" + +let from_file = fun filename -> + let t = from_xml (Xml.parse_file filename) in + { t with filename } + + +(* return a Settings object from telemetry process and modes *) +let get_sys_telemetry_settings = fun telemetry -> + match telemetry with + | None -> None + | Some telemetry -> + (* build a XML node corresponding to the settings *) + let tl_settings = List.fold_left (fun lp p -> + if List.length p.Process.modes > 1 then begin + (* only if more than one mode *) + let p_name = p.Process.name in + let modes = List.map (fun m -> m.Mode.name) p.Process.modes in + let nb_modes = List.length modes in + match nb_modes with + | 0 | 1 -> lp (* Nothing to do if 1 or zero mode *) + | _ -> (* add settings with all modes *) + let (key, _) = List.fold_left (fun (lk, i) m -> + match m.Mode.key_press with + | None -> (lk, i+1) + | Some k -> ((Xml.Element ("key_press", [("key", k); ("value", string_of_int i)], [])) :: lk, i+1) + ) ([], 0) p.Process.modes in + lp @ [Xml.Element ("dl_setting", + [("min", "0"); + ("step", "1"); + ("max", string_of_int (nb_modes-1)); + ("var", "telemetry_mode_"^p_name); + ("shortname", p_name); + ("values", String.concat "|" modes); + ("header", "generated/periodic_telemetry")], key)] + end + else lp + ) [] telemetry.processes in + if List.length tl_settings > 0 then + let xml = Xml.Element("dl_settings",[("name","Telemetry")],tl_settings) in + Some (Settings.from_xml (Xml.Element("settings",[],[xml]))) + else + None + diff --git a/sw/lib/ocaml/xml2h.ml b/sw/lib/ocaml/xml2h.ml index 1088eb3eba..aa7255c3d9 100644 --- a/sw/lib/ocaml/xml2h.ml +++ b/sw/lib/ocaml/xml2h.ml @@ -36,6 +36,14 @@ let define = fun n x -> let define_string = fun n x -> define n ("\""^x^"\"") +let define_out = fun out n x -> + match x with + "" -> fprintf out "#define %s\n" n + | _ -> fprintf out "#define %s %s\n" n x + +let define_string_out = fun out n x -> + define_out out n ("\""^x^"\"") + let xml_error s = failwith ("Bad XML tag: "^s^ " expected") @@ -47,24 +55,24 @@ let sprint_float_array = fun l -> | x::xs -> x ^","^ loop xs in "{" ^ loop l -let begin_out = fun xml_file h_name out -> +let begin_out = fun out xml_file h_name -> fprintf out "/* This file has been generated from %s */\n" xml_file; fprintf out "/* Version %s */\n" (Env.get_paparazzi_version ()); fprintf out "/* Please DO NOT EDIT */\n\n"; fprintf out "#ifndef %s\n" h_name; fprintf out "#define %s\n\n" h_name -let start_and_begin_out = fun xml_file h_name out -> +let start_and_begin_out = fun out xml_file h_name -> let xml = ExtXml.parse_file xml_file in - begin_out xml_file h_name out; + begin_out out xml_file h_name; xml let start_and_begin = fun xml_file h_name -> let xml = ExtXml.parse_file xml_file in - begin_out xml_file h_name stdout; + begin_out stdout xml_file h_name; xml -let begin_c_out = fun xml_file name out -> +let begin_c_out = fun out xml_file name -> fprintf out "/* This file has been generated from %s */\n" xml_file; fprintf out "/* Version %s */\n" (Env.get_paparazzi_version ()); fprintf out "/* Please DO NOT EDIT */\n\n"; @@ -72,12 +80,15 @@ let begin_c_out = fun xml_file name out -> let start_and_begin_c = fun xml_file name -> let xml = ExtXml.parse_file xml_file in - begin_c_out xml_file name stdout; + begin_c_out stdout xml_file name; xml let finish = fun h_name -> printf "\n#endif // %s\n" h_name +let finish_out = fun out h_name -> + fprintf out "\n#endif // %s\n" h_name + let warning s = Printf.fprintf stderr "##################################################\n"; Printf.fprintf stderr " %s\n" s; diff --git a/sw/lib/ocaml/xml2h.mli b/sw/lib/ocaml/xml2h.mli index eef950f3ce..95a7f9ea0a 100644 --- a/sw/lib/ocaml/xml2h.mli +++ b/sw/lib/ocaml/xml2h.mli @@ -26,12 +26,15 @@ exception Error of string val nl : unit -> unit val define : string -> string -> unit val define_string : string -> string -> unit +val define_out : out_channel -> string -> string -> unit +val define_string_out : out_channel -> string -> string -> unit val xml_error : string -> 'a val sprint_float_array : string list -> string -val begin_out : string -> string -> out_channel -> unit -val start_and_begin_out : string -> string -> out_channel -> Xml.xml +val begin_out : out_channel -> string -> string -> unit +val start_and_begin_out : out_channel -> string -> string -> Xml.xml val start_and_begin : string -> string -> Xml.xml -val begin_c_out : string -> string -> out_channel -> unit +val begin_c_out : out_channel -> string -> string -> unit val start_and_begin_c : string -> string -> Xml.xml val finish : string -> unit +val finish_out : out_channel -> string -> unit val warning : string -> unit diff --git a/sw/logalizer/sd2log.ml b/sw/logalizer/sd2log.ml index b53e1e2b4a..1964b32836 100644 --- a/sw/logalizer/sd2log.ml +++ b/sw/logalizer/sd2log.ml @@ -55,9 +55,9 @@ let log_xml = fun ac_id -> Not_found -> failwith (sprintf "Error: A/C %d not found in conf.xml" ac_id) in - let expanded_conf_ac = Env.expand_ac_xml ~raise_exception:false conf_ac in + let ac = Aircraft.parse_aircraft ~parse_all:true "" conf_ac in let expanded_conf = - make_element (Xml.tag conf_xml) (Xml.attribs conf_xml) [expanded_conf_ac] in + make_element (Xml.tag conf_xml) (Xml.attribs conf_xml) ac.Aircraft.xml in make_element "configuration" [] diff --git a/sw/simulator/data.ml b/sw/simulator/data.ml index 8a1e9220e2..a149896a23 100644 --- a/sw/simulator/data.ml +++ b/sw/simulator/data.ml @@ -65,7 +65,7 @@ let aircraft = fun name -> let airframe_file = user_conf_path // ExtXml.attrib aircraft_xml "airframe" in { id = id; name = name; - airframe = Gen_common.expand_includes (string_of_int id) (ExtXml.parse_file airframe_file); + airframe = Airframe.expand_includes (string_of_int id) (ExtXml.parse_file airframe_file); flight_plan = ExtXml.parse_file (user_conf_path // ExtXml.attrib aircraft_xml "flight_plan"); radio = ExtXml.parse_file (user_conf_path // ExtXml.attrib aircraft_xml "radio") } diff --git a/sw/supervision/pc_aircraft.ml b/sw/supervision/pc_aircraft.ml index dae685b9d9..b3d7625e53 100644 --- a/sw/supervision/pc_aircraft.ml +++ b/sw/supervision/pc_aircraft.ml @@ -190,10 +190,10 @@ 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_id ac_xml fp_xml settings_modules -> +let get_settings_modules = fun ac_id aircraft_xml settings_modules -> (* get modules *) - let modules = Gen_common.get_modules_of_config ac_id ac_xml fp_xml in - let modules = List.map (fun m -> m.Gen_common.xml, m.Gen_common.file ) modules in + let ac = Aircraft.parse_aircraft ~parse_af:true ~parse_ap:true ~parse_fp:true "" aircraft_xml in + let modules = List.map (fun m -> (m.Module.xml, m.Module.xml_filename)) ac.Aircraft.all_modules in (* get list of settings files *) let settings = List.fold_left (fun l (m, f) -> (* get list of settings_file xml node if any *) @@ -330,23 +330,9 @@ let ac_combo_handler = fun gui (ac_combo:Gtk_tools.combo) target_combo flash_com let aircraft = Hashtbl.find Utils.aircrafts ac_name in let sample = aircraft_sample ac_name "42" in (* update list of modules settings *) - let af_file = (Env.paparazzi_home // "conf" // (Xml.attrib aircraft "airframe")) in - let af_xml = try Xml.parse_file af_file - with - | Xml.File_not_found x -> - gui#label_airframe#set_text ""; - gui#button_clean#misc#set_sensitive false; - gui#button_build#misc#set_sensitive false; - (Gtk_tools.combo_widget target_combo)#misc#set_sensitive false; - (Gtk_tools.combo_widget flash_combo)#misc#set_sensitive false; - 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 = ExtXml.parse_file fp_file in let ac_id = ExtXml.attrib aircraft "ac_id" in let settings_modules = try - get_settings_modules ac_id af_xml fp_xml (ExtXml.attrib_or_default aircraft "settings_modules" "") + get_settings_modules ac_id aircraft (ExtXml.attrib_or_default aircraft "settings_modules" "") with | Failure x -> prerr_endline x; [] | _ -> [] diff --git a/sw/tools/generators/Makefile b/sw/tools/generators/Makefile index 5761c129bf..9904377e4a 100644 --- a/sw/tools/generators/Makefile +++ b/sw/tools/generators/Makefile @@ -28,9 +28,12 @@ INCLUDES = PKG = -package pprz LINKPKG = $(PKG) -linkpkg -dllpath-pkg pprz,pprzlink -all: gen_aircraft.out gen_airframe.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 gen_srtm.out +GENACCMO = gen_airframe.cmo gen_flight_plan.cmo gen_autopilot.cmo gen_periodic.cmo gen_makefile.cmo gen_radio.cmo gen_modules.cmo gen_settings.cmo gen_aircraft.cmo +GENACCMX = $(GENACCMO:.cmo=.cmx) -gen_flight_plan.out : gen_flight_plan.cmo $(LIBPPRZCMA) $(LIBPPRZLINKCMA) +all: gen_aircraft.out gen_ubx.out gen_mtk.out gen_xsens.out gen_abi.out gen_srtm.out dump_flight_plan.out + +gen_aircraft.out : $(GENACCMO) $(LIBPPRZCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $^ @@ -38,6 +41,10 @@ gen_srtm.out : gen_srtm.ml $(LIBPPRZCMA) $(LIBPPRZLINKCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $< +dump_flight_plan.out : gen_flight_plan.ml dump_flight_plan.ml $(LIBPPRZCMA) + @echo OL $@ + $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $^ + %.out : %.ml $(LIBPPRZCMA) $(LIBPPRZLINKCMA) @echo OL $< $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $< diff --git a/sw/tools/generators/dump_flight_plan.ml b/sw/tools/generators/dump_flight_plan.ml new file mode 100644 index 0000000000..e15b78531c --- /dev/null +++ b/sw/tools/generators/dump_flight_plan.ml @@ -0,0 +1,35 @@ +(* + * Dump a flight plan XML file + * + * 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. + * + *) + +let () = + + let fp_xml, dump_out = + try Sys.argv.(1), Sys.argv.(2) + with _ -> + failwith "Dump FP: please choose a flight plan XML and an output file" + in + + let fp = Flight_plan.from_file fp_xml in + Gen_flight_plan.generate fp ~dump:true fp.Flight_plan.filename dump_out + diff --git a/sw/tools/generators/gen_aircraft.ml b/sw/tools/generators/gen_aircraft.ml index 93ad587a8f..ad1918e264 100644 --- a/sw/tools/generators/gen_aircraft.ml +++ b/sw/tools/generators/gen_aircraft.ml @@ -1,7 +1,7 @@ (* - * Call to Makefile.ac with the appropriate attributes from conf.xml - * * Copyright (C) 2003-2009 Pascal Brisset, Antoine Drouin, ENAC + * Copyright (C) 2017 Gautier Hattenberger + * Cyril Allignol * * This file is part of paparazzi. * @@ -16,23 +16,42 @@ * 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. - * + * along with paparazzi; see the file COPYING. If not, see + * . + *) + +(** + * Main generator tool *) open Printf module U = Unix -open Gen_common +module GC = Gen_common +module Af = Airframe +module AfT = Airframe.Target +module AfF = Airframe.Firmware +module GM = Gen_makefile let (//) = Filename.concat let paparazzi_conf = Env.paparazzi_home // "conf" let default_conf_xml = paparazzi_conf // "conf.xml" +let airframe_h = "airframe.h" +let flight_plan_h = "flight_plan.h" +let flight_plan_xml = "flight_plan.xml" +let radio_h = "radio.h" +let periodic_h = "periodic_telemetry.h" +let modules_h = "modules.h" +let settings_h = "settings.h" +let settings_xml = "settings.xml" +let default_periodic_freq = 60 +let default_modules_freq = 60 + +let get_string_opt = fun x -> match x with Some s -> s | None -> "" + let mkdir = fun d -> assert (Sys.command (sprintf "mkdir -p %s" d) = 0) @@ -56,309 +75,6 @@ let check_unique_id_and_name = fun conf conf_xml -> ) conf -let configure_xml2mk = fun ?(default_configure=false) f xml -> - (* all makefiles variables are forced to uppercase *) - let name = Compat.uppercase_ascii (ExtXml.attrib xml "name") - and value = ExtXml.attrib_or_default xml "value" "" - and default = ExtXml.attrib_or_default xml "default" "" - and case = ExtXml.attrib_or_default xml "case" "" in - (* either print the default or the normal configure variable *) - if default_configure then begin - (* Only print variable if default is set but not value *) - if String.length default > 0 && String.length value = 0 then - fprintf f "%s ?= %s\n" name default; - (* also providing lower and upper case version on request *) - if Str.string_match (Str.regexp ".*lower.*") case 0 then - fprintf f "%s_LOWER = $(shell echo $(%s) | tr A-Z a-z)\n" name name; - if Str.string_match (Str.regexp ".*upper.*") case 0 then - fprintf f "%s_UPPER = $(shell echo $(%s) | tr a-z A-Z)\n" name name - end - else - (* Only print variable if value is not empty *) - if String.length value > 0 then - fprintf f "%s = %s\n" name value; - (* Or if only the name is given (unset a variable *) - if String.length value = 0 && String.length default = 0 && String.length case = 0 then - fprintf f "%s =\n" name - -let include_xml2mk = fun f ?(target="$(TARGET)") ?(vpath=None) xml -> - let name = Xml.attrib xml "name" - and path = match vpath with Some vp -> vp ^ "/" | None -> "" in - let flag = sprintf "%s.CFLAGS += -I%s%s" target path name in - try - (* TODO: add condition in xml syntax ? *) - let cond = Xml.attrib xml "cond" in - fprintf f "%s\n%s\nendif\n" cond flag - with Xml.No_attribute _ -> fprintf f "%s\n" flag - -let flag_xml2mk = fun f ?(target="$(TARGET)") xml -> - let name = Xml.attrib xml "name" - and value = Xml.attrib xml "value" in - let flag = sprintf "%s.%s += -%s" target name value in - try - (* TODO: add condition in xml syntax ? *) - let cond = Xml.attrib xml "cond" in - fprintf f "%s\n%s\nendif\n" cond flag - with Xml.No_attribute _ -> fprintf f "%s\n" flag - -let define_xml2mk = fun f ?(target="$(TARGET)") xml -> - let name = Xml.attrib xml "name" - and value = try Some (Xml.attrib xml "value") with _ -> None in - let _ = try - ignore(Xml.attrib xml "unit"); - prerr_endline ("Warning: 'unit' attribute for '"^name^"' in firmware section is not handled") with _ -> () - in - let flag_type = fun s -> - match ExtXml.attrib_or_default xml "type" "raw", value with - | "string", Some v -> "=\\\""^v^"\\\"" - | _, Some v -> "="^v - | _, _ -> "" - in - let flag = sprintf "%s.CFLAGS += -D%s%s" target name (flag_type value) in - try - (* TODO: add condition in xml syntax ? *) - let cond = Xml.attrib xml "cond" in - fprintf f "%s\n%s\nendif\n" cond flag - with Xml.No_attribute _ -> fprintf f "%s\n" flag - -let raw_xml2mk = fun f name xml -> - match Xml.children xml with - | [Xml.PCData s] -> fprintf f "%s\n" s - | _ -> eprintf "Warning: wrong makefile section in '%s': %s\n" - name (Xml.to_string_fmt xml) - -let file_xml2mk = fun f ?(arch = false) dir_name target xml -> - let name = Xml.attrib xml "name" in - let dir_name = ExtXml.attrib_or_default xml "dir" ("$(" ^ dir_name ^ ")") in - let cond, cond_end = try "\n"^(Xml.attrib xml "cond")^"\n", "\nendif" with Xml.No_attribute _ -> "", "" in - let fmt = - if arch then format_of_string "%s%s.srcs += arch/$(ARCH)/%s/%s%s\n" - else format_of_string "%s%s.srcs += %s/%s%s\n" in - fprintf f fmt cond target dir_name name cond_end - -(* only print the configuration flags for a module - * 'raw' section are not handled here - *) -let module_configure_xml2mk = fun ?(default_configure=false) f target firmware m -> - (* print global config flags *) - List.iter (fun flag -> - match Compat.lowercase_ascii (Xml.tag flag) with - | "configure" -> configure_xml2mk ~default_configure f flag - | _ -> ()) m.param; - (* Look for makefile section *) - ExtXml.iter_tag "makefile" - (fun section -> - (* Look for defines, flags, files, ... if target is matching *) - let section = - let targets = Gen_common.targets_of_field section Env.default_module_targets in - if Gen_common.test_targets target targets then section else Xml.Element ("makefile", [], []) - in - (* keep section if firmware is also matching or not speficied *) - let section = begin - try - if Xml.attrib section "firmware" = firmware then section - else Xml.Element ("makefile", [], []) - with _ -> section end - in - Xml.iter - (fun field -> - match Compat.lowercase_ascii (Xml.tag field) with - | "configure" -> configure_xml2mk ~default_configure f field - | _ -> () - ) section - ) m.xml - -(* module files and flags except configuration flags *) -let module_xml2mk = fun f target firmware m -> - let name = ExtXml.attrib m.xml "name" in - let dir = try Xml.attrib m.xml "dir" with Xml.No_attribute _ -> name in - let dir_name = Compat.uppercase_ascii dir ^ "_DIR" in - (* print global flags as compilation defines and flags *) - fprintf f "\n# makefile for module %s in modules/%s\n" name dir; - List.iter (fun flag -> - match Compat.lowercase_ascii (Xml.tag flag) with - | "define" -> define_xml2mk f ~target flag - | _ -> ()) m.param; - (* Look for makefile section *) - ExtXml.iter_tag "makefile" - (fun section -> - (* Look for defines, flags, files, ... if target is matching *) - let section = - let targets = Gen_common.targets_of_field section Env.default_module_targets in - if Gen_common.test_targets target targets then section else Xml.Element ("makefile", [], []) - in - (* keep section if firmware is also matching or not speficied *) - let section = begin - try - if Xml.attrib section "firmware" = firmware then section - else Xml.Element ("makefile", [], []) - with _ -> section end - in - (* add condition if need *) - let cond = try Some (Xml.attrib section "cond") with _ -> None in - let _ = match cond with Some c -> fprintf f "%s\n" c | None -> () in - Xml.iter - (fun field -> - match Compat.lowercase_ascii (Xml.tag field) with - | "define" -> define_xml2mk f ~target field - | "include" -> include_xml2mk f ~target ~vpath:m.vpath field - | "flag" -> flag_xml2mk f ~target field - | "file" -> file_xml2mk f dir_name target field - | "file_arch" -> file_xml2mk f ~arch:true dir_name target field - | "raw" -> raw_xml2mk f name field - | _ -> () - ) section; - match cond with Some _ -> fprintf f "endif\n" | None -> () - ) m.xml - -let modules_xml2mk = fun f target ac_id xml fp -> - let modules = Gen_common.get_modules_of_config ~target ~verbose:true ac_id xml fp in - (* print modules directories and includes for all targets *) - fprintf f "\n# include modules directory for all targets\n"; - (* get dir list *) - let dir_list = Gen_common.get_modules_dir modules in - (** include modules directory for ALL targets, not just the defined ones **) - fprintf f "$(TARGET).CFLAGS += -Imodules -Iarch/$(ARCH)/modules\n"; - List.iter - (fun dir -> fprintf f "%s_DIR = modules/%s\n" (Compat.uppercase_ascii dir) dir - ) dir_list; - (* add vpath for external modules *) - List.iter - (fun m -> match m.vpath with - | Some vp -> fprintf f "VPATH += %s\n" vp - | _ -> () - ) modules; - fprintf f "\n"; - modules - -(** Search and dump the makefile sections *) -let dump_makefile_section = fun xml makefile_ac airframe_infile location -> - ExtXml.iter_tag "makefile" - (fun x -> - let loc = ExtXml.attrib_or_default x "location" "before" in - match location, loc with - | "before", "before" | "after", "after" -> - fprintf makefile_ac "\n# raw makefile\n"; - raw_xml2mk makefile_ac airframe_infile x - | _ -> () - ) xml - -(** Firmware Children *) -let subsystem_xml2mk = fun f firmware s -> - let name = ExtXml.attrib s "name" - and s_type = try "_" ^ (Xml.attrib s "type") with Xml.No_attribute _ -> "" in - fprintf f "\n# -subsystem: '%s'\n" name; - let s_config, rest = ExtXml.partition_tag "configure" (Xml.children s) in - let s_defines, _ = ExtXml.partition_tag "define" rest in - (*List.iter (configure_xml2mk f) s_config;*) - List.iter (fun def -> define_xml2mk f def) s_defines; - (* include subsystem *) (* TODO test if file exists with the generator ? *) - let s_name = name ^ s_type ^ ".makefile" in - let s_dir = "CFG_" ^ Compat.uppercase_ascii (Xml.attrib firmware "name") in - fprintf f "ifneq ($(strip $(wildcard $(%s)/%s)),)\n" s_dir s_name; - fprintf f "\tinclude $(%s)/%s\n" s_dir s_name; - fprintf f "else\n"; - fprintf f "\tinclude $(CFG_SHARED)/%s\n" s_name; - fprintf f "endif\n" - -let subsystem_configure_xml2mk = fun f s -> - let s_config, _ = ExtXml.partition_tag "configure" (Xml.children s) in - List.iter (configure_xml2mk f) s_config - -(** if xml node valid module, do notihg, otherwise fall back to subsystem *) -let fallback_subsys_xml2mk = fun f global_targets firmware target xml -> - try - 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_id 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 - let targets, rest = ExtXml.partition_tag "target" rest in - let mods, rest = ExtXml.partition_tag "module" rest in - let subsystems, rest = ExtXml.partition_tag "subsystem" rest in - let defines, _ = ExtXml.partition_tag "define" rest in - (* iter on all targets *) - List.iter (fun target -> - (* get configures, defines and subsystems for this target *) - let t_config, rest = ExtXml.partition_tag "configure" (Xml.children target) in - let t_defines, rest = ExtXml.partition_tag "define" rest in - let t_mods, rest = ExtXml.partition_tag "module" rest in - let t_subsystems, _ = ExtXml.partition_tag "subsystem" rest in - (* print makefile for this target *) - let target_name = Xml.attrib target "name" in - 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_id ac_xml fp in - begin (* Check for "processor" attribute *) - try - let proc = Xml.attrib target "processor" in - 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; - List.iter (subsystem_configure_xml2mk makefile_ac) t_subsystems; - List.iter (subsystem_configure_xml2mk makefile_ac) mods; - List.iter (subsystem_configure_xml2mk makefile_ac) t_mods; - List.iter (module_configure_xml2mk makefile_ac target_name firmware_name) modules; (* print normal configure from module xml *) - fprintf makefile_ac "\ninclude $(PAPARAZZI_SRC)/conf/boards/%s.makefile\n" (Xml.attrib target "board"); - fprintf makefile_ac "include $(PAPARAZZI_SRC)/conf/firmwares/%s.makefile\n\n" (Xml.attrib firmware "name"); - List.iter (module_configure_xml2mk ~default_configure:true makefile_ac target_name firmware_name) modules; (* print default configure from module xml *) - fprintf makefile_ac "\n"; - List.iter (fun def -> define_xml2mk makefile_ac def) defines; - List.iter (fun def -> define_xml2mk makefile_ac def) t_defines; - List.iter (module_xml2mk makefile_ac target_name firmware_name) modules; - List.iter (fallback_subsys_xml2mk makefile_ac (Gen_common.Var "") firmware target_name) mods; - List.iter (fallback_subsys_xml2mk makefile_ac (Gen_common.Var "") firmware target_name) t_mods; - List.iter (subsystem_xml2mk makefile_ac firmware) t_subsystems; - List.iter (subsystem_xml2mk makefile_ac firmware) subsystems; - fprintf makefile_ac "\nendif # end of target '%s'\n\n" target_name - ) targets - - -(** Search and dump the firmware section *) -let dump_firmware = fun f ac_id 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_id ac_xml firmware fp - with Xml.No_attribute _ -> failwith "Warning: firmware name is undeclared" - -let dump_firmware_sections = fun makefile_ac ac_id fp xml -> - ExtXml.iter_tag "firmware" - (fun tag -> dump_firmware makefile_ac ac_id xml tag fp) xml - -(** Extracts the makefile sections of an airframe file *) -let extract_makefile = fun ac_id airframe_file flight_plan_file makefile_ac -> - let xml = ExtXml.parse_file airframe_file in - let xml = Gen_common.expand_includes ac_id xml in - let fp = ExtXml.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; - fprintf f "# Version %s\n" (Env.get_paparazzi_version ()); - fprintf f "# Please DO NOT EDIT\n"; - fprintf f "AC_ID=%s\n" ac_id; - - (** 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 ac_id 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 - let is_older = fun target_file dep_files -> not (Sys.file_exists target_file) || let target_file_time = (U.stat target_file).U.st_mtime in @@ -367,80 +83,115 @@ let is_older = fun target_file dep_files -> | f :: fs -> target_file_time < (U.stat f).U.st_mtime || loop fs in loop dep_files - let make_element = fun t a c -> Xml.Element (t,a,c) +let copy_file = fun source dest -> + assert (Sys.command (sprintf "cp %s %s" source dest) = 0) + +(** Generate a configuration element + * Also check dependencies + * + * [a' -> (a' -> unit) -> (string * string list) list -> unit] + *) +let generate_config_element = fun ?(verbose=true) elt f dep_list -> + if List.exists (fun (file, dep) -> is_older file dep) dep_list + then begin if verbose then Printf.printf "(updated)"; f elt end + else if verbose then Printf.printf "(unchanged)" (******************************* MAIN ****************************************) let () = + let ac_name = ref None + and target_name = ref None + and conf_xml = ref default_conf_xml + and gen_conf = ref false + and gen_af = ref false + and gen_fp = ref false + and gen_set = ref false + and gen_rc = ref false + and gen_tl = ref false + and gen_ap = ref false + and gen_all = ref false + and gen_ac_dir = ref None + and gen_conf_dir = ref None + and modules_freq = ref default_modules_freq + and tl_freq = ref default_periodic_freq in + + let options = + [ "-name", Arg.String (fun x -> ac_name := Some x), " Aircraft name (mandatory)"; + "-target", Arg.String (fun x -> target_name := Some x), " Target to build (mandatory)"; + "-conf", Arg.String (fun x -> conf_xml := x), (sprintf " Configuration file (default '%s')" default_conf_xml); + "-ac_conf", Arg.Set gen_conf, " Generate expanded aircraft config file"; + "-airframe", Arg.Set gen_af, " Generate airframe file"; + "-flight_plan", Arg.Set gen_fp, " Generate flight plan file"; + "-settings", Arg.Set gen_set, " Generate settings file"; + "-radio", Arg.Set gen_rc, " Generate radio file"; + "-telemetry", Arg.Set gen_tl, " Generate telemetry file"; + "-autopilot", Arg.Set gen_ap, " Generate autopilot file if needed"; + "-all", Arg.Set gen_all, " Generate all files"; + "-ac_dir", Arg.String (fun x -> gen_ac_dir := Some x), "Directory for aircraft generated headers"; + "-conf_dir", Arg.String (fun x -> gen_conf_dir := Some x), "Directory for aircraft generated config"; + "-periodic_freq", Arg.Int (fun x -> tl_freq := x), (sprintf " Periodic telemetry frequency (default %d)" default_periodic_freq); + "-modules_freq", Arg.Int (fun x -> modules_freq := x), (sprintf " Modules frequency (default %d)" default_modules_freq); + ] in + + Arg.parse + options + (fun x -> Printf.fprintf stderr "%s: Warning: Don't do anything with '%s' argument\n" Sys.argv.(0) x) + "Usage: "; + + (* check aircraft and target options *) + let aircraft = + match !ac_name with + | None -> failwith "An aircraft name is mandatory" + | Some ac -> ac + in + let target = + match !target_name with + | None -> failwith "A target name is mandatory" + | Some t -> t + in + Printf.printf "Aircraft generator: '%s' for target '%s'\n%!" aircraft target; + try - if Array.length Sys.argv < 2 || Array.length Sys.argv > 3 then - failwith (sprintf "Usage: %s [conf.xml]" Sys.executable_name); - let aircraft = Sys.argv.(1) in - let conf_xml = if Array.length Sys.argv = 3 then Sys.argv.(2) else default_conf_xml in - let conf = ExtXml.parse_file conf_xml in - check_unique_id_and_name conf conf_xml; + let conf = ExtXml.parse_file !conf_xml in + check_unique_id_and_name conf !conf_xml; let aircraft_xml = try ExtXml.child conf ~select:(fun x -> Xml.attrib x "name" = aircraft) "aircraft" with - Not_found -> failwith (sprintf "Aircraft '%s' not found in '%s'" aircraft conf_xml) + Not_found -> failwith (sprintf "Aircraft '%s' not found in '%s'" aircraft !conf_xml) in let value = fun attrib -> ExtXml.attrib aircraft_xml attrib in - let aircraft_dir = Env.paparazzi_home // "var" // "aircrafts" // aircraft in - let aircraft_conf_dir = aircraft_dir // "conf" in - - 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; - mkdir (aircraft_dir // "fbw"); - mkdir (aircraft_dir // "autopilot"); - mkdir (aircraft_dir // "sim"); - mkdir aircraft_conf_dir; - mkdir (aircraft_conf_dir // "airframes"); - mkdir (aircraft_conf_dir // "flight_plans"); - mkdir (aircraft_conf_dir // "radios"); - mkdir (aircraft_conf_dir // "settings"); - mkdir (aircraft_conf_dir // "telemetry"); - - let target = try Sys.getenv "TARGET" with _ -> "" in - let modules = Gen_common.get_modules_of_config ~target (value "ac_id") (ExtXml.parse_file abs_airframe_file) (ExtXml.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 *) - let settings = List.fold_left (fun l s -> if Gen_common.is_element_unselected ~verbose:true target modules s then l else l @ [s]) [] (Str.split (Str.regexp " ") settings) in - (* update aircraft_xml *) - let aircraft_xml = ExtXml.subst_attrib "settings" (String.concat " " settings) aircraft_xml in - (* add modules settings *) - let settings_modules = try Env.filter_settings (value "settings_modules") with _ -> "" in - (* remove settings if not supported for the current target *) - let settings_modules = List.fold_left (fun l s -> if Gen_common.is_element_unselected ~verbose:true target modules s then l else l @ [s]) [] (Str.split (Str.regexp " ") settings_modules) in - (* update aircraft_xml *) - let aircraft_xml = ExtXml.subst_attrib "settings_modules" (String.concat " " settings_modules) aircraft_xml in - (* finally, concat all settings *) - let settings = settings @ settings_modules in - let settings = if List.length settings = 0 then - begin - fprintf stderr "\nInfo: No 'settings' attribute specified for A/C '%s', using 'settings/dummy.xml'\n\n%!" aircraft; - "settings/dummy.xml" - end - else String.concat " " settings + (* Prepare building folders *) + let default_aircraft_dir = Env.paparazzi_home // "var" // "aircrafts" // aircraft in + let aircraft_dir, aircraft_conf_dir = match !gen_conf_dir with + | None -> default_aircraft_dir, default_aircraft_dir // "conf" + | Some d -> d, d // "conf" in + let aircraft_gen_dir = match !gen_ac_dir with + | None -> default_aircraft_dir // target // "generated" + | Some d -> d // "generated" + in + mkdir aircraft_conf_dir; + mkdir aircraft_gen_dir; + + + (* + * Parse file if needed + *) + + let ac = Aircraft.parse_aircraft ~parse_af:!gen_af ~parse_ap:!gen_ap ~parse_fp:!gen_fp ~parse_rc:!gen_rc ~parse_tl:!gen_tl ~parse_set:!gen_set ~parse_all:!gen_all ~verbose:true target aircraft_xml in + + (* + * Expands the configuration of the A/C into one single file + *) - (** Expands the configuration of the A/C into one single file *) - let conf_aircraft = Env.expand_ac_xml aircraft_xml in let configuration = make_element "configuration" [] - [ make_element "conf" [] [conf_aircraft]; PprzLink.messages_xml () ] in + [ make_element "conf" [] ac.Aircraft.xml; PprzLink.messages_xml () ] in let conf_aircraft_file = aircraft_conf_dir // "conf_aircraft.xml" in let f = open_out conf_aircraft_file in Printf.fprintf f "%s\n" (ExtXml.to_string_fmt configuration); @@ -456,54 +207,157 @@ let () = Printf.fprintf f "%s\n" md5sum; close_out f; - (** Save the configuration for future use *) + (** Save the configuration for future use *) let d = U.localtime (U.gettimeofday ()) in - let filename = sprintf "%02d_%02d_%02d__%02d_%02d_%02d_%s_%s.conf" (d.U.tm_year mod 100) (d.U.tm_mon+1) (d.U.tm_mday) (d.U.tm_hour) (d.U.tm_min) (d.U.tm_sec) md5sum aircraft in + let filename = sprintf "%02d_%02d_%02d__%02d_%02d_%02d_%s_%s.conf" + (d.U.tm_year mod 100) (d.U.tm_mon+1) (d.U.tm_mday) + (d.U.tm_hour) (d.U.tm_min) (d.U.tm_sec) + md5sum aircraft in let d = Env.paparazzi_home // "var" // "conf" in mkdir d; let f = open_out (d // filename) in Printf.fprintf f "%s\n" (ExtXml.to_string_fmt configuration); - close_out f end; + close_out f; + Ocaml_tools.compress (d // filename) + end; - let airframe_dir = Filename.dirname airframe_file in - let var_airframe_dir = aircraft_conf_dir // airframe_dir in - mkdir var_airframe_dir; - assert (Sys.command (sprintf "cp %s %s" (paparazzi_conf // airframe_file) var_airframe_dir) = 0); - (** Calls the Makefile with target and options *) - let make = fun target options -> - let c = sprintf "make -f Makefile.ac AIRCRAFT=%s AC_ID=%s AIRFRAME_XML=%s TELEMETRY=%s SETTINGS=\"%s\" MD5SUM=\"%s\" %s %s" aircraft (value "ac_id") airframe_file (value "telemetry") settings md5sum options target in - begin (** Quiet is speficied in the Makefile *) - try if Sys.getenv "Q" <> "@" then raise Not_found with - Not_found -> prerr_endline c - end; - let returned_code = Sys.command c in - if returned_code <> 0 then - exit returned_code in - (** Calls the makefile if the optional attribute is available *) - let make_opt = fun target var attr -> - try - let value = Xml.attrib aircraft_xml attr in - make target (sprintf "%s=%s" var value) - with Xml.No_attribute _ -> () in + (* + * Generate output files + *) + + + Printf.printf "Dumping flight plan XML and header...%!"; + let abs_flight_plan_h = aircraft_gen_dir // flight_plan_h in + let abs_flight_plan_dump = aircraft_dir // flight_plan_xml in + begin match ac.Aircraft.flight_plan with + | None -> Printf.printf "(skip)" + | Some flight_plan -> + generate_config_element flight_plan + (fun e -> + Gen_flight_plan.generate + e flight_plan.Flight_plan.filename abs_flight_plan_h) + [ (abs_flight_plan_h, [flight_plan.Flight_plan.filename]) ]; + generate_config_element ~verbose:false flight_plan + (fun e -> + Gen_flight_plan.generate + e ~dump:true flight_plan.Flight_plan.filename abs_flight_plan_dump) + [ (abs_flight_plan_dump, [flight_plan.Flight_plan.filename]) ]; + (* save conf file in aircraft conf dir *) + begin match Aircraft.get_element_relative_path (!gen_fp || !gen_all) aircraft_xml "flight_plan" with + | None -> () + | Some f -> + let dir = (aircraft_conf_dir // (Filename.dirname f)) in + mkdir dir; + copy_file flight_plan.Flight_plan.filename dir; + end; + end; + Printf.printf " done\n%!"; + + Printf.printf "Dumping airframe header...%!"; + let abs_airframe_h = aircraft_gen_dir // airframe_h in + begin match ac.Aircraft.airframe with + | None -> Printf.printf "(skip)" + | Some airframe -> + let inc = List.map (fun i -> + let href = i.Airframe.Include.href in + Str.global_replace (Str.regexp "\\$AC_ID") (value "ac_id") href + ) airframe.Af.includes in + generate_config_element airframe + (fun e -> + Gen_airframe.generate + e (value "ac_id") (get_string_opt !ac_name) + md5sum airframe.Airframe.filename abs_airframe_h) + [ (abs_airframe_h, [airframe.Airframe.filename] @ inc) ]; + (* save conf file in aircraft conf dir *) + begin match Aircraft.get_element_relative_path (!gen_af || !gen_all) aircraft_xml "airframe" with + | None -> () + | Some f -> + let dir = (aircraft_conf_dir // (Filename.dirname f)) in + mkdir dir; + copy_file airframe.Airframe.filename dir; + copy_file (paparazzi_conf // "airframes" // "airframe.dtd") (aircraft_conf_dir // "airframes") + end; + end; + Printf.printf " done\n%!"; + + Printf.printf "Dumping autopilot header...%!"; + begin match ac.Aircraft.autopilots with + | None -> Printf.printf "(skip)" + | Some autopilots -> + List.iter (fun (freq, autopilot) -> + let dep = List.map (fun sm -> + let h_name = paparazzi_conf // ("autopilot_core_"^(Xml.attrib sm "name")^".h") in + (h_name, [autopilot.Autopilot.filename]) + ) (Xml.children autopilot.Autopilot.xml) in + generate_config_element autopilot + (fun e -> Gen_autopilot.generate e freq aircraft_gen_dir) dep + ) autopilots + end; + Printf.printf " done\n%!"; + + Printf.printf "Dumping radio header...%!"; + let abs_radio_h = aircraft_gen_dir // radio_h in + begin match ac.Aircraft.radio with + | None -> Printf.printf "(skip)" + | Some radio -> + generate_config_element radio + (fun e -> Gen_radio.generate e radio.Radio.filename abs_radio_h) + [ (abs_radio_h, [radio.Radio.filename]) ] end; + Printf.printf " done\n%!"; + + Printf.printf "Dumping telemetry header...%!"; + let abs_telemetry_h = aircraft_gen_dir // periodic_h in + begin match ac.Aircraft.telemetry with + | None -> Printf.printf "(skip)" + | Some telemetry -> + generate_config_element telemetry + (fun e -> Gen_periodic.generate e !tl_freq abs_telemetry_h) + [ (abs_telemetry_h, [telemetry.Telemetry.filename]) ] end; + Printf.printf " done\n%!"; + + Printf.printf "Dumping modules header...%!"; + let loaded_modules = Aircraft.get_loaded_modules ac.Aircraft.config_by_target target in + let abs_modules_h = aircraft_gen_dir // modules_h in + generate_config_element loaded_modules + (fun e -> Gen_modules.generate e !modules_freq "" abs_modules_h) + [ abs_modules_h, List.map (fun m -> m.Module.xml_filename) loaded_modules ]; + Printf.printf " done\n%!"; + + + Printf.printf "Dumping settings XML and header...%!"; + let abs_settings_h = aircraft_gen_dir // settings_h in + let abs_settings_xml = aircraft_dir // settings_xml in + begin match ac.Aircraft.settings with + | None -> Printf.printf "(skip)" + | Some settings -> + let dep_list = List.fold_left + (fun l s -> if Sys.file_exists s.Settings.filename then s.Settings.filename :: l else l) [] settings + in + generate_config_element settings + (fun e -> Gen_settings.generate e dep_list abs_settings_xml abs_settings_h) + [ (abs_settings_h, dep_list) ] end; + Printf.printf " done\n%!"; + let temp_makefile_ac = Filename.temp_file "Makefile.ac" "tmp" in - - let () = extract_makefile (value "ac_id") abs_airframe_file abs_flight_plan_file temp_makefile_ac in + begin match ac.Aircraft.airframe, ac.Aircraft.flight_plan with + | Some af, Some fp -> + GM.generate_makefile (value "ac_id") ac.Aircraft.config_by_target temp_makefile_ac + | _ -> Printf.eprintf "Missing airframe or flight_plan" end; (* Create Makefile.ac only if needed *) let makefile_ac = aircraft_dir // "Makefile.ac" in - if is_older makefile_ac (abs_airframe_file ::(List.map (fun m -> m.file) modules)) then - assert(Sys.command (sprintf "mv %s %s" temp_makefile_ac makefile_ac) = 0); + match ac.Aircraft.airframe with + | None -> () + | Some airframe -> + let module_files = List.map (fun m -> m.Module.xml_filename) loaded_modules in + if is_older makefile_ac (airframe.Airframe.filename :: module_files) + then + assert(Sys.command (sprintf "mv %s %s" temp_makefile_ac makefile_ac) = 0) + ; - (* Get TARGET env, needed to build modules.h according to the target *) - let t = try Printf.sprintf "TARGET=%s" (Sys.getenv "TARGET") with _ -> "" in - (* Get FLIGHT_PLAN attribute, needed to build modules.h as well FIXME *) - let t = t ^ try Printf.sprintf " FLIGHT_PLAN=%s" (Xml.attrib aircraft_xml "flight_plan") with _ -> "" in - make_opt "radio_ac_h" "RADIO" "radio"; - make_opt "flight_plan_ac_h" "FLIGHT_PLAN" "flight_plan"; - make "all_ac_h" t with Failure f -> prerr_endline f; exit 1 diff --git a/sw/tools/generators/gen_airframe.ml b/sw/tools/generators/gen_airframe.ml index f24162acce..9c38d9197c 100644 --- a/sw/tools/generators/gen_airframe.ml +++ b/sw/tools/generators/gen_airframe.ml @@ -1,7 +1,7 @@ (* - * XML preprocessing for airframe parameters - * * Copyright (C) 2003-2006 Pascal Brisset, Antoine Drouin + * Copyright (C) 2017 Gautier Hattenberger + * Cyril Allignol * * This file is part of paparazzi. * @@ -16,10 +16,12 @@ * 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. - * + * along with paparazzi; see the file COPYING. If not, see + * . + *) + +(** + * XML preprocessing for airframe parameters *) let max_pprz = 9600. (* !!!! MAX_PPRZ From paparazzi.h !!!! *) @@ -41,6 +43,7 @@ let get_servo_driver = fun servo_name -> Hashtbl.find servos_drivers servo_name with Not_found -> failwith (sprintf "gen_airframe, Unknown servo: %s" servo_name) + let get_list_of_drivers = fun () -> let l = ref [] in Hashtbl.iter @@ -49,21 +52,12 @@ let get_list_of_drivers = fun () -> !l -let define_macro name n x = - let a = fun s -> ExtXml.attrib x s in - printf "#define %s(" name; - match n with (* Do we really need more ??? *) - 1 -> printf "x1) (%s*(x1))\n" (a "coeff1") - | 2 -> printf "x1,x2) (%s*(x1)+ %s*(x2))\n" (a "coeff1") (a "coeff2") - | 3 -> printf "x1,x2,x3) (%s*(x1)+ %s*(x2)+%s*(x3))\n" (a "coeff1") (a "coeff2") (a "coeff3") - | _ -> failwith "define_macro" - -let define_integer name v n = +let define_integer out name v n = let max_val = 1 lsl n in let (v,s) = if v >= 0. then (v, 1) else (-.v, -1) in let print = fun name num den -> - define (name^"_NUM") (string_of_int num); - define (name^"_DEN") (string_of_int den) in + define_out out (name^"_NUM") (string_of_int num); + define_out out (name^"_DEN") (string_of_int den) in let rec continious_frac = fun a x num den s -> let x1 = 1. /. (x -. (float_of_int a)) in let a1 = truncate x1 in @@ -132,7 +126,7 @@ let rec string_from_type = fun name v t -> | _ -> v -let parse_element = fun prefix s -> +let parse_element = fun out prefix s -> match Xml.tag s with "define" -> begin try @@ -144,44 +138,40 @@ let parse_element = fun prefix s -> in let name = (prefix^ExtXml.attrib s "name") in let t = ExtXml.attrib_or_default s "type" "" in - define name (string_from_type name value t); - define_integer name (ExtXml.float_attrib s "value") (ExtXml.int_attrib s "integer"); + define_out out name (string_from_type name value t); + define_integer out name (ExtXml.float_attrib s "value") (ExtXml.int_attrib s "integer"); with _ -> (); end - | "linear" -> - let name = ExtXml.attrib s "name" - and n = int_of_string (ExtXml.attrib s "arity") in - define_macro (prefix^name) n s - | _ -> xml_error "define|linear" + | _ -> xml_error "define" -let print_reverse_servo_table = fun driver servos -> +let print_reverse_servo_table = fun out driver servos -> let d = match driver with "Default" -> "" | _ -> "_"^(Compat.uppercase_ascii driver) in - printf "static inline int get_servo_min%s(int _idx) {\n" d; - printf " switch (_idx) {\n"; + fprintf out "static inline int get_servo_min%s(int _idx) {\n" d; + fprintf out " switch (_idx) {\n"; List.iter (fun c -> let name = ExtXml.attrib c "name" in - printf " case SERVO_%s: return SERVO_%s_MIN;\n" name name; + fprintf out " case SERVO_%s: return SERVO_%s_MIN;\n" name name; ) servos; - printf " default: return 0;\n"; - printf " };\n"; - printf "}\n\n"; - printf "static inline int get_servo_max%s(int _idx) {\n" d; - printf " switch (_idx) {\n"; + fprintf out " default: return 0;\n"; + fprintf out " };\n"; + fprintf out "}\n\n"; + fprintf out "static inline int get_servo_max%s(int _idx) {\n" d; + fprintf out " switch (_idx) {\n"; List.iter (fun c -> let name = ExtXml.attrib c "name" in - printf " case SERVO_%s: return SERVO_%s_MAX;\n" name name; + fprintf out " case SERVO_%s: return SERVO_%s_MAX;\n" name name; ) servos; - printf " default: return 0;\n"; - printf " };\n"; - printf "}\n\n" + fprintf out " default: return 0;\n"; + fprintf out " };\n"; + fprintf out "}\n\n" -let parse_servo = fun driver c -> +let parse_servo = fun out driver c -> let shortname = ExtXml.attrib c "name" in let name = "SERVO_"^shortname and no_servo = int_of_string (ExtXml.attrib c "no") in - define name (string_of_int no_servo); + define_out out name (string_of_int no_servo); let s_min = fos (ExtXml.attrib c "min" ) and neutral = fos (ExtXml.attrib c "neutral") @@ -190,18 +180,18 @@ let parse_servo = fun driver c -> let travel_up = (s_max-.neutral) /. max_pprz and travel_down = (neutral-.s_min) /. max_pprz in - define (name^"_NEUTRAL") (sof neutral); - define (name^"_TRAVEL_UP") (sof travel_up); - define_integer (name^"_TRAVEL_UP") travel_up 16; - define (name^"_TRAVEL_DOWN") (sof travel_down); - define_integer (name^"_TRAVEL_DOWN") travel_down 16; + define_out out (name^"_NEUTRAL") (sof neutral); + define_out out (name^"_TRAVEL_UP") (sof travel_up); + define_integer out (name^"_TRAVEL_UP") travel_up 16; + define_out out (name^"_TRAVEL_DOWN") (sof travel_down); + define_integer out (name^"_TRAVEL_DOWN") travel_down 16; let s_min = min s_min s_max and s_max = max s_min s_max in - define (name^"_MAX") (sof s_max); - define (name^"_MIN") (sof s_min); - nl (); + define_out out (name^"_MAX") (sof s_max); + define_out out (name^"_MIN") (sof s_min); + fprintf out "\n"; (* Memorize the associated driver (if any) and global index (insertion order) *) let global_idx = Hashtbl.length servos_drivers in @@ -215,81 +205,81 @@ let preprocess_value = fun s v prefix -> let s = Str.global_replace pprz_value (sprintf "%s[%s_\\1]" v prefix) s in Str.global_replace var_value "_var_\\1" s -let print_actuators_idx = fun () -> +let print_actuators_idx = fun out -> Hashtbl.iter (fun s (d, i) -> - printf "#define SERVO_%s_IDX %d\n" s i; + fprintf out "#define SERVO_%s_IDX %d\n" s i; (* Set servo macro *) - printf "#define Set_%s_Servo(_v) { \\\n" s; - printf " actuators[SERVO_%s_IDX] = Clip(_v, SERVO_%s_MIN, SERVO_%s_MAX); \\\n" s s s; - printf " Actuator%sSet(SERVO_%s, actuators[SERVO_%s_IDX]); \\\n" d s s; - printf "}\n\n" + fprintf out "#define Set_%s_Servo(_v) { \\\n" s; + fprintf out " actuators[SERVO_%s_IDX] = Clip(_v, SERVO_%s_MIN, SERVO_%s_MAX); \\\n" s s s; + fprintf out " Actuator%sSet(SERVO_%s, actuators[SERVO_%s_IDX]); \\\n" d s s; + fprintf out "}\n\n" ) servos_drivers; - define "ACTUATORS_NB" (string_of_int (Hashtbl.length servos_drivers)); - nl () + define_out out "ACTUATORS_NB" (string_of_int (Hashtbl.length servos_drivers)); + fprintf out "\n" -let parse_command_laws = fun command -> +let parse_command_laws = fun out command -> let a = fun s -> ExtXml.attrib command s in match Xml.tag command with "set" -> let servo = a "servo" and value = a "value" in let v = preprocess_value value "values" "COMMAND" in - printf " command_value = %s; \\\n" v; - printf " command_value *= command_value>0 ? SERVO_%s_TRAVEL_UP_NUM : SERVO_%s_TRAVEL_DOWN_NUM; \\\n" servo servo; - printf " command_value /= command_value>0 ? SERVO_%s_TRAVEL_UP_DEN : SERVO_%s_TRAVEL_DOWN_DEN; \\\n" servo servo; - printf " servo_value = SERVO_%s_NEUTRAL + command_value; \\\n" servo; - printf " Set_%s_Servo(servo_value); \\\n\\\n" servo + fprintf out " command_value = %s; \\\n" v; + fprintf out " command_value *= command_value>0 ? SERVO_%s_TRAVEL_UP_NUM : SERVO_%s_TRAVEL_DOWN_NUM; \\\n" servo servo; + fprintf out " command_value /= command_value>0 ? SERVO_%s_TRAVEL_UP_DEN : SERVO_%s_TRAVEL_DOWN_DEN; \\\n" servo servo; + fprintf out " servo_value = SERVO_%s_NEUTRAL + command_value; \\\n" servo; + fprintf out " Set_%s_Servo(servo_value); \\\n\\\n" servo | "let" -> let var = a "var" and value = a "value" in let v = preprocess_value value "values" "COMMAND" in - printf " int32_t _var_%s = %s; \\\n" var v + fprintf out " int32_t _var_%s = %s; \\\n" var v | "call" -> let f = a "fun" in - printf " %s; \\\n\\\n" f + fprintf out " %s; \\\n\\\n" f | "ratelimit" -> let var = a "var" and value = a "value" and rate_min = a "rate_min" and rate_max = a "rate_max" in let v = preprocess_value value "values" "COMMAND" in - printf " static int32_t _var_%s = 0; _var_%s += Clip((%s) - (_var_%s), (%s), (%s)); \\\n" var var v var rate_min rate_max + fprintf out " static int32_t _var_%s = 0; _var_%s += Clip((%s) - (_var_%s), (%s), (%s)); \\\n" var var v var rate_min rate_max | "define" -> - parse_element "" command + parse_element out "" command | _ -> xml_error "set|let" -let parse_rc_commands = fun rc -> +let parse_rc_commands = fun out rc -> let a = fun s -> ExtXml.attrib rc s in match Xml.tag rc with "set" -> let com = a "command" and value = a "value" in let v = preprocess_value value "_rc_array" "RADIO" in - printf " _commands_array[COMMAND_%s] = %s;\\\n" com v; + fprintf out " _commands_array[COMMAND_%s] = %s;\\\n" com v; | "let" -> let var = a "var" and value = a "value" in let v = preprocess_value value "rc_values" "RADIO" in - printf " int16_t _var_%s = %s;\\\n" var v + fprintf out " int16_t _var_%s = %s;\\\n" var v | "define" -> - parse_element "" rc + parse_element out "" rc | _ -> xml_error "set|let" -let parse_ap_only_commands = fun ap_only -> +let parse_ap_only_commands = fun out ap_only -> let a = fun s -> ExtXml.attrib ap_only s in match Xml.tag ap_only with "copy" -> let com = a "command" in - printf " commands[COMMAND_%s] = ap_commands[COMMAND_%s];\\\n" com com + fprintf out " commands[COMMAND_%s] = ap_commands[COMMAND_%s];\\\n" com com | _ -> xml_error "copy" -let parse_command = fun command no -> +let parse_command = fun out command no -> let command_name = "COMMAND_"^ExtXml.attrib command "name" in - define command_name (string_of_int no); + define_out out command_name (string_of_int no); let failsafe_value = int_of_string (ExtXml.attrib command "failsafe_value") in { failsafe_value = failsafe_value; foo = 0} -let parse_heli_curves = fun heli_curves -> +let parse_heli_curves = fun out heli_curves -> let a = fun s -> ExtXml.attrib heli_curves s in match Xml.tag heli_curves with "curve" -> @@ -305,85 +295,85 @@ let parse_heli_curves = fun heli_curves -> failwith (Printf.sprintf "Amount of throttle points not the same as collective in throttle curve ('%s', '%s')" throttle collective); if nb_throttle <> nb_rpm && nb_rpm <> 0 then failwith (Printf.sprintf "Amount of throttle points not the same as rpm in throttle curve ('%s', '%s', '%s')" throttle collective rpm); - printf " {.nb_points = %i, \\\n" nb_throttle; - printf " .throttle = {%s}, \\\n" throttle; + fprintf out " {.nb_points = %i, \\\n" nb_throttle; + fprintf out " .throttle = {%s}, \\\n" throttle; if nb_rpm <> 0 then - printf " .rpm = {%s}, \\\n" rpm + fprintf out " .rpm = {%s}, \\\n" rpm else - printf " .rpm = {0xFFFF}, \\\n"; - printf " .collective = {%s}}, \\\n" collective + fprintf out " .rpm = {0xFFFF}, \\\n"; + fprintf out " .collective = {%s}}, \\\n" collective | _ -> xml_error "mixer" -let rec parse_section = fun ac_id s -> +let rec parse_section = fun out ac_id s -> match Xml.tag s with "section" -> let prefix = ExtXml.attrib_or_default s "prefix" "" in - define ("SECTION_"^ExtXml.attrib s "name") "1"; - List.iter (parse_element prefix) (Xml.children s); - nl () + define_out out ("SECTION_"^ExtXml.attrib s "name") "1"; + List.iter (parse_element out prefix) (Xml.children s); + fprintf out "\n"; | "servos" -> let driver = ExtXml.attrib_or_default s "driver" "Default" in let servos = Xml.children s in let nb_servos = List.fold_right (fun s m -> max (int_of_string (ExtXml.attrib s "no")) m) servos min_int + 1 in - define (sprintf "SERVOS_%s_NB" (Compat.uppercase_ascii driver)) (string_of_int nb_servos); - printf "#include \"subsystems/actuators/actuators_%s.h\"\n" (Compat.lowercase_ascii driver); - nl (); - List.iter (parse_servo driver) servos; - print_reverse_servo_table driver servos; - nl () + define_out out (sprintf "SERVOS_%s_NB" (Compat.uppercase_ascii driver)) (string_of_int nb_servos); + fprintf out "#include \"subsystems/actuators/actuators_%s.h\"\n" (Compat.lowercase_ascii driver); + fprintf out "\n"; + List.iter (parse_servo out driver) servos; + print_reverse_servo_table out driver servos; + fprintf out "\n" | "commands" -> let commands = Array.of_list (Xml.children s) in - let commands_params = Array.mapi (fun i c -> parse_command c i) commands in - define "COMMANDS_NB" (string_of_int (Array.length commands)); - define "COMMANDS_FAILSAFE" (sprint_float_array (List.map (fun x -> string_of_int x.failsafe_value) (Array.to_list commands_params))); - nl (); nl () + let commands_params = Array.mapi (fun i c -> parse_command out c i) commands in + define_out out "COMMANDS_NB" (string_of_int (Array.length commands)); + define_out out "COMMANDS_FAILSAFE" (sprint_float_array (List.map (fun x -> string_of_int x.failsafe_value) (Array.to_list commands_params))); + fprintf out "\n\n" | "rc_commands" -> - printf "#define SetCommandsFromRC(_commands_array, _rc_array) { \\\n"; - List.iter parse_rc_commands (Xml.children s); - printf "}\n\n" + fprintf out "#define SetCommandsFromRC(_commands_array, _rc_array) { \\\n"; + List.iter (parse_rc_commands out) (Xml.children s); + fprintf out "}\n\n" | "auto_rc_commands" -> - printf "#define SetAutoCommandsFromRC(_commands_array, _rc_array) { \\\n"; - List.iter parse_rc_commands (Xml.children s); - printf "}\n\n" + fprintf out "#define SetAutoCommandsFromRC(_commands_array, _rc_array) { \\\n"; + List.iter (parse_rc_commands out) (Xml.children s); + fprintf out "}\n\n" | "ap_only_commands" -> - printf "#define SetApOnlyCommands(ap_commands) { \\\n"; - List.iter parse_ap_only_commands (Xml.children s); - printf "}\n\n" + fprintf out "#define SetApOnlyCommands(ap_commands) { \\\n"; + List.iter (parse_ap_only_commands out) (Xml.children s); + fprintf out "}\n\n" | "command_laws" -> (* print actuators index and set macros *) - print_actuators_idx (); + print_actuators_idx out; (* print init and commit actuators macros *) let drivers = get_list_of_drivers () in - printf "#define AllActuatorsInit() { \\\n"; - List.iter (fun d -> printf " Actuators%sInit();\\\n" d) drivers; - printf "}\n\n"; - printf "#define AllActuatorsCommit() { \\\n"; - List.iter (fun d -> printf " Actuators%sCommit();\\\n" d) drivers; - printf "}\n\n"; + fprintf out "#define AllActuatorsInit() { \\\n"; + List.iter (fun d -> fprintf out " Actuators%sInit();\\\n" d) drivers; + fprintf out "}\n\n"; + fprintf out "#define AllActuatorsCommit() { \\\n"; + List.iter (fun d -> fprintf out " Actuators%sCommit();\\\n" d) drivers; + fprintf out "}\n\n"; (* print actuators from commands macro *) - printf "#define SetActuatorsFromCommands(values, AP_MODE) { \\\n"; - printf " int32_t servo_value;\\\n"; - printf " int32_t command_value;\\\n\\\n"; - List.iter parse_command_laws (Xml.children s); - printf " AllActuatorsCommit(); \\\n"; - printf "}\n\n"; + fprintf out "#define SetActuatorsFromCommands(values, AP_MODE) { \\\n"; + fprintf out " int32_t servo_value;\\\n"; + fprintf out " int32_t command_value;\\\n\\\n"; + List.iter (parse_command_laws out) (Xml.children s); + fprintf out " AllActuatorsCommit(); \\\n"; + fprintf out "}\n\n"; | "heli_curves" -> let default = ExtXml.attrib_or_default s "default" "0" in let curves = Xml.children s in let nb_points = List.fold_right (fun s m -> max (List.length (Str.split (Str.regexp ",") (ExtXml.attrib s "throttle"))) m) curves 0 in - define "THROTTLE_CURVE_MODE_INIT" default; - define "THROTTLE_CURVES_NB" (string_of_int (List.length curves)); - define "THROTTLE_POINTS_NB" (string_of_int nb_points); - printf "#define THROTTLE_CURVES { \\\n"; - List.iter parse_heli_curves curves; - printf "}\n\n"; + define_out out "THROTTLE_CURVE_MODE_INIT" default; + define_out out "THROTTLE_CURVES_NB" (string_of_int (List.length curves)); + define_out out "THROTTLE_POINTS_NB" (string_of_int nb_points); + fprintf out "#define THROTTLE_CURVES { \\\n"; + List.iter (parse_heli_curves out) curves; + fprintf out "}\n\n"; | "include" -> let filename = Str.global_replace (Str.regexp "\\$AC_ID") ac_id (ExtXml.attrib s "href") in let subxml = ExtXml.parse_file filename in - printf "/* XML %s */" filename; - nl (); - List.iter (parse_section ac_id) (Xml.children subxml) + fprintf out "/* XML %s */" filename; + fprintf out "\n"; + List.iter (parse_section out ac_id) (Xml.children subxml) | "makefile" -> () (** Ignoring this section *) @@ -404,24 +394,14 @@ let hex_to_bin = fun s -> done; Bytes.to_string b -let _ = - if Array.length Sys.argv <> 5 then - failwith (Printf.sprintf "Usage: %s A/C_ident A/C_name MD5SUM xml_file" Sys.argv.(0)); - let xml_file = Sys.argv.(4) - and ac_id = Sys.argv.(1) - and ac_name = Sys.argv.(2) - and md5sum = Sys.argv.(3) in - try - let xml = start_and_begin xml_file h_name in - (* Xml2h.warning ("AIRFRAME MODEL: "^ ac_name); *) - define_string "AIRFRAME_NAME" ac_name; - define "AC_ID" ac_id; - define "MD5SUM" (sprintf "((uint8_t*)\"%s\")" (hex_to_bin md5sum)); - nl (); - List.iter (parse_section ac_id) (Xml.children xml); - finish h_name - 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 +let generate = fun airframe ac_id ac_name md5sum xml_file out_file -> + let out = open_out out_file in + + begin_out out xml_file h_name; + define_string_out out "AIRFRAME_NAME" ac_name; + define_out out "AC_ID" ac_id; + define_out out "MD5SUM" (sprintf "((uint8_t*)\"%s\")" (hex_to_bin md5sum)); + fprintf out "\n"; + List.iter (parse_section out ac_id) (Xml.children airframe.Airframe.xml); + finish_out out h_name + diff --git a/sw/tools/generators/gen_autopilot.ml b/sw/tools/generators/gen_autopilot.ml index 4cd68a27f9..ff6e212c79 100644 --- a/sw/tools/generators/gen_autopilot.ml +++ b/sw/tools/generators/gen_autopilot.ml @@ -28,8 +28,7 @@ open Xml2h let (//) = Filename.concat -let paparazzi_conf = Env.paparazzi_home // "conf" -let autopilot_dir = paparazzi_conf // "autopilot" +let autopilot_dir = Env.paparazzi_conf // "autopilot" (** Formatting with a margin *) let margin = ref 0 @@ -342,7 +341,7 @@ let parse_and_gen_modes xml_file ap_name main_freq h_dir sm = let name = Xml.attrib sm "name" in let name_up = Compat.uppercase_ascii name in (* Generate start of header *) - begin_out xml_file ("AUTOPILOT_CORE_"^name_up^"_H") out_h; + begin_out out_h xml_file ("AUTOPILOT_CORE_"^name_up^"_H"); fprintf out_h "/*** %s ***/\n\n" ap_name; (* Print EXTERN definition *) fprintf out_h "#ifdef AUTOPILOT_CORE_%s_C\n" name_up; @@ -369,7 +368,7 @@ let parse_and_gen_modes xml_file ap_name main_freq h_dir sm = print_global_exceptions (get_exceptions sm) name out_h; print_set_mode modes name out_h; (* Select freq, airframe definition will supersede autopilot one *) - let freq = match main_freq, (Xml.attrib sm "freq") with + let freq = match main_freq, Xml.attrib sm "freq" with | None, f -> f | Some f, _ -> f in @@ -385,106 +384,13 @@ let parse_and_gen_modes xml_file ap_name main_freq h_dir sm = failwith (sprintf "gen_autopilot: fail to move tmp file %s to final location" tmp_file) with _ -> Sys.remove tmp_file -(* Output settings xml file *) -let write_settings = fun xml_file out_set ap -> - fprintf out_set "\n" xml_file; - fprintf out_set "\n" (Env.get_paparazzi_version ()); - fprintf out_set "\n\n"; - fprintf out_set "\n"; - fprintf out_set " \n"; - (* Filter state machines that need to be displayed *) - let sm_filtered = List.filter (fun sm -> - try (Compat.lowercase_ascii (Xml.attrib sm "settings_mode")) = "true" with _ -> false - ) (Xml.children ap) in - if List.length sm_filtered > 0 then begin - (* Create node if there is at least one to display *) - fprintf out_set " \n"; - let write_ap_mode = fun sm -> - let modes = get_modes sm in - let name = Xml.attrib sm "name" in - (* Iter on modes and store min, max and values *) - let (_, min, max, values) = List.fold_left (fun (current, min, max, values) m -> - let print = try Compat.lowercase_ascii (Xml.attrib m "settings") <> "hide" with _ -> true in - let name = Xml.attrib m "name" in - if print then begin - let min = match min with - | None -> Some current - | Some x -> Some x - in - let max = Some current in - let values = values @ [name] in - (current + 1, min, max, values) - end - else begin - let n = match min with None -> [] | _ -> [name] in - (current + 1, min, max, values @ n) - end - ) (0, None, None, []) modes in - (* check handler *) - let handler = try - let sh = Xml.attrib sm "settings_handler" in - let s = Str.split (Str.regexp "|") sh in - match s with - | [m; h] -> sprintf " module=\"%s\" handler=\"%s\"" m h - | _ -> failwith "invalid handler format" - with _ -> sprintf " module=\"autopilot_core_%s\"" name in - (* Print if at least one mode has been found *) - begin match min, max with - | Some min_idx, Some max_idx -> - fprintf out_set " \n" - min_idx max_idx name name (String.concat "|" values) handler - | _, _ -> () - end; - (* check for embedded custom settings *) - let extra_settings = get_settings sm in - List.iter (fun s -> fprintf out_set " %s\n" (Xml.to_string s)) (Xml.children extra_settings) - in - (* Iter and call print function *) - List.iter write_ap_mode sm_filtered; - fprintf out_set " \n"; - end; - fprintf out_set " \n"; - fprintf out_set "\n" - - (** Main generation function - * Usage: main_freq xml_file_input h_dir_output *) -let gen_autopilot main_freq xml_file h_dir out_set = - let ap_xml = try 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 - | 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 - | Xml.File_not_found _ -> (* invalid file, return empty Xml *) - Xml.Element ("autopilot", [], []) - in - let ap_name = ExtXml.attrib_or_default ap_xml "name" "Autopilot" in - let state_machines = get_state_machines ap_xml in - List.iter (parse_and_gen_modes xml_file ap_name main_freq h_dir) state_machines; - write_settings xml_file out_set ap_xml - -(* Main call *) -let () = - if Array.length Sys.argv <> 4 then - failwith (Printf.sprintf "Usage: %s airframe_xml_file out_h_dir out_settings" Sys.argv.(0)); - let xml_file = Sys.argv.(1) - and h_dir = Sys.argv.(2) - and out_set = open_out Sys.argv.(3) in - let (autopilot, ap_freq) = try - 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 - | 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 -> (* No autopilot file found *) - ("", None) - in +let generate = fun autopilot ap_freq out_dir -> try - gen_autopilot ap_freq autopilot h_dir out_set; - close_out out_set + let ap_name = ExtXml.attrib_or_default autopilot.Autopilot.xml "name" "Autopilot" in + let state_machines = get_state_machines autopilot.Autopilot.xml in + List.iter (parse_and_gen_modes autopilot.Autopilot.filename ap_name ap_freq out_dir) state_machines with _ -> fprintf stderr "gen_autopilot: What the heck? Something went wrong...\n"; exit 1 diff --git a/sw/tools/generators/gen_flight_plan.ml b/sw/tools/generators/gen_flight_plan.ml index 3cc3667272..7da594ae4f 100644 --- a/sw/tools/generators/gen_flight_plan.ml +++ b/sw/tools/generators/gen_flight_plan.ml @@ -73,9 +73,9 @@ let step = 2 let right () = margin := !margin + step let left () = margin := !margin - step -let lprintf = fun f -> - printf "%s" (String.make !margin ' '); - printf f +let lprintf = fun out f -> + fprintf out "%s" (String.make !margin ' '); + fprintf out f let float_attrib = fun xml a -> @@ -122,32 +122,32 @@ let localize_waypoint = fun rel_utm_of_wgs84 waypoint -> waypoint -let print_waypoint_utm = fun default_alt waypoint -> +let print_waypoint_utm = fun out default_alt waypoint -> let (x, y) = (float_attrib waypoint "x", float_attrib waypoint "y") and alt = try sof (float_attrib waypoint "height" +. !ground_alt) with _ -> default_alt in let alt = try Xml.attrib waypoint "alt" with _ -> alt in check_altitude (float_of_string alt) waypoint; - printf " {%.1f, %.1f, %s},\\\n" x y alt + fprintf out " {%.1f, %.1f, %s},\\\n" x y alt -let print_waypoint_enu = fun utm0 default_alt waypoint -> +let print_waypoint_enu = fun out utm0 default_alt waypoint -> let (x, y) = (float_attrib waypoint "x", float_attrib waypoint "y") and alt = try sof (float_attrib waypoint "height" +. !ground_alt) with _ -> default_alt in let alt = try Xml.attrib waypoint "alt" with _ -> alt in let ecef0 = Latlong.ecef_of_geo Latlong.WGS84 (Latlong.of_utm Latlong.WGS84 utm0) !ground_alt in let ecef = Latlong.ecef_of_geo Latlong.WGS84 (Latlong.of_utm Latlong.WGS84 (Latlong.utm_add utm0 (x, y))) (float_of_string alt) in let ned = Latlong.array_of_ned (Latlong.ned_of_ecef ecef0 ecef) in - printf " {%.2f, %.2f, %.2f}, /* ENU in meters */ \\\n" ned.(1) ned.(0) (-.ned.(2)) + fprintf out " {%.2f, %.2f, %.2f}, /* ENU in meters */ \\\n" ned.(1) ned.(0) (-.ned.(2)) let convert_angle = fun rad -> Int64.of_float (1e7 *. (Rad>>Deg)rad) -let print_waypoint_lla = fun utm0 default_alt waypoint -> +let print_waypoint_lla = fun out utm0 default_alt waypoint -> let (x, y) = (float_attrib waypoint "x", float_attrib waypoint "y") and alt = try sof (float_attrib waypoint "height" +. !ground_alt) with _ -> default_alt in let alt = try Xml.attrib waypoint "alt" with _ -> alt in let wgs84 = Latlong.of_utm Latlong.WGS84 (Latlong.utm_add utm0 (x, y)) in - printf " {.lat=%Ld, .lon=%Ld, .alt=%.0f}, /* 1e7deg, 1e7deg, mm (above NAV_MSL0, local msl=%.2fm) */ \\\n" (convert_angle wgs84.posn_lat) (convert_angle wgs84.posn_long) (1000. *. float_of_string alt) (Egm96.of_wgs84 wgs84) + fprintf out " {.lat=%Ld, .lon=%Ld, .alt=%.0f}, /* 1e7deg, 1e7deg, mm (above NAV_MSL0, local msl=%.2fm) */ \\\n" (convert_angle wgs84.posn_lat) (convert_angle wgs84.posn_long) (1000. *. float_of_string alt) (Egm96.of_wgs84 wgs84) -let print_waypoint_lla_wgs84 = fun utm0 default_alt waypoint -> +let print_waypoint_lla_wgs84 = fun out utm0 default_alt waypoint -> let (x, y) = (float_attrib waypoint "x", float_attrib waypoint "y") and alt = try sof (float_attrib waypoint "height" +. !ground_alt) with _ -> default_alt in let alt = try Xml.attrib waypoint "alt" with _ -> alt in @@ -155,13 +155,13 @@ let print_waypoint_lla_wgs84 = fun utm0 default_alt waypoint -> if Srtm.available wgs84 then check_altitude_srtm (float_of_string alt) waypoint wgs84; let alt = float_of_string alt +. Egm96.of_wgs84 wgs84 in - printf " {.lat=%Ld, .lon=%Ld, .alt=%.0f}, /* 1e7deg, 1e7deg, mm (above WGS84 ref ellipsoid) */ \\\n" (convert_angle wgs84.posn_lat) (convert_angle wgs84.posn_long) (1000. *. alt) + fprintf out " {.lat=%Ld, .lon=%Ld, .alt=%.0f}, /* 1e7deg, 1e7deg, mm (above WGS84 ref ellipsoid) */ \\\n" (convert_angle wgs84.posn_lat) (convert_angle wgs84.posn_long) (1000. *. alt) -let print_waypoint_global = fun waypoint -> +let print_waypoint_global = fun out waypoint -> try let (_, _) = (float_attrib waypoint "lat", float_attrib waypoint "lon") in - printf " TRUE, \\\n" - with _ -> printf " FALSE, \\\n" + fprintf out " TRUE, \\\n" + with _ -> fprintf out " FALSE, \\\n" let get_index_block = fun x -> try @@ -169,16 +169,16 @@ let get_index_block = fun x -> with Not_found -> failwith (sprintf "Unknown block: '%s'" x) -let print_exception = fun x -> +let print_exception = fun out x -> let c = parsed_attrib x "cond" in let i = get_index_block (ExtXml.attrib x "deroute") in begin try let f = ExtXml.attrib x "exec" in - lprintf "if ((nav_block != %d) && %s) {%s; GotoBlock(%d); return; }\n" i c f i + lprintf out "if ((nav_block != %d) && %s) {%s; GotoBlock(%d); return; }\n" i c f i with ExtXml.Error _ -> ( - lprintf "if ((nav_block != %d) && %s) { GotoBlock(%d); return; }\n" i c i + lprintf out "if ((nav_block != %d) && %s) { GotoBlock(%d); return; }\n" i c i ) end @@ -190,7 +190,7 @@ let home_block = Xml.parse_string "" let stage = ref 0 -let output_label l = lprintf "Label(%s)\n" l +let output_label out l = lprintf out "Label(%s)\n" l let get_index_waypoint = fun x l -> try @@ -211,15 +211,15 @@ let pprz_throttle = fun s -> (********************* Vertical control ********************************************) -let output_vmode = fun stage_xml wp last_wp -> +let output_vmode = fun out stage_xml wp last_wp -> let pitch = try Xml.attrib stage_xml "pitch" with _ -> "0.0" in if Compat.lowercase_ascii (Xml.tag stage_xml) <> "manual" then begin if pitch = "auto" then begin - lprintf "NavVerticalAutoPitchMode(%s);\n" (pprz_throttle (parsed_attrib stage_xml "throttle")) + lprintf out "NavVerticalAutoPitchMode(%s);\n" (pprz_throttle (parsed_attrib stage_xml "throttle")) end else begin - lprintf "NavVerticalAutoThrottleMode(RadOfDeg(%s));\n" (parse pitch); + lprintf out "NavVerticalAutoThrottleMode(RadOfDeg(%s));\n" (parse pitch); end end; @@ -227,7 +227,7 @@ let output_vmode = fun stage_xml wp last_wp -> begin match vmode with "climb" -> - lprintf "NavVerticalClimbMode(%s);\n" (parsed_attrib stage_xml "climb") + lprintf out "NavVerticalClimbMode(%s);\n" (parsed_attrib stage_xml "climb") | "alt" -> let alt = try @@ -255,20 +255,20 @@ let output_vmode = fun stage_xml wp last_wp -> if wp = "" then failwith "alt or waypoint required in alt vmode" else sprintf "WaypointAlt(%s)" wp in - lprintf "NavVerticalAltitudeMode(%s, 0.);\n" alt; + lprintf out "NavVerticalAltitudeMode(%s, 0.);\n" alt; | "xyz" -> () (** Handled in Goto3D() *) | "glide" -> - lprintf "NavGlide(%s, %s);\n" last_wp wp + lprintf out "NavGlide(%s, %s);\n" last_wp wp | "throttle" -> if (pitch = "auto") then failwith "auto pich mode not compatible with vmode=throttle"; - lprintf "NavVerticalThrottleMode(%s);\n" (pprz_throttle (parsed_attrib stage_xml "throttle")) + lprintf out "NavVerticalThrottleMode(%s);\n" (pprz_throttle (parsed_attrib stage_xml "throttle")) | x -> failwith (sprintf "Unknown vmode '%s'" x) end; vmode (****************** Horizontal control *********************************************) -let output_hmode x wp last_wp = +let output_hmode out x wp last_wp = try let hmode = ExtXml.attrib x "hmode" in begin @@ -276,13 +276,13 @@ let output_hmode x wp last_wp = "route" -> if last_wp = "last_wp" then fprintf stderr "NOTICE: Deprecated use of 'route' using last waypoint in %s\n"(Xml.to_string x); - lprintf "NavSegment(%s, %s);\n" last_wp wp - | "direct" -> lprintf "NavGotoWaypoint(%s);\n" wp + lprintf out "NavSegment(%s, %s);\n" last_wp wp + | "direct" -> lprintf out "NavGotoWaypoint(%s);\n" wp | x -> failwith (sprintf "Unknown hmode '%s'" x) end; hmode with - ExtXml.Error _ -> lprintf "NavGotoWaypoint(%s);\n" wp; "direct" (* Default behaviour *) + ExtXml.Error _ -> lprintf out "NavGotoWaypoint(%s);\n" wp; "direct" (* Default behaviour *) @@ -319,57 +319,57 @@ let rec index_stage = fun x -> let inside_function = fun name -> "Inside" ^ Compat.capitalize_ascii name (* pre call utility function *) -let fp_pre_call = fun x -> - try lprintf "%s;\n" (ExtXml.attrib x "pre_call") with _ -> () +let fp_pre_call = fun out x -> + try lprintf out "%s;\n" (ExtXml.attrib x "pre_call") with _ -> () (* post call utility function *) -let fp_post_call = fun x -> - try lprintf "%s;\n" (ExtXml.attrib x "post_call") with _ -> () +let fp_post_call = fun out x -> + try lprintf out "%s;\n" (ExtXml.attrib x "post_call") with _ -> () (* test until condition test if any, post_call before leaving *) -let stage_until = fun x -> +let stage_until = fun out x -> try let cond = parsed_attrib x "until" in - lprintf "if (%s) {\n" cond; + lprintf out "if (%s) {\n" cond; right (); - fp_post_call x; - lprintf "NextStageAndBreak()\n"; + fp_post_call out x; + lprintf out "NextStageAndBreak()\n"; left (); - lprintf "}\n" + lprintf out "}\n" with ExtXml.Error _ -> () (* fallback when "until" attribute doesn't exist *) -let rec print_stage = fun index_of_waypoints x -> - let stage () = incr stage;lprintf "Stage(%d)\n" !stage; right () in +let rec print_stage = fun out index_of_waypoints x -> + let stage out = incr stage; lprintf out "Stage(%d)\n" !stage; right () in begin match Compat.lowercase_ascii (Xml.tag x) with | "return" -> - stage (); - lprintf "Return(%s);\n" (ExtXml.attrib_or_default x "reset_stage" "0"); - lprintf "break;\n" + stage out; + lprintf out "Return(%s);\n" (ExtXml.attrib_or_default x "reset_stage" "0"); + lprintf out "break;\n" | "goto" -> - stage (); - lprintf "Goto(%s)\n" (name_of x) + stage out; + lprintf out "Goto(%s)\n" (name_of x) | "deroute" -> - stage (); - lprintf "GotoBlock(%d);\n" (get_index_block (ExtXml.attrib x "block")); - lprintf "break;\n" + stage out; + lprintf out "GotoBlock(%d);\n" (get_index_block (ExtXml.attrib x "block")); + lprintf out "break;\n" | "exit_block" -> - lprintf "/* Falls through. */\n"; - lprintf "default:\n"; - stage (); - lprintf "NextBlock();\n"; - lprintf "break;\n" + lprintf out "/* Falls through. */\n"; + lprintf out "default:\n"; + stage out; + lprintf out "NextBlock();\n"; + lprintf out "break;\n" | "while" -> let w = gen_label "while" in let e = gen_label "endwhile" in - output_label w; - stage (); + output_label out w; + stage out; let c = try parsed_attrib x "cond" with _ -> "TRUE" in - lprintf "if (! (%s)) Goto(%s) else NextStageAndBreak();\n" c e; - List.iter (print_stage index_of_waypoints) (Xml.children x); - print_stage index_of_waypoints (goto w); - output_label e + lprintf out "if (! (%s)) Goto(%s) else NextStageAndBreak();\n" c e; + List.iter (print_stage out index_of_waypoints) (Xml.children x); + print_stage out index_of_waypoints (goto w); + output_label out e | "for" -> let f = gen_label "for" in let e = gen_label "endfor" in @@ -377,65 +377,65 @@ let rec print_stage = fun index_of_waypoints x -> and from_ = parsed_attrib x "from" and to_expr = parsed_attrib x "to" in let to_var = v ^ "_to" in - lprintf "static int8_t %s;\n" v; - lprintf "static int8_t %s;\n" to_var; + lprintf out "static int8_t %s;\n" v; + lprintf out "static int8_t %s;\n" to_var; (* init *) - stage (); - lprintf "%s = %s - 1;\n" v from_; - lprintf "%s = %s;\n" to_var to_expr; - lprintf "INTENTIONAL_FALLTHRU\n"; + stage out; + lprintf out "%s = %s - 1;\n" v from_; + lprintf out "%s = %s;\n" to_var to_expr; + lprintf out "INTENTIONAL_FALLTHRU\n"; left (); - output_label f; - stage (); - lprintf "if (++%s > %s) Goto(%s) else NextStageAndBreak();\n" v to_var e; - List.iter (print_stage index_of_waypoints) (Xml.children x); - print_stage index_of_waypoints (goto f); - output_label e + output_label out f; + stage out; + lprintf out "if (++%s > %s) Goto(%s) else NextStageAndBreak();\n" v to_var e; + List.iter (print_stage out index_of_waypoints) (Xml.children x); + print_stage out index_of_waypoints (goto f); + output_label out e | "heading" -> - stage (); - fp_pre_call x; - lprintf "NavHeading(RadOfDeg(%s));\n" (parsed_attrib x "course"); - ignore (output_vmode x "" ""); - stage_until x; - fp_post_call x; - lprintf "break;\n" + stage out; + fp_pre_call out x; + lprintf out "NavHeading(RadOfDeg(%s));\n" (parsed_attrib x "course"); + ignore (output_vmode out x "" ""); + stage_until out x; + fp_post_call out x; + lprintf out "break;\n" | "follow" -> - stage (); - fp_pre_call x; + stage out; + fp_pre_call out x; let id = ExtXml.attrib x "ac_id" and d = ExtXml.attrib x "distance" and h = ExtXml.attrib x "height" in - lprintf "NavFollow(%s, %s, %s);\n" id d h; - fp_post_call x; - lprintf "break;\n" + lprintf out "NavFollow(%s, %s, %s);\n" id d h; + fp_post_call out x; + lprintf out "break;\n" | "attitude" -> - stage (); - fp_pre_call x; - lprintf "NavAttitude(RadOfDeg(%s));\n" (parsed_attrib x "roll"); - ignore (output_vmode x "" ""); - stage_until x; - fp_post_call x; - lprintf "break;\n" + stage out; + fp_pre_call out x; + lprintf out "NavAttitude(RadOfDeg(%s));\n" (parsed_attrib x "roll"); + ignore (output_vmode out x "" ""); + stage_until out x; + fp_post_call out x; + lprintf out "break;\n" | "manual" -> - stage (); - fp_pre_call x; - lprintf "NavSetManual(%s, %s, %s);\n" (parsed_attrib x "roll") (parsed_attrib x "pitch") (parsed_attrib x "yaw"); - ignore (output_vmode x "" ""); - stage_until x; - fp_post_call x; - lprintf "break;\n" + stage out; + fp_pre_call out x; + lprintf out "NavSetManual(%s, %s, %s);\n" (parsed_attrib x "roll") (parsed_attrib x "pitch") (parsed_attrib x "yaw"); + ignore (output_vmode out x "" ""); + stage_until out x; + fp_post_call out x; + lprintf out "break;\n" | "go" -> - stage (); - fp_pre_call x; + stage out; + fp_pre_call out x; let wp = try get_index_waypoint (ExtXml.attrib x "wp") index_of_waypoints with ExtXml.Error _ -> - lprintf "waypoints[0].x = %s;\n" (parsed_attrib x "x"); - lprintf "waypoints[0].y = %s;\n" (parsed_attrib x "y"); + lprintf out "waypoints[0].x = %s;\n" (parsed_attrib x "x"); + lprintf out "waypoints[0].y = %s;\n" (parsed_attrib x "y"); "0" in let at = try Some (ExtXml.attrib x "approaching_time") with _ -> None in @@ -451,100 +451,100 @@ let rec print_stage = fun index_of_waypoints x -> get_index_waypoint (ExtXml.attrib x "from") index_of_waypoints with ExtXml.Error _ -> "last_wp" in if last_wp = "last_wp" then - lprintf "if (NavApproaching(%s,%s)) {\n" wp at + lprintf out "if (NavApproaching(%s,%s)) {\n" wp at else - lprintf "if (NavApproachingFrom(%s,%s,%s)) {\n" wp last_wp at; + lprintf out "if (NavApproachingFrom(%s,%s,%s)) {\n" wp last_wp at; right (); - fp_post_call x; - lprintf "NextStageAndBreakFrom(%s);\n" wp; + fp_post_call out x; + lprintf out "NextStageAndBreakFrom(%s);\n" wp; left (); - lprintf "} else {\n"; + lprintf out "} else {\n"; right (); - let hmode = output_hmode x wp last_wp in - let vmode = output_vmode x wp last_wp in + let hmode = output_hmode out x wp last_wp in + let vmode = output_vmode out x wp last_wp in if vmode = "glide" && hmode <> "route" then failwith "glide vmode requires route hmode"; - left (); lprintf "}\n"; - stage_until x; - fp_post_call x; - lprintf "break;\n" + left (); lprintf out "}\n"; + stage_until out x; + fp_post_call out x; + lprintf out "break;\n" | "stay" -> - stage (); - fp_pre_call x; + stage out; + fp_pre_call out x; begin try let wp = get_index_waypoint (ExtXml.attrib x "wp") index_of_waypoints in - ignore (output_hmode x wp ""); - ignore (output_vmode x wp ""); + ignore (output_hmode out x wp ""); + ignore (output_vmode out x wp ""); with Xml2h.Error _ -> - lprintf "NavGotoXY(last_x, last_y);\n"; - ignore(output_vmode x "" "") + lprintf out "NavGotoXY(last_x, last_y);\n"; + ignore(output_vmode out x "" "") end; - stage_until x; - fp_post_call x; - lprintf "break;\n" + stage_until out x; + fp_post_call out x; + lprintf out "break;\n" | "xyz" -> - stage (); - fp_pre_call x; - let r = try parsed_attrib x "radius" with _ -> "100" in - lprintf "Goto3D(%s)\n" r; + stage out; + fp_pre_call out x; + let r = try parsed_attrib x "radius" with _ -> "100" in + lprintf out "Goto3D(%s)\n" r; let x = ExtXml.subst_attrib "vmode" "xyz" x in - ignore (output_vmode x "" ""); (** To handle "pitch" *) - fp_post_call x; - lprintf "break;\n" + ignore (output_vmode out x "" ""); (** To handle "pitch" *) + fp_post_call out x; + lprintf out "break;\n" | "home" -> - stage (); - lprintf "nav_home();\n"; - lprintf "break;\n" + stage out; + lprintf out "nav_home();\n"; + lprintf out "break;\n" | "circle" -> - stage (); - fp_pre_call x; + stage out; + fp_pre_call out x; let wp = get_index_waypoint (ExtXml.attrib x "wp") index_of_waypoints in let r = parsed_attrib x "radius" in - let _vmode = output_vmode x wp "" in - lprintf "NavCircleWaypoint(%s, %s);\n" wp r; - stage_until x; - fp_post_call x; - lprintf "break;\n" + let _vmode = output_vmode out x wp "" in + lprintf out "NavCircleWaypoint(%s, %s);\n" wp r; + stage_until out x; + fp_post_call out x; + lprintf out "break;\n" | "eight" -> - stage (); - lprintf "nav_eight_init();\n"; - lprintf "NextStageAndBreak();\n"; + stage out; + lprintf out "nav_eight_init();\n"; + lprintf out "NextStageAndBreak();\n"; left (); - stage (); - fp_pre_call x; + stage out; + fp_pre_call out x; let center = get_index_waypoint (ExtXml.attrib x "center") index_of_waypoints and turn_about = get_index_waypoint (ExtXml.attrib x "turn_around") index_of_waypoints in - let r = parsed_attrib x "radius" in - let _vmode = output_vmode x center "" in - lprintf "Eight(%s, %s, %s);\n" center turn_about r; - stage_until x; - fp_post_call x; - lprintf "break;\n" + let r = parsed_attrib x "radius" in + let _vmode = output_vmode out x center "" in + lprintf out "Eight(%s, %s, %s);\n" center turn_about r; + stage_until out x; + fp_post_call out x; + lprintf out "break;\n" | "oval" -> - stage (); - lprintf "nav_oval_init();\n"; - lprintf "NextStageAndBreak();\n"; + stage out; + lprintf out "nav_oval_init();\n"; + lprintf out "NextStageAndBreak();\n"; left (); - stage (); - fp_pre_call x; + stage out; + fp_pre_call out x; let p1 = get_index_waypoint (ExtXml.attrib x "p1") index_of_waypoints and p2 = get_index_waypoint (ExtXml.attrib x "p2") index_of_waypoints in let r = parsed_attrib x "radius" in - let _vmode = output_vmode x p1 "" in - lprintf "Oval(%s, %s, %s);\n" p1 p2 r; - stage_until x; - fp_post_call x; - lprintf "break;\n" + let _vmode = output_vmode out x p1 "" in + lprintf out "Oval(%s, %s, %s);\n" p1 p2 r; + stage_until out x; + fp_post_call out x; + lprintf out "break;\n" | "set" -> - stage (); + stage out; let var = ExtXml.attrib x "var" and value = parsed_attrib x "value" in - lprintf "%s = %s;\n" var value; - lprintf "NextStage();\n" + lprintf out "%s = %s;\n" var value; + lprintf out "NextStage();\n" | "call" -> - stage (); + stage out; let statement = ExtXml.attrib x "fun" in (* by default, function is called while returning TRUE *) (* otherwise, function is called once and returned value is ignored *) @@ -553,41 +553,41 @@ let rec print_stage = fun index_of_waypoints x -> let break = Compat.uppercase_ascii (ExtXml.attrib_or_default x "break" "FALSE") in begin match loop with | "TRUE" -> - lprintf "if (! (%s)) {\n" statement; + lprintf out "if (! (%s)) {\n" statement; begin match break with - | "TRUE" -> lprintf " NextStageAndBreak();\n"; - | "FALSE" -> lprintf " NextStage();\n"; + | "TRUE" -> lprintf out " NextStageAndBreak();\n"; + | "FALSE" -> lprintf out " NextStage();\n"; | _ -> failwith "FP: 'call' break attribute must be TRUE or FALSE"; end; - lprintf "} else {\n"; + lprintf out "} else {\n"; begin try let c = parsed_attrib x "until" in - lprintf " if (%s) NextStageAndBreak();\n" c + lprintf out " if (%s) NextStageAndBreak();\n" c with ExtXml.Error _ -> () end; - lprintf " break;\n"; - lprintf "}\n" + lprintf out " break;\n"; + lprintf out "}\n" | "FALSE" -> - lprintf "%s;\n" statement; + lprintf out "%s;\n" statement; begin match break with - | "TRUE" -> lprintf "NextStageAndBreak();\n"; - | "FALSE" -> lprintf "NextStage();\n"; + | "TRUE" -> lprintf out "NextStageAndBreak();\n"; + | "FALSE" -> lprintf out "NextStage();\n"; | _ -> failwith "FP: 'call' break attribute must be TRUE or FALSE"; end; | _ -> failwith "FP: 'call' loop attribute must be TRUE or FALSE" end | "call_once" -> (* call_once is an alias for *) - stage (); + stage out; let statement = ExtXml.attrib x "fun" in (* by default, go to next stage immediately *) let break = Compat.uppercase_ascii (ExtXml.attrib_or_default x "break" "FALSE") in - lprintf "%s;\n" statement; + lprintf out "%s;\n" statement; begin match break with - | "TRUE" -> lprintf "NextStageAndBreak();\n"; - | "FALSE" -> lprintf "NextStage();\n"; + | "TRUE" -> lprintf out "NextStageAndBreak();\n"; + | "FALSE" -> lprintf out "NextStage();\n"; | _ -> failwith "FP: 'call_once' break attribute must be TRUE or FALSE"; end; | "survey_rectangle" -> @@ -595,18 +595,18 @@ let rec print_stage = fun index_of_waypoints x -> and wp1 = get_index_waypoint (ExtXml.attrib x "wp1") index_of_waypoints and wp2 = get_index_waypoint (ExtXml.attrib x "wp2") index_of_waypoints and orientation = ExtXml.attrib_or_default x "orientation" "NS" in - stage (); + stage out; if orientation <> "NS" && orientation <> "WE" then failwith (sprintf "Unknown survey orientation (NS or WE): %s" orientation); - lprintf "NavSurveyRectangleInit(%s, %s, %s, %s);\n" wp1 wp2 grid orientation; - lprintf "NextStageAndBreak();\n"; + lprintf out "NavSurveyRectangleInit(%s, %s, %s, %s);\n" wp1 wp2 grid orientation; + lprintf out "NextStageAndBreak();\n"; left (); - stage (); - fp_pre_call x; - lprintf "NavSurveyRectangle(%s, %s);\n" wp1 wp2; - stage_until x; - fp_post_call x; - lprintf "break;\n" + stage out; + fp_pre_call out x; + lprintf out "NavSurveyRectangle(%s, %s);\n" wp1 wp2; + stage_until out x; + fp_post_call out x; + lprintf out "break;\n" | _s -> failwith "Unreachable" end; left () @@ -653,47 +653,47 @@ let index_blocks = fun xml -> -let print_block = fun index_of_waypoints (b:Xml.xml) block_num -> +let print_block = fun out index_of_waypoints (b:Xml.xml) block_num -> let n = name_of b in (* Block entry *) - lprintf "Block(%d) // %s\n" block_num n; - fp_pre_call b; + lprintf out "Block(%d) // %s\n" block_num n; + fp_pre_call out b; let excpts, stages = List.partition (fun x -> Xml.tag x = "exception") (Xml.children b) in - List.iter print_exception excpts; + List.iter (print_exception out) excpts; - lprintf "switch(nav_stage) {\n"; + lprintf out "switch(nav_stage) {\n"; right (); stage := (-1); - List.iter (print_stage index_of_waypoints) stages; + List.iter (print_stage out index_of_waypoints) stages; - print_stage index_of_waypoints exit_block; + print_stage out index_of_waypoints exit_block; left (); - lprintf "}\n"; + lprintf out "}\n"; (* Block exit *) - fp_post_call b; - lprintf "break;\n\n" + fp_post_call out b; + lprintf out "break;\n\n" -let print_blocks = fun index_of_waypoints bs -> +let print_blocks = fun out index_of_waypoints bs -> let block = ref (-1) in - List.iter (fun b -> incr block; print_block index_of_waypoints b !block) bs + List.iter (fun b -> incr block; print_block out index_of_waypoints b !block) bs let c_suffix = let r = Str.regexp "^[a-zA-Z0-9_]*$" in fun s -> Str.string_match r s 0 -let define_waypoints_indices = fun wpts -> +let define_waypoints_indices = fun out wpts -> let i = ref 0 in List.iter (fun w -> let n = name_of w in if c_suffix n then - Xml2h.define (sprintf "WP_%s" n) (string_of_int !i); + Xml2h.define_out out (sprintf "WP_%s" n) (string_of_int !i); incr i) wpts @@ -743,64 +743,64 @@ let dummy_waypoint = []) -let print_inside_polygon = fun pts -> +let print_inside_polygon = fun out pts -> let (_, pts) = List.split pts in let layers = Geometry_2d.slice_polygon (Array.of_list pts) in let rec f = fun i j -> if i = j then let {G2D.top=yl; left_side=(xg, ag); right_side=(xd, ad)} = layers.(i) in if xg > xd then begin - lprintf "return FALSE;\n" + lprintf out "return FALSE;\n" end else begin if ad <> 0. || ag <> 0. then - lprintf "float dy = _y - %.1f;\n" yl; + lprintf out "float dy = _y - %.1f;\n" yl; let dy_times = fun f -> if f = 0. then "" else sprintf "+dy*%f" f in - lprintf "return (%.1f%s<= _x && _x <= %.1f%s);\n" xg (dy_times ag) xd (dy_times ad) + lprintf out "return (%.1f%s<= _x && _x <= %.1f%s);\n" xg (dy_times ag) xd (dy_times ad) end else let ij2 = (i+j) / 2 in let yl = layers.(ij2).G2D.top in - lprintf "if (_y <= %.1f) {\n" yl; + lprintf out "if (_y <= %.1f) {\n" yl; right (); f i ij2; left (); - lprintf "} else {\n"; + lprintf out "} else {\n"; right (); f (ij2+1) j; left (); - lprintf "}\n" + lprintf out "}\n" in f 0 (Array.length layers - 1);; -let print_inside_polygon_global = fun pts -> - lprintf "uint8_t i, j;\n"; - lprintf "bool c = false;\n"; +let print_inside_polygon_global = fun out pts -> + lprintf out "uint8_t i, j;\n"; + lprintf out "bool c = false;\n"; (* build array of wp id *) let (ids, _) = List.split pts in - lprintf "const uint8_t nb_pts = %d;\n" (List.length pts); - lprintf "const uint8_t wps_id[] = { %s };\n\n" (String.concat ", " ids); + lprintf out "const uint8_t nb_pts = %d;\n" (List.length pts); + lprintf out "const uint8_t wps_id[] = { %s };\n\n" (String.concat ", " ids); (* start algo *) - lprintf "for (i = 0, j = nb_pts - 1; i < nb_pts; j = i++) {\n"; + lprintf out "for (i = 0, j = nb_pts - 1; i < nb_pts; j = i++) {\n"; right (); - lprintf "if (((WaypointY(wps_id[i]) > _y) != (WaypointY(wps_id[j]) > _y)) &&\n"; - lprintf " (_x < (WaypointX(wps_id[j])-WaypointX(wps_id[i])) * (_y-WaypointY(wps_id[i])) / (WaypointY(wps_id[j])-WaypointY(wps_id[i])) + WaypointX(wps_id[i]))) {\n"; + lprintf out "if (((WaypointY(wps_id[i]) > _y) != (WaypointY(wps_id[j]) > _y)) &&\n"; + lprintf out " (_x < (WaypointX(wps_id[j])-WaypointX(wps_id[i])) * (_y-WaypointY(wps_id[i])) / (WaypointY(wps_id[j])-WaypointY(wps_id[i])) + WaypointX(wps_id[i]))) {\n"; right (); - lprintf "if (c == TRUE) { c = FALSE; } else { c = TRUE; }\n"; + lprintf out "if (c == TRUE) { c = FALSE; } else { c = TRUE; }\n"; left(); - lprintf "}\n"; + lprintf out "}\n"; left(); - lprintf "}\n"; - lprintf "return c;\n" + lprintf out "}\n"; + lprintf out "return c;\n" type sector_type = StaticSector | DynamicSector -let print_inside_sector = fun t (s, pts) -> - lprintf "static inline bool %s(float _x, float _y) {\n" (inside_function s); +let print_inside_sector = fun out t (s, pts) -> + lprintf out "static inline bool %s(float _x, float _y) {\n" (inside_function s); right (); begin match t with - | StaticSector -> print_inside_polygon pts - | DynamicSector -> print_inside_polygon_global pts + | StaticSector -> print_inside_polygon out pts + | DynamicSector -> print_inside_polygon_global out pts end; left (); - lprintf "}\n" + lprintf out "}\n" let parse_wpt_sector = fun indexes waypoints xml -> @@ -860,319 +860,306 @@ let parse_variables = fun xml -> | _ -> failwith "Gen_flight_plan: unexpected variables tag" ) xml -let print_var_decl abi_msgs = function - | FP_var (v, t, _) -> printf "extern %s %s;\n" t v +let print_var_decl out abi_msgs = function + | FP_var (v, t, _) -> lprintf out "extern %s %s;\n" t v | _ -> () (* ABI variables are not public *) -let print_var_impl abi_msgs = function - | FP_var (v, t, i) -> printf "%s %s = %s;\n" t v i +let print_var_impl out abi_msgs = function + | FP_var (v, t, i) -> lprintf out "%s %s = %s;\n" t v i | FP_binding (n, Some vs, _, None) -> - printf "static abi_event FP_%s_ev;\n" n; + lprintf out "static abi_event FP_%s_ev;\n" n; let field_types = Hashtbl.find abi_msgs n in - List.iter2 (fun abi_t user -> if not (user = "_") then printf "static %s %s;\n" abi_t user) field_types vs + List.iter2 (fun abi_t user -> if not (user = "_") then lprintf out "static %s %s;\n" abi_t user) field_types vs | FP_binding (n, None, _, Some _) -> - printf "static abi_event FP_%s_ev;\n" n + lprintf out "static abi_event FP_%s_ev;\n" n | _ -> () -let print_auto_init_bindings = fun abi_msgs variables -> +let print_auto_init_bindings = fun out abi_msgs variables -> let print_cb = function | FP_binding (n, Some vs, _, None) -> let field_types = Hashtbl.find abi_msgs n in - printf "static void FP_%s_cb(uint8_t sender_id __attribute__((unused))" n; + lprintf out "static void FP_%s_cb(uint8_t sender_id __attribute__((unused))" n; List.iteri (fun i v -> - if v = "_" then printf ", %s _unused_%d __attribute__((unused))" (List.nth field_types i) i - else printf ", %s _%s" (List.nth field_types i) v + if v = "_" then lprintf out ", %s _unused_%d __attribute__((unused))" (List.nth field_types i) i + else lprintf out ", %s _%s" (List.nth field_types i) v ) vs; - printf ") {\n"; + lprintf out ") {\n"; List.iter (fun v -> - if not (v = "_") then printf " %s = _%s;\n" v v + if not (v = "_") then lprintf out " %s = _%s;\n" v v ) vs; - printf "}\n\n" + lprintf out "}\n\n" | _ -> () in let print_bindings = function | FP_binding (n, _, i, None) -> - printf " AbiBindMsg%s(%s, &FP_%s_ev, FP_%s_cb);\n" n i n n + lprintf out " AbiBindMsg%s(%s, &FP_%s_ev, FP_%s_cb);\n" n i n n | FP_binding (n, _, i, Some h) -> - printf " AbiBindMsg%s(%s, &FP_%s_ev, %s);\n" n i n h + lprintf out " AbiBindMsg%s(%s, &FP_%s_ev, %s);\n" n i n h | _ -> () in List.iter print_cb variables; - printf "static inline void auto_nav_init(void) {\n"; + lprintf out "static inline void auto_nav_init(void) {\n"; List.iter print_bindings variables; - printf "}\n\n" + lprintf out "}\n\n" -let write_settings = fun xml_file out_set variables -> - fprintf out_set "\n" xml_file; - fprintf out_set "\n" (Env.get_paparazzi_version ()); - fprintf out_set "\n\n"; - fprintf out_set "\n"; - fprintf out_set " \n"; - (* remove some incompatible variables and all ABI bindings *) - let att_exists = fun x a -> - try let _ = Xml.attrib x a in true with Xml.No_attribute _ -> false - in - let variables = List.filter (fun x -> - if Xml.tag x = "variable" && att_exists x "min" && att_exists x "max" && att_exists x "step" then - true - else false) variables in - (* add tab only if their are some variables *) - if List.length variables > 0 then - fprintf out_set " \n"; - List.iter (fun v -> - let attribs = Xml.attribs v in - (* remove some incompatible attributes *) - let attribs = List.filter (fun (a, _) -> not (a = "init")) attribs in - let xml = Xml.Element ("dl_setting", attribs @ ["module", "generated/flight_plan"], []) in - fprintf out_set " %s\n" (Xml.to_string xml); - ) variables; - if List.length variables > 0 then - fprintf out_set " \n"; - fprintf out_set " \n"; - fprintf out_set "\n" +(** + * Print flight plan header + *) +let print_flight_plan_h = fun xml utm0 xml_file out_file -> + let out = open_out out_file in + + let waypoints = Xml.children (ExtXml.child xml "waypoints") + and variables = try Xml.children (ExtXml.child xml "variables") with _ -> [] + and blocks = Xml.children (ExtXml.child xml "blocks") + and global_exceptions = try Xml.children (ExtXml.child xml "exceptions") with _ -> [] in + + let h_name = "FLIGHT_PLAN_H" in + fprintf out "/* This file has been generated by gen_flight_plan from %s */\n" xml_file; + fprintf out "/* Version %s */\n" (Env.get_paparazzi_version ()); + fprintf out "/* Please DO NOT EDIT */\n\n"; + + fprintf out "#ifndef %s\n" h_name; + Xml2h.define_out out h_name ""; + fprintf out "\n"; + + (* include general headers *) + fprintf out "#include \"std.h\"\n"; + fprintf out "#include \"generated/modules.h\"\n"; + fprintf out "#include \"subsystems/abi.h\"\n"; + fprintf out "#include \"autopilot.h\"\n\n"; + (* print variables and ABI bindings declaration *) + + let variables = parse_variables variables in + let abi_msgs = extract_abi_msg (Env.paparazzi_home ^ "/conf/abi.xml") "airborne" in + List.iter (fun v -> print_var_decl out abi_msgs v) variables; + fprintf out "\n"; + + (* add custum header part *) + begin + try + let header = ExtXml.child (ExtXml.child xml "header") "0" in + fprintf out "%s\n\n" (Xml.pcdata header) + with _ -> () + end; + + let name = ExtXml.attrib xml "name" in + Xml2h.define_string_out out "FLIGHT_PLAN_NAME" name; + + (* flight plan header *) + let get_float = fun x -> float_attrib xml x in + let qfu = try get_float "qfu" with Xml.No_attribute "qfu" -> 0. + and mdfh = get_float "max_dist_from_home" + and alt = ExtXml.attrib xml "alt" in + security_height := get_float "security_height"; + + (* check altitudes *) + begin + try + if security_height < ref 0. then + begin + fprintf stderr "\nError: Security height cannot be negative (%.0f)\n" !security_height; + exit 1; + end + with _ -> () + end; + ground_alt := get_float "ground_alt"; + let home_mode_height = try + max (get_float "home_mode_height") !security_height + with _ -> !security_height in + check_altitude (float_of_string alt) xml; + check_altitude_srtm (float_of_string alt) xml !fp_wgs84; + + (* print general defines *) + Xml2h.define_out out "NAV_DEFAULT_ALT" (sprintf "%.0f /* nominal altitude of the flight plan */" (float_of_string alt)); + Xml2h.define_out out "NAV_UTM_EAST0" (sprintf "%.0f" utm0.utm_x); + Xml2h.define_out out "NAV_UTM_NORTH0" (sprintf "%.0f" utm0.utm_y); + Xml2h.define_out out "NAV_UTM_ZONE0" (sprintf "%d" utm0.utm_zone); + Xml2h.define_out out "NAV_LAT0" (sprintf "%Ld /* 1e7deg */" (convert_angle !fp_wgs84.posn_lat)); + Xml2h.define_out out "NAV_LON0" (sprintf "%Ld /* 1e7deg */" (convert_angle !fp_wgs84.posn_long)); + Xml2h.define_out out "NAV_ALT0" (sprintf "%.0f /* mm above msl */" (1000. *. !ground_alt)); + Xml2h.define_out out "NAV_MSL0" (sprintf "%.0f /* mm, EGM96 geoid-height (msl) over ellipsoid */" (1000. *. Egm96.of_wgs84 !fp_wgs84)); + + Xml2h.define_out out "QFU" (sprintf "%.1f" qfu); + + let waypoints = dummy_waypoint :: waypoints in + let (hx, hy) = home waypoints in + List.iter (check_distance (hx, hy) mdfh) waypoints; + define_waypoints_indices out waypoints; + + Xml2h.define_out out "WAYPOINTS_UTM" "{ \\"; + List.iter (print_waypoint_utm out alt) waypoints; + lprintf out "};\n"; + Xml2h.define_out out "WAYPOINTS_ENU" "{ \\"; + List.iter (print_waypoint_enu out utm0 alt) waypoints; + lprintf out "};\n"; + Xml2h.define_out out "WAYPOINTS_LLA" "{ \\"; + List.iter (print_waypoint_lla out utm0 alt) waypoints; + lprintf out "};\n"; + Xml2h.define_out out "WAYPOINTS_LLA_WGS84" "{ \\"; + List.iter (print_waypoint_lla_wgs84 out utm0 alt) waypoints; + lprintf out "};\n"; + Xml2h.define_out out "WAYPOINTS_GLOBAL" "{ \\"; + List.iter (print_waypoint_global out) waypoints; + lprintf out "};\n"; + Xml2h.define_out out "NB_WAYPOINT" (string_of_int (List.length waypoints)); + + Xml2h.define_out out "FP_BLOCKS" "{ \\"; + List.iter (fun b -> fprintf out " \"%s\" , \\\n" (ExtXml.attrib b "name")) blocks; + lprintf out "}\n"; + Xml2h.define_out out "NB_BLOCK" (string_of_int (List.length blocks)); + + Xml2h.define_out out "GROUND_ALT" (sof !ground_alt); + Xml2h.define_out out "GROUND_ALT_CM" (sprintf "%.0f" (100.*. !ground_alt)); + Xml2h.define_out out "SECURITY_HEIGHT" (sof !security_height); + Xml2h.define_out out "SECURITY_ALT" (sof (!security_height +. !ground_alt)); + Xml2h.define_out out "HOME_MODE_HEIGHT" (sof home_mode_height); + Xml2h.define_out out "MAX_DIST_FROM_HOME" (sof mdfh); + + (* geofencing warnings and errors *) + begin + try + let geofence_max_alt = get_float "geofence_max_alt" in + if geofence_max_alt < !ground_alt then + begin + fprintf stderr "\nError: Geofence max altitude below ground alt (%.0f < %.0f)\n" geofence_max_alt !ground_alt; + exit 1; + end + else if geofence_max_alt < (!ground_alt +. !security_height) then + begin + fprintf stderr "\nError: Geofence max altitude below security height (%.0f < (%.0f+%.0f))\n" geofence_max_alt !ground_alt !security_height; + exit 1; + end + else if geofence_max_alt < (!ground_alt +. home_mode_height) then + begin + fprintf stderr "\nError: Geofence max altitude below ground alt + home mode height (%.0f < (%.0f+%.0f))\n" geofence_max_alt !ground_alt home_mode_height; + exit 1; + end + else if geofence_max_alt < (float_of_string alt) then + fprintf stderr "\nWarning: Geofence max altitude below default waypoint alt (%.0f < %.0f)\n" geofence_max_alt (float_of_string alt); + Xml2h.define_out out "GEOFENCE_MAX_ALTITUDE" (sof geofence_max_alt); + fprintf stderr "\nWarning: Geofence max altitude set to %.0f\n" geofence_max_alt; + with + _ -> () + end; + + begin + try + let geofence_max_height = get_float "geofence_max_height" in + if geofence_max_height < !security_height then + begin + fprintf stderr "\nError: Geofence max height below security height (%.0f < %.0f)\n" geofence_max_height !security_height; + exit 1; + end + else if geofence_max_height < home_mode_height then + begin + fprintf stderr "\nError: Geofence max height below home mode height (%.0f < %.0f)\n" geofence_max_height home_mode_height; + exit 1; + end + else if (geofence_max_height +. !ground_alt) < (float_of_string alt) then + fprintf stderr "\nWarning: Geofence max AGL below default waypoint AGL (%.0f < %.0f)\n" (geofence_max_height +. !ground_alt) (float_of_string alt); + Xml2h.define_out out "GEOFENCE_MAX_HEIGHT" (sof geofence_max_height); + fprintf stderr "\nWarning: Geofence max AGL set to %.0f\n" geofence_max_height; + with + _ -> () + end; + + (* start "C" part *) + lprintf out "\n#ifdef NAV_C\n\n"; + + (* print variables and ABI initialization *) + List.iter (fun v -> print_var_impl out abi_msgs v) variables; + lprintf out "\n"; + print_auto_init_bindings out abi_msgs variables; + + (* index of waypoints *) + let index_of_waypoints = + let i = ref (-1) in + List.map (fun w -> incr i; (name_of w, !i)) waypoints in + + (* print sectors *) + let sectors_element = try ExtXml.child xml "sectors" with Not_found -> Xml.Element ("", [], []) in + let sectors = List.filter (fun x -> Compat.lowercase_ascii (Xml.tag x) = "sector") (Xml.children sectors_element) in + let sectors_type = List.map (fun x -> match ExtXml.attrib_or_default x "type" "static" with "dynamic" -> DynamicSector | _ -> StaticSector) sectors in + let sectors = List.map (parse_wpt_sector index_of_waypoints waypoints) sectors in + List.iter2 (print_inside_sector out) sectors_type sectors; + + (* print main flight plan state machine *) + lprintf out "\nstatic inline void auto_nav(void) {\n"; + right (); + List.iter (print_exception out) global_exceptions; + lprintf out "switch (nav_block) {\n"; + right (); + print_blocks out index_of_waypoints blocks; + lprintf out "default: break;\n"; + left (); + lprintf out "}\n"; + left (); + lprintf out "}\n"; + lprintf out "#endif // NAV_C\n"; + + (* geofencing sector FIXME why here ? *) + begin + try + let geofence_sector = Xml.attrib xml "geofence_sector" in + lprintf out "#define InGeofenceSector(_x, _y) %s(_x, _y)\n" (inside_function geofence_sector) + with + _ -> () + end; + + Xml2h.finish_out out h_name; + close_out out -(************************** MAIN ******************************************) -let () = - let xml_file = ref "fligh_plan.xml" - and set_file = ref None - and dump = ref false in - Arg.parse [ ("-check", Arg.Set check_expressions, "Enable expression checking"); - ("-dump", Arg.Set dump, "Dump compile result"); - ("-settings", Arg.String (fun f -> set_file := Some f), "Settings file for flight plan variables") ] - (fun f -> xml_file := f) - "Usage:"; - if !xml_file = "" then - failwith (sprintf "Usage: %s " Sys.argv.(0)); - try - let xml = ExtXml.parse_file !xml_file in +(** + * Dump expanded version of the flight plan + *) +let dump_fligh_plan = fun xml out_file -> + let out = open_out out_file in + let blocks = Xml.children (ExtXml.child xml "blocks") in + let xml_stages = Xml.Element ("stages", [], indexed_stages blocks) in + let dump_xml = Xml.Element ("dump", [], [xml; xml_stages]) in + fprintf out "%s\n" (ExtXml.to_string_fmt dump_xml); + close_out out - fp_wgs84 := georef_of_xml xml; - let xml = check_geo_ref !fp_wgs84 xml in +(** + * + * MAIN generation function + * + **) +(* FIXME: for later, get rid of those references? *) +let reinit = fun () -> + index_of_blocks := []; + check_expressions := false; + margin := 0; + stage := 0 - let dir = Filename.dirname !xml_file in - let xml = Fp_proc.process_includes dir xml in - let xml = Fp_proc.process_paths xml in - let xml = Fp_proc.process_relative_waypoints xml in +let generate = fun flight_plan ?(check=false) ?(dump=false) xml_file out_fp -> - (* Add a safety last HOME block *) - let blocks = Xml.children (ExtXml.child xml "blocks") @ [home_block] in + reinit (); + let xml = flight_plan.Flight_plan.xml in + fp_wgs84 := georef_of_xml xml; + let xml = check_geo_ref !fp_wgs84 xml in - let xml = ExtXml.subst_child "blocks" (index_blocks (element "blocks" [] blocks)) xml in - let waypoints = Xml.children (ExtXml.child xml "waypoints") - and variables_xml = try Xml.children (ExtXml.child xml "variables") with _ -> [] - and blocks = Xml.children (ExtXml.child xml "blocks") - and global_exceptions = try Xml.children (ExtXml.child xml "exceptions") with _ -> [] in + let dir = Filename.dirname xml_file in + let xml = Fp_proc.process_includes dir xml in + let xml = Fp_proc.process_paths xml in + let xml = Fp_proc.process_relative_waypoints xml in - let utm0 = utm_of WGS84 !fp_wgs84 in - let rel_utm_of_wgs84 = fun wgs84 -> - (* force utm zone to be the same that reference point *) - let utm = utm_of ~zone:utm0.utm_zone WGS84 wgs84 in - (utm.utm_x -. utm0.utm_x, utm.utm_y -. utm0.utm_y) in - let waypoints = - List.map (localize_waypoint rel_utm_of_wgs84) waypoints in + (* Add a safety last HOME block *) + let blocks = Xml.children (ExtXml.child xml "blocks") @ [home_block] in + let xml = ExtXml.subst_child "blocks" (index_blocks (element "blocks" [] blocks)) xml in + let waypoints = Xml.children (ExtXml.child xml "waypoints") in - let xml = ExtXml.subst_child "waypoints" (element "waypoints" [] waypoints) xml in + let utm0 = utm_of WGS84 !fp_wgs84 in + let rel_utm_of_wgs84 = fun wgs84 -> + (* force utm zone to be the same that reference point *) + let utm = utm_of ~zone:utm0.utm_zone WGS84 wgs84 in + (utm.utm_x -. utm0.utm_x, utm.utm_y -. utm0.utm_y) in + let waypoints = + List.map (localize_waypoint rel_utm_of_wgs84) waypoints in - if !dump then - let xml_stages = Xml.Element ("stages", [], indexed_stages blocks) in - let dump_xml = Xml.Element ("dump", [], [xml; xml_stages]) in - printf "%s\n" (ExtXml.to_string_fmt dump_xml) - else begin - let h_name = "FLIGHT_PLAN_H" in - printf "/* This file has been generated by gen_flight_plan from %s */\n" !xml_file; - printf "/* Version %s */\n" (Env.get_paparazzi_version ()); - printf "/* Please DO NOT EDIT */\n\n"; + let xml = ExtXml.subst_child "waypoints" (element "waypoints" [] waypoints) xml in - printf "#ifndef %s\n" h_name; - Xml2h.define h_name ""; - printf "\n"; + if dump then dump_fligh_plan xml out_fp + else print_flight_plan_h xml utm0 xml_file out_fp - printf "#include \"std.h\"\n"; - printf "#include \"generated/modules.h\"\n"; - printf "#include \"subsystems/abi.h\"\n"; - printf "#include \"autopilot.h\"\n\n"; - - let variables = parse_variables variables_xml in - let abi_msgs = extract_abi_msg (Env.paparazzi_home ^ "/conf/abi.xml") "airborne" in - List.iter (fun v -> print_var_decl abi_msgs v) variables; - printf "\n"; - - begin - try - let header = ExtXml.child (ExtXml.child xml "header") "0" in - printf "%s\n\n" (Xml.pcdata header) - with _ -> () - end; - - let name = ExtXml.attrib xml "name" in - (* Xml2h.warning ("FLIGHT PLAN: "^name); *) - Xml2h.define_string "FLIGHT_PLAN_NAME" name; - - let get_float = fun x -> float_attrib xml x in - let qfu = try get_float "qfu" with Xml.No_attribute "qfu" -> 0. - and mdfh = get_float "max_dist_from_home" - and alt = ExtXml.attrib xml "alt" in - security_height := get_float "security_height"; - begin - try - if security_height < ref 0. then - begin - fprintf stderr "\nError: Security height cannot be negative (%.0f)\n" !security_height; - exit 1; - end - with - _ -> () - end; - ground_alt := get_float "ground_alt"; - let home_mode_height = try - max (get_float "home_mode_height") !security_height - with _ -> !security_height in - - check_altitude (float_of_string alt) xml; - check_altitude_srtm (float_of_string alt) xml !fp_wgs84; - - Xml2h.define "NAV_DEFAULT_ALT" (sprintf "%.0f /* nominal altitude of the flight plan */" (float_of_string alt)); - Xml2h.define "NAV_UTM_EAST0" (sprintf "%.0f" utm0.utm_x); - Xml2h.define "NAV_UTM_NORTH0" (sprintf "%.0f" utm0.utm_y); - Xml2h.define "NAV_UTM_ZONE0" (sprintf "%d" utm0.utm_zone); - Xml2h.define "NAV_LAT0" (sprintf "%Ld /* 1e7deg */" (convert_angle !fp_wgs84.posn_lat)); - Xml2h.define "NAV_LON0" (sprintf "%Ld /* 1e7deg */" (convert_angle !fp_wgs84.posn_long)); - Xml2h.define "NAV_ALT0" (sprintf "%.0f /* mm above msl */" (1000. *. !ground_alt)); - Xml2h.define "NAV_MSL0" (sprintf "%.0f /* mm, EGM96 geoid-height (msl) over ellipsoid */" (1000. *. Egm96.of_wgs84 !fp_wgs84)); - - Xml2h.define "QFU" (sprintf "%.1f" qfu); - - let (hx, hy) = home waypoints in - List.iter (check_distance (hx, hy) mdfh) waypoints; - let waypoints = dummy_waypoint :: waypoints in - define_waypoints_indices waypoints; - - Xml2h.define "WAYPOINTS_UTM" "{ \\"; - List.iter (print_waypoint_utm alt) waypoints; - lprintf "};\n"; - Xml2h.define "WAYPOINTS_ENU" "{ \\"; - List.iter (print_waypoint_enu utm0 alt) waypoints; - lprintf "};\n"; - Xml2h.define "WAYPOINTS_LLA" "{ \\"; - List.iter (print_waypoint_lla utm0 alt) waypoints; - lprintf "};\n"; - Xml2h.define "WAYPOINTS_LLA_WGS84" "{ \\"; - List.iter (print_waypoint_lla_wgs84 utm0 alt) waypoints; - lprintf "};\n"; - Xml2h.define "WAYPOINTS_GLOBAL" "{ \\"; - List.iter print_waypoint_global waypoints; - lprintf "};\n"; - Xml2h.define "NB_WAYPOINT" (string_of_int (List.length waypoints)); - - Xml2h.define "FP_BLOCKS" "{ \\"; - List.iter (fun b -> printf " \"%s\" , \\\n" (ExtXml.attrib b "name")) blocks; - lprintf "}\n"; - Xml2h.define "NB_BLOCK" (string_of_int (List.length blocks)); - - Xml2h.define "GROUND_ALT" (sof !ground_alt); - Xml2h.define "GROUND_ALT_CM" (sprintf "%.0f" (100.*. !ground_alt)); - Xml2h.define "SECURITY_HEIGHT" (sof !security_height); - Xml2h.define "SECURITY_ALT" (sof (!security_height +. !ground_alt)); - Xml2h.define "HOME_MODE_HEIGHT" (sof home_mode_height); - Xml2h.define "MAX_DIST_FROM_HOME" (sof mdfh); - begin - try - let geofence_max_alt = get_float "geofence_max_alt" in - if geofence_max_alt < !ground_alt then - begin - fprintf stderr "\nError: Geofence max altitude below ground alt (%.0f < %.0f)\n" geofence_max_alt !ground_alt; - exit 1; - end - else if geofence_max_alt < (!ground_alt +. !security_height) then - begin - fprintf stderr "\nError: Geofence max altitude below security height (%.0f < (%.0f+%.0f))\n" geofence_max_alt !ground_alt !security_height; - exit 1; - end - else if geofence_max_alt < (!ground_alt +. home_mode_height) then - begin - fprintf stderr "\nError: Geofence max altitude below ground alt + home mode height (%.0f < (%.0f+%.0f))\n" geofence_max_alt !ground_alt home_mode_height; - exit 1; - end - else if geofence_max_alt < (float_of_string alt) then - fprintf stderr "\nWarning: Geofence max altitude below default waypoint alt (%.0f < %.0f)\n" geofence_max_alt (float_of_string alt); - Xml2h.define "GEOFENCE_MAX_ALTITUDE" (sof geofence_max_alt); - fprintf stderr "\nWarning: Geofence max altitude set to %.0f\n" geofence_max_alt; - with - _ -> () - end; - - begin - try - let geofence_max_height = get_float "geofence_max_height" in - if geofence_max_height < !security_height then - begin - fprintf stderr "\nError: Geofence max height below security height (%.0f < %.0f)\n" geofence_max_height !security_height; - exit 1; - end - else if geofence_max_height < home_mode_height then - begin - fprintf stderr "\nError: Geofence max height below home mode height (%.0f < %.0f)\n" geofence_max_height home_mode_height; - exit 1; - end - else if (geofence_max_height +. !ground_alt) < (float_of_string alt) then - fprintf stderr "\nWarning: Geofence max AGL below default waypoint AGL (%.0f < %.0f)\n" (geofence_max_height +. !ground_alt) (float_of_string alt); - Xml2h.define "GEOFENCE_MAX_HEIGHT" (sof geofence_max_height); - fprintf stderr "\nWarning: Geofence max AGL set to %.0f\n" geofence_max_height; - with - _ -> () - end; - - - (* output settings file if needed *) - begin - match !set_file with - | Some f -> - let out_set = open_out f in - write_settings !xml_file out_set variables_xml; - close_out out_set - | None -> () - end; - - lprintf "\n#ifdef NAV_C\n\n"; - - List.iter (fun v -> print_var_impl abi_msgs v) variables; - lprintf "\n"; - print_auto_init_bindings abi_msgs variables; - - let index_of_waypoints = - let i = ref (-1) in - List.map (fun w -> incr i; (name_of w, !i)) waypoints in - - let sectors_element = try ExtXml.child xml "sectors" with Not_found -> Xml.Element ("", [], []) in - let sectors = List.filter (fun x -> Compat.lowercase_ascii (Xml.tag x) = "sector") (Xml.children sectors_element) in - let sectors_type = List.map (fun x -> match ExtXml.attrib_or_default x "type" "static" with "dynamic" -> DynamicSector | _ -> StaticSector) sectors in - let sectors = List.map (parse_wpt_sector index_of_waypoints waypoints) sectors in - List.iter2 print_inside_sector sectors_type sectors; - - lprintf "\nstatic inline void auto_nav(void) {\n"; - right (); - List.iter print_exception global_exceptions; - lprintf "switch (nav_block) {\n"; - right (); - print_blocks index_of_waypoints blocks; - lprintf "default: break;\n"; - left (); - lprintf "}\n"; - left (); - lprintf "}\n"; - lprintf "#endif // NAV_C\n"; - - begin - try - let geofence_sector = Xml.attrib xml "geofence_sector" in - lprintf "#define InGeofenceSector(_x, _y) %s(_x, _y)\n" (inside_function geofence_sector) - with - _ -> () - end; - - Xml2h.finish h_name - end - with - Failure x -> - fprintf stderr "%s: %s\n" !xml_file x; exit 1 diff --git a/sw/tools/generators/gen_makefile.ml b/sw/tools/generators/gen_makefile.ml new file mode 100644 index 0000000000..f1548076af --- /dev/null +++ b/sw/tools/generators/gen_makefile.ml @@ -0,0 +1,167 @@ +(* + * Copyright (C) 2017 Gautier Hattenberger + * Cyril Allignol + * + * 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 + * . + *) + +(** + * Generate makefile from the aircraft configuration + *) + +open Printf +open Gen_common + +module AC = Aircraft + +(* TODO: add condition in xml syntax ? *) + +let get_string_opt = fun x -> match x with Some s -> s | None -> "" + +let (//) = Filename.concat + +let configure2mk = fun ?(default_configure=false) f c -> + (* all makefiles variables are forced to uppercase *) + let name = Compat.uppercase_ascii c.Module.cname + and value = get_string_opt c.Module.cvalue + and default = get_string_opt c.Module.default + and case = get_string_opt c.Module.case in + (* either print the default or the normal configure variable *) + if default_configure then begin + (* Only print variable if default is set but not value *) + if String.length default > 0 && String.length value = 0 then + fprintf f "%s ?= %s\n" name default + end else begin + (* Only print variable if value is not empty *) + if String.length value > 0 then + fprintf f "%s = %s\n" name value; + (* Or if only the name is given (unset a variable *) + if String.length value = 0 && String.length default = 0 && String.length case = 0 then + fprintf f "%s =\n" name + end; + (* also providing lower and upper case version on request *) + if Str.string_match (Str.regexp ".*lower.*") case 0 then + fprintf f "%s_LOWER = $(shell echo $(%s) | tr A-Z a-z)\n" name name; + if Str.string_match (Str.regexp ".*upper.*") case 0 then + fprintf f "%s_UPPER = $(shell echo $(%s) | tr a-z A-Z)\n" name name + +let include2mk = fun f ?(target="$(TARGET)") ?(vpath=None) inc -> + let name = inc.Module.element + and path = match vpath with Some vp -> vp ^ "/" | None -> "" in + let flag = sprintf "%s.CFLAGS += -I%s%s" target path name in + match inc.Module.condition with + | None -> fprintf f "%s\n" flag + | Some cond -> fprintf f "%s\n%s\nendif\n" cond flag + +let flag2mk = fun f ?(target="$(TARGET)") x -> + let name = x.Module.flag + and value = x.Module.value in + let flag = sprintf "%s.%s += -%s" target name value in + match x.Module.fcond with + | None -> fprintf f "%s\n" flag + | Some cond -> fprintf f "%s\n%s\nendif\n" cond flag + +let define2mk = fun f ?(target="$(TARGET)") define -> + let name = define.Module.dname + and value = define.Module.dvalue in + let flag_type = fun s -> + match define.Module.dtype, value with + | Some "string", Some v -> "=\\\""^v^"\\\"" + | _, Some v -> "="^v + | _, _ -> "" + in + let flag = sprintf "%s.CFLAGS += -D%s%s" target name (flag_type value) in + match define.Module.cond with + | None -> fprintf f "%s\n" flag + | Some cond -> fprintf f "%s\n%s\nendif\n" cond flag + +let raw2mk = fun f raw -> + fprintf f "%s\n" raw + +let file2mk = fun f ?(arch = false) dir_name target file -> + let name = file.Module.filename in + let dir_name = match file.Module.directory with Some d -> d + | None -> "$(" ^ dir_name ^ ")" in + let cond, cond_end = match file.Module.filecond with None -> "", "" + | Some c -> "\n"^c^"\n", "\nendif" in + let fmt = + if arch then format_of_string "%s%s.srcs += arch/$(ARCH)/%s/%s%s\n" + else format_of_string "%s%s.srcs += %s/%s%s\n" in + fprintf f fmt cond target dir_name name cond_end + +(* module files and flags except configuration flags *) +let module2mk = fun f target firmware m -> + let name = m.Module.name in + let dir = match m.Module.dir with Some d -> d | None -> name in + let dir_name = Compat.uppercase_ascii dir ^ "_DIR" in + (* iter makefile section *) + List.iter (fun mk -> + if Module.check_mk target firmware mk then begin + begin match mk.Module.condition with Some c -> fprintf f "%s\n" c | None -> () end; + List.iter (define2mk f ~target) mk.Module.defines; + List.iter (include2mk f ~target (*FIXME vpath*)) mk.Module.inclusions; + List.iter (flag2mk f ~target) mk.Module.flags; + List.iter (file2mk f dir_name target) mk.Module.files; + List.iter (file2mk f ~arch:true dir_name target) mk.Module.files_arch; + List.iter (raw2mk f) mk.Module.raws; + match mk.Module.condition with Some _ -> fprintf f "endif\n" | None -> () + end + ) m.Module.makefiles + +let dump_target_conf = fun out target conf -> + fprintf out "\n####################################################\n"; + fprintf out "# makefile target '%s' for firmware '%s'\n" target conf.AC.firmware_name; + fprintf out "####################################################\n\n"; + fprintf out "ifeq ($(TARGET), %s)\n\n" target; + let dir_list = singletonize (List.fold_left (fun l (_, m) -> match m.Module.dir with + | None -> m.Module.name::l | Some d -> d::l) [] conf.AC.modules) in + List.iter (fun d -> fprintf out "%s_DIR = modules/%s\n" (Compat.uppercase_ascii d) d) dir_list; + List.iter (fun p -> + fprintf out "VPATH += %s\n" p; + fprintf out "$(TARGET).CFLAGS += -I%s/modules\n" p + ) Env.modules_ext_paths; + fprintf out "\n"; + if conf.AC.autopilot then + fprintf out "USE_GENERATED_AUTOPILOT = TRUE\n"; + List.iter (configure2mk out) conf.AC.configures; + fprintf out "\ninclude $(PAPARAZZI_SRC)/conf/boards/%s.makefile\n" conf.AC.board_type; + fprintf out "include $(PAPARAZZI_SRC)/conf/firmwares/%s.makefile\n\n" conf.AC.firmware_name; + List.iter (configure2mk ~default_configure:true out) conf.AC.configures_default; + fprintf out "\n"; + List.iter (define2mk out) conf.AC.defines; + fprintf out "\n"; + List.iter (fun (l, m) -> match l with + | AC.UserLoad | AC.AutoLoad -> module2mk out target conf.AC.firmware_name m + | _ -> () + ) conf.AC.modules; + fprintf out "\nendif # end of target '%s'\n\n" target + + +(** Generate makefile configuration files *) +let generate_makefile = fun ac_id confs makefile_out -> + let out = open_out makefile_out in + fprintf out "# This file has been generated by gen_aircraft\n"; + fprintf out "# Version %s\n" (Env.get_paparazzi_version ()); + fprintf out "# Please DO NOT EDIT\n"; + fprintf out "AC_ID=%s\n\n" ac_id; + fprintf out "$(TARGET).CFLAGS += -Imodules -Iarch/$(ARCH)/modules\n"; + + (** Iter on targets and dump makefile *) + Hashtbl.iter (fun target c -> dump_target_conf out target c) confs; + + close_out out + diff --git a/sw/tools/generators/gen_modules.ml b/sw/tools/generators/gen_modules.ml index 7906b25afa..21292c4bdd 100644 --- a/sw/tools/generators/gen_modules.ml +++ b/sw/tools/generators/gen_modules.ml @@ -1,7 +1,7 @@ (* * XML preprocessing for modules * - * Copyright (C) 2009 Gautier Hattenberger + * Copyright (C) 2009-2020 Gautier Hattenberger * * This file is part of paparazzi. * @@ -26,9 +26,6 @@ open Printf open Xml2h module GC = Gen_common -(** Default main frequency = 60Hz *) -let freq = ref 60 - (** Formatting with a margin *) let margin = ref 0 let step = 2 @@ -36,24 +33,19 @@ let step = 2 let right () = margin := !margin + step let left () = margin := !margin - step -let out_h = stdout - let lprintf = fun out f -> fprintf out "%s" (String.make !margin ' '); fprintf out f -let print_headers = fun modules -> - lprintf out_h "#include \"std.h\"\n"; +let print_headers = fun out modules -> + lprintf out "#include \"std.h\"\n"; List.iter (fun m -> - let dir_name = try Xml.attrib m "dir" with _ -> Xml.attrib m "name" in - try - let headers = ExtXml.child m "header" in - List.iter (fun h -> - let dir = ExtXml.attrib_or_default h "dir" dir_name in - lprintf out_h "#include \"%s/%s\"\n" dir (Xml.attrib h "name")) - (Xml.children headers) - with _ -> ()) - modules + let dirname = match m.Module.dir with None -> m.Module.name | Some d -> d in + List.iter (fun h -> + let dir = match h.Module.directory with None -> dirname | Some d -> d in + Printf.fprintf out "#include \"%s/%s\"\n" dir h.Module.filename + ) m.Module.headers + ) modules let get_status_name = fun f n -> let func = (Xml.attrib f "fun") in @@ -63,6 +55,21 @@ let get_status_shortname = fun f -> let func = (Xml.attrib f "fun") in String.sub func 0 (try String.index func '(' with _ -> (String.length func)) +(*let fprint_status = fun ch mod_name p -> + match p.autorun with + | True | False -> + Printf.fprintf ch "EXTERN_MODULES uint8_t %s;\n" (status_name mod_name p) + | Lock -> () + +let fprint_periodic_init = fun ch mod_name p -> + match p.autorun with + | True -> Printf.fprintf ch "%s = %s;" (status_name mod_name p) "MODULES_START" + | False -> Printf.fprintf ch "%s = %s;" (status_name mod_name p) "MODULES_IDLE" + | Lock -> () + +let fprint_init = fun ch init -> Printf.fprintf ch "%s;\n" init +*) + let get_period_and_freq = fun f max_freq -> let period = try Some (float_of_string (Xml.attrib f "period")) with _ -> None and freq = try Some (float_of_string (Xml.attrib f "freq")) with _ -> None in @@ -74,6 +81,14 @@ let get_period_and_freq = fun f max_freq -> fprintf stderr "Warning: both period and freq are defined but only period is used for function %s\n" (ExtXml.attrib f "fun"); (_p, 1. /. _p) +(*let fprint_period_freq = fun ch max_freq p -> + let period, freq = match p.period_freq with + | Unset -> 1. /. max_freq, max_freq + | Set (p, f) -> p, f in + let cap_fname = Compat.uppercase_ascii p.fname in + Printf.fprintf ch "#define %s_PERIOD %f\n" cap_fname period; + Printf.fprintf ch "#define %s_FREQ %f\n" cap_fname freq*) + (* Extract function name and return in capital letters *) let get_cap_name = fun f -> let name = Str.full_split (Str.regexp "[()]") f in @@ -83,43 +98,43 @@ let get_cap_name = fun f -> | [Str.Text t; Str.Delim "("; Str.Text _ ; Str.Delim ")"] -> Compat.uppercase_ascii t | _ -> failwith "Gen_modules: not a valid function name" -let print_function_freq = fun modules -> - let max_freq = float !freq in - nl (); +let print_function_freq = fun out freq modules -> + let max_freq = float freq in + fprintf out "\n"; List.iter (fun m -> List.iter (fun i -> match Xml.tag i with "periodic" -> let fname = get_cap_name (Xml.attrib i "fun") in let p, f = get_period_and_freq i max_freq in - lprintf out_h "#define %s_PERIOD %f\n" fname p; - lprintf out_h "#define %s_FREQ %f\n" fname f; + lprintf out "#define %s_PERIOD %f\n" fname p; + lprintf out "#define %s_FREQ %f\n" fname f; | _ -> ()) - (Xml.children m)) + (Xml.children m.Module.xml)) modules let is_status_lock = fun p -> let mode = ExtXml.attrib_or_default p "autorun" "LOCK" in mode = "LOCK" -let print_status = fun modules -> - nl (); +let print_status = fun out modules -> + fprintf out "\n"; List.iter (fun m -> - let module_name = ExtXml.attrib m "name" in + let module_name = m.Module.name in List.iter (fun i -> match Xml.tag i with "periodic" -> if not (is_status_lock i) then begin - lprintf out_h "EXTERN_MODULES uint8_t %s;\n" (get_status_name i module_name); + lprintf out "EXTERN_MODULES uint8_t %s;\n" (get_status_name i module_name); end | _ -> ()) - (Xml.children m)) + (Xml.children m.Module.xml)) modules let modules_of_task = fun modules -> let h = Hashtbl.create 1 in List.iter (fun m -> - let task = ExtXml.attrib_or_default m "task" "default" in + let task = match m.Module.task with None -> "default" | Some t -> t in if Hashtbl.mem h task then Hashtbl.replace h task (List.append [m] (Hashtbl.find h task)) else @@ -127,54 +142,54 @@ let modules_of_task = fun modules -> ) modules; h -let print_init = fun task modules -> - lprintf out_h "\nstatic inline void modules_%s_init(void) {\n" task; +let print_init = fun out task modules -> + lprintf out "\nstatic inline void modules_%s_init(void) {\n" task; right (); List.iter (fun m -> - let module_name = ExtXml.attrib m "name" in + let module_name = m.Module.name in List.iter (fun i -> match Xml.tag i with - "init" -> lprintf out_h "%s;\n" (Xml.attrib i "fun") + "init" -> lprintf out "%s;\n" (Xml.attrib i "fun") | "periodic" -> if not (is_status_lock i) then - lprintf out_h "%s = %s;\n" (get_status_name i module_name) (try match Xml.attrib i "autorun" with + lprintf out "%s = %s;\n" (get_status_name i module_name) (try match Xml.attrib i "autorun" with "TRUE" | "true" -> "MODULES_START" | "FALSE" | "false" | "LOCK" | "lock" -> "MODULES_IDLE" | _ -> failwith "Error: Unknown autorun value (possible values are: TRUE, FALSE, LOCK(default))" with _ -> "MODULES_IDLE" (* this should not be possible anyway *)) | _ -> ()) - (Xml.children m)) + (Xml.children m.Module.xml)) modules; left (); - lprintf out_h "}\n" + lprintf out "}\n" -let print_init_functions = fun modules -> +let print_init_functions = fun out modules -> let h = modules_of_task modules in - Hashtbl.iter print_init h; - lprintf out_h "\nstatic inline void modules_init(void) {\n"; + Hashtbl.iter (print_init out) h; + lprintf out "\nstatic inline void modules_init(void) {\n"; right (); - Hashtbl.iter (fun t _ -> lprintf out_h "modules_%s_init();\n" t) h; + Hashtbl.iter (fun t _ -> lprintf out "modules_%s_init();\n" t) h; left (); - lprintf out_h "}\n" + lprintf out "}\n" -let print_periodic = fun task modules -> - let min_period = 1. /. float !freq - and max_period = 65536. /. float !freq - and min_freq = float !freq /. 65536. - and max_freq = float !freq in +let print_periodic = fun out freq task modules -> + let min_period = 1. /. float freq + and max_period = 65536. /. float freq + and min_freq = float freq /. 65536. + and max_freq = float freq in - lprintf out_h "\nstatic inline void modules_%s_periodic_task(void) {\n" task; + lprintf out "\nstatic inline void modules_%s_periodic_task(void) {\n" task; right (); (** Computes the required modulos *) let functions_modulo = List.flatten (List.map (fun m -> - let periodic = List.filter (fun i -> (String.compare (Xml.tag i) "periodic") == 0) (Xml.children m) in - let module_name = ExtXml.attrib m "name" in + let periodic = List.filter (fun i -> (String.compare (Xml.tag i) "periodic") == 0) (Xml.children m.Module.xml) in + let module_name = m.Module.name in List.map (fun x -> let p, _ = get_period_and_freq x max_freq in if p < min_period || p > max_period then fprintf stderr "Warning: period is bound between %.3fs and %.3fs (%fHz and %.1fHz) for function %s\n%!" min_period max_period max_freq min_freq (ExtXml.attrib x "fun"); - ((x, module_name), min 65535 (max 1 (int_of_float (p *. float_of_int !freq)))) + ((x, module_name), min 65535 (max 1 (int_of_float (p *. float_of_int freq)))) ) periodic) modules) in let modulos = GC.singletonize (List.map snd functions_modulo) in @@ -182,32 +197,32 @@ let print_periodic = fun task modules -> List.iter (fun modulo -> let v = sprintf "i%d" modulo in let _type = if modulo >= 256 then "uint16_t" else "uint8_t" in - lprintf out_h "static %s %s; %s++; if (%s>=%d) %s=0;\n" _type v v v modulo v;) + lprintf out "static %s %s; %s++; if (%s>=%d) %s=0;\n" _type v v v modulo v;) modulos; (** Print start and stop functions *) List.iter (fun m -> - let module_name = ExtXml.attrib m "name" in - let periodic = List.filter (fun i -> (String.compare (Xml.tag i) "periodic") == 0) (Xml.children m) in - nl (); + let module_name = m.Module.name in + let periodic = List.filter (fun i -> (String.compare (Xml.tag i) "periodic") == 0) (Xml.children m.Module.xml) in + fprintf out "\n"; List.iter (fun f -> if (is_status_lock f) then begin - try lprintf out_h "%s;\n" (Xml.attrib f "start") with _ -> (); + try lprintf out "%s;\n" (Xml.attrib f "start") with _ -> (); try let stop = Xml.attrib f "stop" in fprintf stderr "Warning: stop %s function will not be called\n" stop with _ -> (); end else begin let status = get_status_name f module_name in - lprintf out_h "if (%s == MODULES_START) {\n" status; + lprintf out "if (%s == MODULES_START) {\n" status; right (); - ignore(try lprintf out_h "%s;\n" (Xml.attrib f "start") with _ -> ()); - lprintf out_h "%s = MODULES_RUN;\n" status; + ignore(try lprintf out "%s;\n" (Xml.attrib f "start") with _ -> ()); + lprintf out "%s = MODULES_RUN;\n" status; left (); - lprintf out_h "}\n"; - lprintf out_h "if (%s == MODULES_STOP) {\n" status; + lprintf out "}\n"; + lprintf out "if (%s == MODULES_STOP) {\n" status; right (); - ignore(try lprintf out_h "%s;\n" (Xml.attrib f "stop") with _ -> ()); - lprintf out_h "%s = MODULES_IDLE;\n" status; + ignore(try lprintf out "%s;\n" (Xml.attrib f "stop") with _ -> ()); + lprintf out "%s = MODULES_IDLE;\n" status; left (); - lprintf out_h "}\n"; + lprintf out "}\n"; end ) periodic) @@ -216,20 +231,20 @@ let print_periodic = fun task modules -> let functions = List.sort (fun (_,p) (_,p') -> compare p p') functions_modulo in let i = ref 0 in (** Basic balancing:1 function every 10Hz FIXME *) let l = ref [] in - nl (); + fprintf out "\n"; let test_delay = fun x -> try let _ = Xml.attrib x "delay" in true with _ -> false in List.iter (fun ((func, name), p) -> let function_name = ExtXml.attrib func "fun" in if p = 1 then begin if (is_status_lock func) then - lprintf out_h "%s;\n" function_name + lprintf out "%s;\n" function_name else begin - lprintf out_h "if (%s == MODULES_RUN) {\n" (get_status_name func name); + lprintf out "if (%s == MODULES_RUN) {\n" (get_status_name func name); right (); - lprintf out_h "%s;\n" function_name; + lprintf out "%s;\n" function_name; left (); - lprintf out_h "}\n"; + lprintf out "}\n"; end end else @@ -242,9 +257,9 @@ let print_periodic = fun task modules -> let else_ = if List.mem_assoc p !l && not (List.mem (p, delay_p) !l) then "else " else "" in if (is_status_lock func) then - lprintf out_h "%sif (i%d == %d) {\n" else_ p delay_p + lprintf out "%sif (i%d == %d) {\n" else_ p delay_p else - lprintf out_h "%sif (i%d == %d && %s == MODULES_RUN) {\n" else_ p delay_p (get_status_name func name); + lprintf out "%sif (i%d == %d && %s == MODULES_RUN) {\n" else_ p delay_p (get_status_name func name); l := (p, delay_p) :: !l; end else begin @@ -252,59 +267,59 @@ let print_periodic = fun task modules -> i := !i mod p; let else_ = if List.mem_assoc p !l && not (List.mem (p, !i) !l) then "else " else "" in if (is_status_lock func) then - lprintf out_h "%sif (i%d == %d) {\n" else_ p !i + lprintf out "%sif (i%d == %d) {\n" else_ p !i else - lprintf out_h "%sif (i%d == %d && %s == MODULES_RUN) {\n" else_ p !i (get_status_name func name); + lprintf out "%sif (i%d == %d && %s == MODULES_RUN) {\n" else_ p !i (get_status_name func name); l := (p, !i) :: !l; let incr = p / ((List.length (List.filter (fun (_,p') -> compare p p' == 0) functions)) + 1) in i := !i + incr; end; right (); - lprintf out_h "%s;\n" function_name; + lprintf out "%s;\n" function_name; left (); - lprintf out_h "}\n" + lprintf out "}\n" end; ) functions; left (); - lprintf out_h "}\n" + lprintf out "}\n" -let print_periodic_functions = fun modules -> +let print_periodic_functions = fun out freq modules -> let h = modules_of_task modules in - Hashtbl.iter print_periodic h; - lprintf out_h "\nstatic inline void modules_periodic_task(void) {\n"; + Hashtbl.iter (print_periodic out freq) h; + lprintf out "\nstatic inline void modules_periodic_task(void) {\n"; right (); - Hashtbl.iter (fun t _ -> lprintf out_h "modules_%s_periodic_task();\n" t) h; + Hashtbl.iter (fun t _ -> lprintf out "modules_%s_periodic_task();\n" t) h; left (); - lprintf out_h "}\n" + lprintf out "}\n" -let print_event = fun task modules -> - lprintf out_h "\nstatic inline void modules_%s_event_task(void) {\n" task; +let print_event = fun out task modules -> + lprintf out "\nstatic inline void modules_%s_event_task(void) {\n" task; right (); List.iter (fun m -> List.iter (fun i -> match Xml.tag i with - "event" -> lprintf out_h "%s;\n" (Xml.attrib i "fun") + "event" -> lprintf out "%s;\n" (Xml.attrib i "fun") | _ -> ()) - (Xml.children m)) + (Xml.children m.Module.xml)) modules; left (); - lprintf out_h "}\n" + lprintf out "}\n" -let print_event_functions = fun modules -> +let print_event_functions = fun out modules -> let h = modules_of_task modules in - Hashtbl.iter print_event h; - lprintf out_h "\nstatic inline void modules_event_task(void) {\n"; + Hashtbl.iter (print_event out) h; + lprintf out "\nstatic inline void modules_event_task(void) {\n"; right (); - Hashtbl.iter (fun t _ -> lprintf out_h "modules_%s_event_task();\n" t) h; + Hashtbl.iter (fun t _ -> lprintf out "modules_%s_event_task();\n" t) h; left (); - lprintf out_h "}\n" + lprintf out "}\n" -let print_datalink_functions = fun modules -> - lprintf out_h "\n#include \"pprzlink/messages.h\"\n"; - lprintf out_h "#include \"generated/airframe.h\"\n"; - lprintf out_h "static inline void modules_parse_datalink(uint8_t msg_id __attribute__ ((unused)), +let print_datalink_functions = fun out modules -> + lprintf out "\n#include \"pprzlink/messages.h\"\n"; + lprintf out "#include \"generated/airframe.h\"\n"; + lprintf out "static inline void modules_parse_datalink(uint8_t msg_id __attribute__ ((unused)), struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf __attribute__((unused))) {\n"; @@ -314,27 +329,27 @@ let print_datalink_functions = fun modules -> List.iter (fun i -> match Xml.tag i with "datalink" -> - lprintf out_h "%sif (msg_id == DL_%s) { %s; }\n" !else_ (ExtXml.attrib i "message") (ExtXml.attrib i "fun"); + lprintf out "%sif (msg_id == DL_%s) { %s; }\n" !else_ (ExtXml.attrib i "message") (ExtXml.attrib i "fun"); else_ := "else " | _ -> ()) - (Xml.children m)) + (Xml.children m.Module.xml)) modules; left (); - lprintf out_h "}\n" + lprintf out "}\n" -let parse_modules modules = - print_headers modules; - print_function_freq modules; - print_status modules; - nl (); - print_init_functions modules; - print_periodic_functions modules; - print_event_functions modules; - nl (); - fprintf out_h "#ifdef MODULES_DATALINK_C\n"; - print_datalink_functions modules; - nl (); - fprintf out_h "#endif // MODULES_DATALINK_C\n" +let parse_modules out freq modules = + print_headers out modules; + print_function_freq out freq modules; + print_status out modules; + fprintf out "\n"; + print_init_functions out modules; + print_periodic_functions out freq modules; + print_event_functions out modules; + fprintf out "\n"; + fprintf out "#ifdef MODULES_DATALINK_C\n"; + print_datalink_functions out modules; + fprintf out "\n"; + fprintf out "#endif // MODULES_DATALINK_C\n" let test_section_modules = fun xml -> List.fold_right (fun x r -> ExtXml.tag_is x "modules" || r) (Xml.children xml) false @@ -365,8 +380,8 @@ let get_pcdata = fun xml tag -> let check_dependencies = fun modules names -> List.iter (fun m -> try - let module_name = Xml.attrib m "name" in - let dep_string = get_pcdata m "depends" in + let module_name = m.Module.name in + let dep_string = get_pcdata m.Module.xml "depends" in (*fprintf stderr "\n\nWARNING: parsing dep string: %s\n\n" dep_string; fprintf stderr "\n\nWARNING: names: %s" (String.concat "," names);*) let require = deps_of_string dep_string in @@ -381,7 +396,7 @@ let check_dependencies = fun modules names -> exit 1 end) require; - let conflict_string = get_pcdata m "conflicts" in + let conflict_string = get_pcdata m.Module.xml "conflicts" in let conflict_l = List.flatten (deps_of_string conflict_string) in List.iter (fun con -> if List.exists (fun c -> String.compare c con == 0) names then @@ -390,83 +405,28 @@ let check_dependencies = fun modules names -> with _ -> () ) modules -let write_settings = fun xml_file out_set modules -> - fprintf out_set "\n" xml_file; - fprintf out_set "\n" (Env.get_paparazzi_version ()); - fprintf out_set "\n\n"; - fprintf out_set "\n"; - fprintf out_set " \n"; - let setting_exist = ref false in - List.iter (fun m -> - let module_name = ExtXml.attrib m "name" in - List.iter (fun i -> - match Xml.tag i with - "periodic" -> - if not (is_status_lock i) then begin - if (not !setting_exist) then begin - fprintf out_set " \n"; - setting_exist := true; - end; - fprintf out_set " \n" - (get_status_name i module_name) (get_status_shortname i) - end - | _ -> ()) - (Xml.children m)) - modules; - if !setting_exist then fprintf out_set " \n"; - fprintf out_set " \n"; - fprintf out_set "\n" let h_name = "MODULES_H" -let () = - if Array.length Sys.argv <> 6 then - failwith (Printf.sprintf "Usage: %s ac_id out_settings_file default_freq fp_file xml_file" Sys.argv.(0)); - let xml_file = Sys.argv.(5) - and fp_file = Sys.argv.(4) - and default_freq = int_of_string(Sys.argv.(3)) - and out_set = open_out Sys.argv.(2) - and ac_id = Sys.argv.(1) in - try - let xml = start_and_begin xml_file h_name in - fprintf out_h "#define MODULES_IDLE 0\n"; - fprintf out_h "#define MODULES_RUN 1\n"; - fprintf out_h "#define MODULES_START 2\n"; - fprintf out_h "#define MODULES_STOP 3\n"; - nl (); - (* Extract main_freq parameter *) - let modules = try (ExtXml.child xml "modules") with _ -> Xml.Element("modules",[],[]) in - let main_freq = try (int_of_string (Xml.attrib modules "main_freq")) with _ -> default_freq in - freq := main_freq; - fprintf out_h "#define MODULES_FREQUENCY %d\n" !freq; - nl (); - fprintf out_h "#ifdef MODULES_C\n"; - fprintf out_h "#define EXTERN_MODULES\n"; - fprintf out_h "#else\n"; - fprintf out_h "#define EXTERN_MODULES extern\n"; - fprintf out_h "#endif"; - nl (); - (* Extract modules list *) - let modules = - try - let target = Sys.getenv "TARGET" in - GC.get_modules_of_config ~target ac_id xml (ExtXml.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 - (* Extract xml modules nodes *) - let modules_list = List.map (fun m -> m.GC.xml) modules in - check_dependencies modules_list modules_name; - parse_modules modules_list; - finish h_name; - write_settings xml_file out_set modules_list; - close_out out_set; - 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 +let generate = fun modules freq xml_file out_file -> + let out = open_out out_file in + + begin_out out xml_file h_name; + define_out out "MODULES_IDLE " "0"; + define_out out "MODULES_RUN " "1"; + define_out out "MODULES_START" "2"; + define_out out "MODULES_STOP " "3"; + fprintf out "\n"; + + define_out out "MODULES_FREQUENCY" (string_of_int freq); + fprintf out "\n"; + fprintf out "#ifdef MODULES_C\n"; + fprintf out "#define EXTERN_MODULES\n"; + fprintf out "#else\n"; + fprintf out "#define EXTERN_MODULES extern\n"; + fprintf out "#endif\n"; + + parse_modules out freq modules; + + finish_out out h_name + diff --git a/sw/tools/generators/gen_periodic.ml b/sw/tools/generators/gen_periodic.ml index 311f81aa3c..3ef2eb536e 100644 --- a/sw/tools/generators/gen_periodic.ml +++ b/sw/tools/generators/gen_periodic.ml @@ -1,7 +1,7 @@ (* - * XML preprocessing for periodic messages - * * Copyright (C) 2003 Pascal Brisset, Antoine Drouin + * Copyright (C) 2017 Gautier Hattenberger + * Cyril Allignol * * This file is part of paparazzi. * @@ -16,12 +16,15 @@ * 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. + * along with paparazzi; see the file COPYING. If not, see + * . * *) +(** + * XML preprocessing for periodic messages + *) + open Printf module GC = Gen_common @@ -103,39 +106,6 @@ let output_modes = fun out_h process_name telem_type modes freq -> lprintf out_h "}\n") modes -let write_settings = fun xml_file out_set telemetry_xml -> - (* filter xml file to remove unneeded process and modes (more than 1 mode per process) *) - let filtered_xml = List.filter (fun p -> List.length (Xml.children p) > 1) (Xml.children telemetry_xml) in - fprintf out_set "\n" xml_file; - fprintf out_set "\n" (Env.get_paparazzi_version ()); - fprintf out_set "\n\n"; - fprintf out_set "\n"; - if List.length filtered_xml > 0 then begin - fprintf out_set " \n"; - fprintf out_set " \n"; - List.iter (fun p -> - (* for each (pre-filtered) process *) - let process_name = Xml.attrib p "name" in - (* convert the xml list of mode to a string list *) - let modes = List.map (fun m -> Xml.attrib m "name") (Xml.children p) in - let nb_modes = List.length modes in - match nb_modes with - | 0 | 1 -> () (* Nothing to do if 1 or zero mode *) - | _ -> (* add settings with all modes *) - fprintf out_set " \n" (nb_modes-1) process_name process_name (String.concat "|" modes); - let i = ref 0 in - List.iter (fun m -> try - let key = Xml.attrib m "key_press" in - fprintf out_set " \n" key (string_of_int !i); - incr i - with _ -> incr i) (Xml.children p); - fprintf out_set " \n" - ) filtered_xml; - fprintf out_set " \n"; - fprintf out_set " \n"; - end; - fprintf out_set "\n" - let print_message_table = fun out_h xml -> let telemetry_types = Hashtbl.create 2 in (* For each process *) @@ -158,7 +128,7 @@ let print_message_table = fun out_h xml -> (* Print ID *) fprintf out_h "/* Periodic telemetry messages of type %s */\n" telem_type; let nb = Hashtbl.fold (fun n _ i -> - Xml2h.define (sprintf "TELEMETRY_%s_MSG_%s_IDX" telem_type n) (sprintf "%d" i); + Xml2h.define_out out_h (sprintf "TELEMETRY_%s_MSG_%s_IDX" telem_type n) (sprintf "%d" i); i+1 ) messages 0 in fprintf out_h "#define TELEMETRY_%s_NB_MSG %d\n" telem_type nb; @@ -181,19 +151,19 @@ let print_process_send = fun out_h xml freq -> fprintf out_h "\n/* Periodic telemetry (type %s): %s process */\n" telem_type process_name; let p_id = ref 0 in - Xml2h.define (sprintf "TELEMETRY_PROCESS_%s" process_name) (string_of_int !p_id); + Xml2h.define_out out_h (sprintf "TELEMETRY_PROCESS_%s" process_name) (string_of_int !p_id); incr p_id; (** For each mode of this process *) let _ = List.fold_left (fun i mode -> let name = ExtXml.attrib mode "name" in - Xml2h.define (sprintf "TELEMETRY_MODE_%s_%s" process_name name) (string_of_int i); + Xml2h.define_out out_h (sprintf "TELEMETRY_MODE_%s_%s" process_name name) (string_of_int i); (* Output the periods of the messages *) List.iter (fun x -> let p = ExtXml.attrib x "period" and n = ExtXml.attrib x "name" in (* FIXME really needed ? *) - Xml2h.define (sprintf "PERIOD_%s_%s_%d" n process_name i) (sprintf "(%s)" p) + Xml2h.define_out out_h (sprintf "PERIOD_%s_%s_%d" n process_name i) (sprintf "(%s)" p) ) (Xml.children mode); i + 1 ) 0 modes in @@ -217,42 +187,30 @@ let print_process_send = fun out_h xml freq -> (Xml.children xml) - -let _ = - if Array.length Sys.argv <> 5 then begin - failwith (sprintf "Usage: %s frequency_in_hz out_settings_file" Sys.argv.(0)) - end; - - let freq = int_of_string(Sys.argv.(3)) in - let telemetry_xml = - try - Xml.parse_file Sys.argv.(2) - with Dtd.Check_error e -> failwith (Dtd.check_error e) - in - - let out_h = stdout in +(** + * main generation function + *) +let generate = fun telemetry freq out_file -> + let out = open_out out_file in (** Print header *) - fprintf out_h "/* This file has been generated by gen_periodic from %s and %s */\n" Sys.argv.(1) Sys.argv.(2); - fprintf out_h "/* Version %s */\n" (Env.get_paparazzi_version ()); - fprintf out_h "/* Please DO NOT EDIT */\n\n"; - fprintf out_h "#ifndef _VAR_PERIODIC_H_\n"; - fprintf out_h "#define _VAR_PERIODIC_H_\n\n"; - fprintf out_h "#include \"std.h\"\n"; - fprintf out_h "#include \"generated/airframe.h\"\n"; - fprintf out_h "#include \"subsystems/datalink/telemetry_common.h\"\n\n"; - fprintf out_h "#define TELEMETRY_FREQUENCY %d\n\n" freq; + fprintf out "/* This file has been generated by gen_periodic */\n"; + fprintf out "/* Version %s */\n" (Env.get_paparazzi_version ()); + fprintf out "/* Please DO NOT EDIT */\n\n"; + fprintf out "#ifndef _VAR_PERIODIC_H_\n"; + fprintf out "#define _VAR_PERIODIC_H_\n\n"; + fprintf out "#include \"std.h\"\n"; + fprintf out "#include \"generated/airframe.h\"\n"; + fprintf out "#include \"subsystems/datalink/telemetry_common.h\"\n\n"; + fprintf out "#define TELEMETRY_FREQUENCY %d\n\n" freq; (** Print the telemetry table with ID *) - print_message_table out_h telemetry_xml; + print_message_table out telemetry.Telemetry.xml; (** Print process sending functions *) - print_process_send out_h telemetry_xml freq; + print_process_send out telemetry.Telemetry.xml freq; - (** Output XML settings file with telemetry modes *) - let out_set = open_out Sys.argv.(4) in - write_settings Sys.argv.(2) out_set telemetry_xml; - close_out out_set; + fprintf out "#endif // _VAR_PERIODIC_H_\n"; - fprintf out_h "#endif // _VAR_PERIODIC_H_\n"; + close_out out diff --git a/sw/tools/generators/gen_radio.ml b/sw/tools/generators/gen_radio.ml index df74cbe86d..a0c39c3d25 100644 --- a/sw/tools/generators/gen_radio.ml +++ b/sw/tools/generators/gen_radio.ml @@ -2,6 +2,7 @@ * XML preprocessing for radio-control parameters * * Copyright (C) 2003 Pascal Brisset, Antoine Drouin + * Copyright (C) 2017 Gautier Hattenberger, Cyril Allignol * * This file is part of paparazzi. * @@ -23,21 +24,10 @@ *) open Printf -open Xml2h +open Radio let h_name = "RADIO_H" -let fos = float_of_string -type us = int - -type channel = { - name : string; - min : us; - max : us; - neutral : us; - averaged : bool } - - (* Characters used in Gen_airframe.pprz_value *) let check_function_name = fun s -> for i = 0 to String.length s - 1 do @@ -47,24 +37,6 @@ let check_function_name = fun s -> failwith (sprintf "Character '%c' not allowed in function name '%s'" s.[i] s) done -let parse_channel = - let no_channel = ref 0 in - fun c -> - let name = ExtXml.attrib c "function" in - check_function_name name; - let ctl = "RADIO_CTL_"^ExtXml.attrib c "ctl" - and fct = "RADIO_" ^ name in - define ctl (string_of_int !no_channel); - define fct ctl; - no_channel := !no_channel + 1; - let int_attrib = fun x -> int_of_string (ExtXml.attrib c x) in - { min = int_attrib "min"; - neutral = int_attrib "neutral"; - max = int_attrib "max"; - averaged = ExtXml.attrib_or_default c "average" "0" <> "0"; - name = name - } - let norm1_ppm = fun c -> if c.neutral = c.min then @@ -72,38 +44,38 @@ let norm1_ppm = fun c -> else sprintf "tmp_radio * (tmp_radio >=0 ? (MAX_PPRZ/(float)(RC_PPM_SIGNED_TICKS_OF_USEC(%d-%d))) : (MIN_PPRZ/(float)(RC_PPM_SIGNED_TICKS_OF_USEC(%d-%d))))" c.max c.neutral c.min c.neutral, "MIN_PPRZ" -let gen_normalize_ppm_fir = fun channels -> - printf "#define NormalizePpmFIR(_ppm, _rc) {\\\n"; - printf " static uint8_t avg_cpt = 0; /* Counter for averaging */\\\n"; - printf " int16_t tmp_radio;\\\n"; +let gen_normalize_ppm_fir = fun out channels -> + fprintf out "#define NormalizePpmFIR(_ppm, _rc) {\\\n"; + fprintf out " static uint8_t avg_cpt = 0; /* Counter for averaging */\\\n"; + fprintf out " int16_t tmp_radio;\\\n"; List.iter (fun c -> let value, min_pprz = norm1_ppm c in - if c.averaged then begin - printf " _rc.avg_values[RADIO_%s] += _ppm[RADIO_%s];\\\n" c.name c.name + if c.average then begin + fprintf out " _rc.avg_values[RADIO_%s] += _ppm[RADIO_%s];\\\n" c.cname c.cname end else begin - printf " tmp_radio = _ppm[RADIO_%s] - RC_PPM_TICKS_OF_USEC(%d);\\\n" c.name c.neutral; - printf " _rc.values[RADIO_%s] = %s;\\\n" c.name value; - printf " Bound(_rc.values[RADIO_%s], %s, MAX_PPRZ); \\\n\\\n" c.name min_pprz; + fprintf out " tmp_radio = _ppm[RADIO_%s] - RC_PPM_TICKS_OF_USEC(%d);\\\n" c.cname c.neutral; + fprintf out " _rc.values[RADIO_%s] = %s;\\\n" c.cname value; + fprintf out " Bound(_rc.values[RADIO_%s], %s, MAX_PPRZ); \\\n\\\n" c.cname min_pprz; end ) channels; - printf " avg_cpt++;\\\n"; - printf " if (avg_cpt == RC_AVG_PERIOD) {\\\n"; - printf " avg_cpt = 0;\\\n"; + fprintf out " avg_cpt++;\\\n"; + fprintf out " if (avg_cpt == RC_AVG_PERIOD) {\\\n"; + fprintf out " avg_cpt = 0;\\\n"; List.iter (fun c -> - if c.averaged then begin + if c.average then begin let value, min_pprz = norm1_ppm c in - printf " tmp_radio = _rc.avg_values[RADIO_%s] / RC_AVG_PERIOD - RC_PPM_TICKS_OF_USEC(%d);\\\n" c.name c.neutral; - printf " _rc.values[RADIO_%s] = %s;\\\n" c.name value; - printf " _rc.avg_values[RADIO_%s] = 0;\\\n" c.name; - printf " Bound(_rc.values[RADIO_%s], %s, MAX_PPRZ); \\\n\\\n" c.name min_pprz; + fprintf out " tmp_radio = _rc.avg_values[RADIO_%s] / RC_AVG_PERIOD - RC_PPM_TICKS_OF_USEC(%d);\\\n" c.cname c.neutral; + fprintf out " _rc.values[RADIO_%s] = %s;\\\n" c.cname value; + fprintf out " _rc.avg_values[RADIO_%s] = 0;\\\n" c.cname; + fprintf out " Bound(_rc.values[RADIO_%s], %s, MAX_PPRZ); \\\n\\\n" c.cname min_pprz; end ) channels; - printf " }\\\n"; - printf "}\n" + fprintf out " }\\\n"; + fprintf out "}\n\n" let norm1_ppm2 = fun c -> if c.neutral = c.min then @@ -111,81 +83,60 @@ let norm1_ppm2 = fun c -> else sprintf "(tmp_radio >=0 ? (tmp_radio * MAX_PPRZ) / (RC_PPM_SIGNED_TICKS_OF_USEC(%d-%d)) : (tmp_radio * MIN_PPRZ) / (RC_PPM_SIGNED_TICKS_OF_USEC(%d-%d)))" c.max c.neutral c.min c.neutral, "MIN_PPRZ" -let gen_normalize_ppm_iir = fun channels -> - printf "#define NormalizePpmIIR(_ppm, _rc) {\\\n"; - printf " int32_t tmp_radio;\\\n"; - printf " int32_t tmp_value;\\\n\\\n"; +let gen_normalize_ppm_iir = fun out channels -> + fprintf out "#define NormalizePpmIIR(_ppm, _rc) {\\\n"; + fprintf out " int32_t tmp_radio;\\\n"; + fprintf out " int32_t tmp_value;\\\n\\\n"; List.iter (fun c -> let value, min_pprz = norm1_ppm2 c in - printf " tmp_radio = _ppm[RADIO_%s] - RC_PPM_TICKS_OF_USEC(%d);\\\n" c.name c.neutral; - printf " tmp_value = %s;\\\n" value; - printf " Bound(tmp_value, %s, MAX_PPRZ); \\\n" min_pprz; - if c.averaged then - printf " _rc.values[RADIO_%s] = (pprz_t)((RADIO_FILTER * _rc.values[RADIO_%s] + tmp_value) / (RADIO_FILTER + 1));\\\n\\\n" c.name c.name + fprintf out " tmp_radio = _ppm[RADIO_%s] - RC_PPM_TICKS_OF_USEC(%d);\\\n" c.cname c.neutral; + fprintf out " tmp_value = %s;\\\n" value; + fprintf out " Bound(tmp_value, %s, MAX_PPRZ); \\\n" min_pprz; + if c.average then + fprintf out " _rc.values[RADIO_%s] = (pprz_t)((RADIO_FILTER * _rc.values[RADIO_%s] + tmp_value) / (RADIO_FILTER + 1));\\\n\\\n" c.cname c.cname else - printf " _rc.values[RADIO_%s] = (pprz_t)(tmp_value);\\\n\\\n" c.name + fprintf out " _rc.values[RADIO_%s] = (pprz_t)(tmp_value);\\\n\\\n" c.cname ) channels; - (*printf " rc_values_contains_avg_channels = TRUE;\\\n";*) - printf "}\n" + fprintf out "}\n\n" +let generate = fun radio xml_file out_file -> + let out = open_out out_file in + fprintf out "/* This file has been generated by gen_radio from %s */\n" xml_file; + fprintf out "/* Version %s */\n" (Env.get_paparazzi_version ()); + fprintf out "/* Please DO NOT EDIT */\n\n"; + fprintf out "#ifndef %s\n" h_name; + fprintf out "#define %s\n\n" h_name; + fprintf out "#define RADIO_NAME %s\n\n" radio.name; + fprintf out "#define RADIO_CTL_NB %s\n\n" (string_of_int (List.length radio.channels)); + fprintf out "#define RADIO_FILTER 7\n\n"; -let _ = - if Array.length Sys.argv < 2 then - failwith "Usage: gen_radio xml_file"; - let xml_file = Sys.argv.(1) in - let xml = ExtXml.parse_file xml_file in + List.iteri (fun i c -> + check_function_name c.cname; + fprintf out "#define RADIO_%s %d\n" c.cname i; + fprintf out "#define RADIO_%s_NEUTRAL %d\n" c.cname c.neutral; + let (mini, maxi) = if c.reverse then (c.max, c.min) else (c.min, c.max) in + fprintf out "#define RADIO_%s_MIN %d\n" c.cname mini; + fprintf out "#define RADIO_%s_MAX %d\n\n" c.cname maxi; + ) radio.channels; - printf "/* This file has been generated by gen_radio from %s */\n" xml_file; - printf "/* Version %s */\n" (Env.get_paparazzi_version ()); - printf "/* Please DO NOT EDIT */\n\n"; - printf "#ifndef %s\n" h_name; - define h_name ""; - nl (); - let channels = Xml.children xml in - let n = ExtXml.attrib xml "name" in - (* Xml2h.warning ("RADIO MODEL: "^n); *) - define_string "RADIO_NAME" n; - nl (); - (*define "RADIO_CONTROL_NB_CHANNEL" (string_of_int (List.length channels));*) - define "RADIO_CTL_NB" (string_of_int (List.length channels)); - nl (); - define "RADIO_FILTER" "7"; - nl (); + let ppm_pulse_type = match radio.pulse_type with + | PositivePulse -> "POSITIVE" + | NegativePulse -> "NEGATIVE" + in + fprintf out "#define PPM_PULSE_TYPE PPM_PULSE_TYPE_%s\n" ppm_pulse_type; + fprintf out "#define PPM_DATA_MIN_LEN (%dul)\n" radio.data_min; + fprintf out "#define PPM_DATA_MAX_LEN (%dul)\n" radio.data_max; + fprintf out "#define PPM_SYNC_MIN_LEN (%dul)\n" radio.sync_min; + fprintf out "#define PPM_SYNC_MAX_LEN (%dul)\n\n" radio.sync_max; - let channels_params = List.map parse_channel channels in - nl (); + gen_normalize_ppm_fir out radio.channels; + gen_normalize_ppm_iir out radio.channels; - List.iter - (fun c -> - begin - printf "#define RADIO_%s_NEUTRAL %d\n" c.name c.neutral; - printf "#define RADIO_%s_MIN %d\n" c.name c.min; - printf "#define RADIO_%s_MAX %d\n" c.name c.max; - end - ) - channels_params; - nl(); + fprintf out "\n#endif // %s\n" h_name; - let ppm_pulse_type = ExtXml.attrib xml "pulse_type" in - let ppm_data_min = ExtXml.attrib xml "data_min" in - let ppm_data_max = ExtXml.attrib xml "data_max" in - let ppm_sync_min = ExtXml.attrib xml "sync_min" in - let ppm_sync_max = ExtXml.attrib xml "sync_max" in + close_out out - printf "#define PPM_PULSE_TYPE PPM_PULSE_TYPE_%s\n" ppm_pulse_type; - printf "#define PPM_DATA_MIN_LEN (%sul)\n" ppm_data_min; - printf "#define PPM_DATA_MAX_LEN (%sul)\n" ppm_data_max; - printf "#define PPM_SYNC_MIN_LEN (%sul)\n" ppm_sync_min; - printf "#define PPM_SYNC_MAX_LEN (%sul)\n" ppm_sync_max; - nl (); - - gen_normalize_ppm_fir channels_params; - nl (); - gen_normalize_ppm_iir channels_params; - nl (); - - printf "\n#endif // %s\n" h_name diff --git a/sw/tools/generators/gen_settings.ml b/sw/tools/generators/gen_settings.ml index 4cd56214c2..a3581fe360 100644 --- a/sw/tools/generators/gen_settings.ml +++ b/sw/tools/generators/gen_settings.ml @@ -27,13 +27,14 @@ open Printf open Xml2h + let margin = ref 0 let step = 2 -let tab () = printf "%s" (String.make !margin ' ') +let tab = fun out -> fprintf out "%s" (String.make !margin ' ') let right () = margin := !margin + step let left () = margin := !margin - step -let lprintf = fun f -> tab (); printf f +let lprintf = fun out f -> tab out; fprintf out f let rec flatten = fun xml r -> if ExtXml.tag_is xml "dl_setting" then @@ -46,48 +47,40 @@ let rec flatten = fun xml r -> List.fold_right flatten (x::xs) r -module StringSet = Set.Make(struct type t = string let compare = compare end) - - -let print_dl_settings = fun settings -> - let settings = flatten settings [] in +let print_dl_settings = fun out settings settings_xml -> + let settings_xml = flatten settings_xml [] in (** include headers **) - let modules = ref StringSet.empty in - List.iter - (fun s -> - try - modules := StringSet.add (ExtXml.attrib s "module") !modules - with ExtXml.Error e -> () - ) - settings; - - lprintf "\n"; - StringSet.iter (fun m -> lprintf "#include \"%s.h\"\n" m) !modules; - lprintf "#include \"generated/modules.h\"\n"; - lprintf "\n"; + lprintf out "\n"; + List.iter (fun s -> + List.iter (fun h -> + lprintf out "#include \"%s.h\"\n" h + ) (Settings.get_headers s) + ) settings; + lprintf out "#include \"generated/modules.h\"\n"; + lprintf out "\n"; (** Datalink knowing what settings mean **) - Xml2h.define "SETTINGS_NAMES" "{ \\"; - List.iter (fun b -> printf " { \"%s\" }, \\\n" (ExtXml.attrib b "var")) settings; - lprintf "};\n"; + Xml2h.define_out out "SETTINGS_NAMES" "{ \\"; + List.iter (fun b -> fprintf out " { \"%s\" }, \\\n" (ExtXml.attrib b "var")) settings_xml; + lprintf out "};\n"; - Xml2h.define "SETTINGS_NAMES_SHORT" "{ \\"; + Xml2h.define_out out "SETTINGS_NAMES_SHORT" "{ \\"; List.iter (fun b -> let varname = Str.split (Str.regexp "[_.]+") (ExtXml.attrib b "var") in let shortname = List.fold_left (fun acc c -> try acc ^"_"^ (Str.first_chars c 3) with _ -> acc ^"_"^ c ) "" varname in let shorted = try String.sub shortname 1 16 with _ -> String.sub shortname 1 ((String.length shortname)-1) in - printf " \"%s\" , \\\n" shorted - ) settings; - lprintf "};\n"; - Xml2h.define "NB_SETTING" (string_of_int (List.length settings)); + fprintf out " \"%s\" , \\\n" shorted + ) settings_xml; + lprintf out "};\n"; + Xml2h.define_out out "NB_SETTING" (string_of_int (List.length settings_xml)); (** Macro to call to set one variable *) - lprintf "#define DlSetting(_idx, _value) { \\\n"; + lprintf out "#define DlSetting(_idx, _value) { \\\n"; right (); - lprintf "switch (_idx) { \\\n"; + lprintf out "switch (_idx) { \\\n"; right (); let idx = ref 0 in List.iter @@ -97,65 +90,67 @@ let print_dl_settings = fun settings -> try let h = ExtXml.attrib s "handler" in begin - try - let m = ExtXml.attrib s "module" in - lprintf "case %d: %s_%s( _value ); _value = %s; break;\\\n" !idx (Filename.basename m) h v - with - ExtXml.Error e -> prerr_endline (sprintf "Error: You need to specify the module attribute for setting %s to use the handler %s" v h); exit 1 + let m1 = ExtXml.attrib_or_default s "module" "" in + let m2 = ExtXml.attrib_or_default s "header" "" in + match m1, m2 with + | "", "" -> prerr_endline (sprintf "Error: You need to specify the header (or module for legacy) attribute for setting %s to use the handler %s" v h); exit 1 + | "", _ -> lprintf out "case %d: %s_%s( _value ); _value = %s; break;\\\n" !idx (Filename.basename m2) h v + | _, "" -> lprintf out "case %d: %s_%s( _value ); _value = %s; break;\\\n" !idx (Filename.basename m1) h v + | _, _ -> prerr_endline (sprintf "Error: You can't specify both module and header attributes for setting %s to use the handler %s" v h); exit 1 end; with - ExtXml.Error e -> lprintf "case %d: %s = _value; break;\\\n" !idx v + ExtXml.Error e -> lprintf out "case %d: %s = _value; break;\\\n" !idx v end; incr idx ) - settings; - lprintf "default: break;\\\n"; + settings_xml; + lprintf out "default: break;\\\n"; left (); - lprintf "}\\\n"; + lprintf out "}\\\n"; left (); - lprintf "}\n"; + lprintf out "}\n"; let nb_values = !idx in (** Macro to call to downlink current values *) - lprintf "#define PeriodicSendDlValue(_trans, _dev) { \\\n"; + lprintf out "#define PeriodicSendDlValue(_trans, _dev) { \\\n"; if nb_values > 0 then begin right (); - lprintf "static uint8_t i;\\\n"; - lprintf "float var;\\\n"; - lprintf "if (i >= %d) i = 0;\\\n" nb_values; + lprintf out "static uint8_t i;\\\n"; + lprintf out "float var;\\\n"; + lprintf out "if (i >= %d) i = 0;\\\n" nb_values; let idx = ref 0 in - lprintf "switch (i) { \\\n"; + lprintf out "switch (i) { \\\n"; right (); List.iter (fun s -> let v = ExtXml.attrib s "var" in - lprintf "case %d: var = %s; break;\\\n" !idx v; incr idx) - settings; - lprintf "default: var = 0.; break;\\\n"; + lprintf out "case %d: var = %s; break;\\\n" !idx v; incr idx) + settings_xml; + lprintf out "default: var = 0.; break;\\\n"; left (); - lprintf "}\\\n"; - lprintf "pprz_msg_send_DL_VALUE(_trans, _dev, AC_ID, &i, &var);\\\n"; - lprintf "i++;\\\n"; + lprintf out "}\\\n"; + lprintf out "pprz_msg_send_DL_VALUE(_trans, _dev, AC_ID, &i, &var);\\\n"; + lprintf out "i++;\\\n"; left () end; - lprintf "}\n"; + lprintf out "}\n"; (** Inline function to get a setting value *) - lprintf "static inline float settings_get_value(uint8_t i) {\n"; + lprintf out "static inline float settings_get_value(uint8_t i) {\n"; right (); let idx = ref 0 in - lprintf "switch (i) {\n"; + lprintf out "switch (i) {\n"; right (); List.iter (fun s -> let v = ExtXml.attrib s "var" in - lprintf "case %d: return %s;\n" !idx v; incr idx) - settings; - lprintf "default: return 0.;\n"; + lprintf out "case %d: return %s;\n" !idx v; incr idx) + settings_xml; + lprintf out "default: return 0.;\n"; left (); - lprintf "}\n"; + lprintf out "}\n"; left (); - lprintf "}\n" + lprintf out "}\n" let inttype = function @@ -176,39 +171,39 @@ let inttype = function (* Generate code for persistent settings *) -let print_persistent_settings = fun settings -> - let settings = flatten settings [] in +let print_persistent_settings = fun out settings settings_xml -> + let settings_xml = flatten settings_xml [] in let pers_settings = - List.filter (fun x -> try let _ = Xml.attrib x "persistent" in true with _ -> false) settings in + List.filter (fun x -> try let _ = Xml.attrib x "persistent" in true with _ -> false) settings_xml in (* structure declaration *) (* if List.length pers_settings > 0 then begin *) - lprintf "\n/* Persistent Settings */\n"; - lprintf "struct PersistentSettings {\n"; + lprintf out "\n/* Persistent Settings */\n"; + lprintf out "struct PersistentSettings {\n"; right(); let idx = ref 0 in List.iter (fun s -> let v = ExtXml.attrib s "var" in let t = try ExtXml.attrib s "type" with _ -> "float" in - lprintf "%s s_%d; /* %s */\n" (inttype t) !idx v; incr idx) + lprintf out "%s s_%d; /* %s */\n" (inttype t) !idx v; incr idx) pers_settings; left(); - lprintf "};\n\n"; - lprintf "extern struct PersistentSettings pers_settings;\n\n"; + lprintf out "};\n\n"; + lprintf out "extern struct PersistentSettings pers_settings;\n\n"; (* Inline function to store persistent settings *) idx := 0; - lprintf "static inline void persistent_settings_store( void ) {\n"; + lprintf out "static inline void persistent_settings_store( void ) {\n"; right(); List.iter (fun s -> let v = ExtXml.attrib s "var" in - lprintf "pers_settings.s_%d = %s;\n" !idx v; incr idx) + lprintf out "pers_settings.s_%d = %s;\n" !idx v; incr idx) pers_settings; left(); - lprintf "}\n\n"; + lprintf out "}\n\n"; (* Inline function to load persistent settings *) idx := 0; - lprintf "static inline void persistent_settings_load( void ) {\n"; + lprintf out "static inline void persistent_settings_load( void ) {\n"; right(); List.iter (fun s -> @@ -217,194 +212,31 @@ let print_persistent_settings = fun settings -> try let h = ExtXml.attrib s "handler" and m = ExtXml.attrib s "module" in - lprintf "%s_%s( pers_settings.s_%d );\n" (Filename.basename m) h !idx ; - (* lprintf "%s = pers_settings.s_%d;\n" v !idx *) (* do we want to set the value too or just call the handler ? *) + lprintf out "%s_%s( pers_settings.s_%d );\n" (Filename.basename m) h !idx ; + (* lprintf out "%s = pers_settings.s_%d;\n" v !idx *) (* do we want to set the value too or just call the handler ? *) with - ExtXml.Error e -> lprintf "%s = pers_settings.s_%d;\n" v !idx + ExtXml.Error e -> lprintf out "%s = pers_settings.s_%d;\n" v !idx end; incr idx) pers_settings; left(); - lprintf "}\n" + lprintf out "}\n" (* end *) +let h_name = "SETTINGS_H" -(* - Blaaaaaa2 -*) -let calib_mode_of_rc = function -"gain_1_up" -> 1, "up" - | "gain_1_down" -> 1, "down" - | "gain_2_up" -> 2, "up" - | "gain_2_down" -> 2, "down" - | x -> failwith (sprintf "Unknown rc: %s" x) +let generate = fun settings xml_files out_xml out_file -> + let out = open_out out_file in -let param_macro_of_type = fun x -> "ParamVal"^Compat.capitalize_ascii x + (* generate XML concatenated file *) + let xml = Settings.get_settings_xml settings in + let f = open_out out_xml in + fprintf f "%s\n" (ExtXml.to_string_fmt xml); + close_out f; -let parse_rc_setting = fun xml -> - let cursor, cm = calib_mode_of_rc (ExtXml.attrib xml "rc") - and var = ExtXml.attrib xml "var" - and range = float_of_string (ExtXml.attrib xml "range") in - let t = (ExtXml.attrib xml "type") in - let param_macro = param_macro_of_type t in - let dot_pos = - try String.rindex var '.' + 1 with - Not_found -> 0 in - let var_nostruct = String.sub var dot_pos (String.length var - dot_pos) in - let var_init = var_nostruct ^ "_init" in + (* generate C file *) + begin_out out (String.concat " " xml_files) h_name; + print_dl_settings out settings xml; + print_persistent_settings out settings xml; + finish_out out h_name - lprintf "if (rc_settings_mode == RC_SETTINGS_MODE_%s) { \\\n" (Compat.uppercase_ascii cm); - right (); - lprintf "static %s %s; \\\n" (inttype t) var_init; - lprintf "static int16_t slider%d_init; \\\n" cursor; - lprintf "if (mode_changed) { \\\n"; - right (); - lprintf "%s = %s; \\\n" var_init var; - lprintf "slider%d_init = RcChannel(RADIO_GAIN%d); \\\n" cursor cursor; - left (); lprintf "} \\\n"; - lprintf "%s = %s(%s, %f, RcChannel(RADIO_GAIN%d), slider%d_init); \\\n" var param_macro var_init range cursor cursor; - lprintf "slider_%d_val = (float)%s; \\\n" cursor var; - left (); lprintf "} \\\n" - - -let parse_rc_mode = fun xml -> - 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" - -let parse_rc_modes = fun xml -> - List.iter parse_rc_mode (Xml.children xml) - -(* - Check if target t is marked as supported in the targets string. - The targets string is a pipe delimited list of supported targets, e.g. "ap|nps" - To specifiy a list with unsupported targets, prefix with ! - e.g. "!sim|nps" to mark support for all targets except sim and nps. -*) -let supports_target = fun t targets -> - if String.length targets > 0 && targets.[0] = '!' then - not (Str.string_match (Str.regexp (".*"^t^".*")) targets 0) - else - Str.string_match (Str.regexp (".*"^t^".*")) targets 0 - -let join_xml_files = fun xml_sys_files xml_user_files -> - let dl_settings = ref [] - and rc_settings = ref [] in - let target = try Sys.getenv "TARGET" with _ -> "" in - List.iter (fun xml_file -> - (* look for a specific name after settings file (in case of modules) *) - let split = Str.split (Str.regexp "~") xml_file in - let xml_file, name = match split with - | [f; n] -> f, n - | _ -> xml_file, "" - in - let xml = ExtXml.parse_file xml_file in - let these_rc_settings = - try Xml.children (ExtXml.child xml "rc_settings") with - Not_found -> [] in - let these_dl_settings = - try - (* test if the file is plain settings file or a module file *) - let xml = - if Xml.tag xml = "module" - then begin - (* test if the module is loaded or not *) - if List.exists (fun n -> - if Xml.tag n = "makefile" then begin - let t = ExtXml.attrib_or_default n "target" Env.default_module_targets in - supports_target target t - end - else false - ) (Xml.children xml) - then - List.filter (fun t -> - (* filter xml nodes and keep them if: - * it is a settings node - * the current target is supported in the 'target' attribute - * if no 'target' attribute always keep it - *) - Xml.tag t = "settings" && supports_target target (ExtXml.attrib_or_default t "target" target) - ) (Xml.children xml) - else [] - end - else begin - (* if the top node has a target attribute, - only add if matches current target *) - let t = ExtXml.attrib_or_default xml "target" "" in - if t = "" || (supports_target target t) then - [xml] - else - [] - end - in - (* include settings if name is matching *) - List.fold_left (fun l x -> - if (ExtXml.attrib_or_default x "name" "") = name then - l @ (Xml.children (ExtXml.child x "dl_settings")) - else l - ) [] xml - with - | Not_found -> [] in - rc_settings := these_rc_settings @ !rc_settings; - dl_settings := these_dl_settings @ !dl_settings) - xml_user_files; - - (* add system settings grouped under the same tab *) - let dl_sys = List.map (fun xml_file -> - let xml = ExtXml.parse_file xml_file in - (* take "second stage" dl_settings nodes *) - try - let dl = ExtXml.child xml "dl_settings" in - Xml.children dl - with Not_found -> [] - ) xml_sys_files in - dl_settings := Xml.Element("dl_settings", [("name", "System")], List.rev (List.flatten dl_sys)) :: !dl_settings; - - (* return final node *) - Xml.Element("rc_settings",[],!rc_settings), Xml.Element("dl_settings",[],!dl_settings) - - - -let _ = - if Array.length Sys.argv < 4 then - failwith (Printf.sprintf "Usage: %s output_xml_file input_xml_file(s) input_xml_modules" Sys.argv.(0)); - let h_name = "SETTINGS_H" - and xml_files = Array.to_list (Array.sub Sys.argv 2 (Array.length Sys.argv - 2)) in - (* split system settings and user settings based on '*' separator *) - let xml_sys_files, xml_user_files, _ = List.fold_left (fun (sys, user, delim) x -> - if x = "--" then (sys, user, true) - else if delim then (sys, x :: user, delim) - else (x :: sys, user, delim)) ([], [], false) xml_files - in - - try - printf "/* This file has been generated by gen_settings from %s */\n" (String.concat " " xml_files); - printf "/* Version %s */\n" (Env.get_paparazzi_version ()); - printf "/* Please DO NOT EDIT */\n\n"; - - printf "#ifndef %s\n" h_name; - define h_name ""; - nl (); - - let rc_settings, dl_settings = join_xml_files xml_sys_files xml_user_files in - - let xml = Xml.Element ("settings", [], [rc_settings; dl_settings]) in - let f = open_out Sys.argv.(1) in - fprintf f "%s\n" (ExtXml.to_string_fmt xml); - close_out f; - - lprintf "#define RCSettings(mode_changed) { \\\n"; - right (); - parse_rc_modes rc_settings; - left (); lprintf "}\n"; - - print_dl_settings dl_settings; - - print_persistent_settings dl_settings; - - finish h_name - with - Xml.Error e -> prerr_endline (Xml.error e); exit 1 - | Dtd.Prove_error e -> prerr_endline (Dtd.prove_error e); exit 1 - | Dtd.Parse_error e -> prerr_endline (Dtd.parse_error e); exit 1