diff --git a/.gitignore b/.gitignore index 4cc7b9421a..de78b4a36c 100644 --- a/.gitignore +++ b/.gitignore @@ -94,6 +94,7 @@ /sw/ground_segment/tmtc/diadec /sw/ground_segment/tmtc/ivy_serial_bridge /sw/ground_segment/tmtc/GSM/SMS_GS +/sw/ground_segment/tmtc/app_server /sw/ground_segment/tmtc/gpsd2ivy # /sw/ground_segment/joystick @@ -140,13 +141,13 @@ /sw/supervision/gtk_pc.ml # /sw/tools/ -/sw/tools/fp_parser.ml /sw/tools/wiki_gen/wiki_gen /sw/tools/mergelogs # /sw/ground_segment/misc /sw/ground_segment/misc/davis2ivy /sw/ground_segment/misc/kestrel2ivy +/sw/ground_segment/misc/natnet2ivy # /sw/airborne/arch/lpc21/test/bootloader diff --git a/Makefile b/Makefile index 294068a846..6a00332709 100644 --- a/Makefile +++ b/Makefile @@ -64,7 +64,7 @@ SIMULATOR=sw/simulator MULTIMON=sw/ground_segment/multimon COCKPIT=sw/ground_segment/cockpit TMTC=sw/ground_segment/tmtc -TOOLS=$(PAPARAZZI_SRC)/sw/tools +GENERATORS=$(PAPARAZZI_SRC)/sw/tools/generators JOYSTICK=sw/ground_segment/joystick EXT=sw/ext @@ -121,7 +121,7 @@ conf/%.xml :conf/%_example.xml ground_segment: print_build_version update_google_version conf libpprz subdirs commands static ground_segment.opt: ground_segment cockpit.opt tmtc.opt -static: cockpit tmtc tools sim_static joystick static_h +static: cockpit tmtc generators sim_static joystick static_h libpprz: $(MAKE) -C $(LIB)/ocaml @@ -141,8 +141,8 @@ tmtc: libpprz cockpit multimon tmtc.opt: libpprz cockpit.opt multimon $(MAKE) -C $(TMTC) opt -tools: libpprz - $(MAKE) -C $(TOOLS) +generators: libpprz + $(MAKE) -C $(GENERATORS) joystick: libpprz $(MAKE) -C $(JOYSTICK) @@ -168,68 +168,68 @@ $(LOGALIZER): libpprz static_h: $(GEN_HEADERS) -$(MESSAGES_H) : $(MESSAGES_XML) tools +$(MESSAGES_H) : $(MESSAGES_XML) generators $(Q)test -d $(STATICINCLUDE) || mkdir -p $(STATICINCLUDE) @echo GENERATE $@ $(eval $@_TMP := $(shell $(MKTEMP))) - $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) $(TOOLS)/gen_messages.out $< telemetry > $($@_TMP) + $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) $(GENERATORS)/gen_messages.out $< telemetry > $($@_TMP) $(Q)mv $($@_TMP) $@ $(Q)chmod a+r $@ -$(MESSAGES2_H) : $(MESSAGES_XML) tools +$(MESSAGES2_H) : $(MESSAGES_XML) generators $(Q)test -d $(STATICINCLUDE) || mkdir -p $(STATICINCLUDE) @echo GENERATE $@ $(eval $@_TMP := $(shell $(MKTEMP))) - $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) $(TOOLS)/gen_messages2.out $< telemetry > $($@_TMP) + $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) $(GENERATORS)/gen_messages2.out $< telemetry > $($@_TMP) $(Q)mv $($@_TMP) $@ $(Q)chmod a+r $@ -$(UBX_PROTOCOL_H) : $(UBX_XML) tools +$(UBX_PROTOCOL_H) : $(UBX_XML) generators @echo GENERATE $@ $(eval $@_TMP := $(shell $(MKTEMP))) - $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) $(TOOLS)/gen_ubx.out $< > $($@_TMP) + $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) $(GENERATORS)/gen_ubx.out $< > $($@_TMP) $(Q)mv $($@_TMP) $@ $(Q)chmod a+r $@ -$(MTK_PROTOCOL_H) : $(MTK_XML) tools +$(MTK_PROTOCOL_H) : $(MTK_XML) generators @echo GENERATE $@ $(eval $@_TMP := $(shell $(MKTEMP))) - $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) $(TOOLS)/gen_mtk.out $< > $($@_TMP) + $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) $(GENERATORS)/gen_mtk.out $< > $($@_TMP) $(Q)mv $($@_TMP) $@ $(Q)chmod a+r $@ -$(XSENS_PROTOCOL_H) : $(XSENS_XML) tools +$(XSENS_PROTOCOL_H) : $(XSENS_XML) generators @echo GENERATE $@ $(eval $@_TMP := $(shell $(MKTEMP))) - $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) $(TOOLS)/gen_xsens.out $< > $($@_TMP) + $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) $(GENERATORS)/gen_xsens.out $< > $($@_TMP) $(Q)mv $($@_TMP) $@ $(Q)chmod a+r $@ -$(DL_PROTOCOL_H) : $(MESSAGES_XML) tools +$(DL_PROTOCOL_H) : $(MESSAGES_XML) generators @echo GENERATE $@ $(eval $@_TMP := $(shell $(MKTEMP))) - $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) $(TOOLS)/gen_messages.out $< datalink > $($@_TMP) + $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) $(GENERATORS)/gen_messages.out $< datalink > $($@_TMP) $(Q)mv $($@_TMP) $@ $(Q)chmod a+r $@ -$(DL_PROTOCOL2_H) : $(MESSAGES_XML) tools +$(DL_PROTOCOL2_H) : $(MESSAGES_XML) generators @echo GENERATE $@ $(eval $@_TMP := $(shell $(MKTEMP))) - $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) $(TOOLS)/gen_messages2.out $< datalink > $($@_TMP) + $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) $(GENERATORS)/gen_messages2.out $< datalink > $($@_TMP) $(Q)mv $($@_TMP) $@ $(Q)chmod a+r $@ -$(ABI_MESSAGES_H) : $(ABI_XML) tools +$(ABI_MESSAGES_H) : $(ABI_XML) generators @echo GENERATE $@ $(eval $@_TMP := $(shell $(MKTEMP))) - $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) $(TOOLS)/gen_abi.out $< airborne > $($@_TMP) + $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) $(GENERATORS)/gen_abi.out $< airborne > $($@_TMP) $(Q)mv $($@_TMP) $@ $(Q)chmod a+r $@ include Makefile.ac -ac_h ac fbw ap: static conf tools ext +ac_h ac fbw ap: static conf generators ext sim: sim_static @@ -307,7 +307,7 @@ test: all replace_current_conf_xml run_tests restore_conf_xml .PHONY: all print_build_version update_google_version dox ground_segment ground_segment.opt \ -subdirs $(SUBDIRS) conf ext libpprz multimon cockpit cockpit.opt tmtc tmtc.opt tools\ +subdirs $(SUBDIRS) conf ext libpprz multimon cockpit cockpit.opt tmtc tmtc.opt generators\ static sim_static lpctools commands \ clean cleanspaces ab_clean dist_clean distclean dist_clean_irreversible \ test replace_current_conf_xml run_tests restore_conf_xml diff --git a/Makefile.ac b/Makefile.ac index 9939768bcd..b2f40075c6 100644 --- a/Makefile.ac +++ b/Makefile.ac @@ -31,7 +31,7 @@ AIRBORNE=sw/airborne MESSAGES_XML = $(CONF)/messages.xml ACINCLUDE = $(PAPARAZZI_HOME)/var/$(AIRCRAFT) AIRCRAFT_CONF_DIR = $(ACINCLUDE)/conf -AC_GENERATED = $(ACINCLUDE)/generated +AC_GENERATED = $(ACINCLUDE)/$(TARGET)/generated AIRFRAME_H=$(AC_GENERATED)/airframe.h PERIODIC_H=$(AC_GENERATED)/periodic_telemetry.h @@ -104,16 +104,16 @@ $(AIRFRAME_H) : $(CONF)/$(AIRFRAME_XML) $(CONF_XML) $(AIRCRAFT_MD5) $(Q)test -d $(AC_GENERATED) || mkdir -p $(AC_GENERATED) @echo GENERATE $@ $(eval $@_TMP := $(shell $(MKTEMP))) - $(Q)$(TOOLS)/gen_airframe.out $(AC_ID) $(AIRCRAFT) $(MD5SUM) $< > $($@_TMP) + $(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 -$(RADIO_H) : $(CONF)/$(RADIO) $(CONF_XML) $(TOOLS)/gen_radio.out +$(RADIO_H) : $(CONF)/$(RADIO) $(CONF_XML) $(GENERATORS)/gen_radio.out $(Q)test -d $(AC_GENERATED) || mkdir -p $(AC_GENERATED) @echo GENERATE $@ $(eval $@_TMP := $(shell $(MKTEMP))) - $(Q)$(TOOLS)/gen_radio.out $< > $($@_TMP) + $(Q)$(GENERATORS)/gen_radio.out $< > $($@_TMP) $(Q)mv $($@_TMP) $@ $(Q)chmod a+r $@ $(Q)cp $< $(AIRCRAFT_CONF_DIR)/radios @@ -122,59 +122,59 @@ $(PERIODIC_H) : $(CONF)/$(AIRFRAME_XML) $(MESSAGES_XML) $(CONF_XML) $(CONF)/$(TE $(Q)test -d $(AC_GENERATED) || mkdir -p $(AC_GENERATED) @echo GENERATE $@ $(eval $@_TMP := $(shell $(MKTEMP))) - $(Q)$(TOOLS)/gen_periodic.out $(CONF)/$(AIRFRAME_XML) $(MESSAGES_XML) $(CONF)/$(TELEMETRY) $(TELEMETRY_FREQUENCY) $(SETTINGS_TELEMETRY) > $($@_TMP) + $(Q)$(GENERATORS)/gen_periodic.out $(CONF)/$(AIRFRAME_XML) $(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) $(TOOLS)/gen_flight_plan.out +$(FLIGHT_PLAN_H) : $(CONF)/$(FLIGHT_PLAN) $(CONF_XML) $(GENERATORS)/gen_flight_plan.out $(Q)test -d $(AC_GENERATED) || mkdir -p $(AC_GENERATED) @echo GENERATE $@ $(eval $@_TMP := $(shell $(MKTEMP))) - $(Q)$(TOOLS)/gen_flight_plan.out $< > $($@_TMP) + $(Q)$(GENERATORS)/gen_flight_plan.out $< > $($@_TMP) $(Q)mv $($@_TMP) $@ $(Q)chmod a+r $@ $(Q)cp $< $(AIRCRAFT_CONF_DIR)/flight_plans -$(FLIGHT_PLAN_XML) : $(CONF)/$(FLIGHT_PLAN) $(CONF_XML) $(TOOLS)/gen_flight_plan.out +$(FLIGHT_PLAN_XML) : $(CONF)/$(FLIGHT_PLAN) $(CONF_XML) $(GENERATORS)/gen_flight_plan.out @echo GENERATE $@ $(eval $@_TMP := $(shell $(MKTEMP))) - $(Q)$(TOOLS)/gen_flight_plan.out -dump $< > $($@_TMP) + $(Q)$(GENERATORS)/gen_flight_plan.out -dump $< > $($@_TMP) $(Q)mv $($@_TMP) $@ $(Q)chmod a+r $@ -$(SETTINGS_H) : $(SETTINGS_XMLS) $(CONF_XML) $(SETTINGS_MODULES) $(SETTINGS_TELEMETRY) $(TOOLS)/gen_settings.out +$(SETTINGS_H) : $(SETTINGS_XMLS) $(CONF_XML) $(SETTINGS_MODULES) $(SETTINGS_TELEMETRY) $(GENERATORS)/gen_settings.out $(Q)test -d $(AC_GENERATED) || mkdir -p $(AC_GENERATED) @echo GENERATE $@ $(eval $@_TMP := $(shell $(MKTEMP))) - $(Q)$(TOOLS)/gen_settings.out $(SETTINGS_XML) $(SETTINGS_TELEMETRY) $(SETTINGS_XMLS) $(SETTINGS_MODULES) > $($@_TMP) + $(Q)$(GENERATORS)/gen_settings.out $(SETTINGS_XML) $(SETTINGS_TELEMETRY) $(SETTINGS_XMLS) $(SETTINGS_MODULES) > $($@_TMP) $(Q)mv $($@_TMP) $@ $(Q)chmod a+r $@ $(Q)cp $(SETTINGS_XMLS) $(AIRCRAFT_CONF_DIR)/settings -$(MODULES_H) : $(CONF)/$(AIRFRAME_XML) $(TOOLS)/gen_modules.out $(CONF)/modules/*.xml +$(MODULES_H) : $(CONF)/$(AIRFRAME_XML) $(GENERATORS)/gen_modules.out $(CONF)/modules/*.xml $(Q)test -d $(AC_GENERATED) || mkdir -p $(AC_GENERATED) @echo GENERATE $@ $(eval $@_TMP := $(shell $(MKTEMP))) - $(Q)$(TOOLS)/gen_modules.out $(SETTINGS_MODULES) $(DEFAULT_MODULES_FREQUENCY) $< > $($@_TMP) + $(Q)$(GENERATORS)/gen_modules.out $(SETTINGS_MODULES) $(DEFAULT_MODULES_FREQUENCY) $< > $($@_TMP) $(Q)mv $($@_TMP) $@ $(Q)chmod a+r $@ -$(AUTOPILOT_H) : $(CONF)/$(AIRFRAME_XML) $(TOOLS)/gen_autopilot.out $(CONF)/autopilot/*.xml +$(AUTOPILOT_H) : $(CONF)/$(AIRFRAME_XML) $(GENERATORS)/gen_autopilot.out $(CONF)/autopilot/*.xml $(Q)test -d $(AC_GENERATED) || mkdir -p $(AC_GENERATED) @echo GENERATE $@ $(eval $@_TMP := $(shell $(MKTEMP))) - $(Q)$(TOOLS)/gen_autopilot.out $(CONF)/$(AIRFRAME_XML) $($@_TMP) + $(Q)$(GENERATORS)/gen_autopilot.out $(CONF)/$(AIRFRAME_XML) $($@_TMP) $(Q)mv $($@_TMP) $@ $(Q)chmod a+r $@ $(SETTINGS_MODULES) : $(MODULES_H) $(SETTINGS_TELEMETRY) : $(PERIODIC_H) -%.ac_h : $(TOOLS)/gen_aircraft.out +%.ac_h : $(GENERATORS)/gen_aircraft.out $(Q)if (expr "$(AIRCRAFT)"); then : ; else echo "AIRCRAFT undefined: type 'make AIRCRAFT=AircraftName ...'"; exit 1; fi @echo BUILD $(AIRCRAFT), TARGET $* - +$(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) TARGET=$* Q=$(Q) $(TOOLS)/gen_aircraft.out $(AIRCRAFT) + $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) TARGET=$* Q=$(Q) $(GENERATORS)/gen_aircraft.out $(AIRCRAFT) %.compile: %.ac_h print_version cd $(AIRBORNE); $(MAKE) TARGET=$* all diff --git a/README.md b/README.md index 4cd013f77b..24aaa7027c 100644 --- a/README.md +++ b/README.md @@ -4,7 +4,7 @@ Paparazzi UAS Paparazzi is an attempt to develop a free software Unmanned (Air) Vehicle System. As of today the system is being used successfuly by a number of hobyists, universities and companies all over the world, on vehicle of various size ( 100g to 25Kg ) and of various nature ( fixed wing, rotorcrafts, boats and surface vehicles). -Up to date information is available in the wiki http://paparazzi.enac.fr +Up to date information is available in the wiki http://wiki.paparazziuav.org and from the mailing list [paparazzi-devel@nongnu.org] (http://savannah.nongnu.org/mail/?group=paparazzi) and the IRC channel (freenode, #paparazzi). @@ -13,10 +13,10 @@ and the IRC channel (freenode, #paparazzi). Required Software ----------------- -Installation is described in the wiki (http://paparazzi.enac.fr/wiki/Installation). +Installation is described in the wiki (http://wiki.paparazziuav.org/wiki/Installation). For Ubuntu users, required packages are available in the [paparazzi-uav PPA] (https://launchpad.net/~paparazzi-uav/+archive/ppa), -Debian users can use http://paparazzi.enac.fr/debian +Debian users can use the [OpenSUSE Build Service repository] (http://download.opensuse.org/repositories/home:/flixr:/paparazzi-uav/Debian_7.0/) Debian/Ubuntu packages: - **paparazzi-dev** is the meta-package that depends on everything needed to compile and run the ground segment and the simulator. diff --git a/conf/Makefile.chibios-libopencm3 b/conf/Makefile.chibios-libopencm3 index 91280de61b..06a1eff254 100644 --- a/conf/Makefile.chibios-libopencm3 +++ b/conf/Makefile.chibios-libopencm3 @@ -41,7 +41,7 @@ CHIBIOS_BOARD_DIR = $(PAPARAZZI_SRC)/sw/airborne/boards/$(BOARD)/chibios-libopen CHIBIOS_LIB_DIR = $(PAPARAZZI_SRC)/sw/airborne/subsystems/chibios-libopencm3 CHIBIOS_EXT = $(PAPARAZZI_SRC)/sw/ext/chibios OPENCM3_EXT = $(PAPARAZZI_SRC)/sw/ext/libopencm3 -PPRZ_GENERATED = $(PAPARAZZI_SRC)/var/$(AIRCRAFT)/generated +PPRZ_GENERATED = $(PAPARAZZI_SRC)/var/$(AIRCRAFT)/$(TARGET)/generated PPRZ_CHIBIOS_OCM3 = $(SRC_FIRMWARE)/chibios-libopencm3 # Launch with "make Q=''" to get full command display diff --git a/conf/Makefile.local b/conf/Makefile.local index 0958fa2da5..d02b081e07 100644 --- a/conf/Makefile.local +++ b/conf/Makefile.local @@ -7,8 +7,8 @@ PAPARAZZI_HOME=$(HOME)/paparazzi endif ifeq ($(PAPARAZZI_SRC),) -TOOLS=$(DESTDIR)/usr/share/paparazzi/bin +GENERATORS=$(DESTDIR)/usr/share/paparazzi/bin else -TOOLS=$(PAPARAZZI_SRC)/sw/tools +GENERATORS=$(PAPARAZZI_SRC)/sw/tools/generators endif diff --git a/conf/Makefile.stm32-upload b/conf/Makefile.stm32-upload index 1bff711bf6..4066ab7a79 100644 --- a/conf/Makefile.stm32-upload +++ b/conf/Makefile.stm32-upload @@ -59,9 +59,14 @@ else ifeq ($(FLASH_MODE),DFU-UTIL) # # DFU flash mode using dfu-util DFU_ADDR ?= 0x08000000 +DFU_SIZE ?= $(shell stat --printf=%s $^) upload: $(OBJDIR)/$(TARGET).bin @echo "Using dfu-util at $(DFU_ADDR)" $(Q)dfu-util -d 0483:df11 -c 1 -i 0 -a 0 -s $(DFU_ADDR) -D $^ + $(Q)rm -f $(OBJDIR)/verify.bla + $(Q)dfu-util -d 0483:df11 -c 1 -i 0 -a 0 -s $(DFU_ADDR):$(DFU_SIZE) -U $(OBJDIR)/verify.bla + $(Q)diff $^ $(OBJDIR)/verify.bla + $(Q)rm -f $(OBJDIR)/verify.bla # # serial flash mode diff --git a/conf/abi.xml b/conf/abi.xml index 1b4d345a29..ff6cd327c0 100644 --- a/conf/abi.xml +++ b/conf/abi.xml @@ -11,6 +11,11 @@ + + + + + diff --git a/conf/airframes/CDW/DualBoardApFbw.xml b/conf/airframes/CDW/DualBoardApFbw.xml index f46838d67c..99d5a5103d 100644 --- a/conf/airframes/CDW/DualBoardApFbw.xml +++ b/conf/airframes/CDW/DualBoardApFbw.xml @@ -218,7 +218,7 @@ - + diff --git a/conf/airframes/CDW/classix.xml b/conf/airframes/CDW/classix.xml index 6f3f3a87d3..968e9f3e06 100644 --- a/conf/airframes/CDW/classix.xml +++ b/conf/airframes/CDW/classix.xml @@ -180,10 +180,6 @@ - - - - @@ -230,7 +226,7 @@ - + diff --git a/conf/airframes/CDW/test/yapa3_aspirin2.xml b/conf/airframes/CDW/test/yapa3_aspirin2.xml index 231cb1a8be..9cb255fd5c 100644 --- a/conf/airframes/CDW/test/yapa3_aspirin2.xml +++ b/conf/airframes/CDW/test/yapa3_aspirin2.xml @@ -214,7 +214,7 @@ diff --git a/conf/airframes/CDW/yapa_xsens.xml b/conf/airframes/CDW/yapa_xsens.xml index 8f2d011d30..919721742a 100644 --- a/conf/airframes/CDW/yapa_xsens.xml +++ b/conf/airframes/CDW/yapa_xsens.xml @@ -199,7 +199,7 @@ - + diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml index e1946c7a97..d85b06817b 100644 --- a/conf/airframes/ENAC/quadrotor/blender.xml +++ b/conf/airframes/ENAC/quadrotor/blender.xml @@ -5,7 +5,7 @@ - + @@ -254,8 +254,6 @@
- -
diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml index 632b993672..0e6a25ce3c 100644 --- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml +++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml @@ -3,8 +3,9 @@ - + + @@ -177,13 +179,16 @@
- -
+
+ + +
+
@@ -228,8 +233,7 @@
- - + diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml index 00ade274dc..b9ab81647b 100644 --- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml +++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml @@ -185,29 +185,6 @@
-
- - -
- - - - - - - - diff --git a/conf/airframes/TUDelft/delfly.xml b/conf/airframes/TUDelft/delfly.xml new file mode 100644 index 0000000000..c07812c4b7 --- /dev/null +++ b/conf/airframes/TUDelft/delfly.xml @@ -0,0 +1,199 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + +
+ +
+ + + + +
+ +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + +
+ +
+ + + + +
+ +
+ + +
+ +
+ + + + + + +
+ +
+ + + + + + + + +
+ + diff --git a/conf/airframes/ardrone2_raw_optitrack.xml b/conf/airframes/ardrone2_raw_optitrack.xml index 6d919681fd..262d253bb2 100644 --- a/conf/airframes/ardrone2_raw_optitrack.xml +++ b/conf/airframes/ardrone2_raw_optitrack.xml @@ -18,7 +18,7 @@ - + @@ -26,6 +26,11 @@ + + + + + @@ -102,6 +107,9 @@
+ + +
@@ -133,7 +141,7 @@ - + @@ -155,17 +163,17 @@ - + - + - + - + - + @@ -191,8 +199,16 @@ - - + + + + + + + + + +
diff --git a/conf/airframes/examples/MentorEnergy.xml b/conf/airframes/examples/MentorEnergy.xml index e320508b39..188d9a0f54 100644 --- a/conf/airframes/examples/MentorEnergy.xml +++ b/conf/airframes/examples/MentorEnergy.xml @@ -79,7 +79,7 @@ - + diff --git a/conf/airframes/examples/Twinstar_energyadaptive.xml b/conf/airframes/examples/Twinstar_energyadaptive.xml index 5863d2b287..9eea22514c 100644 --- a/conf/airframes/examples/Twinstar_energyadaptive.xml +++ b/conf/airframes/examples/Twinstar_energyadaptive.xml @@ -58,7 +58,7 @@ twog_1.0 + aspirin + ETS baro + ETS speed - + diff --git a/conf/airframes/examples/funjet_cam.xml b/conf/airframes/examples/funjet_cam.xml index 5f98dddb82..f2674a8d81 100644 --- a/conf/airframes/examples/funjet_cam.xml +++ b/conf/airframes/examples/funjet_cam.xml @@ -43,7 +43,7 @@ - + diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml index ee7a9f1bf8..b875a461ef 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml @@ -218,8 +218,6 @@
- - diff --git a/conf/airframes/examples/quadrotor_lisa_mx.xml b/conf/airframes/examples/quadrotor_lisa_mx.xml index 1cf5bc6ee3..95e406d9c3 100644 --- a/conf/airframes/examples/quadrotor_lisa_mx.xml +++ b/conf/airframes/examples/quadrotor_lisa_mx.xml @@ -37,7 +37,7 @@ - + @@ -52,6 +52,8 @@ + + diff --git a/conf/airframes/examples/quadrotor_navgo.xml b/conf/airframes/examples/quadrotor_navgo.xml index 194bba93bb..0a7da95fae 100644 --- a/conf/airframes/examples/quadrotor_navgo.xml +++ b/conf/airframes/examples/quadrotor_navgo.xml @@ -239,8 +239,6 @@
- -
diff --git a/conf/airframes/examples/separate_fbw_ap.xml b/conf/airframes/examples/separate_fbw_ap.xml index a2747c1603..abb0185263 100644 --- a/conf/airframes/examples/separate_fbw_ap.xml +++ b/conf/airframes/examples/separate_fbw_ap.xml @@ -219,7 +219,7 @@ - + diff --git a/conf/airframes/obsolete/vor.xml b/conf/airframes/obsolete/vor.xml deleted file mode 100644 index 6793fa5f24..0000000000 --- a/conf/airframes/obsolete/vor.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - - -ARCH=lpc21 - - -FLASH_MODE = IAP -main.ARCHDIR = $(ARCH) - -main.CFLAGS += -DBOARD_CONFIG=\"boards/olimex_lpc_h2148.h\" -main.srcs = firmwares/vor/lpc_vor_main.c - -main.CFLAGS += -DPERIODIC_FREQUENCY='29880.' -main.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c - -main.CFLAGS += -DUSE_LED - -main.srcs += $(SRC_ARCH)/armVIC.c - -main.srcs += firmwares/vor/lpc_vor_convertions.c - -main.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B9600 -main.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c - -main.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=UART0 -main.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c - -main.srcs += firmwares/vor/vor_int_demod_decim.c - - - - - diff --git a/conf/boa.conf b/conf/boa.conf deleted file mode 100644 index 0ac987eaf5..0000000000 --- a/conf/boa.conf +++ /dev/null @@ -1,241 +0,0 @@ -# Boa v0.94 configuration file -# File format has not changed from 0.93 -# File format has changed little from 0.92 -# version changes are noted in the comments -# -# The Boa configuration file is parsed with a custom parser. If it -# reports an error, the line number will be provided; it should be easy -# to spot. The syntax of each of these rules is very simple, and they -# can occur in any order. Where possible these directives mimic those -# of NCSA httpd 1.3; I saw no reason to introduce gratuitous -# differences. - -# $Id$ - -# The "ServerRoot" is not in this configuration file. It can be -# compiled into the server (see defines.h) or specified on the command -# line with the -c option, for example: -# -# boa -c /usr/local/boa - - -# Port: The port Boa runs on. The default port for http servers is 80. -# If it is less than 1024, the server must be started as root. - -Port 8889 - -# Listen: the Internet address to bind(2) to. If you leave it out, -# it takes the behavior before 0.93.17.2, which is to bind to all -# addresses (INADDR_ANY). You only get one "Listen" directive, -# if you want service on multiple IP addresses, you have three choices: -# 1. Run boa without a "Listen" directive -# a. All addresses are treated the same; makes sense if the addresses -# are localhost, ppp, and eth0. -# b. Use the VirtualHost directive below to point requests to different -# files. Should be good for a very large number of addresses (web -# hosting clients). -# 2. Run one copy of boa per IP address, each has its own configuration -# with a "Listen" directive. No big deal up to a few tens of addresses. -# Nice separation between clients. -# The name you provide gets run through inet_aton(3), so you have to use dotted -# quad notation. This configuration is too important to trust some DNS. - -#Listen 192.68.0.5 - -# User: The name or UID the server should run as. -# Group: The group name or GID the server should run as. - -User www-data -Group www-data - -# ServerAdmin: The email address where server problems should be sent. -# Note: this is not currently used, except as an environment variable -# for CGIs. - -#ServerAdmin root@localhost - -# PidFile: where to put the pid of the process. -# Comment out to write no pid file. -# Note: Because Boa drops privileges at startup, and the -# pid file is written by the UID/GID before doing so, Boa -# does not attempt removal of the pid file. -# PidFile /var/run/boa.pid - -# ErrorLog: The location of the error log file. If this does not start -# with /, it is considered relative to the server root. -# Set to /dev/null if you don't want errors logged. -# If unset, defaults to /dev/stderr -# Please NOTE: Sending the logs to a pipe ('|'), as shown below, -# is somewhat experimental and might fail under heavy load. -# "Usual libc implementations of printf will stall the whole -# process if the receiving end of a pipe stops reading." -#ErrorLog "|/usr/sbin/cronolog --symlink=/var/log/boa/error_log /var/log/boa/error-%Y%m%d.log" - -ErrorLog PAPARAZZI_HOME/var/logs/boa.log - -# AccessLog: The location of the access log file. If this does not -# start with /, it is considered relative to the server root. -# Comment out or set to /dev/null (less effective) to disable. -# Useful to set to /dev/stdout for use with daemontools. -# Access logging. -# Please NOTE: Sending the logs to a pipe ('|'), as shown below, -# is somewhat experimental and might fail under heavy load. -# "Usual libc implementations of printf will stall the whole -# process if the receiving end of a pipe stops reading." -#AccessLog "|/usr/sbin/cronolog --symlink=/var/log/boa/access_log /var/log/boa/access-%Y%m%d.log" - -AccessLog PAPARAZZI_HOME/var/logs/boa_access.log - -# CGILog /var/log/boa/cgi_log -# CGILog: The location of the CGI stderr log file. If this does not -# start with /, it is considered relative to the server root. -# The log file would contain any contents send to /dev/stderr -# by the CGI. If this is commented out, it defaults to whatever -# ErrorLog points. Set to /dev/null to disable CGI stderr logging. -# Please NOTE: Sending the logs to a pipe ('|'), as shown below, -# is somewhat experimental and might fail under heavy load. -# "Usual libc implementations of printf will stall the whole -# process if the receiving end of a pipe stops reading." -#CGILog "|/usr/sbin/cronolog --symlink=/var/log/boa/cgi_log /var/log/boa/cgi-%Y%m%d.log" - -# CGIumask 027 (no mask for user, read-only for group, and nothing for user) -# CGIumask 027 -# The CGIumask is set immediately before execution of the CGI. - -# UseLocaltime: Logical switch. Uncomment to use localtime -# instead of UTC time -#UseLocaltime - -# VerboseCGILogs: this is just a logical switch. -# It simply notes the start and stop times of cgis in the error log -# Comment out to disable. - -#VerboseCGILogs - -# ServerName: the name of this server that should be sent back to -# clients if different than that returned by gethostname + gethostbyname - -#ServerName www.your.org.here - -# VirtualHost: a logical switch. -# Comment out to disable. -# Given DocumentRoot /var/www, requests on interface 'A' or IP 'IP-A' -# become /var/www/IP-A. -# Example: http://localhost/ becomes /var/www/127.0.0.1 -# -# Not used until version 0.93.17.2. This "feature" also breaks commonlog -# output rules, it prepends the interface number to each access_log line. -# You are expected to fix that problem with a postprocessing script. - -#VirtualHost - - -# VHostRoot: the root location for all virtually hosted data -# Comment out to disable. -# Incompatible with 'Virtualhost' and 'DocumentRoot'!! -# Given VHostRoot /var/www, requests to host foo.bar.com, -# where foo.bar.com is ip a.b.c.d, -# become /var/www/a.b.c.d/foo.bar.com -# Hostnames are "cleaned", and must conform to the rules -# specified in rfc1034, which are be summarized here: -# -# Hostnames must start with a letter, end with a letter or digit, -# and have as interior characters only letters, digits, and hyphen. -# Hostnames must not exceed 63 characters in length. - -#VHostRoot /var/www - -# DefaultVHost -# Define this in order to have a default hostname when the client does not -# specify one, if using VirtualHostName. If not specified, the word -# "default" will be used for compatibility with older clients. - -#DefaultVHost foo.bar.com - -# DocumentRoot: The root directory of the HTML documents. -# Comment out to disable server non user files. - -DocumentRoot PAPARAZZI_HOME - -# UserDir: The name of the directory which is appended onto a user's home -# directory if a ~user request is received. - -UserDir public_html - -# DirectoryIndex: Name of the file to use as a pre-written HTML -# directory index. Please MAKE AND USE THESE FILES. On the -# fly creation of directory indexes can be _slow_. -# Comment out to always use DirectoryMaker - -DirectoryIndex index.html - -# DirectoryMaker: Name of program used to create a directory listing. -# Comment out to disable directory listings. If both this and -# DirectoryIndex are commented out, accessing a directory will give -# an error (though accessing files in the directory are still ok). - -DirectoryMaker /usr/lib/boa/boa_indexer - -# DirectoryCache: If DirectoryIndex doesn't exist, and DirectoryMaker -# has been commented out, the the on-the-fly indexing of Boa can be used -# to generate indexes of directories. Be warned that the output is -# extremely minimal and can cause delays when slow disks are used. -# Note: The DirectoryCache must be writable by the same user/group that -# Boa runs as. - -# DirectoryCache /var/spool/boa/dircache - -# KeepAliveMax: Number of KeepAlive requests to allow per connection -# Comment out, or set to 0 to disable keepalive processing - -KeepAliveMax 1000 - -# KeepAliveTimeout: seconds to wait before keepalive connection times out - -KeepAliveTimeout 10 - -# MimeTypes: This is the file that is used to generate mime type pairs -# and Content-Type fields for boa. -# Set to /dev/null if you do not want to load a mime types file. -# Do *not* comment out (better use AddType!) - -MimeTypes /etc/mime.types - -# DefaultType: MIME type used if the file extension is unknown, or there -# is no file extension. - -DefaultType text/plain - -# CGIPath: The value of the $PATH environment variable given to CGI progs. - -CGIPath /bin:/usr/bin:/usr/local/bin - -# SinglePostLimit: The maximum allowable number of bytes in -# a single POST. Default is normally 1MB. - -# AddType: adds types without editing mime.types -# Example: AddType type extension [extension ...] - -# Uncomment the next line if you want .cgi files to execute from anywhere -AddType application/x-httpd-cgi cgi - -# Redirect, Alias, and ScriptAlias all have the same semantics -- they -# match the beginning of a request and take appropriate action. Use -# Redirect for other servers, Alias for the same server, and ScriptAlias -# to enable directories for script execution. - -# Redirect allows you to tell clients about documents which used to exist in -# your server's namespace, but do not anymore. This allows you to tell the -# clients where to look for the relocated document. -# Example: Redirect /bar http://elsewhere/feh/bar - -# Aliases: Aliases one path to another. -# Example: Alias /path1/bar /path2/foo - -Alias /doc /usr/share/doc - -# ScriptAlias: Maps a virtual path to a directory for serving scripts -# Example: ScriptAlias /htbin/ /www/htbin/ - -ScriptAlias /cgi-bin/ PAPARAZZI_HOME/sw/cgi/ - diff --git a/conf/chibios/chibios_rules.mk b/conf/chibios/chibios_rules.mk index 60bdc3e03c..8d74f87b0c 100644 --- a/conf/chibios/chibios_rules.mk +++ b/conf/chibios/chibios_rules.mk @@ -116,10 +116,10 @@ all: $(OBJS) $(OUTFILES) MAKE_ALL_RULE_HOOK MAKE_ALL_RULE_HOOK: -$(OBJS): | $(BUILDDIR) +$(OBJS): | $(OBJDIR) $(BUILDDIR) $(OBJDIR) $(LSTDIR): -ifneq ($(USE_VERBOSE_COMPILE),yes) +ifeq ($(USE_VERBOSE_COMPILE),yes) @echo Compiler Options @echo $(CC) -c $(CFLAGS) -I. $(IINCDIR) main.c -o main.o @echo diff --git a/conf/control_panel_example.xml b/conf/control_panel_example.xml index 91b86c0d49..f8956a1ca9 100644 --- a/conf/control_panel_example.xml +++ b/conf/control_panel_example.xml @@ -41,11 +41,11 @@ - + - + - + @@ -68,8 +68,6 @@ - - @@ -78,7 +76,11 @@ -
+ + + + +
diff --git a/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile b/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile index 0ab3fc9cca..2afb6ad8d0 100644 --- a/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile +++ b/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile @@ -1,7 +1,7 @@ # Hey Emacs, this is a -*- makefile -*- ins_srcs += $(SRC_SUBSYSTEMS)/ins.c -ins_srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c +ins_srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough_utm.c ap.CFLAGS += $(ins_CFLAGS) diff --git a/conf/firmwares/subsystems/rotorcraft/gps_datalink.makefile b/conf/firmwares/subsystems/rotorcraft/gps_datalink.makefile new file mode 100644 index 0000000000..b674b38242 --- /dev/null +++ b/conf/firmwares/subsystems/rotorcraft/gps_datalink.makefile @@ -0,0 +1,18 @@ +# Hey Emacs, this is a -*- makefile -*- + +GPS_LED ?= none + +ap.srcs += $(SRC_SUBSYSTEMS)/gps.c +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_datalink.h\" +ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_datalink.c + +ap.CFLAGS += -DUSE_GPS -DGPS_DATALINK + +ifneq ($(GPS_LED),none) + ap.CFLAGS += -DGPS_LED=$(GPS_LED) +endif + +nps.CFLAGS += -DUSE_GPS +nps.srcs += $(SRC_SUBSYSTEMS)/gps.c +nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" +nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c diff --git a/conf/firmwares/subsystems/rotorcraft/gps_sim_hitl.makefile b/conf/firmwares/subsystems/rotorcraft/gps_sim_hitl.makefile new file mode 100644 index 0000000000..c2ea1367a8 --- /dev/null +++ b/conf/firmwares/subsystems/rotorcraft/gps_sim_hitl.makefile @@ -0,0 +1,15 @@ +# Hey Emacs, this is a -*- makefile -*- + +ap.srcs += $(SRC_SUBSYSTEMS)/gps.c +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_hitl.h\" +ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_hitl.c +ap.CFLAGS += -DUSE_GPS -DHITL + +ifneq ($(GPS_LED),none) + ap.CFLAGS += -DGPS_LED=$(GPS_LED) +endif + +nps.CFLAGS += -DUSE_GPS +nps.srcs += $(SRC_SUBSYSTEMS)/gps.c +nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" +nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c diff --git a/conf/firmwares/subsystems/rotorcraft/ins_gps_passthrough.makefile b/conf/firmwares/subsystems/rotorcraft/ins_gps_passthrough.makefile new file mode 100644 index 0000000000..d9ede096ab --- /dev/null +++ b/conf/firmwares/subsystems/rotorcraft/ins_gps_passthrough.makefile @@ -0,0 +1,11 @@ +# Hey Emacs, this is a -*- makefile -*- + +ins_srcs += $(SRC_SUBSYSTEMS)/ins.c +ins_srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c + + +ap.CFLAGS += $(ins_CFLAGS) +ap.srcs += $(ins_srcs) + +nps.CFLAGS += $(ins_CFLAGS) +nps.srcs += $(ins_srcs) diff --git a/conf/firmwares/subsystems/shared/actuators_dualpwm.makefile b/conf/firmwares/subsystems/shared/actuators_dualpwm.makefile new file mode 100644 index 0000000000..a0a1af7a3f --- /dev/null +++ b/conf/firmwares/subsystems/shared/actuators_dualpwm.makefile @@ -0,0 +1,10 @@ +ifeq ($(ARCH), lpc21) +$(error Error: dualpwm actuators only implemented for stm32) +endif + +$(TARGET).CFLAGS += -DACTUATORS +$(TARGET).srcs += $(SRC_ARCH)/subsystems/actuators/actuators_dualpwm_arch.c +ifeq ($(ARCH), stm32) +$(TARGET).srcs += $(SRC_ARCH)/subsystems/actuators/actuators_shared_arch.c +endif + diff --git a/conf/firmwares/subsystems/shared/actuators_pwm.makefile b/conf/firmwares/subsystems/shared/actuators_pwm.makefile index c476520918..cdb82abe1d 100644 --- a/conf/firmwares/subsystems/shared/actuators_pwm.makefile +++ b/conf/firmwares/subsystems/shared/actuators_pwm.makefile @@ -2,3 +2,6 @@ $(TARGET).CFLAGS += -DACTUATORS $(TARGET).srcs += $(SRC_ARCH)/subsystems/actuators/actuators_pwm_arch.c +ifeq ($(ARCH), stm32) +$(TARGET).srcs += $(SRC_ARCH)/subsystems/actuators/actuators_shared_arch.c +endif diff --git a/conf/firmwares/subsystems/shared/imu_ardrone2.makefile b/conf/firmwares/subsystems/shared/imu_ardrone2.makefile index ac06f5acf4..3a30478234 100644 --- a/conf/firmwares/subsystems/shared/imu_ardrone2.makefile +++ b/conf/firmwares/subsystems/shared/imu_ardrone2.makefile @@ -19,6 +19,8 @@ ap.srcs += $(imu_srcs) # Set the AHRS propegation frequencies AHRS_PROPAGATE_FREQUENCY ?= 200 AHRS_CORRECT_FREQUENCY ?= 200 +ap.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY) +ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY) # # Simulator diff --git a/conf/flight_plans/basic.xml b/conf/flight_plans/basic.xml index 0b7465334e..778f0c9bbc 100644 --- a/conf/flight_plans/basic.xml +++ b/conf/flight_plans/basic.xml @@ -40,7 +40,7 @@ - + diff --git a/conf/flight_plans/flight_plan.dtd b/conf/flight_plans/flight_plan.dtd index fb18d5ce0e..33599ac043 100644 --- a/conf/flight_plans/flight_plan.dtd +++ b/conf/flight_plans/flight_plan.dtd @@ -141,6 +141,7 @@ pitch CDATA #IMPLIED alt CDATA #IMPLIED height CDATA #IMPLIED approaching_time CDATA #IMPLIED +exceeding_time CDATA #IMPLIED throttle CDATA #IMPLIED climb CDATA #IMPLIED until CDATA #IMPLIED> @@ -151,6 +152,7 @@ vmode CDATA #IMPLIED pitch CDATA #IMPLIED alt CDATA #IMPLIED approaching_time CDATA #IMPLIED +exceeding_time CDATA #IMPLIED throttle CDATA #IMPLIED climb CDATA #IMPLIED> diff --git a/conf/flight_plans/mission_fw.xml b/conf/flight_plans/mission_fw.xml index daef0aaf3d..8359db5d26 100644 --- a/conf/flight_plans/mission_fw.xml +++ b/conf/flight_plans/mission_fw.xml @@ -31,7 +31,7 @@ - + diff --git a/conf/flight_plans/poles.xml b/conf/flight_plans/poles.xml index 0d0e539340..7241254523 100644 --- a/conf/flight_plans/poles.xml +++ b/conf/flight_plans/poles.xml @@ -37,7 +37,7 @@ - + diff --git a/conf/flight_plans/zamboni_survey_test.xml b/conf/flight_plans/zamboni_survey_test.xml index 7972b4d116..810c6a779d 100644 --- a/conf/flight_plans/zamboni_survey_test.xml +++ b/conf/flight_plans/zamboni_survey_test.xml @@ -75,7 +75,7 @@ - + diff --git a/conf/messages.xml b/conf/messages.xml index 4be902ef3e..65af79ddc1 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1498,8 +1498,7 @@ - - + @@ -1889,7 +1888,27 @@ - + + + + + + + + + + + + + + + + + + + + + @@ -1947,7 +1966,7 @@ - + @@ -2430,35 +2449,21 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + diff --git a/conf/modules/digital_cam.xml b/conf/modules/digital_cam.xml index 4bef1874e4..ca2e908b5f 100644 --- a/conf/modules/digital_cam.xml +++ b/conf/modules/digital_cam.xml @@ -1,44 +1,34 @@ - - - Digital camera control (trigger using led) - - - - - + Digital camera control (trigger using GPIO) + + + + + + + +
- +
- + - + - - + - - + - diff --git a/conf/modules/digital_cam_servo.xml b/conf/modules/digital_cam_servo.xml index 141ca80fa0..f6b75b0296 100644 --- a/conf/modules/digital_cam_servo.xml +++ b/conf/modules/digital_cam_servo.xml @@ -1,29 +1,15 @@ - - Digital camera control (trigger using servo) - - + + + + + + +
@@ -35,14 +21,10 @@ - - + - - + - - diff --git a/conf/modules/follow.xml b/conf/modules/follow.xml new file mode 100644 index 0000000000..c852b6c481 --- /dev/null +++ b/conf/modules/follow.xml @@ -0,0 +1,23 @@ + + + + + Follow a certain AC_ID trough remote GPS + + + + + + + +
+ +
+ + + + + + + + diff --git a/conf/modules/meteo_france_DAQ.xml b/conf/modules/meteo_france_DAQ.xml index 945cf2abed..008b69bc51 100644 --- a/conf/modules/meteo_france_DAQ.xml +++ b/conf/modules/meteo_france_DAQ.xml @@ -10,7 +10,7 @@ - + diff --git a/conf/modules/rotorcraft_cam.xml b/conf/modules/rotorcraft_cam.xml index b40f9896ce..fd734808ec 100644 --- a/conf/modules/rotorcraft_cam.xml +++ b/conf/modules/rotorcraft_cam.xml @@ -12,12 +12,16 @@ Four modes: - HEADING: the servo position and the heading of the rotorcraft are set with angles - WP: the camera is tracking a waypoint (Default: CAM) -The CAM_SWITCH can be used to power the camera in normal modes and disable it when in NONE mode. +If ROTORCRAFT_CAM_SWITCH_GPIO is defined, this gpio is set/cleared to switch the power +of the camera on in normal modes and disable it when in NONE mode. +On boards with CAM_SWITCH, ROTORCRAFT_CAM_SWITCH_GPIO can be defined to CAM_SWITCH_GPIO. +
- - + + + diff --git a/conf/modules/sonar_adc.xml b/conf/modules/sonar_adc.xml index e0dd84eca3..83506640ff 100644 --- a/conf/modules/sonar_adc.xml +++ b/conf/modules/sonar_adc.xml @@ -7,8 +7,9 @@ Reads an anlog sonar sensor and outputs sonar distance in [m] - + +
@@ -24,7 +25,6 @@ - diff --git a/conf/modules/sonar_adc_ins.xml b/conf/modules/sonar_adc_ins.xml deleted file mode 100644 index 7f0257f98e..0000000000 --- a/conf/modules/sonar_adc_ins.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - - - Sonar ADC (INS bindings). - Sonar ADC driver with INS binding, wich oes the same than sonar_adc module with an event function to feed INS subsystem. - Even if SONAR_OFFSET and _SCALE can be set, currently only the raw value and the INS_SONAR_SENS will be used in ins filters. - - - -
- - -
- - - - - - - - - - - - - diff --git a/conf/radios/T10CG_SBUS.xml b/conf/radios/T10CG_SBUS.xml index 137caa7b20..747b8329b3 100644 --- a/conf/radios/T10CG_SBUS.xml +++ b/conf/radios/T10CG_SBUS.xml @@ -45,11 +45,11 @@ - - - - - - - + + + + + + + diff --git a/conf/settings/control/rotorcraft_guidance.xml b/conf/settings/control/rotorcraft_guidance.xml index 114b18aa83..b8b7c49a1c 100644 --- a/conf/settings/control/rotorcraft_guidance.xml +++ b/conf/settings/control/rotorcraft_guidance.xml @@ -14,6 +14,7 @@ + diff --git a/conf/settings/estimation/ins_sonar.xml b/conf/settings/estimation/ins_sonar.xml index 0c016fa593..e709d2c6f5 100644 --- a/conf/settings/estimation/ins_sonar.xml +++ b/conf/settings/estimation/ins_sonar.xml @@ -3,7 +3,7 @@ - + diff --git a/doc/control_loops/fixedwing/README b/doc/control_loops/fixedwing/README index 560d8c902e..b42204618f 100644 --- a/doc/control_loops/fixedwing/README +++ b/doc/control_loops/fixedwing/README @@ -1,3 +1,3 @@ Diagram of the control loops used on the fiwed-wing airframes -Source files of the images used on the Wiki [http;//paparazzi.enac.fr/wiki/Control_loops] +Source files of the images used on the Wiki [http://wiki.paparazziuav.org/wiki/Control_Loops] diff --git a/doc/pprz_algebra/headfile.pdf b/doc/pprz_algebra/headfile.pdf index 2f348026e4..5a7f2527be 100644 Binary files a/doc/pprz_algebra/headfile.pdf and b/doc/pprz_algebra/headfile.pdf differ diff --git a/sw/airborne/Makefile b/sw/airborne/Makefile index 8688244cfa..4da0867799 100644 --- a/sw/airborne/Makefile +++ b/sw/airborne/Makefile @@ -27,7 +27,7 @@ Q=@ OBJDIR = $(PAPARAZZI_HOME)/var/$(AIRCRAFT)/$(TARGET) VARINCLUDE=$(PAPARAZZI_HOME)/var/include -ACINCLUDE = $(PAPARAZZI_HOME)/var/$(AIRCRAFT) +ACINCLUDE = $(PAPARAZZI_HOME)/var/$(AIRCRAFT)/$(TARGET) INCLUDES = -I$(PAPARAZZI_SRC)/sw/include -I$(PAPARAZZI_SRC)/sw/airborne -I$(PAPARAZZI_SRC)/conf/autopilot -I$(PAPARAZZI_SRC)/sw/airborne/arch/$($(TARGET).ARCHDIR) -I$(VARINCLUDE) -I$(ACINCLUDE) diff --git a/sw/airborne/arch/lpc21/usb_tunnel.c b/sw/airborne/arch/lpc21/usb_tunnel.c index 055cf89a7c..c87e331fdc 100644 --- a/sw/airborne/arch/lpc21/usb_tunnel.c +++ b/sw/airborne/arch/lpc21/usb_tunnel.c @@ -38,6 +38,14 @@ /* minimum LED blink on time 10Hz = 100ms */ #define BLINK_MIN 10 +#ifndef USB_TUNNEL_UART +#if USE_UART0 +#define USB_TUNNEL_UART uart0 +#else +#define USB_TUNNEL_UART uart1 +#endif +#endif + int main( void ) { unsigned char inc; unsigned int rx_time=0, tx_time=0; @@ -53,44 +61,37 @@ int main( void ) { mcu_int_enable(); +#if USE_LED_3 LED_ON(3); - -#if USE_UART0 - while(1) { - if (T0TC > (rx_time+((PCLK / T0_PCLK_DIV) / BLINK_MIN))) LED_OFF(1); - if (T0TC > (tx_time+((PCLK / T0_PCLK_DIV) / BLINK_MIN))) LED_OFF(2); - if (uart_char_available(&uart0) && VCOM_check_free_space(1)) { - LED_ON(1); - rx_time = T0TC; - inc = uart_getch(&uart0); - VCOM_putchar(inc); - } - if (VCOM_check_available() && uart_check_free_space(&uart0, 1)) { - LED_ON(2); - tx_time = T0TC; - inc = VCOM_getchar(); - uart_transmit(&uart0, inc); - } - } -#else - while(1) { - if (T0TC > (rx_time+((PCLK / T0_PCLK_DIV) / BLINK_MIN))) LED_OFF(1); - if (T0TC > (tx_time+((PCLK / T0_PCLK_DIV) / BLINK_MIN))) LED_OFF(2); - if (uart_char_available(&uart1) && VCOM_check_free_space(1)) { - LED_ON(1); - rx_time = T0TC; - inc = uart_getch(&uart1); - VCOM_putchar(inc); - } - if (VCOM_check_available() && uart_check_free_space(&uart1, 1)) { - LED_ON(2); - tx_time = T0TC; - inc = VCOM_getchar(); - uart_transmit(&uart1, inc); - } - } #endif + while(1) { + +#if USE_LED_1 + if (T0TC > (rx_time+((PCLK / T0_PCLK_DIV) / BLINK_MIN))) LED_OFF(1); +#endif +#if USE_LED_2 + if (T0TC > (tx_time+((PCLK / T0_PCLK_DIV) / BLINK_MIN))) LED_OFF(2); +#endif + + if (uart_char_available(&USB_TUNNEL_UART) && VCOM_check_free_space(1)) { +#if USE_LED_1 + LED_ON(1); +#endif + rx_time = T0TC; + inc = uart_getch(&USB_TUNNEL_UART); + VCOM_putchar(inc); + } + if (VCOM_check_available() && uart_check_free_space(&USB_TUNNEL_UART, 1)) { +#if USE_LED_2 + LED_ON(2); +#endif + tx_time = T0TC; + inc = VCOM_getchar(); + uart_transmit(&USB_TUNNEL_UART, inc); + } + } + return 0; } diff --git a/sw/airborne/arch/sim/ivy_transport.h b/sw/airborne/arch/sim/ivy_transport.h index 1c406b3811..d84ff19234 100644 --- a/sw/airborne/arch/sim/ivy_transport.h +++ b/sw/airborne/arch/sim/ivy_transport.h @@ -18,6 +18,8 @@ extern int ivy_dl_enabled; #define Space() ivy_p += sprintf(ivy_p, " "); #define Comma() ivy_p += sprintf(ivy_p, ","); +#define DelimStart() ivy_p += sprintf(ivy_p, "|"); +#define DelimEnd() ivy_p += sprintf(ivy_p, "|"); #define IvyTransportPutcByAddr(_dev,x) ivy_p += sprintf(ivy_p, "%c", *x); #define IvyTransportPutCharByAddr(_dev,x) IvyTransportPutcByAddr(_dev,x) Space() @@ -39,10 +41,11 @@ extern int ivy_dl_enabled; #define IvyTransportPutArray(_dev,_put, _n, _x) { \ int __i; \ + DelimStart(); \ for(__i = 0; __i < _n; __i++) { \ _put(_dev,&_x[__i]); \ Comma(); \ - } Space() \ + } DelimEnd(); Space(); \ } #define IvyTransportPutInt8Array(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutIntByAddr, _n, _x) diff --git a/sw/airborne/arch/sim/subsystems/actuators/actuators_dualpwm_arch.c b/sw/airborne/arch/sim/subsystems/actuators/actuators_dualpwm_arch.c new file mode 100644 index 0000000000..9fcfc6c883 --- /dev/null +++ b/sw/airborne/arch/sim/subsystems/actuators/actuators_dualpwm_arch.c @@ -0,0 +1,31 @@ +/* + * Copyright (C) 2010 The Paparazzi Team + * + * 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. + */ + +/** @file arch/sim/subsystems/actuators/actuators_dualpwm_arch.c + * dummy servos handling for sim + */ + +#include "subsystems/actuators/actuators_dualpwm_arch.h" + +void actuators_dualpwm_arch_init(void) { + +} + diff --git a/sw/airborne/arch/sim/subsystems/actuators/actuators_dualpwm_arch.h b/sw/airborne/arch/sim/subsystems/actuators/actuators_dualpwm_arch.h new file mode 100644 index 0000000000..2a22eee305 --- /dev/null +++ b/sw/airborne/arch/sim/subsystems/actuators/actuators_dualpwm_arch.h @@ -0,0 +1,36 @@ +/* + * Copyright (C) 2010 The Paparazzi Team + * + * 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. + */ + +/** @file arch/sim/subsystems/actuators/actuators_dualpwm_arch.h + * dummy servos handling for sim + */ + +#ifndef ACTUATORS_DUALPWM_ARCH_H +#define ACTUATORS_DUALPWM_ARCH_H + +#define SERVOS_TICS_OF_USEC(_v) (_v) + +#define ActuatorDualpwmSet(_i, _v) {} +#define ActuatorsDualPwmCommit() {} + +extern void actuators_dualpwm_arch_init(void); + +#endif /* ACTUATORS_DUALPWM_ARCH_H */ diff --git a/sw/airborne/arch/stm32/subsystems/actuators/actuators_dualpwm_arch.c b/sw/airborne/arch/stm32/subsystems/actuators/actuators_dualpwm_arch.c new file mode 100644 index 0000000000..108bb9e834 --- /dev/null +++ b/sw/airborne/arch/stm32/subsystems/actuators/actuators_dualpwm_arch.c @@ -0,0 +1,174 @@ +/* + * Copyright (C) 2010 The Paparazzi Team + * + * 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. + */ + +/** @file arch/stm32/subsystems/actuators/actuators_dualpwm_arch.c + * STM32 dual PWM servos handling. + */ + +//VALID TIMERS IS TIM5 ON THE LISA/M + +#include "subsystems/actuators/actuators_shared_arch.h" +#include "subsystems/actuators/actuators_dualpwm_arch.h" +#include "subsystems/actuators/actuators_dualpwm.h" + +#include +#include +#include +#include + + +uint32_t ratio_4ms, ratio_16ms; + + +uint32_t actuators_dualpwm_values[ACTUATORS_PWM_NB]; + +/** PWM arch init called by generic pwm driver + */ +void actuators_dualpwm_arch_init(void) { + + /*----------------------------------- + * Configure timer peripheral clocks + *-----------------------------------*/ +#if PWM_USE_TIM1 + rcc_periph_clock_enable(RCC_TIM1); +#endif +#if PWM_USE_TIM2 + rcc_periph_clock_enable(RCC_TIM2); +#endif +#if PWM_USE_TIM3 + rcc_periph_clock_enable(RCC_TIM3); +#endif +#if PWM_USE_TIM4 + rcc_periph_clock_enable(RCC_TIM4); +#endif +#if PWM_USE_TIM5 + rcc_periph_clock_enable(RCC_TIM5); +#endif +#if PWM_USE_TIM8 + rcc_periph_clock_enable(RCC_TIM8); +#endif +#if PWM_USE_TIM9 + rcc_periph_clock_enable(RCC_TIM9); +#endif +#if PWM_USE_TIM12 + rcc_periph_clock_enable(RCC_TIM12); +#endif + + /*---------------- + * Configure GPIO + *----------------*/ +#if defined(STM32F1) + /* TIM3 GPIO for PWM1..4 */ + AFIO_MAPR |= AFIO_MAPR_TIM3_REMAP_FULL_REMAP; +#endif + +#ifdef DUAL_PWM_SERVO_5 + set_servo_gpio(DUAL_PWM_SERVO_5_GPIO, DUAL_PWM_SERVO_5_PIN, DUAL_PWM_SERVO_5_AF, DUAL_PWM_SERVO_5_RCC); +#endif +#ifdef DUAL_PWM_SERVO_6 + set_servo_gpio(DUAL_PWM_SERVO_6_GPIO, DUAL_PWM_SERVO_6_PIN, DUAL_PWM_SERVO_6_AF, DUAL_PWM_SERVO_6_RCC); +#endif + +#if DUAL_PWM_USE_TIM5 + set_servo_timer(TIM5, TIM5_SERVO_HZ, PWM_TIM5_CHAN_MASK); + + nvic_set_priority(NVIC_TIM5_IRQ, 2); + nvic_enable_irq(NVIC_TIM5_IRQ); + timer_enable_irq(TIM5, TIM_DIER_CC1IE); +#endif + + //calculation the values to put into the timer registers to generate pulses every 4ms and 16ms. + ratio_4ms = (ONE_MHZ_CLK / 250)-1; + ratio_16ms = (ONE_MHZ_CLK / 62.5)-1; + +} + + +/** Interuption called at the end of the timer. In our case alternatively very 4ms and 16ms (twice every 20ms) + */ +#if DUAL_PWM_USE_TIM5 +void tim5_isr(void){ + + dual_pwm_isr(); +} +#endif + + + + +/** Fonction that clears the flag of interuption in order to reactivate the interuption + */ +void clear_timer_flag(void){ + +#if DUAL_PWM_USE_TIM5 + timer_clear_flag(TIM5, TIM_SR_CC1IF); +#endif +} + + +void set_dual_pwm_timer_s_period(uint32_t period){ + +#if DUAL_PWM_USE_TIM5 + timer_set_period(TIM5, period); +#endif +} + + +void set_dual_pwm_timer_s_oc(uint32_t oc_value){ + +#if DUAL_PWM_USE_TIM5 + timer_set_oc_value(DUAL_PWM_SERVO_5_TIMER, DUAL_PWM_SERVO_5_OC, oc_value); +#endif +} + + + + +void dual_pwm_isr(void){ + + static int num_pulse = 0; //status of the timer. Are we controling the first or the second servo + + clear_timer_flag(); + + if(num_pulse == 1){ + + set_dual_pwm_timer_s_period(ratio_16ms); + set_dual_pwm_timer_s_oc(actuators_dualpwm_values[FIRST_DUAL_PWM_SERVO]); + + num_pulse = 0; + }else{ + + set_dual_pwm_timer_s_period(ratio_4ms); + set_dual_pwm_timer_s_oc(actuators_dualpwm_values[SECOND_DUAL_PWM_SERVO]); + + num_pulse = 1; + } +} + + +/** Set pulse widths from actuator values, assumed to be in us + */ +void actuators_dualpwm_commit(void) { + + //we don't need to commit the values into this function as far as it's done in the interuption + //(wich is called every 4ms and 16ms alternatively (twice every 20ms)) + +} diff --git a/sw/airborne/arch/stm32/subsystems/actuators/actuators_dualpwm_arch.h b/sw/airborne/arch/stm32/subsystems/actuators/actuators_dualpwm_arch.h new file mode 100644 index 0000000000..5f6bfbb139 --- /dev/null +++ b/sw/airborne/arch/stm32/subsystems/actuators/actuators_dualpwm_arch.h @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2010 The Paparazzi Team + * + * 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. + */ + +/** @file arch/stm32/subsystems/actuators/actuators_dualpwm_arch.h + * STM32 PWM servos handling. + */ + +#ifndef ACTUATORS_dualpwm_ARCH_H +#define ACTUATORS_dualpwm_ARCH_H + +#include "std.h" + +#include BOARD_CONFIG + +#ifndef ACTUATORS_dualpwm_NB +#define ACTUATORS_dualpwm_NB 8 +#endif + + +extern uint32_t actuators_dualpwm_values[ACTUATORS_PWM_NB]; + +extern void actuators_dualpwm_commit(void); + +extern void dual_pwm_isr(void); + +extern void clear_timer_flag(void); + +extern void set_dual_pwm_timer_s_period(uint32_t period); + +extern void set_dual_pwm_timer_s_oc(uint32_t oc_value); + +#define SERVOS_TICS_OF_USEC(_v) (_v) + +#define ActuatorDualpwmSet(_i, _v) { actuators_dualpwm_values[_i] = _v; } +#define ActuatorsDualpwmCommit actuators_dualpwm_commit + +#endif /* ACTUATORS_dualpwm_ARCH_H */ diff --git a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c index 0ce80993df..a47b6fed50 100644 --- a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c +++ b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c @@ -25,6 +25,7 @@ //VALID TIMERS ARE TIM1,2,3,4,5,8,9,12 +#include "subsystems/actuators/actuators_shared_arch.h" #include "subsystems/actuators/actuators_pwm_arch.h" #include "subsystems/actuators/actuators_pwm.h" @@ -33,176 +34,9 @@ #include -#if defined(STM32F1) -//#define PCLK 72000000 -#define PCLK AHB_CLK -#elif defined(STM32F4) -//#define PCLK 84000000 -#define PCLK AHB_CLK/2 -#endif - -#define ONE_MHZ_CLK 1000000 - -#ifdef STM32F1 -/** - * HCLK = 72MHz, Timer clock also 72MHz since - * TIM1_CLK = APB2 = 72MHz - * TIM2_CLK = 2 * APB1 = 2 * 32MHz - */ -#define TIMER_APB1_CLK AHB_CLK -#define TIMER_APB2_CLK AHB_CLK -#endif - -#ifdef STM32F4 -/* Since APB prescaler != 1 : - * Timer clock frequency (before prescaling) is twice the frequency - * of the APB domain to which the timer is connected. - */ -#define TIMER_APB1_CLK (rcc_ppre1_frequency * 2) -#define TIMER_APB2_CLK (rcc_ppre2_frequency * 2) -#endif - - -/** Default servo update rate in Hz */ -#ifndef SERVO_HZ -#define SERVO_HZ 40 -#endif - -// Update rate can be adapted for each timer -#ifndef TIM1_SERVO_HZ -#define TIM1_SERVO_HZ SERVO_HZ -#endif -#ifndef TIM2_SERVO_HZ -#define TIM2_SERVO_HZ SERVO_HZ -#endif -#ifndef TIM3_SERVO_HZ -#define TIM3_SERVO_HZ SERVO_HZ -#endif -#ifndef TIM4_SERVO_HZ -#define TIM4_SERVO_HZ SERVO_HZ -#endif -#ifndef TIM5_SERVO_HZ -#define TIM5_SERVO_HZ SERVO_HZ -#endif -#ifndef TIM9_SERVO_HZ -#define TIM9_SERVO_HZ SERVO_HZ -#endif -#ifndef TIM12_SERVO_HZ -#define TIM12_SERVO_HZ SERVO_HZ -#endif - - -/** @todo: these should go into libopencm3 */ -#define TIM9 TIM9_BASE -#define TIM12 TIM12_BASE - int32_t actuators_pwm_values[ACTUATORS_PWM_NB]; -/** Set PWM channel configuration - */ -static inline void actuators_pwm_arch_channel_init(uint32_t timer_peripheral, - enum tim_oc_id oc_id) { - - timer_disable_oc_output(timer_peripheral, oc_id); - //There is no such register in TIM9 and 12. - if (timer_peripheral != TIM9 && timer_peripheral != TIM12) - timer_disable_oc_clear(timer_peripheral, oc_id); - timer_enable_oc_preload(timer_peripheral, oc_id); - timer_set_oc_slow_mode(timer_peripheral, oc_id); - timer_set_oc_mode(timer_peripheral, oc_id, TIM_OCM_PWM1); - timer_set_oc_polarity_high(timer_peripheral, oc_id); - timer_enable_oc_output(timer_peripheral, oc_id); - // Used for TIM1 and TIM8, the function does nothing if other timer is specified. - timer_enable_break_main_output(timer_peripheral); -} - -/** Set GPIO configuration - */ -#if defined(STM32F4) -static inline void set_servo_gpio(uint32_t gpioport, uint16_t pin, uint8_t af_num, enum rcc_periph_clken clken) { - rcc_periph_clock_enable(clken); - gpio_mode_setup(gpioport, GPIO_MODE_AF, GPIO_PUPD_NONE, pin); - gpio_set_af(gpioport, af_num, pin); -} -#elif defined(STM32F1) -static inline void set_servo_gpio(uint32_t gpioport, uint16_t pin, uint8_t none __attribute__((unused)), enum rcc_periph_clken clken) { - rcc_periph_clock_enable(clken); - rcc_periph_clock_enable(RCC_AFIO); - gpio_set_mode(gpioport, GPIO_MODE_OUTPUT_50_MHZ, - GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, pin); -} -#endif - -/** Set Timer configuration - */ -static inline void set_servo_timer(uint32_t timer, uint32_t period, uint8_t channels_mask) { - timer_reset(timer); - - /* Timer global mode: - * - No divider. - * - Alignement edge. - * - Direction up. - */ - if ((timer == TIM9) || (timer == TIM12)) - //There are no EDGE and DIR settings in TIM9 and TIM12 - timer_set_mode(timer, TIM_CR1_CKD_CK_INT, 0, 0); - else - timer_set_mode(timer, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP); - - - // TIM1, 8 and 9 use APB2 clock, all others APB1 - if (timer != TIM1 && timer != TIM8 && timer != TIM9) { - timer_set_prescaler(timer, (TIMER_APB1_CLK / ONE_MHZ_CLK) - 1); // 1uS - } else { - // TIM9, 1 and 8 use APB2 clock - timer_set_prescaler(timer, (TIMER_APB2_CLK / ONE_MHZ_CLK) - 1); - } - - timer_disable_preload(timer); - - timer_continuous_mode(timer); - - timer_set_period(timer, (ONE_MHZ_CLK / period) - 1); - - /* Disable outputs and configure channel if needed. */ - if (bit_is_set(channels_mask, 0)) { - actuators_pwm_arch_channel_init(timer, TIM_OC1); - } - if (bit_is_set(channels_mask, 1)) { - actuators_pwm_arch_channel_init(timer, TIM_OC2); - } - if (bit_is_set(channels_mask, 2)) { - actuators_pwm_arch_channel_init(timer, TIM_OC3); - } - if (bit_is_set(channels_mask, 3)) { - actuators_pwm_arch_channel_init(timer, TIM_OC4); - } - - /* - * Set initial output compare values. - * Note: Maybe we should preload the compare registers with some sensible - * values before we enable the timer? - */ - //timer_set_oc_value(timer, TIM_OC1, 1000); - //timer_set_oc_value(timer, TIM_OC2, 1000); - //timer_set_oc_value(timer, TIM_OC3, 1000); - //timer_set_oc_value(timer, TIM_OC4, 1000); - - /* -- Enable timer -- */ - /* - * ARR reload enable. - * Note: In our case it does not matter much if we do preload or not. As it - * is unlikely we will want to change the frequency of the timer during - * runtime anyways. - */ - timer_enable_preload(timer); - - /* Counter enable. */ - timer_enable_counter(timer); - -} - /** PWM arch init called by generic pwm driver */ void actuators_pwm_arch_init(void) { diff --git a/sw/airborne/arch/stm32/subsystems/actuators/actuators_shared_arch.c b/sw/airborne/arch/stm32/subsystems/actuators/actuators_shared_arch.c new file mode 100644 index 0000000000..6c4cd624ce --- /dev/null +++ b/sw/airborne/arch/stm32/subsystems/actuators/actuators_shared_arch.c @@ -0,0 +1,135 @@ +/* + * Copyright (C) 2010 The Paparazzi Team + * + * 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. + */ + +/** @file arch/stm32/subsystems/actuators/actuators_shared_arch.c + * STM32 PWM and dualPWM servos shared functions. + */ + +#include "subsystems/actuators/actuators_shared_arch.h" + + +/** Set GPIO configuration + */ +#if defined(STM32F4) +void set_servo_gpio(uint32_t gpioport, uint16_t pin, uint8_t af_num, enum rcc_periph_clken clken) { + rcc_periph_clock_enable(clken); + gpio_mode_setup(gpioport, GPIO_MODE_AF, GPIO_PUPD_NONE, pin); + gpio_set_af(gpioport, af_num, pin); +} +#elif defined(STM32F1) +void set_servo_gpio(uint32_t gpioport, uint16_t pin, uint8_t none __attribute__((unused)), enum rcc_periph_clken clken) { + rcc_periph_clock_enable(clken); + rcc_periph_clock_enable(RCC_AFIO); + gpio_set_mode(gpioport, GPIO_MODE_OUTPUT_50_MHZ, + GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, pin); +} +#endif + + + +/** Set PWM channel configuration + */ +void actuators_pwm_arch_channel_init(uint32_t timer_peripheral, + enum tim_oc_id oc_id) { + + timer_disable_oc_output(timer_peripheral, oc_id); + //There is no such register in TIM9 and 12. + if (timer_peripheral != TIM9 && timer_peripheral != TIM12) + timer_disable_oc_clear(timer_peripheral, oc_id); + timer_enable_oc_preload(timer_peripheral, oc_id); + timer_set_oc_slow_mode(timer_peripheral, oc_id); + timer_set_oc_mode(timer_peripheral, oc_id, TIM_OCM_PWM1); + timer_set_oc_polarity_high(timer_peripheral, oc_id); + timer_enable_oc_output(timer_peripheral, oc_id); + // Used for TIM1 and TIM8, the function does nothing if other timer is specified. + timer_enable_break_main_output(timer_peripheral); +} + + +/** Set Timer configuration + */ +void set_servo_timer(uint32_t timer, uint32_t period, uint8_t channels_mask) { + timer_reset(timer); + + /* Timer global mode: + * - No divider. + * - Alignement edge. + * - Direction up. + */ + if ((timer == TIM9) || (timer == TIM12)) + //There are no EDGE and DIR settings in TIM9 and TIM12 + timer_set_mode(timer, TIM_CR1_CKD_CK_INT, 0, 0); + else + timer_set_mode(timer, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP); + + + // TIM1, 8 and 9 use APB2 clock, all others APB1 + if (timer != TIM1 && timer != TIM8 && timer != TIM9) { + timer_set_prescaler(timer, (TIMER_APB1_CLK / ONE_MHZ_CLK) - 1); // 1uS + } else { + // TIM9, 1 and 8 use APB2 clock + timer_set_prescaler(timer, (TIMER_APB2_CLK / ONE_MHZ_CLK) - 1); + } + + timer_disable_preload(timer); + + timer_continuous_mode(timer); + + timer_set_period(timer, (ONE_MHZ_CLK / period) - 1); + + /* Disable outputs and configure channel if needed. */ + if (bit_is_set(channels_mask, 0)) { + actuators_pwm_arch_channel_init(timer, TIM_OC1); + } + if (bit_is_set(channels_mask, 1)) { + actuators_pwm_arch_channel_init(timer, TIM_OC2); + } + if (bit_is_set(channels_mask, 2)) { + actuators_pwm_arch_channel_init(timer, TIM_OC3); + } + if (bit_is_set(channels_mask, 3)) { + actuators_pwm_arch_channel_init(timer, TIM_OC4); + } + + /* + * Set initial output compare values. + * Note: Maybe we should preload the compare registers with some sensible + * values before we enable the timer? + */ + //timer_set_oc_value(timer, TIM_OC1, 1000); + //timer_set_oc_value(timer, TIM_OC2, 1000); + //timer_set_oc_value(timer, TIM_OC3, 1000); + //timer_set_oc_value(timer, TIM_OC4, 1000); + + /* -- Enable timer -- */ + /* + * ARR reload enable. + * Note: In our case it does not matter much if we do preload or not. As it + * is unlikely we will want to change the frequency of the timer during + * runtime anyways. + */ + timer_enable_preload(timer); + + /* Counter enable. */ + timer_enable_counter(timer); + +} + diff --git a/sw/airborne/arch/stm32/subsystems/actuators/actuators_shared_arch.h b/sw/airborne/arch/stm32/subsystems/actuators/actuators_shared_arch.h new file mode 100644 index 0000000000..7911b8ceab --- /dev/null +++ b/sw/airborne/arch/stm32/subsystems/actuators/actuators_shared_arch.h @@ -0,0 +1,113 @@ +/* + * Copyright (C) 2010 The Paparazzi Team + * + * 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. + */ + +/** @file arch/stm32/subsystems/actuators/actuators_shared_arch.h + * STM32 PWM and dualPWM servos shared functions. + */ + +#ifndef ACTUATORS_PWM_SHARED_ARCH_H +#define ACTUATORS_PWM_SHARED_ARCH_H + +#include "std.h" + +#include BOARD_CONFIG + +#include +#include +#include +#include + + + +#if defined(STM32F1) +//#define PCLK 72000000 +#define PCLK AHB_CLK +#elif defined(STM32F4) +//#define PCLK 84000000 +#define PCLK AHB_CLK/2 +#endif + +#define ONE_MHZ_CLK 1000000 + +#ifdef STM32F1 +/** + * HCLK = 72MHz, Timer clock also 72MHz since + * TIM1_CLK = APB2 = 72MHz + * TIM2_CLK = 2 * APB1 = 2 * 32MHz + */ +#define TIMER_APB1_CLK AHB_CLK +#define TIMER_APB2_CLK AHB_CLK +#endif + +#ifdef STM32F4 +/* Since APB prescaler != 1 : + * Timer clock frequency (before prescaling) is twice the frequency + * of the APB domain to which the timer is connected. + */ +#define TIMER_APB1_CLK (rcc_ppre1_frequency * 2) +#define TIMER_APB2_CLK (rcc_ppre2_frequency * 2) +#endif + +/** Default servo update rate in Hz */ +#ifndef SERVO_HZ +#define SERVO_HZ 40 +#endif + +// Update rate can be adapted for each timer +#ifndef TIM1_SERVO_HZ +#define TIM1_SERVO_HZ SERVO_HZ +#endif +#ifndef TIM2_SERVO_HZ +#define TIM2_SERVO_HZ SERVO_HZ +#endif +#ifndef TIM3_SERVO_HZ +#define TIM3_SERVO_HZ SERVO_HZ +#endif +#ifndef TIM4_SERVO_HZ +#define TIM4_SERVO_HZ SERVO_HZ +#endif +#ifndef TIM5_SERVO_HZ +#define TIM5_SERVO_HZ SERVO_HZ +#endif +#ifndef TIM9_SERVO_HZ +#define TIM9_SERVO_HZ SERVO_HZ +#endif +#ifndef TIM12_SERVO_HZ +#define TIM12_SERVO_HZ SERVO_HZ +#endif + + +/** @todo: these should go into libopencm3 */ +#define TIM9 TIM9_BASE +#define TIM12 TIM12_BASE + + +#if defined(STM32F4) +extern void set_servo_gpio(uint32_t gpioport, uint16_t pin, uint8_t af_num, enum rcc_periph_clken clken); +#elif defined(STM32F1) +extern void set_servo_gpio(uint32_t gpioport, uint16_t pin, uint8_t none __attribute__((unused)), enum rcc_periph_clken clken); +#endif + +extern void actuators_pwm_arch_channel_init(uint32_t timer_peripheral, enum tim_oc_id oc_id); + +extern void set_servo_timer(uint32_t timer, uint32_t period, uint8_t channels_mask); + +#endif /* ACTUATORS_PWM_SHARED_ARCH_H */ diff --git a/sw/airborne/boards/apogee_1.0.h b/sw/airborne/boards/apogee_1.0.h index 7a6dedf61e..d68f10884b 100644 --- a/sw/airborne/boards/apogee_1.0.h +++ b/sw/airborne/boards/apogee_1.0.h @@ -91,17 +91,8 @@ #define LED_8_GPIO_OFF gpio_clear #define LED_8_AFIO_REMAP ((void)0) -/* Power Switch, on PB12, 1 on LED_ON, 0 on LED_OFF */ -#ifndef USE_LED_9 -#define USE_LED_9 1 -#endif -#define LED_9_GPIO GPIOB -#define LED_9_GPIO_PIN GPIO12 -#define LED_9_GPIO_ON gpio_set -#define LED_9_GPIO_OFF gpio_clear -#define LED_9_AFIO_REMAP ((void)0) - -#define POWER_SWITCH_LED 9 +/* Power Switch, on PB12 */ +#define POWER_SWITCH_GPIO GPIOB,GPIO12 /* Pint to set Uart2 RX polarity, on PB13, output high inverts, low doesn't */ diff --git a/sw/airborne/boards/ardrone/baro_board.c b/sw/airborne/boards/ardrone/baro_board.c index 73c33868f3..67528f4761 100644 --- a/sw/airborne/boards/ardrone/baro_board.c +++ b/sw/airborne/boards/ardrone/baro_board.c @@ -31,10 +31,21 @@ #include "baro_board.h" #include "navdata.h" +/** Use an extra median filter to filter baro data + */ +#if USE_BARO_MEDIAN_FILTER +#include "filters/median_filter.h" +struct MedianFilterInt baro_median; +#endif + #define BMP180_OSS 0 // Parrot ARDrone uses no oversampling -void baro_init(void) {} +void baro_init(void) { +#if USE_BARO_MEDIAN_FILTER + init_median_filter(&baro_median); +#endif +} void baro_periodic(void) {} @@ -74,7 +85,11 @@ void ardrone_baro_event(void) // first read temperature because pressure calibration depends on temperature // TODO send Temperature message baro_apply_calibration_temp(navdata.temperature_pressure); - float pressure = (float)baro_apply_calibration(navdata.pressure); + int32_t press_raw = baro_apply_calibration(navdata.pressure); +#if USE_BARO_MEDIAN_FILTER + press_raw = update_median_filter(&baro_median, press_raw); +#endif + float pressure = (float)press_raw; AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); } navdata_baro_available = FALSE; diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c index 099034a73c..7e30c91127 100644 --- a/sw/airborne/boards/ardrone/navdata.c +++ b/sw/airborne/boards/ardrone/navdata.c @@ -40,6 +40,7 @@ #include "std.h" #include "navdata.h" #include "subsystems/ins.h" +#include "subsystems/abi.h" #define NAVDATA_PACKET_SIZE 60 #define NAVDATA_START_BYTE 0x3a @@ -51,9 +52,20 @@ navdata_port nav_port; static int nav_fd = 0; measures_t navdata; -#include "subsystems/sonar.h" -uint16_t sonar_meas = 0; +/** Sonar offset. + * Offset value in ADC + * equals to the ADC value so that height is zero + */ +#ifndef SONAR_OFFSET +#define SONAR_OFFSET 880 +#endif +/** Sonar scale. + * Sensor sensitivity in m/adc (float) + */ +#ifndef SONAR_SCALE +#define SONAR_SCALE 0.00047 +#endif // FIXME(ben): there must be a better home for these ssize_t full_write(int fd, const uint8_t *buf, size_t count) @@ -469,8 +481,8 @@ void navdata_update() // Check if there is a new sonar measurement and update the sonar if (navdata.ultrasound >> 15) { - sonar_meas = (navdata.ultrasound & 0x7FFF); - ins_update_sonar(); + float sonar_meas = (float)((navdata.ultrasound & 0x7FFF) - SONAR_OFFSET) * SONAR_SCALE; + AbiSendMsgAGL(AGL_SONAR_ARDRONE2_ID, &sonar_meas); } #endif diff --git a/sw/airborne/boards/booz_1.0.h b/sw/airborne/boards/booz_1.0.h index 7877386dae..11bd4defaa 100644 --- a/sw/airborne/boards/booz_1.0.h +++ b/sw/airborne/boards/booz_1.0.h @@ -42,22 +42,9 @@ #define LED_4_BANK 1 #define LED_4_PIN 31 -#ifndef USE_LED_5 -#define USE_LED_5 1 -#endif -#define LED_5_BANK 1 -#define LED_5_PIN 18 - -#define POWER_SWITCH_LED 5 - -#ifndef USE_LED_6 -#define USE_LED_6 1 -#endif -#define LED_6_BANK 1 -#define LED_6_PIN 22 - -#define CAM_SWITCH_LED 6 +#define POWER_SWITCH_GPIO GPIOB,GPIO18 +#define CAM_SWITCH_GPIO GPIOB,GPIO22 /* PPM : rc rx on P0.28 ( CAP0.2 ) */ #define PPM_PINSEL PINSEL1 diff --git a/sw/airborne/boards/hb_1.1.h b/sw/airborne/boards/hb_1.1.h index 02f00187e0..ad0c7a6511 100644 --- a/sw/airborne/boards/hb_1.1.h +++ b/sw/airborne/boards/hb_1.1.h @@ -17,39 +17,13 @@ /* Peripheral bus clock freq. */ #define PCLK (CCLK / PBSD_VAL) -/* Onboard LEDs */ -/* led 1 and led 2 are not seperate leds, but leds indicating the power switch status */ +/* power switch status */ +#define POWER_SWITCH_GPIO GPIOB,GPIO18 +#define POWER_SWITCH_2_GPIO GPIOB,GPIO19 -#ifndef USE_LED_1 -#define USE_LED_1 1 -#endif -#define LED_1_BANK 1 -#define LED_1_PIN 18 - -#ifndef USE_LED_2 -#define USE_LED_2 1 -#endif -#define LED_2_BANK 1 -#define LED_2_PIN 19 - -#define POWER_SWITCH_LED 1 -#define POWER_SWITCH_2_LED 2 - -/* there are no actual leds 3 and 4, these defines are just to conveniently switch the buzzer and the cam switch */ -#ifndef USE_LED_3 -#define USE_LED_3 1 -#endif -#define LED_3_BANK 1 -#define LED_3_PIN 20 - -#ifndef USE_LED_4 -#define USE_LED_4 1 -#endif -#define LED_4_BANK 1 -#define LED_4_PIN 25 - -#define BUZZER_LED 3 -#define CAM_SWITCH_LED 4 +/* buzzer and cam switch */ +#define BUZZER_GPIO GPIOB,GPIO20 +#define CAM_SWITCH_GPIO GPIOB,GPIO25 /* P0.22 aka MAT0.0 */ #define SERVO_CLOCK_PIN 22 diff --git a/sw/airborne/boards/lisa_l_1.0.h b/sw/airborne/boards/lisa_l_1.0.h index 3ef3a14c6d..b8aa6cba2f 100644 --- a/sw/airborne/boards/lisa_l_1.0.h +++ b/sw/airborne/boards/lisa_l_1.0.h @@ -13,9 +13,6 @@ #endif #define LED_STP08 -// FIXME, this is just to make it compile -#define POWER_SWITCH_LED 5 - /* SPI slave mapping */ #define SPI_SELECT_SLAVE0_PERIPH RCC_GPIOA diff --git a/sw/airborne/boards/lisa_m_common.h b/sw/airborne/boards/lisa_m_common.h index 3fc39b5d0c..6f0a26da27 100644 --- a/sw/airborne/boards/lisa_m_common.h +++ b/sw/airborne/boards/lisa_m_common.h @@ -224,8 +224,18 @@ #define USE_PWM2 1 #define USE_PWM3 1 #define USE_PWM4 1 -#define USE_PWM5 1 -#define USE_PWM6 1 + +#if DUAL_PWM_ON + #define DUAL_PWM_USE_TIM5 1 + + #define USE_DUAL_PWM5 1 + #define USE_DUAL_PWM6 1 +#else + #define USE_PWM5 1 + #define USE_PWM6 1 +#endif + + #if USE_SERVOS_7AND8 #if USE_I2C1 @@ -296,31 +306,60 @@ #endif #if USE_PWM5 -#define PWM_SERVO_5 4 -#define PWM_SERVO_5_TIMER TIM5 -#define PWM_SERVO_5_RCC RCC_GPIOA -#define PWM_SERVO_5_GPIO GPIOA -#define PWM_SERVO_5_PIN GPIO0 -#define PWM_SERVO_5_AF 0 -#define PWM_SERVO_5_OC TIM_OC1 -#define PWM_SERVO_5_OC_BIT (1<<0) + #define PWM_SERVO_5 4 + #define PWM_SERVO_5_TIMER TIM5 + #define PWM_SERVO_5_RCC RCC_GPIOA + #define PWM_SERVO_5_GPIO GPIOA + #define PWM_SERVO_5_PIN GPIO0 + #define PWM_SERVO_5_AF 0 + #define PWM_SERVO_5_OC TIM_OC1 + #define PWM_SERVO_5_OC_BIT (1<<0) +#elif USE_DUAL_PWM5 + #define DUAL_PWM_SERVO_5 4 + + #define FIRST_DUAL_PWM_SERVO DUAL_PWM_SERVO_5 + + #define DUAL_PWM_SERVO_5_TIMER TIM5 + #define DUAL_PWM_SERVO_5_RCC RCC_GPIOA + #define DUAL_PWM_SERVO_5_GPIO GPIOA + #define DUAL_PWM_SERVO_5_PIN GPIO0 + #define DUAL_PWM_SERVO_5_AF 0 + #define DUAL_PWM_SERVO_5_OC TIM_OC1 + #define PWM_SERVO_5_OC_BIT (1<<0) #else -#define PWM_SERVO_5_OC_BIT 0 + #define PWM_SERVO_5_OC_BIT 0 #endif #if USE_PWM6 -#define PWM_SERVO_6 5 -#define PWM_SERVO_6_TIMER TIM5 -#define PWM_SERVO_6_RCC RCC_GPIOA -#define PWM_SERVO_6_GPIO GPIOA -#define PWM_SERVO_6_PIN GPIO1 -#define PWM_SERVO_6_AF 0 -#define PWM_SERVO_6_OC TIM_OC2 -#define PWM_SERVO_6_OC_BIT (1<<1) + #define PWM_SERVO_6 5 + #define PWM_SERVO_6_TIMER TIM5 + #define PWM_SERVO_6_RCC RCC_GPIOA + #define PWM_SERVO_6_GPIO GPIOA + #define PWM_SERVO_6_PIN GPIO1 + #define PWM_SERVO_6_AF 0 + #define PWM_SERVO_6_OC TIM_OC2 + #define PWM_SERVO_6_OC_BIT (1<<1) +#elif USE_DUAL_PWM6 + #define DUAL_PWM_SERVO_6 5 + + #define SECOND_DUAL_PWM_SERVO DUAL_PWM_SERVO_6 + + #define DUAL_PWM_SERVO_6_TIMER TIM5 + #define DUAL_PWM_SERVO_6_RCC RCC_GPIOA + #define DUAL_PWM_SERVO_6_GPIO GPIOA + #define DUAL_PWM_SERVO_6_PIN GPIO1 + #define DUAL_PWM_SERVO_6_AF 0 + #define DUAL_PWM_SERVO_6_OC TIM_OC2 + #define PWM_SERVO_6_OC_BIT (1<<1) #else -#define PWM_SERVO_6_OC_BIT 0 + #define PWM_SERVO_6_OC_BIT 0 #endif + + + + + #if USE_PWM7 #define PWM_SERVO_7 6 #define PWM_SERVO_7_TIMER TIM4 diff --git a/sw/airborne/boards/lisa_s_0.1.h b/sw/airborne/boards/lisa_s_0.1.h index 827ad14917..c6c5449a5a 100644 --- a/sw/airborne/boards/lisa_s_0.1.h +++ b/sw/airborne/boards/lisa_s_0.1.h @@ -90,7 +90,10 @@ #define DefaultVoltageOfAdc(adc) (0.0045*adc) -#define BOARD_HAS_BARO 1 +/* by default activate onboard baro */ +#ifndef USE_BARO_BOARD +#define USE_BARO_BOARD 1 +#endif /* SPI slave mapping */ @@ -290,8 +293,18 @@ #define USE_PWM3 1 #define USE_PWM4 1 -#define USE_PWM5 1 -#define USE_PWM6 1 + +//TODO : test that part +//TODO : merge the USE_SERVOS_1AND2 and DUAL_PWM_ON +#if DUAL_PWM_ON + #define DUAL_PWM_USE_TIM5 1 + + #define USE_DUAL_PWM5 1 + #define USE_DUAL_PWM6 1 +#else + #define USE_PWM5 1 + #define USE_PWM6 1 +#endif // Servo numbering on LisaM silkscreen/docs starts with 1 @@ -352,31 +365,56 @@ #endif #if USE_PWM5 -#define PWM_SERVO_5 2 -#define PWM_SERVO_5_TIMER TIM5 -#define PWM_SERVO_5_RCC RCC_GPIOA -#define PWM_SERVO_5_GPIO GPIOA -#define PWM_SERVO_5_PIN GPIO0 -#define PWM_SERVO_5_AF 0 -#define PWM_SERVO_5_OC TIM_OC1 -#define PWM_SERVO_5_OC_BIT (1<<0) + #define PWM_SERVO_5 2 + #define PWM_SERVO_5_TIMER TIM5 + #define PWM_SERVO_5_RCC RCC_GPIOA + #define PWM_SERVO_5_GPIO GPIOA + #define PWM_SERVO_5_PIN GPIO0 + #define PWM_SERVO_5_AF 0 + #define PWM_SERVO_5_OC TIM_OC1 + #define PWM_SERVO_5_OC_BIT (1<<0) +#elif USE_DUAL_PWM5 + #define DUAL_PWM_SERVO_5 2 + + #define FIRST_DUAL_PWM_SERVO DUAL_PWM_SERVO_5 + + #define DUAL_PWM_SERVO_5_TIMER TIM5 + #define DUAL_PWM_SERVO_5_RCC RCC_GPIOA + #define DUAL_PWM_SERVO_5_GPIO GPIOA + #define DUAL_PWM_SERVO_5_PIN GPIO0 + #define DUAL_PWM_SERVO_5_AF 0 + #define DUAL_PWM_SERVO_5_OC TIM_OC1 + #define PWM_SERVO_5_OC_BIT (1<<0) #else -#define PWM_SERVO_5_OC_BIT 0 + #define PWM_SERVO_5_OC_BIT 0 #endif #if USE_PWM6 -#define PWM_SERVO_6 3 -#define PWM_SERVO_6_TIMER TIM5 -#define PWM_SERVO_6_RCC RCC_GPIOA -#define PWM_SERVO_6_GPIO GPIOA -#define PWM_SERVO_6_PIN GPIO1 -#define PWM_SERVO_6_AF 0 -#define PWM_SERVO_6_OC TIM_OC2 -#define PWM_SERVO_6_OC_BIT (1<<1) + #define PWM_SERVO_6 3 + #define PWM_SERVO_6_TIMER TIM5 + #define PWM_SERVO_6_RCC RCC_GPIOA + #define PWM_SERVO_6_GPIO GPIOA + #define PWM_SERVO_6_PIN GPIO1 + #define PWM_SERVO_6_AF 0 + #define PWM_SERVO_6_OC TIM_OC2 + #define PWM_SERVO_6_OC_BIT (1<<1) +#elif USE_DUAL_PWM6 + #define DUAL_PWM_SERVO_6 3 + + #define SECOND_DUAL_PWM_SERVO DUAL_PWM_SERVO_6 + + #define DUAL_PWM_SERVO_6_TIMER TIM5 + #define DUAL_PWM_SERVO_6_RCC RCC_GPIOA + #define DUAL_PWM_SERVO_6_GPIO GPIOA + #define DUAL_PWM_SERVO_6_PIN GPIO1 + #define DUAL_PWM_SERVO_6_AF 0 + #define DUAL_PWM_SERVO_6_OC TIM_OC2 + #define PWM_SERVO_6_OC_BIT (1<<1) #else -#define PWM_SERVO_6_OC_BIT 0 + #define PWM_SERVO_6_OC_BIT 0 #endif + /* servos 1-4 or 3-4 on TIM4 depending on USE_SERVOS_1AND2 */ #define PWM_TIM4_CHAN_MASK (PWM_SERVO_1_OC_BIT|PWM_SERVO_2_OC_BIT|PWM_SERVO_3_OC_BIT|PWM_SERVO_4_OC_BIT) /* servos 5-6 on TIM5 */ diff --git a/sw/airborne/boards/logom_2.6.h b/sw/airborne/boards/logom_2.6.h index 1b6f716667..f3d11d99b8 100644 --- a/sw/airborne/boards/logom_2.6.h +++ b/sw/airborne/boards/logom_2.6.h @@ -53,33 +53,15 @@ #define LED_3_BANK 0 #define LED_3_PIN 11 -#ifndef USE_LED_4 -#define USE_LED_4 1 -#endif -#define LED_4_BANK 1 -#define LED_4_PIN 17 +#define POWER_SWITCH_GPIO GPIOB,GPIO17 -#define POWER_SWITCH_LED 4 +#define CAM_SWITCH_GPIO GPIOB,GPIO18 -#ifndef USE_LED_5 -#define USE_LED_5 1 -#endif -#define LED_5_BANK 1 -#define LED_5_PIN 18 +#define GPS_RESET_GPIO GPIOB,GPIO19 -#define CAM_SWITCH_LED 5 - -#ifndef USE_LED_6 -#define USE_LED_6 1 -#endif -#define LED_6_BANK 1 -#define LED_6_PIN 19 - -#define GPS_RESET 6 - -#define Configure_GPS_RESET_Pin() LED_INIT(GPS_RESET) -#define Set_GPS_RESET_Pin_LOW() LED_ON(GPS_RESET) -#define Open_GPS_RESET_Pin() ClearBit(LED_DIR(GPS_RESET), LED_PIN(GPS_RESET)) +#define Configure_GPS_RESET_Pin() gpio_setup_output(GPS_RESET_GPIO) +#define Set_GPS_RESET_Pin_LOW() gpio_clear(GPS_RESET_GPIO) +#define Open_GPS_RESET_Pin() gpio_setup_input(GPS_RESET_GPIO) /* P0.29 aka MAT0.3 (P2) */ #define SERVO_CLOCK_PIN 29 diff --git a/sw/airborne/boards/px4fmu_1.7.h b/sw/airborne/boards/px4fmu_1.7.h index 1b3baa27a4..12472111d1 100644 --- a/sw/airborne/boards/px4fmu_1.7.h +++ b/sw/airborne/boards/px4fmu_1.7.h @@ -199,9 +199,10 @@ -/* Activate onboard baro */ -#define BOARD_HAS_BARO 1 - +/* Activate onboard baro by default */ +#ifndef USE_BARO_BOARD +#define USE_BARO_BOARD 1 +#endif /* Default actuators driver */ diff --git a/sw/airborne/boards/sdlog_1.0.h b/sw/airborne/boards/sdlog_1.0.h index 74fd1cb368..0cd319b776 100644 --- a/sw/airborne/boards/sdlog_1.0.h +++ b/sw/airborne/boards/sdlog_1.0.h @@ -57,33 +57,15 @@ #define LED_3_BANK 1 #define LED_3_PIN 23 -#ifndef USE_LED_4 -#define USE_LED_4 1 -#endif -#define LED_4_BANK 1 -#define LED_4_PIN 18 +#define POWER_SWITCH_GPIO GPIOB,GPIO18 -#define POWER_SWITCH_LED 4 +#define CAM_SWITCH_GPIO GPIOB,GPIO22 -#ifndef USE_LED_5 -#define USE_LED_5 1 -#endif -#define LED_5_BANK 1 -#define LED_5_PIN 22 +#define GPS_RESET_GPIO GPIOB,GPIO21 -#define CAM_SWITCH_LED 5 - -#ifndef USE_LED_6 -#define USE_LED_6 1 -#endif -#define LED_6_BANK 1 -#define LED_6_PIN 21 - -#define GPS_RESET 6 - -#define Configure_GPS_RESET_Pin() LED_INIT(GPS_RESET) -#define Set_GPS_RESET_Pin_LOW() LED_ON(GPS_RESET) -#define Open_GPS_RESET_Pin() ClearBit(LED_DIR(GPS_RESET), LED_PIN(GPS_RESET)) +#define Configure_GPS_RESET_Pin() gpio_setup_output(GPS_RESET_GPIO) +#define Set_GPS_RESET_Pin_LOW() gpio_clear(GPS_RESET_GPIO) +#define Open_GPS_RESET_Pin() gpio_setup_input(GPS_RESET_GPIO) /* P0.5 aka MAT0.1 */ #define SERVO_CLOCK_PIN 5 diff --git a/sw/airborne/boards/tiny_1.1.h b/sw/airborne/boards/tiny_1.1.h index 5a79318107..c942521e61 100644 --- a/sw/airborne/boards/tiny_1.1.h +++ b/sw/airborne/boards/tiny_1.1.h @@ -38,13 +38,7 @@ #define LED_2_PIN 19 /* Switch pin */ -#ifndef USE_LED_3 -#define USE_LED_3 1 -#endif -#define LED_3_BANK 0 -#define LED_3_PIN 11 - -#define POWER_SWITCH_LED 3 +#define POWER_SWITCH_GPIO GPIOA,GPIO11 /* Default actuators driver */ #define DEFAULT_ACTUATORS "subsystems/actuators/actuators_4015.h" diff --git a/sw/airborne/boards/tiny_2.0.h b/sw/airborne/boards/tiny_2.0.h index 4f91c445b0..d205470a6d 100644 --- a/sw/airborne/boards/tiny_2.0.h +++ b/sw/airborne/boards/tiny_2.0.h @@ -41,13 +41,7 @@ #define LED_3_BANK 1 #define LED_3_PIN 23 -#ifndef USE_LED_4 -#define USE_LED_4 1 -#endif -#define LED_4_BANK 1 -#define LED_4_PIN 18 - -#define POWER_SWITCH_LED 4 +#define POWER_SWITCH_GPIO GPIOB,GPIO18 /* Default actuators driver */ #define DEFAULT_ACTUATORS "subsystems/actuators/actuators_4017.h" diff --git a/sw/airborne/boards/tiny_2.1.h b/sw/airborne/boards/tiny_2.1.h index 3ee8c1e7de..b16b12c983 100644 --- a/sw/airborne/boards/tiny_2.1.h +++ b/sw/airborne/boards/tiny_2.1.h @@ -58,33 +58,15 @@ #define LED_3_BANK 1 #define LED_3_PIN 23 -#ifndef USE_LED_4 -#define USE_LED_4 1 -#endif -#define LED_4_BANK 1 -#define LED_4_PIN 18 +#define POWER_SWITCH_GPIO GPIOB,GPIO18 -#define POWER_SWITCH_LED 4 +#define CAM_SWITCH_GPIO GPIOB,GPIO22 -#ifndef USE_LED_5 -#define USE_LED_5 1 -#endif -#define LED_5_BANK 1 -#define LED_5_PIN 22 +#define GPS_RESET_GPIO GPIOB,GPIO21 -#define CAM_SWITCH_LED 5 - -#ifndef USE_LED_6 -#define USE_LED_6 1 -#endif -#define LED_6_BANK 1 -#define LED_6_PIN 21 - -#define GPS_RESET 6 - -#define Configure_GPS_RESET_Pin() LED_INIT(GPS_RESET) -#define Set_GPS_RESET_Pin_LOW() LED_ON(GPS_RESET) -#define Open_GPS_RESET_Pin() ClearBit(LED_DIR(GPS_RESET), LED_PIN(GPS_RESET)) +#define Configure_GPS_RESET_Pin() gpio_setup_output(GPS_RESET_GPIO) +#define Set_GPS_RESET_Pin_LOW() gpio_clear(GPS_RESET_GPIO) +#define Open_GPS_RESET_Pin() gpio_setup_input(GPS_RESET_GPIO) /* Default actuators driver */ #define DEFAULT_ACTUATORS "subsystems/actuators/actuators_4017.h" diff --git a/sw/airborne/filters/low_pass_filter.h b/sw/airborne/filters/low_pass_filter.h index 7191163cde..46f6e6fca1 100644 --- a/sw/airborne/filters/low_pass_filter.h +++ b/sw/airborne/filters/low_pass_filter.h @@ -29,6 +29,8 @@ #include "std.h" +#define INT32_FILT_FRAC 8 + /** First order low pass filter structure. * * using bilinear z transform @@ -119,6 +121,7 @@ struct SecondOrderLowPass { float o[2]; ///< output history }; + /** Init second order low pass filter. * * @param filter second order low pass filter structure @@ -165,6 +168,72 @@ static inline float get_second_order_low_pass(struct SecondOrderLowPass * filter return filter->o[0]; } +struct SecondOrderLowPass_int { + int32_t a[2]; ///< denominator gains + int32_t b[2]; ///< numerator gains + int32_t i[2]; ///< input history + int32_t o[2]; ///< output history + int32_t loop_gain; ///< loop gain +}; + +/** Init second order low pass filter(fixed point version). + * + * @param filter second order low pass filter structure + * @param tau time constant of the second order low pass filter , 1/(2*pi*cut_off_3db) + * @param Q Q value of the second order low pass filter + * @param sample_time sampling period of the signal + * @param value initial value of the filter + */ +static inline void init_second_order_low_pass_int(struct SecondOrderLowPass_int* filter, float cut_off, float Q, float sample_time, int32_t value) { + struct SecondOrderLowPass filter_temp; + float tau = 7 / (44*cut_off); + float K = sample_time / (2*tau); + float poly = K*K + K/Q + 1.; + float loop_gain_f; + + filter_temp.a[0] = 2.*(K*K - 1.) / poly; + filter_temp.a[1] = (K*K - K/Q + 1.) / poly; + filter_temp.b[0] = K*K / poly; + filter_temp.b[1] = 2*filter_temp.b[0]; + loop_gain_f = 1/filter_temp.b[0]; + + filter->a[0] = BFP_OF_REAL((filter_temp.a[0] * loop_gain_f), INT32_FILT_FRAC); + filter->a[1] = BFP_OF_REAL((filter_temp.a[1] * loop_gain_f), INT32_FILT_FRAC); + filter->b[0] = BFP_OF_REAL(1, INT32_FILT_FRAC); + filter->b[1] = 2*filter->b[0]; + filter->i[0] = filter->i[1] = filter->o[0] = filter->o[1] = value; + filter->loop_gain = BFP_OF_REAL(loop_gain_f, INT32_FILT_FRAC); +} + +/** Update second order low pass filter state with a new value(fixed point version). + * + * @param filter second order low pass filter structure + * @param value new input value of the filter + * @return new filtered value + */ +static inline int32_t update_second_order_low_pass_int(struct SecondOrderLowPass_int* filter, int32_t value) { + int32_t out = filter->b[0] * value + + filter->b[1] * filter->i[0] + + filter->b[0] * filter->i[1] + - filter->a[0] * filter->o[0] + - filter->a[1] * filter->o[1]; + + filter->i[1] = filter->i[0]; + filter->i[0] = value; + filter->o[1] = filter->o[0]; + filter->o[0] = out / (filter->loop_gain); + return filter->o[0]; +} + +/** Get current value of the second order low pass filter(fixed point version). + * + * @param filter second order low pass filter structure + * @return current value of the filter + */ +static inline int32_t get_second_order_low_pass_int(struct SecondOrderLowPass_int * filter) { + return filter->o[0]; +} + /** Second order Butterworth low pass filter. */ typedef struct SecondOrderLowPass Butterworth2LowPass; @@ -204,6 +273,45 @@ static inline float get_butterworth_2_low_pass(Butterworth2LowPass * filter) { return filter->o[0]; } +/** Second order Butterworth low pass filter(fixed point version). + */ +typedef struct SecondOrderLowPass_int Butterworth2LowPass_int; + +/** Init a second order Butterworth filter. + * + * based on the generic second order filter + * with Q = 0.7071 = 1/sqrt(2) + * + * http://en.wikipedia.org/wiki/Butterworth_filter + * + * @param filter second order Butterworth low pass filter structure + * @param tau time constant of the second order low pass filter + * @param sample_time sampling period of the signal + * @param value initial value of the filter + */ +static inline void init_butterworth_2_low_pass_int(Butterworth2LowPass_int* filter, float cut_off, float sample_time, int32_t value) { + init_second_order_low_pass_int((struct SecondOrderLowPass_int*)filter, cut_off, 0.7071, sample_time, value); +} + +/** Update second order Butterworth low pass filter state with a new value(fixed point version). + * + * @param filter second order Butterworth low pass filter structure + * @param value new input value of the filter + * @return new filtered value + */ +static inline int32_t update_butterworth_2_low_pass_int(Butterworth2LowPass_int* filter, int32_t value) { + return update_second_order_low_pass_int((struct SecondOrderLowPass_int*)filter, value); +} + +/** Get current value of the second order Butterworth low pass filter(fixed point version). + * + * @param filter second order Butterworth low pass filter structure + * @return current value of the filter + */ +static inline int32_t get_butterworth_2_low_pass_int(Butterworth2LowPass_int* filter) { + return filter->o[0]; +} + /** Fourth order Butterworth low pass filter. * * using two cascaded second order filters @@ -253,5 +361,54 @@ static inline float get_butterworth_4_low_pass(Butterworth4LowPass * filter) { return filter->lp2.o[0]; } +/** Fourth order Butterworth low pass filter(fixed point version). + * + * using two cascaded second order filters + */ +typedef struct { + struct SecondOrderLowPass_int lp1; + struct SecondOrderLowPass_int lp2; +} Butterworth4LowPass_int; + +/** Init a fourth order Butterworth filter(fixed point version). + * + * based on two generic second order filters + * with Q1 = 1.30651 + * and Q2 = 0.541184 + * + * http://en.wikipedia.org/wiki/Butterworth_filter + * + * @param filter fourth order Butterworth low pass filter structure + * @param tau time constant of the fourth order low pass filter + * @param sample_time sampling period of the signal + * @param value initial value of the filter + */ +static inline void init_butterworth_4_low_pass_int(Butterworth4LowPass_int * filter, float cut_off, float sample_time, int32_t value) { + init_second_order_low_pass_int(&filter->lp1, cut_off, 1.30651, sample_time, value); + init_second_order_low_pass_int(&filter->lp2, cut_off, 0.51184, sample_time, value); +} + +/** Update fourth order Butterworth low pass filter state with a new value(fixed point version). + * + * using two cascaded second order filters + * + * @param filter fourth order Butterworth low pass filter structure + * @param value new input value of the filter + * @return new filtered value + */ +static inline int32_t update_butterworth_4_low_pass_int(Butterworth4LowPass_int * filter, int32_t value) { + int32_t tmp = update_second_order_low_pass_int(&filter->lp1, value); + return update_second_order_low_pass_int(&filter->lp2, tmp); +} + +/** Get current value of the fourth order Butterworth low pass filter(fixed point version). + * + * @param filter fourth order Butterworth low pass filter structure + * @return current value of the filter + */ +static inline int32_t get_butterworth_4_low_pass_int(Butterworth4LowPass_int * filter) { + return filter->lp2.o[0]; +} + #endif diff --git a/sw/airborne/firmwares/fixedwing/autopilot.c b/sw/airborne/firmwares/fixedwing/autopilot.c index 2b2ada238f..d78c730936 100644 --- a/sw/airborne/firmwares/fixedwing/autopilot.c +++ b/sw/airborne/firmwares/fixedwing/autopilot.c @@ -33,6 +33,10 @@ #include "subsystems/nav.h" #include "generated/settings.h" +#ifdef POWER_SWITCH_GPIO +#include "mcu_periph/gpio.h" +#endif + uint8_t pprz_mode; bool_t kill_throttle; uint8_t mcu1_status; @@ -146,6 +150,10 @@ void autopilot_init(void) { gps_lost = FALSE; power_switch = FALSE; +#ifdef POWER_SWITCH_GPIO + gpio_setup_output(POWER_SWITCH_GPIO); + gpio_clear(POWER_SWITCH_GPIO); +#endif /* register some periodic message */ register_periodic_telemetry(DefaultPeriodic, "ALIVE", send_alive); diff --git a/sw/airborne/firmwares/fixedwing/autopilot.h b/sw/airborne/firmwares/fixedwing/autopilot.h index 035755af5d..42de3013ba 100644 --- a/sw/airborne/firmwares/fixedwing/autopilot.h +++ b/sw/airborne/firmwares/fixedwing/autopilot.h @@ -117,14 +117,16 @@ extern void autopilot_send_mode(void); */ extern bool_t power_switch; -#ifdef POWER_SWITCH_LED +#ifdef POWER_SWITCH_GPIO +#include "mcu_periph/gpio.h" #define autopilot_SetPowerSwitch(_x) { \ power_switch = _x; \ - if (_x) LED_ON(POWER_SWITCH_LED) else LED_OFF(POWER_SWITCH_LED); \ + if (_x) { gpio_set(POWER_SWITCH_GPIO); } \ + else { gpio_clear(POWER_SWITCH_GPIO); } \ } -#else // POWER_SWITCH_LED +#else // POWER_SWITCH_GPIO #define autopilot_SetPowerSwitch(_x) { power_switch = _x; } -#endif // POWER_SWITCH_LED +#endif // POWER_SWITCH_GPIO /* CONTROL_RATE will be removed in the next release diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c index 629ffa1210..25d1dee48f 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c @@ -309,11 +309,6 @@ inline static void h_ctl_roll_loop( void ) { // Compute errors float err = h_ctl_ref_roll_angle - stateGetNedToBodyEulers_f()->phi; struct FloatRates* body_rate = stateGetBodyRates_f(); -#ifdef SITL - static float last_err = 0; - body_rate->p = (err - last_err)/(1/60.); // FIXME should be done in ahrs sim - last_err = err; -#endif float d_err = h_ctl_ref_roll_rate - body_rate->p; if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) { diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index b1836658f9..08e5ad742c 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -39,7 +39,10 @@ #include "firmwares/rotorcraft/navigation.h" #include "firmwares/rotorcraft/guidance.h" #include "firmwares/rotorcraft/stabilization.h" -#include "led.h" + +#ifdef POWER_SWITCH_GPIO +#include "mcu_periph/gpio.h" +#endif uint8_t autopilot_mode; uint8_t autopilot_mode_auto2; @@ -116,9 +119,9 @@ static void send_alive(void) { static void send_status(void) { uint32_t imu_nb_err = 0; #if USE_MOTOR_MIXING - uint8_t _blmc_nb_err = motor_mixing.nb_failure; + uint8_t _motor_nb_err = motor_mixing.nb_saturation + motor_mixing.nb_failure * 10; #else - uint8_t _blmc_nb_err = 0; + uint8_t _motor_nb_err = 0; #endif #if USE_GPS uint8_t fix = gps.fix; @@ -127,7 +130,7 @@ static void send_status(void) { #endif uint16_t time_sec = sys_time.nb_sec; DOWNLINK_SEND_ROTORCRAFT_STATUS(DefaultChannel, DefaultDevice, - &imu_nb_err, &_blmc_nb_err, + &imu_nb_err, &_motor_nb_err, &radio_control.status, &radio_control.frame_rate, &fix, &autopilot_mode, &autopilot_in_flight, &autopilot_motors_on, @@ -217,8 +220,9 @@ void autopilot_init(void) { autopilot_flight_time = 0; autopilot_rc = TRUE; autopilot_power_switch = FALSE; -#ifdef POWER_SWITCH_LED - LED_ON(POWER_SWITCH_LED); // POWER OFF +#ifdef POWER_SWITCH_GPIO + gpio_setup_output(POWER_SWITCH_GPIO); + gpio_clear(POWER_SWITCH_GPIO); // POWER OFF #endif autopilot_arming_init(); diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h index 9203c6e43f..fa60e65afd 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot.h @@ -106,15 +106,16 @@ extern uint16_t autopilot_flight_time; autopilot_set_motors_on(TRUE); \ } -#ifdef POWER_SWITCH_LED -#define autopilot_SetPowerSwitch(_v) { \ - autopilot_power_switch = _v; \ - if (_v) { LED_OFF(POWER_SWITCH_LED); } \ - else { LED_ON(POWER_SWITCH_LED); } \ +#ifdef POWER_SWITCH_GPIO +#include "mcu_periph/gpio.h" +#define autopilot_SetPowerSwitch(_v) { \ + autopilot_power_switch = _v; \ + if (_v) { gpio_set(POWER_SWITCH_GPIO); } \ + else { gpio_clear(POWER_SWITCH_GPIO); } \ } #else -#define autopilot_SetPowerSwitch(_v) { \ - autopilot_power_switch = _v; \ +#define autopilot_SetPowerSwitch(_v) { \ + autopilot_power_switch = _v; \ } #endif diff --git a/sw/airborne/firmwares/rotorcraft/datalink.c b/sw/airborne/firmwares/rotorcraft/datalink.c index 96001f4198..928a35492e 100644 --- a/sw/airborne/firmwares/rotorcraft/datalink.c +++ b/sw/airborne/firmwares/rotorcraft/datalink.c @@ -46,10 +46,15 @@ #include "subsystems/radio_control.h" #endif +#if defined GPS_DATALINK +#include "subsystems/gps/gps_datalink.h" +#endif + #include "firmwares/rotorcraft/navigation.h" #include "math/pprz_geodetic_int.h" #include "state.h" +#include "led.h" #define IdOfMsg(x) (x[1]) @@ -134,6 +139,28 @@ void dl_parse_msg(void) { DL_RC_4CH_yaw(dl_buffer)); break; #endif // RADIO_CONTROL_TYPE_DATALINK +#if defined GPS_DATALINK + case DL_REMOTE_GPS : + // Check if the GPS is for this AC + if (DL_REMOTE_GPS_ac_id(dl_buffer) != AC_ID) break; + + // Parse the GPS + parse_gps_datalink( + DL_REMOTE_GPS_numsv(dl_buffer), + DL_REMOTE_GPS_ecef_x(dl_buffer), + DL_REMOTE_GPS_ecef_y(dl_buffer), + DL_REMOTE_GPS_ecef_z(dl_buffer), + DL_REMOTE_GPS_lat(dl_buffer), + DL_REMOTE_GPS_lon(dl_buffer), + DL_REMOTE_GPS_alt(dl_buffer), + DL_REMOTE_GPS_hmsl(dl_buffer), + DL_REMOTE_GPS_ecef_xd(dl_buffer), + DL_REMOTE_GPS_ecef_yd(dl_buffer), + DL_REMOTE_GPS_ecef_zd(dl_buffer), + DL_REMOTE_GPS_tow(dl_buffer), + DL_REMOTE_GPS_course(dl_buffer)); + break; +#endif default: break; } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h index 4f43278139..bafea46816 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h @@ -112,4 +112,8 @@ extern void guidance_h_run(bool_t in_flight); guidance_h_use_ref = _val && GUIDANCE_H_USE_REF; \ } +#define guidance_h_SetMaxSpeed(_val) { \ + gh_set_max_speed(_val); \ + } + #endif /* GUIDANCE_H_H */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c index fb8be1fa67..fccf2df444 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c @@ -50,9 +50,13 @@ struct Int64Vect2 gh_pos_ref; static const int32_t gh_max_accel = BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, GH_ACCEL_REF_FRAC); -/** @todo GH_MAX_SPEED must be limited to 2^14 to avoid overflow */ +float gh_max_speed = GUIDANCE_H_REF_MAX_SPEED; + #define GH_MAX_SPEED_REF_FRAC 7 -static const int32_t gh_max_speed = BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED, GH_MAX_SPEED_REF_FRAC); +/** gh_max_speed in fixed point representation with #GH_MAX_SPEED_REF_FRAC + * must be limited to 2^14 to avoid overflow + */ +static int32_t gh_max_speed_int = BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED, GH_MAX_SPEED_REF_FRAC); /** second order model natural frequency */ #ifndef GUIDANCE_H_REF_OMEGA @@ -88,6 +92,14 @@ static void gh_compute_ref_max_speed(struct Int32Vect2* ref_vector); static void gh_saturate_ref_accel(void); static void gh_saturate_ref_speed(void); + +float gh_set_max_speed(float max_speed) { + /* limit to 100m/s as int version would overflow at 2^14 = 128 m/s */ + gh_max_speed = Min(fabs(max_speed), 100.0f); + gh_max_speed_int = BFP_OF_REAL(gh_max_speed, GH_MAX_SPEED_REF_FRAC); + return gh_max_speed; +} + void gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel) { struct Int64Vect2 new_pos; new_pos.x = ((int64_t)pos.x)<<(GH_POS_REF_FRAC - INT32_POS_FRAC); @@ -174,9 +186,9 @@ static void gh_compute_ref_max(struct Int32Vect2* ref_vector) { /* Compute maximum acceleration*/ gh_max_accel_ref.x = INT_MULT_RSHIFT(gh_max_accel, c_route_ref, INT32_TRIG_FRAC); gh_max_accel_ref.y = INT_MULT_RSHIFT(gh_max_accel, s_route_ref, INT32_TRIG_FRAC); - /* Compute maximum speed*/ - gh_max_speed_ref.x = INT_MULT_RSHIFT(gh_max_speed, c_route_ref, INT32_TRIG_FRAC); - gh_max_speed_ref.y = INT_MULT_RSHIFT(gh_max_speed, s_route_ref, INT32_TRIG_FRAC); + /* Compute maximum reference x/y velocity from absolute max_speed */ + gh_max_speed_ref.x = INT_MULT_RSHIFT(gh_max_speed_int, c_route_ref, INT32_TRIG_FRAC); + gh_max_speed_ref.y = INT_MULT_RSHIFT(gh_max_speed_int, s_route_ref, INT32_TRIG_FRAC); /* restore gh_speed_ref range (Q14.17) */ INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC)); } @@ -204,9 +216,9 @@ static void gh_compute_ref_max_speed(struct Int32Vect2* ref_vector) { } else { gh_compute_route_ref(ref_vector); - /* Compute maximum speed*/ - gh_max_speed_ref.x = INT_MULT_RSHIFT(gh_max_speed, c_route_ref, INT32_TRIG_FRAC); - gh_max_speed_ref.y = INT_MULT_RSHIFT(gh_max_speed, s_route_ref, INT32_TRIG_FRAC); + /* Compute maximum reference x/y velocity from absolute max_speed */ + gh_max_speed_ref.x = INT_MULT_RSHIFT(gh_max_speed_int, c_route_ref, INT32_TRIG_FRAC); + gh_max_speed_ref.y = INT_MULT_RSHIFT(gh_max_speed_int, s_route_ref, INT32_TRIG_FRAC); /* restore gh_speed_ref range (Q14.17) */ INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC)); } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h index 0c533cb6e8..76dbb25b64 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h @@ -32,11 +32,16 @@ #include "math/pprz_algebra_int.h" #include "generated/airframe.h" -/** Speed saturation */ +/** Default speed saturation */ #ifndef GUIDANCE_H_REF_MAX_SPEED #define GUIDANCE_H_REF_MAX_SPEED 5. #endif +/** Current maximum speed for waypoint navigation. + * Defaults to #GUIDANCE_H_REF_MAX_SPEED + */ +extern float gh_max_speed; + /** Accel saturation. * tanf(RadOfDeg(30.))*9.81 = 5.66 */ @@ -76,4 +81,11 @@ extern void gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct In extern void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp); extern void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp); +/** + * Set a new maximum speed for waypoint navigation. + * @param max_speed speed saturation in m/s + * @return new maximum speed + */ +extern float gh_set_max_speed(float max_speed); + #endif /* GUIDANCE_H_REF_H */ diff --git a/sw/airborne/firmwares/vor/Makefile b/sw/airborne/firmwares/vor/Makefile deleted file mode 100644 index d55b7cd51d..0000000000 --- a/sw/airborne/firmwares/vor/Makefile +++ /dev/null @@ -1,34 +0,0 @@ -# Quiet compilation -Q=@ - -all: - -CFLAGS = -Wall -I.. -LDFLAGS = -lm - -i86_vor_test_float_demod: i86_vor_test_float_demod.c vor_float_demod.c - gcc $(CFLAGS) $^ -o $@ $(LDFLAGS) -lsndfile - -i86_vor_test_int_demod_decim: i86_vor_test_int_demod_decim.c vor_int_demod_decim.c - gcc $(CFLAGS) $^ -o $@ $(LDFLAGS) -lsndfile - -i86_vor_test_int_demod: i86_vor_test_int_demod.c vor_int_demod.c - gcc $(CFLAGS) $^ -o $@ $(LDFLAGS) -lsndfile - -i86_vor_test_filters: i86_vor_test_filters.c - gcc $(CFLAGS) $^ -o $@ $(LDFLAGS) -lsndfile - -vor_filter_params.c: - scilab -nw -nogui -nwni -f gen_filter_params.sce - -play_vor: play_vor.c - gcc $(CFLAGS) $^ -o $@ $(LDFLAGS) -lsndfile -lasound - -play_audio: sndfile-play.c - gcc $(CFLAGS) $^ -o $@ $(LDFLAGS) -lsndfile -lasound - -clean: - $(Q)rm -f i86_vor_test_float_demod \ - i86_vor_test_int_demod \ - i86_vor_test_filters \ - *~ \#* diff --git a/sw/airborne/firmwares/vor/display.sce b/sw/airborne/firmwares/vor/display.sce deleted file mode 100644 index d5a7cbc6c2..0000000000 --- a/sw/airborne/firmwares/vor/display.sce +++ /dev/null @@ -1,20 +0,0 @@ -clear(); -M = fscanfMat('output.log'); -time = M(:,1); -in_float = M(:,2); -out_float = M(:,3); -in_int = M(:,4); -out_int = M(:,5); -out_int_f = M(:,6); - -clf(); -drawlater -subplot(2,1,1) -plot2d(time, in_float, 2); -plot2d(time, out_float, 3); -plot2d(time, out_int_f, 5); - -subplot(2,1,2) -plot2d(time, in_int, 2); -plot2d(time, out_int, 3); -drawnow \ No newline at end of file diff --git a/sw/airborne/firmwares/vor/i86_vor_audio.h b/sw/airborne/firmwares/vor/i86_vor_audio.h deleted file mode 100644 index 7ef1c5be59..0000000000 --- a/sw/airborne/firmwares/vor/i86_vor_audio.h +++ /dev/null @@ -1,36 +0,0 @@ -#ifndef I86_VOR_AUDIO_H -#define I86_VOR_AUDIO_H - -#include -#include -#include - -static short* wav_buf; -static float* float_buf; -static uint16_t* adc_buf; -unsigned nb_samples; - -static void vor_audio_read_wav(const char* filename) { - - SNDFILE *sndfile ; - SF_INFO sfinfo ; - - sndfile = sf_open (filename, SFM_READ, &sfinfo); - nb_samples = sfinfo.frames; - wav_buf = malloc(sizeof(short)*nb_samples); - sf_read_short (sndfile, wav_buf, nb_samples); - - float_buf = malloc(sizeof(float)*nb_samples); - adc_buf = malloc(sizeof(uint16_t)*nb_samples); - - int i; - for (i=0; i -#include -#include -#include -#include - -#include "i86_vor_audio.h" - -#include "vor_int_filters.h" -#include "vor_float_filters.h" - - -int main(int argc, char** argv) { - - vor_audio_read_wav("signal_VOR_BF_50_200dB.wav"); - - int i; - float te = 1/29880.; - - for (i=0; i> 24) & 0x07; - vor_adc_sample = (uint16_t)(tmp >> 6) & 0x03FF; - if (vor_adc_sample_available) { - vor_adc_overrun++; - LED_ON(1); - } - vor_adc_sample_available = TRUE; - - /* trigger next convertion */ - T0MR1 += PERIODIC_TASK_PERIOD; - /* lower clock */ - T0EMR &= ~TEMR_EM1; - - VICVectAddr = 0x00000000; // clear this interrupt from the VIC - ISR_EXIT(); // recover registers and return -} diff --git a/sw/airborne/firmwares/vor/lpc_vor_convertions.h b/sw/airborne/firmwares/vor/lpc_vor_convertions.h deleted file mode 100644 index 6b1e967c35..0000000000 --- a/sw/airborne/firmwares/vor/lpc_vor_convertions.h +++ /dev/null @@ -1,29 +0,0 @@ -#ifndef LPC_VOR_CONVERTIONS_H -#define LPC_VOR_CONVERTIONS_H - -#include "LPC21xx.h" - -#include "std.h" - - -extern volatile uint16_t vor_adc_sample; -extern volatile bool_t vor_adc_sample_available; -extern volatile uint32_t vor_adc_overrun; - -// DAC on P0.25 -#define VorDacInit() { \ - /* turn on DAC pins */ \ - PINSEL1 &= 1 << 19; \ - PINSEL1 |= ~(1 << 18); \ - } - - -#define VorDacSet(a) { \ - DACR = a << 6; \ - } - - -extern void vor_adc_init( void ); - -#endif /* LPC_VOR_CONVERTIONS_H */ - diff --git a/sw/airborne/firmwares/vor/lpc_vor_main.c b/sw/airborne/firmwares/vor/lpc_vor_main.c deleted file mode 100644 index acbd00c390..0000000000 --- a/sw/airborne/firmwares/vor/lpc_vor_main.c +++ /dev/null @@ -1,101 +0,0 @@ -#include "std.h" -#include "mcu.h" -#include "mcu_periph/sys_time.h" -#include "led.h" -#include "interrupt_hw.h" -#include "mcu_periph/uart.h" -#include "subsystems/datalink/uart_print.h" -//#include "messages.h" -//#include "subsystems/datalink/downlink.h" - -#include "lpc_vor_convertions.h" -#include "vor_int_demod_decim.h" - - -static inline void main_init( void ); -static inline void main_report( void ); - -int main( void ) { - main_init(); - - while(1) { - if (vor_adc_sample_available) { - int16_t off_sample = vor_adc_sample - 512; - // off_sample *= 2; - - vor_int_demod_run (off_sample); - - if (vid_qdr_available) { - vid_qdr_available = FALSE; - main_report(); - } - - VorDacSet(vor_adc_sample); - vor_adc_sample_available = FALSE; - } - - } - return 0; -} - -/*//(vid_qdr*360.0)/(double)N_VAR_FM)49800*/ -static inline void main_report( void ) { - static uint8_t cnt = 0; - static int32_t my_qdr; - static uint8_t tmp; - - switch (cnt) { - - case 0: - my_qdr = vid_qdr * 360 / 4980; - if (my_qdr < 0) my_qdr+=3600; - if (my_qdr > 3600) my_qdr-=3600; - uart_transmit(&uart0, '\r'); - break; - case 1: - tmp = my_qdr / 1000; - my_qdr = my_qdr - 1000*tmp; - uart_transmit(&uart0, '0'+tmp); - break; - case 2: - tmp = my_qdr / 100; - my_qdr = my_qdr - 100*tmp; - uart_transmit(&uart0, '0'+tmp); - break; - case 3: - tmp = my_qdr / 10; - my_qdr = my_qdr - 10*tmp; - uart_transmit(&uart0, '0'+tmp); - break; - case 4: - uart_transmit(&uart0, '.'); - break; - case 5: - uart_transmit(&uart0, '0'+my_qdr); - break; - case 6: - uart_transmit(&uart0, '\r'); - break; - } - - cnt++; - if (cnt >7) cnt = 0; -} - - - - -static inline void main_init( void ) { - mcu_init(); - sys_time_init(); - led_init(); - uart0_init(); - vor_int_demod_init(); - VorDacInit(); - vor_adc_init(); - mcu_int_enable(); -} - - - - diff --git a/sw/airborne/firmwares/vor/sci_gen_c_filters.sce b/sw/airborne/firmwares/vor/sci_gen_c_filters.sce deleted file mode 100644 index 17326a5410..0000000000 --- a/sw/airborne/firmwares/vor/sci_gen_c_filters.sce +++ /dev/null @@ -1,73 +0,0 @@ -// -// -// Generates C file and corresponding headers for filters used in -// C implementation -// - -clear(); - -exec('sci_vor_filters.sci'); -[filters] = vor_get_filters(); - -filter_name = [ "BP_VAR" - "BP_REF" - "LP_DECIM" - "LP_VAR" - "LP_REF" - "LP_FM" ]; - -function print_poly(fid, name, p, header) - c_p = coeff(p); - l_p = length(c_p); - l_ps = name+'_LEN'; - if (header) - fprintf(fid, '#define %s %i\n', l_ps, l_p); - fprintf(fid, 'extern FLOAT_T %s[];\n', name); - else - fprintf(fid, 'FLOAT_T %s[%s]={\n', name, l_ps); - for i=l_p:-1:1 - fprintf(fid,'%10.40f,\n',c_p(i)); - end - fprintf(fid,'};\n'); - end -endfunction - -// -// header -// - -filename = 'vor_filter_params'; -fid = mopen(filename+'.h', 'w'); - -fprintf(fid,'#ifndef VOR_LF_FILTER_PARAMS_H\n'); -fprintf(fid,'#define VOR_LF_FILTER_PARAMS_H\n\n'); - -for i=1:FILTER_NB - f = filters(i); - fn = filter_name(i); - fprintf(fid,'/* %s filter */\n', fn); - print_poly(fid, fn+"_NUM", f.tf.num, 1); - print_poly(fid, fn+"_DEN", f.tf.den, 1); - fprintf(fid,'\n'); -end - -fprintf(fid,'#endif /* VOR_LF_FILTER_PARAMS_H */\n'); -mclose(fid); - -// -// C -// - -fid = mopen(filename+'.c', 'w'); - fprintf(fid,'#include ""%s.h""\n\n', filename); -for i=1:FILTER_NB - f = filters(i); - fn = filter_name(i); - print_poly(fid, fn+"_NUM", f.tf.num, 0); - print_poly(fid, fn+"_DEN", f.tf.den, 0); - fprintf(fid,'\n'); -end - -mclose(fid); - - diff --git a/sw/airborne/firmwares/vor/sci_vor_audio.sci b/sw/airborne/firmwares/vor/sci_vor_audio.sci deleted file mode 100644 index 7a315368fb..0000000000 --- a/sw/airborne/firmwares/vor/sci_vor_audio.sci +++ /dev/null @@ -1,39 +0,0 @@ -// -// $Id$ -// -// Copyright (C) 2008 Antoine Blais, Antoine Drouin -// -// 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. -// - -// -// Digital VOR ( VHF Omni-directional Radio Range ) receiver -// audio files handling -// - - -// Load input signal from wav file ( 16 bits mono ) -function [time, samples] = vor_audio_read_wav(filename) - - [samples, Fe, nbits] = wavread(filename); - samples = samples * 2^6; - Te = 1/Fe; - time = [0:length(samples)-1] * Te; - -endfunction - diff --git a/sw/airborne/firmwares/vor/sci_vor_demod.sce b/sw/airborne/firmwares/vor/sci_vor_demod.sce deleted file mode 100644 index 63a2f4d7ae..0000000000 --- a/sw/airborne/firmwares/vor/sci_vor_demod.sce +++ /dev/null @@ -1,116 +0,0 @@ -// -// $Id$ -// -// Copyright (C) 2008 Antoine Blais, Antoine Drouin -// -// 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. -// - -// -// Digital VOR ( VHF Omni-directional Radio Range ) receiver -// demodulator mainloop -// - -clear(); -//xdel(winsid()); - -exec('sci_vor_audio.sci'); -[time, in_sig] = vor_audio_read_wav('signal_VOR_BF_50_200dB.wav'); - -exec('sci_vor_filters.sci'); -[filters] = vor_get_filters(); - -// our 3 PLLs variables - -// 30 REF -freq_REF = vor_F0;// oscillator base frequency -phi_REF = %pi; // oscillator phase -err_REF = 0; // phase error -alpha_REF = -1.2; // error re-injection coeeficient. - -// 30 VAR -freq_VAR = vor_Fvor; -phi_VAR = %pi; -err_VAR = 0; -alpha_VAR = -0.5; - -// DEMODULATED 30 REF -freq_FM = vor_Fvor; -phi_FM = %pi; -err_FM = 0; -alpha_FM = -1; - - -// display frequency -f_display = 2.; -n_affich = round(vor_Fe / f_display); - -//-----------------------------------------------------------------// -// PLL - called at FE -for i=1:length(time); - - ti = time(i); - - phi_REF = phi_REF - alpha_REF * err_REF; // phase error re-injection - phaseREF = 2 * %pi * freq_REF * ti + phi_REF; // local oscillator phase - loREF = sin(phaseREF); // local oscillator signal - - // get REF signal by bandpassing input signal - [signal_REF, filters(BP_REF)] = vor_filter_run(filters(BP_REF), in_sig(i)); - - // multiply REF signal by local oscillator signal - yREF = signal_REF * loREF; - - // get phase error by low passing the result of the multiplication. - [err_REF, filters(LP_REF)] = vor_filter_run(filters(LP_REF), yREF); - - // filter 30 REF before decimating it - [eREFdecim, filters(LP_DECIM)] = vor_filter_run(filters(LP_DECIM), err_REF); - - // get VAR signal by bandpassing input signal - [signal_VAR, filters(BP_VAR)] = vor_filter_run(filters(BP_VAR), in_sig(i)); - - if (modulo(i, vor_decim_factor) == 1) - - phi_VAR = phi_VAR - alpha_VAR * err_VAR; // phase error re-injection - phaseVAR = 2 * %pi * freq_VAR * ti + phi_VAR; // local oscillator phase - loVAR = -sin(phaseVAR); // Local carrier - yVAR = signal_VAR * loVAR; - - [err_VAR, filters(LP_VAR)] = vor_filter_run(filters(LP_VAR), yVAR); - - phi_FM = phi_FM - alpha_FM * err_FM; // phase error re-injection - phaseFM = 2 * %pi * freq_FM * ti + phi_FM; // local oscillator phase - loFM = -sin(phaseFM); // Local carrier - yFM = eREFdecim * loFM; - - [err_FM, filters(LP_FM)] = vor_filter_run(filters(LP_FM), yFM); - - end - - - if (modulo(i, n_affich) == 0) - leVAR = pmodulo(phi_VAR*180/%pi - filters(BP_VAR).Dphi,360); - leREF = pmodulo(phi_FM*180/%pi - filters(LP_REF).Dphi - filters(BP_REF).Dphi - filters(BP_VAR).Dphi,360); - leQDR = pmodulo((phi_VAR-phi_FM)*180/%pi + filters(LP_REF).Dphi + filters(BP_REF).Dphi,360); - mprintf("Phase variable %2.2f",leVAR); - mprintf(" Phase reference %2.2f",leREF); - mprintf(" QDR %2.2f",leQDR);disp(""); - end - -end diff --git a/sw/airborne/firmwares/vor/sci_vor_filters.sci b/sw/airborne/firmwares/vor/sci_vor_filters.sci deleted file mode 100644 index 6b9769c7c7..0000000000 --- a/sw/airborne/firmwares/vor/sci_vor_filters.sci +++ /dev/null @@ -1,107 +0,0 @@ -// -// $Id$ -// -// Copyright (C) 2008 Antoine Blais, Antoine Drouin -// -// 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. -// - -// -// Digital VOR ( VHF Omni-directional Radio Range ) receiver -// filters stuff -// - -// VOR signal parameters -vor_Fvor = 30; -vor_F0 = 9960; -// Frequency modulation ratio of 30 REF -vor_n = 16; -// Frequency excursion produced by 30 REF -vor_Df = vor_n * vor_Fvor; -// Sampling frequency -vor_Fe = 3. * vor_F0; -// decimation factor of 30 VAR and demodulated 30 REF (FM) -vor_decim_factor = 83*3; - -BP_VAR = 1; -BP_REF = 2; -LP_DECIM = 3; -LP_VAR = 4; -LP_REF = 5; -LP_FM = 6; -FILTER_NB = 6; - -function [filters] = vor_get_filters() - - vor_filter_format = ['vor_filter';'tf'; 'ss'; 'state'; 'Dphi']; - - - _tf = iir(3,'lp','butt',[800/vor_Fe 0],[0 0]); - _ss = tf2ss(_tf); - [foo, state0] = flts(0, _ss); - [_dB, _Dphi] = dbphi(repfreq(_tf, vor_Fvor/vor_Fe)); - tl_BP_VAR = tlist(vor_filter_format, _tf, _ss, state0, _Dphi); - - - fc1 = (vor_F0-vor_Df)/vor_Fe*0.9; - fc2 = (vor_F0+vor_Df)/vor_Fe*1.1; - _tf = iir(3,'bp','butt',[fc1 fc2],[0 0]); - _ss = tf2ss(_tf); - [foo, state0] = flts(0, _ss); - [_dB, _Dphi] = dbphi(repfreq(_tf,-0.5,vor_F0 / vor_Fe)); - _Dphi = modulo(_Dphi($) / vor_F0 * vor_Fvor,360); - tl_BP_REF = tlist(vor_filter_format, _tf, _ss, state0, _Dphi); - - - _tf = iir(3,'lp','butt',[800/vor_Fe 0],[0 0]); - _ss = tf2ss(_tf); - [foo, state0] = flts(0, _ss); - _Dphi = 0.; - tl_LP_DECIM = tlist(vor_filter_format, _tf, _ss, state0, _Dphi); - - - _tf = iir(6,'lp','butt',[10/vor_Fe*vor_decim_factor 0],[0 0]); - _ss = tf2ss(_tf); - [foo, state0] = flts(0, _ss); - _Dphi = 0.; - tl_LP_VAR = tlist(vor_filter_format, _tf, _ss, state0, _Dphi); - - - _tf = iir(3,'lp','butt',[3000/vor_Fe 0],[0 0]); - _ss = tf2ss(_tf); - [foo, state0] = flts(0, _ss); - [_dB,_Dphi] = dbphi(repfreq(_tf, vor_Fvor / vor_Fe)); - tl_LP_REF = tlist(vor_filter_format, _tf, _ss, state0, _Dphi); - - - _tf = iir(6,'lp','butt',[10/vor_Fe*vor_decim_factor 0],[0 0]); - _ss = tf2ss(_tf); - [foo, state0] = flts(0, _ss); - _Dphi = 0.; - tl_LP_FM = tlist(vor_filter_format, _tf, _ss, state0, _Dphi); - - - filters = list(tl_BP_VAR, tl_BP_REF, tl_LP_DECIM, tl_LP_VAR, tl_LP_REF, tl_LP_FM); - -endfunction - - -function [out, filter] = vor_filter_run(filter, in) - [out, filter.state] = flts(in, filter.ss, filter.state); -endfunction - diff --git a/sw/airborne/firmwares/vor/signal_VOR_BF_50_200dB.wav b/sw/airborne/firmwares/vor/signal_VOR_BF_50_200dB.wav deleted file mode 100644 index 2ac0605fa6..0000000000 Binary files a/sw/airborne/firmwares/vor/signal_VOR_BF_50_200dB.wav and /dev/null differ diff --git a/sw/airborne/firmwares/vor/vor_float_demod.c b/sw/airborne/firmwares/vor/vor_float_demod.c deleted file mode 100644 index b12303ff5c..0000000000 --- a/sw/airborne/firmwares/vor/vor_float_demod.c +++ /dev/null @@ -1,99 +0,0 @@ -#include "vor_float_demod.h" - -#include -#include - -#include "vor_float_filters.h" - -const float vfd_te = 1./29880.; - -const float vfd_ref_freq = 9960.; - float vfd_ref_phi; - float vfd_ref_err; -const float vfd_ref_alpha = -1.2; - -const float vfd_var_freq = 30.0; - float vfd_var_phi; - float vfd_var_err; -const float vfd_var_alpha = -0.5; - -const float vfd_fm_freq = 30.0; - float vfd_fm_phi; - float vfd_fm_err; -const float vfd_fm_alpha = -1.0; - - float vfd_qdr; - - float vfd_var_sig; - float vfd_fm_local_sig; - -static uint32_t i; -static uint32_t decim; -static uint32_t vfd_DECIM = 3 * 83; - -void vor_float_demod_init( void) { - - i = 0; - - vfd_ref_phi = M_PI; - vfd_ref_err = 0.; - - vfd_var_phi = M_PI; - vfd_var_err = 0.; - - vfd_fm_phi = M_PI; - vfd_fm_err = 0.; - -} - -void vor_float_demod_run ( float sample) { - - const float ti = i * vfd_te; - - // phase error re-injection - vfd_ref_phi -= vfd_ref_alpha * vfd_ref_err; - // local oscillator phase - const float vfd_ref_phase = 2. * M_PI * vfd_ref_freq * ti + vfd_ref_phi; - // local oscillator signal - const float vfd_ref_local_sig = sin(vfd_ref_phase); - - // get REF signal by bandpassing input signal - const float vfd_ref_sig = vor_float_filter_bp_ref(sample); - - // multiply input signal by local oscillator signal - const float vfd_ref_y = vfd_ref_sig * vfd_ref_local_sig; - - // get phase error by low passing the result of the multiplication. - vfd_ref_err = vor_float_filter_lp_ref(vfd_ref_y); - - // filter 30 REF before decimating it - const float vfd_ref_err_decim = vor_float_filter_lp_decim(vfd_ref_err); - - // get VAR signal by bandpassing input signal - // const float vfd_var_sig = vor_float_filter_bp_var(sample); - vfd_var_sig = vor_float_filter_bp_var(sample); - - if (decim >= vfd_DECIM) { - decim = 0; - - vfd_var_phi -= vfd_var_alpha * vfd_var_err; - const float vfd_var_phase = 2. * M_PI * vfd_var_freq * ti + vfd_var_phi; - const float vfd_var_local_sig = -sin( vfd_var_phase); - const float vfd_var_y = vfd_var_sig * vfd_var_local_sig; - vfd_var_err = vor_float_filter_lp_var(vfd_var_y); - - vfd_fm_phi -= vfd_fm_alpha * vfd_fm_err; - const float vfd_fm_phase = 2. * M_PI * vfd_fm_freq * ti + vfd_fm_phi; - // const float vfd_fm_local_sig = -sin( vfd_fm_phase ); - vfd_fm_local_sig = -sin( vfd_fm_phase ); - const float vfd_fm_y = vfd_ref_err_decim * vfd_fm_local_sig; - vfd_fm_err = vor_float_filter_lp_fm(vfd_fm_y); - - vfd_qdr = vfd_var_phi - vfd_fm_phi; - } - - - i++; - decim++; - -} diff --git a/sw/airborne/firmwares/vor/vor_float_filters.h b/sw/airborne/firmwares/vor/vor_float_filters.h deleted file mode 100644 index 19cb21ccac..0000000000 --- a/sw/airborne/firmwares/vor/vor_float_filters.h +++ /dev/null @@ -1,310 +0,0 @@ -#ifndef VOR_FLOAT_FILTERS_H -#define VOR_FLOAT_FILTERS_H - -//typedef float (*vor_float_filter_fun)( float xn); - -inline float vor_float_filter_bp_var( float xn) { - - const float a0 = 0.0000294918571461433008684162315748977790; - const float a1 = 0.0000884755714384299093815122727590960494; - const float a2 = 0.0000884755714384299093815122727590960494; - const float a3 = 0.0000294918571461433008684162315748977790; - - const float b1 = -2.8738524677701420273479016032069921493530; - const float b2 = 2.7555363566291544152875303552718833088875; - const float b3 = -0.8814479540018430592240861187747213989496; - - static float x1 = 0; - static float x2 = 0; - static float x3 = 0; - - static float y1 = 0; - static float y2 = 0; - static float y3 = 0; - - float yn = a0 * xn - + a1 * x1 - + a2 * x2 - + a3 * x3 - - b1 * y1 - - b2 * y2 - - b3 * y3; - - x3 = x2; - x2 = x1; - x1 = xn; - - y3 = y2; - y2 = y1; - y1 = yn; - - return yn; -} - -inline float vor_float_filter_bp_ref( float xn) { - - const float a0 = 0.0175489156490784836694984960558940656483; - // const float a1 = 0.0000000000000000000000000000000000000000; - const float a2 = -0.0526467469472354510084954881676821969450; - // const float a3 = 0.0000000000000000000000000000000000000000; - const float a4 = 0.0526467469472354510084954881676821969450; - // const float a5 = 0.0000000000000000000000000000000000000000; - const float a6 = -0.0175489156490784836694984960558940656483; - - const float b1 = 2.5508195725874163173330089193768799304962; - const float b2 = 3.9857308274503626677187639870680868625641; - const float b3 = 3.8245798569419009460546021728077903389931; - const float b4 = 2.6296760724926846464200025366153568029404; - const float b5 = 1.0926715614934312537087635064381174743176; - const float b6 = 0.2825578543465128156242371915141120553017; - - static float x1 = 0; - static float x2 = 0; - static float x3 = 0; - static float x4 = 0; - static float x5 = 0; - static float x6 = 0; - - static float y1 = 0; - static float y2 = 0; - static float y3 = 0; - static float y4 = 0; - static float y5 = 0; - static float y6 = 0; - - float yn = a0 * xn - // + a1 * x1 - + a2 * x2 - // + a3 * x3 - + a4 * x4 - // + a5 * x5 - + a6 * x6 - - b1 * y1 - - b2 * y2 - - b3 * y3 - - b4 * y4 - - b5 * y5 - - b6 * y6; - - x6 = x5; - x5 = x4; - x4 = x3; - x3 = x2; - x2 = x1; - x1 = xn; - - y6 = y5; - y5 = y4; - y4 = y3; - y3 = y2; - y2 = y1; - y1 = yn; - - return yn; -} - - -inline float vor_float_filter_lp_decim( float xn) { - - const float a0 = 0.0000294918571461433008684162315748977790; - const float a1 = 0.0000884755714384299093815122727590960494; - const float a2 = 0.0000884755714384299093815122727590960494; - const float a3 = 0.0000294918571461433008684162315748977790; - - const float b1 = -2.8738524677701420273479016032069921493530; - const float b2 = 2.7555363566291544152875303552718833088875; - const float b3 = -0.8814479540018430592240861187747213989496; - - static float x1 = 0.; - static float x2 = 0.; - static float x3 = 0.; - - static float y1 = 0.; - static float y2 = 0.; - static float y3 = 0.; - - float yn = a0 * xn - + a1 * x1 - + a2 * x2 - + a3 * x3 - - b1 * y1 - - b2 * y2 - - b3 * y3; - - x3 = x2; - x2 = x1; - x1 = xn; - - y3 = y2; - y2 = y1; - y1 = yn; - - return yn; -} - -inline float vor_float_filter_lp_var( float xn) { - - const float a0 = 0.0001325928962621713658003030911203268261; - const float a1 = 0.0007955573775730281948018185467219609563; - const float a2 = 0.0019888934439325706496348722396305674920; - const float a3 = 0.0026518579252434275328464963195074233226; - const float a4 = 0.0019888934439325706496348722396305674920; - const float a5 = 0.0007955573775730281948018185467219609563; - const float a6 = 0.0001325928962621713658003030911203268261; - - const float b1 = -3.9811884900947003274040980613790452480316; - const float b2 = 6.8452604202756832663112618320155888795853; - const float b3 = -6.4498289229953256196381516929250210523605; - const float b4 = 3.4951386512490887348292289971141144633293; - const float b5 = -1.0292502263335985279724127394729293882847; - const float b6 = 0.1283545132596315418993526691338047385216; - - static float x1 = 0.; - static float x2 = 0.; - static float x3 = 0.; - static float x4 = 0.; - static float x5 = 0.; - static float x6 = 0.; - - static float y1 = 0.; - static float y2 = 0.; - static float y3 = 0.; - static float y4 = 0.; - static float y5 = 0.; - static float y6 = 0.; - - float yn = a0 * xn - + a1 * x1 - + a2 * x2 - + a3 * x3 - + a4 * x4 - + a5 * x5 - + a6 * x6 - - b1 * y1 - - b2 * y2 - - b3 * y3 - - b4 * y4 - - b5 * y5 - - b6 * y6; - - x6 = x5; - x5 = x4; - x4 = x3; - x3 = x2; - x2 = x1; - x1 = xn; - - y6 = y5; - y5 = y4; - y4 = y3; - y3 = y2; - y2 = y1; - y1 = yn; - - return yn; -} - - -inline float vor_float_filter_lp_ref( float xn) { - - const float a0 = 0.0182843871571847782497854950634064152837; - const float a1 = 0.0548531614715543347493564851902192458510; - const float a2 = 0.0548531614715543347493564851902192458510; - const float a3 = 0.0182843871571847782497854950634064152837; - - const float b1 = -1.7551740553005390488294779061106964945793; - const float b2 = 1.1780240265537846866550353297498077154160; - const float b3 = -0.2765748739957675783607271569053409621119; - - static float x1 = 0.; - static float x2 = 0.; - static float x3 = 0.; - - static float y1 = 0.; - static float y2 = 0.; - static float y3 = 0.; - - float yn = a0 * xn - + a1 * x1 - + a2 * x2 - + a3 * x3 - - b1 * y1 - - b2 * y2 - - b3 * y3; - - x3 = x2; - x2 = x1; - x1 = xn; - - y3 = y2; - y2 = y1; - y1 = yn; - - return yn; -} - - -inline float vor_float_filter_lp_fm( float xn) { - - const float a0 = 0.0001325928962621713658003030911203268261; - const float a1 = 0.0007955573775730281948018185467219609563; - const float a2 = 0.0019888934439325706496348722396305674920; - const float a3 = 0.0026518579252434275328464963195074233226; - const float a4 = 0.0019888934439325706496348722396305674920; - const float a5 = 0.0007955573775730281948018185467219609563; - const float a6 = 0.0001325928962621713658003030911203268261; - - const float b1 = -3.9811884900947003274040980613790452480316; - const float b2 = 6.8452604202756832663112618320155888795853; - const float b3 = -6.4498289229953256196381516929250210523605; - const float b4 = 3.4951386512490887348292289971141144633293; - const float b5 = -1.0292502263335985279724127394729293882847; - const float b6 = 0.1283545132596315418993526691338047385216; - - static float x1 = 0.; - static float x2 = 0.; - static float x3 = 0.; - static float x4 = 0.; - static float x5 = 0.; - static float x6 = 0.; - - static float y1 = 0.; - static float y2 = 0.; - static float y3 = 0.; - static float y4 = 0.; - static float y5 = 0.; - static float y6 = 0.; - - float yn = a0 * xn - + a1 * x1 - + a2 * x2 - + a3 * x3 - + a4 * x4 - + a5 * x5 - + a6 * x6 - - b1 * y1 - - b2 * y2 - - b3 * y3 - - b4 * y4 - - b5 * y5 - - b6 * y6; - - x6 = x5; - x5 = x4; - x4 = x3; - x3 = x2; - x2 = x1; - x1 = xn; - - y6 = y5; - y5 = y4; - y4 = y3; - y3 = y2; - y2 = y1; - y1 = yn; - - return yn; - -} - -#endif /* VOR_FLOAT_FILTERS_H */ diff --git a/sw/airborne/firmwares/vor/vor_int_demod.c b/sw/airborne/firmwares/vor/vor_int_demod.c deleted file mode 100644 index 49ba9e4471..0000000000 --- a/sw/airborne/firmwares/vor/vor_int_demod.c +++ /dev/null @@ -1,188 +0,0 @@ -/* - * Copyright (C) 2008 Antoine Blais, Antoine Drouin - * - * 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. - * - */ - - -// timing -// 30khz 33 us -// processing 30Kz with sine 100 us -// processing 30Kz without sine 23-26 us -// -// total processing : 270us -// total processing without sine 54us -// - -#include "vor_int_demod.h" - -#include - -#include "vor_int_filters.h" - - -#define INT_OF_FLOAT(f,r) ((f) * (float)r) - - -#define VID_REF_DANGLE VID_ANGLE_INT_OF_PFLOAT(2. * M_PI * 9960. * 502. / 15000000.) -#define VID_VAR_DANGLE VID_ANGLE_INT_OF_PFLOAT(2. * M_PI * 30. * 502. / 15000000. * 3 * 83) -#define VID_FM_DANGLE VID_VAR_DANGLE - -#define VID_TWO_PI VID_ANGLE_INT_OF_PFLOAT(2. * M_PI) - -// Variables pour la démodulation REF - int32_t vid_ref_angle; - int32_t vid_ref_phi; - float vid_ref_local_sig_float; -const int32_t vid_ref_alpha = INT_OF_FLOAT(-1.2, 15); - int32_t vid_ref_sig; - int32_t vid_ref_err; - -// Variable pour la décimation REF -int32_t vid_ref_err_decim; - -// Variable pour la démodulation du VAR - int32_t vid_var_angle; - int32_t vid_var_phi; - float vid_var_local_sig_float; -const int32_t vid_var_alpha = INT_OF_FLOAT(-0.5, 15); - int32_t vid_var_sig; - int32_t vid_var_err; - -// Variable pour la démodulation FM - int32_t vid_fm_angle; - int32_t vid_fm_phi; - float vid_fm_local_sig_float; -const int32_t vid_fm_alpha = INT_OF_FLOAT(-1., 15); - int32_t vid_fm_sig; - int32_t vid_fm_err; - - int32_t vid_qdr; - - -int32_t vid_ref_local_sig; -int32_t vid_ref_y; - -static uint32_t i; -static uint32_t decim; -static uint32_t vid_DECIM = 3 * 83; - -void vor_int_demod_init( void) { - - i = 0; - vid_ref_angle = 0; - vid_ref_phi = VID_ANGLE_INT_OF_PFLOAT(M_PI); - vid_ref_err = 0; - -} - - -void vor_int_demod_run ( int16_t sample) { - - // phase error re-injection - int32_t dphi = vid_ref_alpha * vid_ref_err; - dphi = dphi <<3; - vid_ref_phi -= dphi ; - - // local oscillator phase - vid_ref_angle += VID_REF_DANGLE; - if (vid_ref_angle >= VID_TWO_PI) vid_ref_angle -= VID_TWO_PI; - int32_t vid_ref_phase = vid_ref_angle + vid_ref_phi; - if (vid_ref_phase >= VID_TWO_PI) vid_ref_phase -= VID_TWO_PI; - - // local oscillator signal - const float vid_ref_phase_float = VID_ANGLE_FLOAT_OF_INT(vid_ref_phase); - vid_ref_local_sig_float = sin(vid_ref_phase_float); - - // test sign ? - /// const int32_t vid_ref_local_sig = VID_ANGLE_INT_OF_PFLOAT(vid_ref_local_sig_float); - vid_ref_local_sig = vid_ref_local_sig_float * (float)(1<<15); - // get REF signal by bandpassing input signal - /// const int32_t vid_ref_sig = vor_int_filter_bp_ref(sample); - vid_ref_sig = vor_int_filter_bp_ref(sample); - // multiply input signal by local oscillator signal - // const int32_t vid_ref_y = vid_ref_sig * vid_ref_local_sig; - vid_ref_y = vid_ref_sig * vid_ref_local_sig; - vid_ref_y = vid_ref_y >> 15; - // get phase error by low passing the result of the multiplication. - vid_ref_err = vor_int_filter_lp_ref(vid_ref_y); - - // filter 30 REF before decimating it - // const int32_t vid_ref_err_decim = vor_int_filter_lp_decim(vid_ref_err); - vid_ref_err_decim = vor_int_filter_lp_decim(vid_ref_err<<3); - vid_ref_err_decim = vid_ref_err_decim >> 3; - - // get VAR signal by bandpassing input signal - vid_var_sig = vor_int_filter_bp_var(sample); - - if (1) { - // if (decim >= vid_DECIM) { - decim = 0; - - // phase error re-injection - int32_t dvar_phi = vid_var_alpha * vid_var_err; - dvar_phi = dvar_phi <<3; - vid_var_phi -= dvar_phi; - - // local var oscillator phase - vid_var_angle += VID_VAR_DANGLE; - - if (vid_var_angle >= VID_TWO_PI) vid_var_angle -= VID_TWO_PI; - int32_t vid_var_phase = vid_var_angle + vid_var_phi; - if (vid_var_phase >= VID_TWO_PI) vid_var_phase -= VID_TWO_PI; - - // local var oscillator signal - const float vid_var_phase_float = VID_ANGLE_FLOAT_OF_INT(vid_var_phase); - vid_var_local_sig_float = -sin(vid_var_phase_float); - - // FIXME - INT mult multiply input signal by local oscillator signal - const int32_t vid_var_y = vid_var_sig * vid_var_local_sig_float; - // get phase error by low passing the result of the multiplication. - vid_var_err = vor_int_filter_lp_var(vid_var_y); - - - // phase error re-injection - int32_t dfm_phi = vid_fm_alpha * vid_fm_err; - dfm_phi = dfm_phi <<3; - vid_fm_phi -= dfm_phi; - - // local var oscillator phase - vid_fm_angle += VID_FM_DANGLE; - - if (vid_fm_angle >= VID_TWO_PI) vid_fm_angle -= VID_TWO_PI; - int32_t vid_fm_phase = vid_fm_angle + vid_fm_phi; - if (vid_fm_phase >= VID_TWO_PI) vid_fm_phase -= VID_TWO_PI; - - // local var oscillator signal - const float vid_fm_phase_float = VID_ANGLE_FLOAT_OF_INT(vid_fm_phase); - vid_fm_local_sig_float = -sin(vid_fm_phase_float); - - // FIXME - INT mult multiply input signal by local oscillator signal - const int32_t vid_fm_y = vid_ref_err_decim * vid_fm_local_sig_float; - // get phase error by low passing the result of the multiplication. - vid_fm_err = vor_int_filter_lp_fm(vid_fm_y); - - vid_qdr = vid_var_phi - vid_fm_phi; - - } - - i++; - decim++; -} - diff --git a/sw/airborne/firmwares/vor/vor_int_demod.h b/sw/airborne/firmwares/vor/vor_int_demod.h deleted file mode 100644 index 78d5159327..0000000000 --- a/sw/airborne/firmwares/vor/vor_int_demod.h +++ /dev/null @@ -1,60 +0,0 @@ -/* - * Copyright (C) 2008 Antoine Blais, Antoine Drouin - * - * 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. - * - */ - -#ifndef VOR_INT_DEMOD_H -#define VOR_INT_DEMOD_H - -#include - -extern void vor_int_demod_init( void); -extern void vor_int_demod_run ( int16_t sample); - -extern int32_t vid_ref_angle; -extern int32_t vid_ref_phi; -extern int32_t vid_ref_err; -extern const int32_t vid_ref_alpha; - - -extern int32_t vid_ref_err_decim; - -extern int32_t vid_var_phi; -extern float vid_var_local_sig_float; - -extern int32_t vid_fm_phi; -extern float vid_fm_local_sig_float; - -extern int32_t vid_qdr; - -#define VID_ANGLE_RES 16 -#define VID_ANGLE_FACT (1 << VID_ANGLE_RES) -#define VID_ANGLE_INT_OF_PFLOAT(a) ((a) * (float)VID_ANGLE_FACT + 0.5) -#define VID_ANGLE_INT_OF_NFLOAT(a) ((a) * (float)VID_ANGLE_FACT - 0.5) -#define VID_ANGLE_FLOAT_OF_INT(a) ((float)(a) / (float)VID_ANGLE_FACT) - -extern float vid_ref_local_sig_float; -extern int32_t vid_ref_sig; -extern int32_t vid_ref_local_sig; -extern int32_t vid_var_sig; -extern int32_t vid_fm_local_sig; -extern int32_t vid_ref_y; - -#endif /* VOR_INT_DEMOD_H */ diff --git a/sw/airborne/firmwares/vor/vor_int_demod_decim.c b/sw/airborne/firmwares/vor/vor_int_demod_decim.c deleted file mode 100644 index 8112cc2013..0000000000 --- a/sw/airborne/firmwares/vor/vor_int_demod_decim.c +++ /dev/null @@ -1,257 +0,0 @@ -#include "vor_int_demod_decim.h" - -#include "vor_int_filters_decim.h" - -#include "lo.h" - -// Variables pour la démodulation REF -int32_t vid_ref_sig; - -int32_t vid_loREF; -int32_t vid_phaseREF; -int32_t vid_ref_err; -int32_t vid_yREF; - -#undef VIF_RES -#define VIF_RES 10 -#define VID_REF (1<<10) -const int32_t vid_alphaREF = VIF_PCOEF(0.003); -const int32_t vid_DELTA_9960 = 500*1; - -// Variable pour la décimation REF -int32_t vid_ref_err_decim1; -int32_t vid_ref_err_decim2; - -// Variable pour la démodulation du VAR -int32_t vid_var_sig; - -int32_t vid_var_err_decim1; -int32_t vid_var_err_decim2; - -int32_t vid_loVAR; -int32_t vid_phaseVAR; -int32_t vid_var_err; -int32_t vid_yVAR; -int32_t vid_cumVAR; - -#define VID_VAR (1<<5) -const int32_t vid_DELTA_30 = 27*300; - -// Variable pour la démodulation FM -int32_t vid_loFM; -int32_t vid_phaseFM; -int32_t vid_fm_err; -int32_t vid_yFM; -int32_t vid_cumFM; - -#define VID_FM (1<<5) - -// Le QDR -int32_t vid_qdr; -uint8_t vid_qdr_available; -const int32_t correcQDR = 454; // 5 - -uint32_t decim1 = 0; -#define vid_DECIM1 2 -uint32_t decim2 = 0; -#define vid_DECIM2 9 -uint32_t decim3 = 0; -#define vid_DECIM3 9 - -// Variables liées au séquencement des switchs -int32_t fix_var_sig; -int32_t fix_var_err_decim1; -int32_t fix_var_err_decim2; - -#define pllREF() { \ - int32_t vid_phiREF = -vid_alphaREF*vid_ref_err; \ - vid_phiREF /= VID_REF; \ - vid_phaseREF += vid_DELTA_9960 + vid_phiREF; \ - if (vid_phaseREF >= N_REF) { \ - vid_phaseREF -= N_REF; \ - } \ - vid_loREF = loREF[vid_phaseREF]; \ - vid_yREF = vid_ref_sig*vid_loREF; \ - vid_yREF /= VID_LO; \ - vid_ref_err = vor_int_filter_lp_ref(vid_yREF); \ - } - -#define pllVAR() { \ - vid_var_err /= VID_VAR; \ - vid_cumVAR += vid_var_err; \ - vid_phaseVAR += vid_DELTA_30 + vid_var_err; \ - if (vid_phaseVAR >= N_VAR_FM) { \ - vid_phaseVAR -= N_VAR_FM; \ - } \ - vid_loVAR = lo[vid_phaseVAR]; \ - vid_yVAR = vid_var_err_decim2*vid_loVAR; \ - vid_yVAR /= VID_LO; \ - vid_var_err = vor_int_filter_lp_var4(vid_yVAR); \ - } - -#define pllFM() { \ - vid_fm_err /= VID_FM; \ - vid_cumFM += vid_fm_err; \ - vid_phaseFM += vid_DELTA_30 + vid_fm_err; \ - if (vid_phaseFM >= N_VAR_FM) { \ - vid_phaseFM -= N_VAR_FM; \ - } \ - vid_loFM = lo[vid_phaseFM]; \ - vid_yFM = vid_ref_err_decim2*vid_loFM; \ - vid_yFM /= VID_LO; \ - vid_fm_err = vor_int_filter_lp_fm4(vid_yFM); \ - } - -#define calculQDR() { \ - vid_qdr = vid_cumVAR - vid_cumFM; \ - \ - if (vid_qdr >= N_VAR_FM) { \ - vid_qdr -= N_VAR_FM; \ - vid_cumVAR = vid_qdr; \ - vid_cumFM = 0; \ - } \ - else { \ - if (vid_qdr <= -N_VAR_FM) { \ - vid_qdr += N_VAR_FM; \ - vid_cumVAR = 0; \ - vid_cumFM = vid_qdr; \ - } \ - else { \ - vid_cumVAR = vid_qdr; \ - vid_cumFM = 0; \ - } \ - } \ - \ - vid_qdr -= correcQDR; \ - } - -void vor_int_demod_init( void) { - vid_qdr_available = FALSE; -} - -void vor_int_demod_run ( int16_t sample) { - - decim1++; - - // Le signal arrive sur 10 bits, on le met sur 16 bits - sample = sample*(1<<6); - - // get VAR signal by bandpassing input signal - vid_var_sig = vor_int_filter_bp_var(sample); - - // get REF signal by bandpassing input signal - vid_ref_sig = vor_int_filter_bp_ref(sample); - - //================================================================= - switch (decim1) { - - //--------------------------------------------------------------- - case 1 : { - - decim2++; - - pllREF() - - // filter 30 REF before decimation 2 - vid_ref_err_decim1 = vor_int_filter_lp_decim1r(vid_ref_err); - - fix_var_sig = vid_var_sig; - - break; - } - //--------------------------------------------------------------- - - //--------------------------------------------------------------- - case vid_DECIM1 : { - - decim1 = 0; - - // filter 30 VAR before decimation 2 - vid_var_err_decim1 = vor_int_filter_lp_decim1v(fix_var_sig); - - //============================================================= - switch (decim2) { - - //----------------------------------------------------------- - case (vid_DECIM2-2) : { - - decim3++; - - // filter 30 REF before decimation 3 - vid_ref_err_decim2 = - vor_int_filter_lp_decim2r(vid_ref_err_decim1); - - fix_var_err_decim1 = vid_var_err_decim1; - - break; - } - //----------------------------------------------------------- - - //----------------------------------------------------------- - case (vid_DECIM2-1) : { - - // filter 30 VAR before decimation 3 - vid_var_err_decim2 = - vor_int_filter_lp_decim2v(fix_var_err_decim1); - - break; - } - //----------------------------------------------------------- - - //----------------------------------------------------------- - case (vid_DECIM2) : { - - decim2 = 0; - - //========================================================= - switch (decim3) { - - //------------------------------------------------------- - case (vid_DECIM3-2) : { - - pllFM(); - - fix_var_err_decim2 = vid_var_err_decim2; - - break; - } - //------------------------------------------------------- - - //------------------------------------------------------- - case (vid_DECIM3-1) : { - - vid_var_err_decim2 = fix_var_err_decim2; - - pllVAR(); - - break; - } - //------------------------------------------------------- - - //------------------------------------------------------- - case vid_DECIM3 : { - - decim3 = 0; - - calculQDR(); - vid_qdr_available = TRUE; - break; - } - //------------------------------------------------------- - } - //========================================================= - - break; - } - //----------------------------------------------------------- - } - //============================================================= - - break; - } - //--------------------------------------------------------------- - } - //================================================================= - -} - diff --git a/sw/airborne/firmwares/vor/vor_int_demod_decim.h b/sw/airborne/firmwares/vor/vor_int_demod_decim.h deleted file mode 100644 index 010062be1d..0000000000 --- a/sw/airborne/firmwares/vor/vor_int_demod_decim.h +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef VOR_INT_DEMOD_DECIM_H -#define VOR_INT_DEMOD_DECIM_H - -#include -#include "std.h" - - -extern void vor_int_demod_init( void); -extern void vor_int_demod_run( int16_t sample); - -extern int32_t vid_ref_sig; - -extern int32_t vid_loREF; -extern int32_t vid_yREF; -extern int32_t vid_ref_err; -extern int32_t vid_ref_err_decim1; -extern int32_t vid_ref_err_decim2; - -extern int32_t vid_var_sig; -extern int32_t vid_var_err_decim1; -extern int32_t vid_var_err_decim2; - -extern int32_t vid_loVAR; -extern int32_t vid_yVAR; -extern int32_t vid_var_err; -extern int32_t vid_cumVAR; - -extern int32_t vid_loFM; -extern int32_t vid_yFM; -extern int32_t vid_fm_err; -extern int32_t vid_cumFM; - -extern int32_t vid_qdr; -extern uint8_t vid_qdr_available; - - -extern const int32_t N_VAR_FM; - -#endif /* VOR_INT_DEMOD_DECIM_H */ diff --git a/sw/airborne/firmwares/vor/vor_int_filters.h b/sw/airborne/firmwares/vor/vor_int_filters.h deleted file mode 100644 index 73b5c80790..0000000000 --- a/sw/airborne/firmwares/vor/vor_int_filters.h +++ /dev/null @@ -1,379 +0,0 @@ -#ifndef VOR_INT_FILTERS_H -#define VOR_INT_FILTERS_H - - -#define VIF_CRES 16 -#define VIF_CFACT (1<> VIF_CRES); - - x3 = x2; - x2 = x1; - x1 = xn; - - y3 = y2; - y2 = _y1; - _y1 = _yn; - - return (_yn >> VIF_SSCALE); -} - -inline int32_t vor_int_filter_bp_ref( int16_t xn) { - - const int32_t a0 = VIF_PCOEF( 0.0175489156490784836694984960558940656483); - // const int32_t a1 = VIF_PCOEF( 0.0000000000000000000000000000000000000000); - const int32_t a2 = VIF_NCOEF(-0.0526467469472354510084954881676821969450); - // const int32_t a3 = VIF_PCOEF( 0.0000000000000000000000000000000000000000); - const int32_t a4 = VIF_PCOEF( 0.0526467469472354510084954881676821969450); - // const int32_t a5 = VIF_PCOEF( 0.0000000000000000000000000000000000000000); - const int32_t a6 = VIF_NCOEF(-0.0175489156490784836694984960558940656483); - - const int32_t b1 = VIF_PCOEF( 2.5508195725874163173330089193768799304962); - const int32_t b2 = VIF_PCOEF( 3.9857308274503626677187639870680868625641); - const int32_t b3 = VIF_PCOEF( 3.8245798569419009460546021728077903389931); - const int32_t b4 = VIF_PCOEF( 2.6296760724926846464200025366153568029404); - const int32_t b5 = VIF_PCOEF( 1.0926715614934312537087635064381174743176); - const int32_t b6 = VIF_PCOEF( 0.2825578543465128156242371915141120553017); - - // printf("%d %d %d %d %d %d %d\n", a0, a1, a2, a3, a4, a5, a6); - // printf("%d %d %d %d %d %d\n", b1, b2, b3, b4, b5, b6); - - static int16_t x1 = 0; - static int16_t x2 = 0; - static int16_t x3 = 0; - static int16_t x4 = 0; - static int16_t x5 = 0; - static int16_t x6 = 0; - - static int32_t _y1 = 0; - static int32_t y2 = 0; - static int32_t y3 = 0; - static int32_t y4 = 0; - static int32_t y5 = 0; - static int32_t y6 = 0; - - /* scale input */ - xn = (xn << VIF_SSCALE); - - int32_t _yn = a0 * xn - // + a1 * x1 - + a2 * x2 - // + a3 * x3 - + a4 * x4 - // + a5 * x5 - + a6 * x6 - - b1 * _y1 - - b2 * y2 - - b3 * y3 - - b4 * y4 - - b5 * y5 - - b6 * y6; - // printf("%d\n", _yn); - _yn = (_yn >> VIF_CRES); - - x6 = x5; - x5 = x4; - x4 = x3; - x3 = x2; - x2 = x1; - x1 = xn; - - y6 = y5; - y5 = y4; - y4 = y3; - y3 = y2; - y2 = _y1; - _y1 = _yn; - - return (_yn >> VIF_SSCALE); -} - - - -#define VIF_CRES_t 16 -#define VIF_CFACT_t (1<> VIF_CRES); - - x3 = x2; - x2 = x1; - x1 = xn; - - y3 = y2; - y2 = _y1; - _y1 = _yn; - - return (_yn >> VIF_SSCALE); -} - - -inline int32_t vor_int_filter_lp_var( int32_t xn) { - - const int32_t a0 = VIF_PCOEF( 0.0001325928962621713658003030911203268261); - const int32_t a1 = VIF_PCOEF( 0.0007955573775730281948018185467219609563); - const int32_t a2 = VIF_PCOEF( 0.0019888934439325706496348722396305674920); - const int32_t a3 = VIF_PCOEF( 0.0026518579252434275328464963195074233226); - const int32_t a4 = VIF_PCOEF( 0.0019888934439325706496348722396305674920); - const int32_t a5 = VIF_PCOEF( 0.0007955573775730281948018185467219609563); - const int32_t a6 = VIF_PCOEF( 0.0001325928962621713658003030911203268261); - - const int32_t b1 = VIF_NCOEF(-3.9811884900947003274040980613790452480316); - const int32_t b2 = VIF_PCOEF( 6.8452604202756832663112618320155888795853); - const int32_t b3 = VIF_NCOEF(-6.4498289229953256196381516929250210523605); - const int32_t b4 = VIF_PCOEF( 3.4951386512490887348292289971141144633293); - const int32_t b5 = VIF_NCOEF(-1.0292502263335985279724127394729293882847); - const int32_t b6 = VIF_PCOEF( 0.1283545132596315418993526691338047385216); - - // printf("%d %d %d %d %d %d %d\n", a0, a1, a2, a3, a4, a5, a6); - // printf("%d %d %d %d %d %d\n", b1, b2, b3, b4, b5, b6); - - static int16_t x1 = 0; - static int16_t x2 = 0; - static int16_t x3 = 0; - static int16_t x4 = 0; - static int16_t x5 = 0; - static int16_t x6 = 0; - - static int32_t _y1 = 0; - static int32_t y2 = 0; - static int32_t y3 = 0; - static int32_t y4 = 0; - static int32_t y5 = 0; - static int32_t y6 = 0; - - /* scale input */ - xn = (xn << VIF_SSCALE); - - int32_t _yn = a0 * xn - + a1 * x1 - + a2 * x2 - + a3 * x3 - + a4 * x4 - + a5 * x5 - + a6 * x6 - - b1 * _y1 - - b2 * y2 - - b3 * y3 - - b4 * y4 - - b5 * y5 - - b6 * y6; - // printf("%d\n", _yn); - _yn = (_yn >> VIF_CRES); - - x6 = x5; - x5 = x4; - x4 = x3; - x3 = x2; - x2 = x1; - x1 = xn; - - y6 = y5; - y5 = y4; - y4 = y3; - y3 = y2; - y2 = _y1; - _y1 = _yn; - - return (_yn >> VIF_SSCALE); -} - -inline int32_t vor_int_filter_lp_ref( int32_t xn) { - - const int32_t a0 = VIF_PCOEF( 0.0182843871571847782497854950634064152837); - const int32_t a1 = VIF_PCOEF( 0.0548531614715543347493564851902192458510); - const int32_t a2 = VIF_PCOEF( 0.0548531614715543347493564851902192458510); - const int32_t a3 = VIF_PCOEF( 0.0182843871571847782497854950634064152837); - - const int32_t b1 = VIF_NCOEF(-1.7551740553005390488294779061106964945793); - const int32_t b2 = VIF_PCOEF( 1.1780240265537846866550353297498077154160); - const int32_t b3 = VIF_NCOEF(-0.2765748739957675783607271569053409621119); - - // printf("%d %d %d %d\n", a0, a1, a2, a3); - // printf("%d %d %d\n", b1, b2, b3); - - static int16_t x1 = 0; - static int16_t x2 = 0; - static int16_t x3 = 0; - - static int32_t _y1 = 0; - static int32_t y2 = 0; - static int32_t y3 = 0; - - /* scale input */ - xn = (xn << VIF_SSCALE); - - int32_t _yn = a0 * xn - + a1 * x1 - + a2 * x2 - + a3 * x3 - - b1 * _y1 - - b2 * y2 - - b3 * y3; - // printf("%d\n", _yn); - _yn = (_yn >> VIF_CRES); - - x3 = x2; - x2 = x1; - x1 = xn; - - y3 = y2; - y2 = _y1; - _y1 = _yn; - - return (_yn >> VIF_SSCALE); -} - -inline int32_t vor_int_filter_lp_fm( int32_t xn) { - - const int32_t a0 = VIF_PCOEF( 0.0001325928962621713658003030911203268261); - const int32_t a1 = VIF_PCOEF( 0.0007955573775730281948018185467219609563); - const int32_t a2 = VIF_PCOEF( 0.0019888934439325706496348722396305674920); - const int32_t a3 = VIF_PCOEF( 0.0026518579252434275328464963195074233226); - const int32_t a4 = VIF_PCOEF( 0.0019888934439325706496348722396305674920); - const int32_t a5 = VIF_PCOEF( 0.0007955573775730281948018185467219609563); - const int32_t a6 = VIF_PCOEF( 0.0001325928962621713658003030911203268261); - - const int32_t b1 = VIF_NCOEF(-3.9811884900947003274040980613790452480316); - const int32_t b2 = VIF_PCOEF( 6.8452604202756832663112618320155888795853); - const int32_t b3 = VIF_NCOEF(-6.4498289229953256196381516929250210523605); - const int32_t b4 = VIF_PCOEF( 3.4951386512490887348292289971141144633293); - const int32_t b5 = VIF_NCOEF(-1.0292502263335985279724127394729293882847); - const int32_t b6 = VIF_PCOEF( 0.1283545132596315418993526691338047385216); - - // printf("%d %d %d %d %d %d %d\n", a0, a1, a2, a3, a4, a5, a6); - // printf("%d %d %d %d %d %d\n", b1, b2, b3, b4, b5, b6); - - static int16_t x1 = 0; - static int16_t x2 = 0; - static int16_t x3 = 0; - static int16_t x4 = 0; - static int16_t x5 = 0; - static int16_t x6 = 0; - - static int32_t _y1 = 0; - static int32_t y2 = 0; - static int32_t y3 = 0; - static int32_t y4 = 0; - static int32_t y5 = 0; - static int32_t y6 = 0; - - /* scale input */ - xn = (xn << VIF_SSCALE); - - int32_t _yn = a0 * xn - + a1 * x1 - + a2 * x2 - + a3 * x3 - + a4 * x4 - + a5 * x5 - + a6 * x6 - - b1 * _y1 - - b2 * y2 - - b3 * y3 - - b4 * y4 - - b5 * y5 - - b6 * y6; - // printf("%d\n", _yn); - _yn = (_yn >> VIF_CRES); - - x6 = x5; - x5 = x4; - x4 = x3; - x3 = x2; - x2 = x1; - x1 = xn; - - y6 = y5; - y5 = y4; - y4 = y3; - y3 = y2; - y2 = _y1; - _y1 = _yn; - - return (_yn >> VIF_SSCALE); - -} - -#endif /* VOR_INT_FILTERS_H */ diff --git a/sw/airborne/firmwares/vor/vor_int_filters_decim.h b/sw/airborne/firmwares/vor/vor_int_filters_decim.h deleted file mode 100644 index 6f0c867701..0000000000 --- a/sw/airborne/firmwares/vor/vor_int_filters_decim.h +++ /dev/null @@ -1,592 +0,0 @@ -#ifndef VOR_INT_FILTERS_DECIM_H -#define VOR_INT_FILTERS_DECIM_H - -extern inline int32_t vor_int_filter_bp_ref( int16_t xn); -extern inline int32_t vor_int_filter_lp_ref( int16_t xn); -extern inline int32_t vor_int_filter_bp_var( int16_t xn); -extern inline int32_t vor_int_filter_lp_decim1v( int16_t xn); -extern inline int32_t vor_int_filter_lp_decim1r( int16_t xn); -extern inline int32_t vor_int_filter_lp_decim2v( int16_t xn); -extern inline int32_t vor_int_filter_lp_decim2r( int16_t xn); -extern inline int32_t vor_int_filter_lp_var3( int16_t xn); -extern inline int32_t vor_int_filter_lp_fm3( int16_t xn); -extern inline int32_t vor_int_filter_lp_var4( int16_t xn); -extern inline int32_t vor_int_filter_lp_fm4( int16_t xn); - -#undef VIF_RES -#define VIF_RES 14 -#define VIF_FACT (1<socket_out = socket( PF_INET, SOCK_DGRAM, pte->p_proto); - setsockopt(me->socket_out, SOL_SOCKET, SO_REUSEADDR, - &so_reuseaddr, sizeof(so_reuseaddr)); + if(port_out >= 0) { + // Create the output socket (enable reuse of the address, and broadcast if necessary) + create_socket(&me->socket_out, 0, 1, broadcast); - /* only set broadcast option if explicitly enabled */ - if (broadcast) - setsockopt(me->socket_out, SOL_SOCKET, SO_BROADCAST, - &broadcast, sizeof(broadcast)); + // Setup the output address + me->addr_out.sin_family = PF_INET; + me->addr_out.sin_port = htons(port_out); + me->addr_out.sin_addr.s_addr = inet_addr(str_ip_out); + } - me->addr_out.sin_family = PF_INET; - me->addr_out.sin_port = htons(port_out); - me->addr_out.sin_addr.s_addr = inet_addr(str_ip_out); + if(port_in >= 0) { + // Creat the input socket (enable reuse of the address, and disable broadcast) + create_socket(&me->socket_in, 0, 1, 0); - me->socket_in = socket( PF_INET, SOCK_DGRAM, pte->p_proto); - setsockopt(me->socket_in, SOL_SOCKET, SO_REUSEADDR, - &so_reuseaddr, sizeof(so_reuseaddr)); + // Create the input address + me->addr_in.sin_family = PF_INET; + me->addr_in.sin_port = htons(port_in); + me->addr_in.sin_addr.s_addr = htonl(INADDR_ANY); - me->addr_in.sin_family = PF_INET; - me->addr_in.sin_port = htons(port_in); - me->addr_in.sin_addr.s_addr = htonl(INADDR_ANY); - - bind(me->socket_in, (struct sockaddr *)&me->addr_in, sizeof(me->addr_in)); + bind(me->socket_in, (struct sockaddr *)&me->addr_in, sizeof(me->addr_in)); + } return me; +} +int network_subscribe_multicast(struct FmsNetwork* me, const char* multicast_addr) { + // Create the request + struct ip_mreq mreq; + mreq.imr_multiaddr.s_addr = inet_addr(multicast_addr); + mreq.imr_interface.s_addr = htonl(INADDR_ANY); + + // Send the request + return setsockopt(me->socket_in, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char *)&mreq, sizeof(mreq)); +} + +int network_set_recvbuf(struct FmsNetwork* me, int buf_size) { + // Set and check + unsigned int optval_size = 4; + int buf_ret; + setsockopt(me->socket_in, SOL_SOCKET, SO_RCVBUF, (char *)&buf_size, optval_size); + getsockopt(me->socket_in, SOL_SOCKET, SO_RCVBUF, (char *)&buf_ret, &optval_size); + + if(buf_size != buf_ret) + return -1; + + return 0; } int network_write(struct FmsNetwork* me, char* buf, int len) { + // Check if the output address is set + if(!me->socket_out) + return -1; + ssize_t byte_written = sendto(me->socket_out, buf, len, MSG_DONTWAIT, (struct sockaddr*)&me->addr_out, sizeof(me->addr_out)); if ( byte_written != len) { @@ -52,6 +88,9 @@ int network_write(struct FmsNetwork* me, char* buf, int len) { ///< returns: -1 = error, 0 = no data, >0 = nrofbytesread int network_read(struct FmsNetwork* me, unsigned char* buf, int len) { + // Check if the input address is set + if(!me->socket_in) + return -1; socklen_t slen = sizeof(struct sockaddr_in); // MSG_DONTWAIT => nonblocking flag diff --git a/sw/airborne/fms/fms_network.h b/sw/airborne/fms/fms_network.h index 347fa99368..df28d8371c 100644 --- a/sw/airborne/fms/fms_network.h +++ b/sw/airborne/fms/fms_network.h @@ -16,6 +16,8 @@ struct FmsNetwork { extern struct FmsNetwork* network_new(const char* str_ip_out, const int port_out, const int port_in, const int broadcast); +extern int network_subscribe_multicast(struct FmsNetwork* me, const char* multicast_addr); +extern int network_set_recvbuf(struct FmsNetwork* me, int buf_size); extern int network_write(struct FmsNetwork* me, char* buf, int len); extern int network_read(struct FmsNetwork* me, unsigned char* buf, int len); diff --git a/sw/airborne/lisa/lisa_stm_passthrough_main.c b/sw/airborne/lisa/lisa_stm_passthrough_main.c index d87ff7ee0b..c9054966e8 100644 --- a/sw/airborne/lisa/lisa_stm_passthrough_main.c +++ b/sw/airborne/lisa/lisa_stm_passthrough_main.c @@ -27,6 +27,7 @@ #include "subsystems/commands.h" #include "subsystems/actuators.h" #include "subsystems/actuators/actuators_pwm.h" +#include "subsystems/actuators/actuators_dualpwm.h" #include "subsystems/imu.h" #include "subsystems/radio_control.h" #include "autopilot.h" diff --git a/sw/airborne/math/pprz_geodetic_int.c b/sw/airborne/math/pprz_geodetic_int.c index 44f5cb6fed..e98cdc4136 100644 --- a/sw/airborne/math/pprz_geodetic_int.c +++ b/sw/airborne/math/pprz_geodetic_int.c @@ -22,7 +22,6 @@ #include "pprz_geodetic_int.h" #include "pprz_algebra_int.h" -#define HIGH_RES_TRIG_FRAC 20 void ltp_of_ecef_rmat_from_lla_i(struct Int32Mat33* ltp_of_ecef, struct LlaCoor_i* lla) { diff --git a/sw/airborne/math/pprz_geodetic_int.h b/sw/airborne/math/pprz_geodetic_int.h index cbeac4ac5f..413026a78e 100644 --- a/sw/airborne/math/pprz_geodetic_int.h +++ b/sw/airborne/math/pprz_geodetic_int.h @@ -126,6 +126,8 @@ extern void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, st #define EM7RAD_OF_RAD(_r) ((_r)*1e7) #define RAD_OF_EM7RAD(_r) ((_r)/1e7) +#define HIGH_RES_TRIG_FRAC 20 + #define VECT3_ENU_OF_NED(_o, _i) { \ (_o).x = (_i).y; \ (_o).y = (_i).x; \ @@ -183,4 +185,28 @@ extern void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, st #define INT32_VECT2_NED_OF_ENU(_o, _i) INT32_VECT2_ENU_OF_NED(_o,_i) +#define HIGH_RES_RMAT_BFP_OF_REAL(_ei, _ef) { \ + (_ei).m[0] = BFP_OF_REAL((_ef).m[0], HIGH_RES_TRIG_FRAC); \ + (_ei).m[1] = BFP_OF_REAL((_ef).m[1], HIGH_RES_TRIG_FRAC); \ + (_ei).m[2] = BFP_OF_REAL((_ef).m[2], HIGH_RES_TRIG_FRAC); \ + (_ei).m[3] = BFP_OF_REAL((_ef).m[3], HIGH_RES_TRIG_FRAC); \ + (_ei).m[4] = BFP_OF_REAL((_ef).m[4], HIGH_RES_TRIG_FRAC); \ + (_ei).m[5] = BFP_OF_REAL((_ef).m[5], HIGH_RES_TRIG_FRAC); \ + (_ei).m[6] = BFP_OF_REAL((_ef).m[6], HIGH_RES_TRIG_FRAC); \ + (_ei).m[7] = BFP_OF_REAL((_ef).m[7], HIGH_RES_TRIG_FRAC); \ + (_ei).m[8] = BFP_OF_REAL((_ef).m[8], HIGH_RES_TRIG_FRAC); \ + } + +#define HIGH_RES_RMAT_FLOAT_OF_BFP(_ef, _ei) { \ + (_ef).m[0] = FLOAT_OF_BFP((_ei).m[0], HIGH_RES_TRIG_FRAC); \ + (_ef).m[1] = FLOAT_OF_BFP((_ei).m[1], HIGH_RES_TRIG_FRAC); \ + (_ef).m[2] = FLOAT_OF_BFP((_ei).m[2], HIGH_RES_TRIG_FRAC); \ + (_ef).m[3] = FLOAT_OF_BFP((_ei).m[3], HIGH_RES_TRIG_FRAC); \ + (_ef).m[4] = FLOAT_OF_BFP((_ei).m[4], HIGH_RES_TRIG_FRAC); \ + (_ef).m[5] = FLOAT_OF_BFP((_ei).m[5], HIGH_RES_TRIG_FRAC); \ + (_ef).m[6] = FLOAT_OF_BFP((_ei).m[6], HIGH_RES_TRIG_FRAC); \ + (_ef).m[7] = FLOAT_OF_BFP((_ei).m[7], HIGH_RES_TRIG_FRAC); \ + (_ef).m[8] = FLOAT_OF_BFP((_ei).m[8], HIGH_RES_TRIG_FRAC); \ + } + #endif /* PPRZ_GEODETIC_INT_H */ diff --git a/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c b/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c index c40a5cb84f..ecfd075b88 100644 --- a/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c +++ b/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c @@ -87,7 +87,7 @@ float airborne_ant_pan_servo = 0; svPlanePosition.fx = stateGetPositionEnu_f()->y; svPlanePosition.fy = stateGetPositionEnu_f()->x; - svPlanePosition.fz = stateGetPositionEnu_f()->z; + svPlanePosition.fz = stateGetPositionUtm_f()->alt; Home_Position.fx = WaypointY(WP_HOME); Home_Position.fy = WaypointX(WP_HOME); diff --git a/sw/airborne/modules/benchmark/flight_benchmark.c b/sw/airborne/modules/benchmark/flight_benchmark.c index a2eaa74d57..00e67b5c6c 100644 --- a/sw/airborne/modules/benchmark/flight_benchmark.c +++ b/sw/airborne/modules/benchmark/flight_benchmark.c @@ -65,7 +65,7 @@ void flight_benchmark_periodic( void ) { #endif #ifdef BENCHMARK_ALTITUDE - Err_altitude = fabs(stateGetPositionEnu_f()->z - v_ctl_altitude_setpoint); + Err_altitude = fabs(stateGetPositionUtm_f()->alt - v_ctl_altitude_setpoint); if (Err_altitude>ToleranceAltitude){ Err_altitude = Err_altitude-ToleranceAltitude; SquareSumErr_altitude += (Err_altitude * Err_altitude); diff --git a/sw/airborne/modules/cam_control/cam.c b/sw/airborne/modules/cam_control/cam.c index 19cf19d53e..d72b7e0dbe 100644 --- a/sw/airborne/modules/cam_control/cam.c +++ b/sw/airborne/modules/cam_control/cam.c @@ -252,7 +252,7 @@ void cam_target( void ) { #else struct EnuCoor_f* pos = stateGetPositionEnu_f(); struct FloatEulers* att = stateGetNedToBodyEulers_f(); - vPoint(pos->x, pos->y, pos->z, + vPoint(pos->x, pos->y, stateGetPositionUtm_f()->alt, att->phi, att->theta, *stateGetHorizontalSpeedDir_f(), cam_target_x, cam_target_y, cam_target_alt, &cam_pan_c, &cam_tilt_c); diff --git a/sw/airborne/modules/cam_control/rotorcraft_cam.c b/sw/airborne/modules/cam_control/rotorcraft_cam.c index 31764b36a2..4110212852 100644 --- a/sw/airborne/modules/cam_control/rotorcraft_cam.c +++ b/sw/airborne/modules/cam_control/rotorcraft_cam.c @@ -33,8 +33,9 @@ * - HEADING: the servo position and the heading of the rotorcraft are set with angles * - WP: the camera is tracking a waypoint (Default: CAM) * - * The CAM_SWITCH can be used to power the camera in normal modes - * and disable it when in NONE mode + * If ROTORCRAFT_CAM_SWITCH_GPIO is defined, this gpio is set/cleared to switch the power + * of the camera on in normal modes and disable it when in NONE mode. + * On boards with CAM_SWITCH, ROTORCRAFT_CAM_SWITCH_GPIO can be defined to CAM_SWITCH_GPIO. */ #include "modules/cam_control/rotorcraft_cam.h" @@ -46,6 +47,25 @@ #include "subsystems/datalink/telemetry.h" + +/** Gpio output to turn camera power power on. + * Control whether to set or clear the ROTORCRAFT_CAM_SWITCH_GPIO to turn on the camera power. + * Should be defined to either gpio_set (default) or gpio_clear. + * Not used if ROTORCRAFT_CAM_SWITCH_GPIO is not defined. + */ +#ifndef ROTORCRAFT_CAM_ON +#define ROTORCRAFT_CAM_ON gpio_set +#endif + +/** Gpio output to turn camera power power off. + * Control whether to set or clear the ROTORCRAFT_CAM_SWITCH_GPIO to turn off the camera power. + * Should be defined to either gpio_set or gpio_clear (default). + * Not used if ROTORCRAFT_CAM_SWITCH_GPIO is not defined. + */ +#ifndef ROTORCRAFT_CAM_OFF +#define ROTORCRAFT_CAM_OFF gpio_clear +#endif + uint8_t rotorcraft_cam_mode; #define _SERVO_PARAM(_s,_p) SERVO_ ## _s ## _ ## _p @@ -73,7 +93,22 @@ static void send_cam(void) { &rotorcraft_cam_tilt,&rotorcraft_cam_pan); } +void rotorcraft_cam_set_mode(uint8_t mode) { + rotorcraft_cam_mode = mode; +#ifdef ROTORCRAFT_CAM_SWITCH_GPIO + if (rotorcraft_cam_mode == ROTORCRAFT_CAM_MODE_NONE) { + ROTORCRAFT_CAM_OFF(ROTORCRAFT_CAM_SWITCH_GPIO); + } + else { + ROTORCRAFT_CAM_ON(ROTORCRAFT_CAM_SWITCH_GPIO); + } +#endif +} + void rotorcraft_cam_init(void) { +#ifdef ROTORCRAFT_CAM_SWITCH_GPIO + gpio_setup_output(ROTORCRAFT_CAM_SWITCH_GPIO); +#endif rotorcraft_cam_SetCamMode(ROTORCRAFT_CAM_DEFAULT_MODE); #if ROTORCRAFT_CAM_USE_TILT rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_NEUTRAL; @@ -137,4 +172,3 @@ void rotorcraft_cam_periodic(void) { ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pwm); #endif } - diff --git a/sw/airborne/modules/cam_control/rotorcraft_cam.h b/sw/airborne/modules/cam_control/rotorcraft_cam.h index 9a95a9b4c8..0fb3c422c2 100644 --- a/sw/airborne/modules/cam_control/rotorcraft_cam.h +++ b/sw/airborne/modules/cam_control/rotorcraft_cam.h @@ -40,11 +40,11 @@ #ifndef ROTORCRAFT_CAM_H #define ROTORCRAFT_CAM_H +#include "std.h" #include "generated/airframe.h" #include "generated/flight_plan.h" #include "math/pprz_algebra_int.h" -#include "std.h" -#include "led.h" +#include "mcu_periph/gpio.h" #define ROTORCRAFT_CAM_MODE_NONE 0 #define ROTORCRAFT_CAM_MODE_MANUAL 1 @@ -56,27 +56,6 @@ #define ROTORCRAFT_CAM_DEFAULT_MODE ROTORCRAFT_CAM_MODE_NONE #endif -/** Cam power control. - * By default CAM_SWITCH is used - * Warning: - * LED_ON set GPIO low on some boards (lpc) - * LED_OFF set GPIO high on some boards (lpc) - */ -#ifndef ROTORCRAFT_CAM_ON -#ifdef CAM_SWITCH_LED -#define ROTORCRAFT_CAM_ON LED_OFF(CAM_SWITCH_LED) -#else -#define ROTORCRAFT_CAM_ON {} -#endif -#endif -#ifndef ROTORCRAFT_CAM_OFF -#ifdef CAM_SWITCH_LED -#define ROTORCRAFT_CAM_OFF LED_ON(CAM_SWITCH_LED) -#else -#define ROTORCRAFT_CAM_OFF {} -#endif -#endif - /** Cam tilt control. * By default use tilt control if a servo is assigned */ @@ -118,14 +97,13 @@ extern int16_t rotorcraft_cam_tilt_pwm; extern void rotorcraft_cam_init(void); extern void rotorcraft_cam_periodic(void); +extern void rotorcraft_cam_set_mode(uint8_t mode); /** Set camera mode. * Camera is powered down in NONE mode if CAM_{ON|OFF} are defined */ #define rotorcraft_cam_SetCamMode(_v) { \ - rotorcraft_cam_mode = _v; \ - if (rotorcraft_cam_mode == ROTORCRAFT_CAM_MODE_NONE) { ROTORCRAFT_CAM_OFF; } \ - else { ROTORCRAFT_CAM_ON; } \ + rotorcraft_cam_set_mode(_v); \ } /** Cam control from datalink message. diff --git a/sw/airborne/modules/cartography/photogrammetry_calculator.c b/sw/airborne/modules/cartography/photogrammetry_calculator.c index f18bd57ce4..e6a8580f03 100644 --- a/sw/airborne/modules/cartography/photogrammetry_calculator.c +++ b/sw/airborne/modules/cartography/photogrammetry_calculator.c @@ -25,19 +25,22 @@ #include "generated/airframe.h" #include "generated/flight_plan.h" -// Flightplan Variables +/** Default sweep angle in radians from north */ #ifndef PHOTOGRAMMETRY_SWEEP_ANGLE #define PHOTOGRAMMETRY_SWEEP_ANGLE 0 #endif +/** overlap 1-99 percent */ #ifndef PHOTOGRAMMETRY_OVERLAP #define PHOTOGRAMMETRY_OVERLAP 50 #endif +/** sidelap 1-99 percent */ #ifndef PHOTOGRAMMETRY_SIDELAP #define PHOTOGRAMMETRY_SIDELAP 50 #endif +/** mm pixel projection size */ #ifndef PHOTOGRAMMETRY_RESOLUTION #define PHOTOGRAMMETRY_RESOLUTION 50 #endif diff --git a/sw/airborne/modules/cartography/photogrammetry_calculator.h b/sw/airborne/modules/cartography/photogrammetry_calculator.h index 36bbf00e0a..14e3a52dd7 100644 --- a/sw/airborne/modules/cartography/photogrammetry_calculator.h +++ b/sw/airborne/modules/cartography/photogrammetry_calculator.h @@ -54,10 +54,10 @@ Add to flightplan or airframe file: Add to flightplan @verbatim
-#define PHOTOGRAMMETRY_SWEEP_ANGLE 53 // Degrees from the North -#define PHOTOGRAMMETRY_OVERLAP 50 // 1-99 Procent -#define PHOTOGRAMMETRY_SIDELAP 50 // 1-99 Procent -#define PHOTOGRAMMETRY_RESOLUTION 80 // mm pixel projection size +#define PHOTOGRAMMETRY_SWEEP_ANGLE RadOfDeg(53) // angle in radians from the North +#define PHOTOGRAMMETRY_OVERLAP 50 // 1-99 Procent +#define PHOTOGRAMMETRY_SIDELAP 50 // 1-99 Procent +#define PHOTOGRAMMETRY_RESOLUTION 80 // mm pixel projection size
@@ -139,7 +139,7 @@ void photogrammetry_calculator_update_flightplan2camera(void); #define PhotogrammetryCalculatorPolygonSurvey(_WP, _COUNT) { \ WaypointAlt(_WP) = photogrammetry_height + GROUND_ALT; \ int _ang = 90 - DegOfRad(photogrammetry_sweep_angle); \ - if (_ang > 90) _ang -= 180; if (_ang < -90) _ang += 180; \ + while (_ang > 90) _ang -= 180; while (_ang < -90) _ang += 180; \ InitializePolygonSurvey((_WP), (_COUNT), 2*photogrammetry_sidestep, _ang); \ } diff --git a/sw/airborne/modules/digital_cam/gpio_cam_ctrl.c b/sw/airborne/modules/digital_cam/gpio_cam_ctrl.c new file mode 100644 index 0000000000..44b0a8c68b --- /dev/null +++ b/sw/airborne/modules/digital_cam/gpio_cam_ctrl.c @@ -0,0 +1,165 @@ +/* + * Copyright (C) 2010-2014 The Paparazzi Team + * + * 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. + */ + +/** + * @file modules/digital_cam/gpio_cam_ctrl.c + * Control the camera via GPIO pins. + * + * Configuration (DC_SHUTTER is mandatory, others optional): + * @code{.xml} + * + * + * + * + * + * @endcode + */ + +#include "gpio_cam_ctrl.h" +#include "generated/airframe.h" + +// Include Standard Camera Control Interface +#include "dc.h" + +#include "mcu_periph/gpio.h" + +#ifndef DC_PUSH +#define DC_PUSH gpio_set +#endif + +#ifndef DC_RELEASE +#define DC_RELEASE gpio_clear +#endif + +#ifndef DC_SHUTTER_DELAY +#define DC_SHUTTER_DELAY 2 /* 4Hz -> 0.5s */ +#endif + +#ifndef DC_POWER_OFF_DELAY +#define DC_POWER_OFF_DELAY 3 +#endif + +#ifdef DC_SHUTTER_LED +#warning DC_SHUTTER_LED is obsolete, please use DC_SHUTTER_GPIO +#endif +#ifndef DC_SHUTTER_GPIO +#error DC: Please specify at least a DC_SHUTTER_GPIO (e.g. ) +#endif + + +// Button Timer +uint8_t dc_timer; + +void gpio_cam_ctrl_init(void) +{ + // Call common DC init + dc_init(); + + // Do gpio specific DC init + dc_timer = 0; + + gpio_setup_output(DC_SHUTTER_GPIO); + DC_RELEASE(DC_SHUTTER_GPIO); +#ifdef DC_ZOOM_IN_GPIO + gpio_setup_output(DC_ZOOM_IN_GPIO); + DC_RELEASE(DC_ZOOM_IN_GPIO); +#endif +#ifdef DC_ZOOM_OUT_GPIO + gpio_setup_output(DC_ZOOM_OUT_GPIO); + DC_RELEASE(DC_ZOOM_OUT_GPIO); +#endif +#ifdef DC_POWER_GPIO + gpio_setup_output(DC_POWER_GPIO); + DC_RELEASE(DC_POWER_GPIO); +#endif +#ifdef DC_POWER_OFF_GPIO + gpio_setup_output(DC_POWER_OFF_GPIO); + DC_RELEASE(DC_POWER_OFF_GPIO); +#endif +} + +void gpio_cam_ctrl_periodic( void ) +{ +#ifdef DC_SHOOT_ON_BUTTON_RELEASE + if (dc_timer==1) { + dc_send_shot_position(); + } +#endif + + if (dc_timer) { + dc_timer--; + } else { + DC_RELEASE(DC_SHUTTER_GPIO); +#ifdef DC_ZOOM_IN_GPIO + DC_RELEASE(DC_ZOOM_IN_GPIO); +#endif +#ifdef DC_ZOOM_OUT_GPIO + DC_RELEASE(DC_ZOOM_OUT_GPIO); +#endif +#ifdef DC_POWER_GPIO + DC_RELEASE(DC_POWER_GPIO); +#endif +#ifdef DC_POWER_OFF_GPIO + DC_RELEASE(DC_POWER_OFF_GPIO); +#endif + } + + // Common DC Periodic task + dc_periodic_4Hz(); +} + +/* Command The Camera */ +void dc_send_command(uint8_t cmd) +{ + dc_timer = DC_SHUTTER_DELAY; + switch (cmd) + { + case DC_SHOOT: + DC_PUSH(DC_SHUTTER_GPIO); +#ifndef DC_SHOOT_ON_BUTTON_RELEASE + dc_send_shot_position(); +#endif + break; +#ifdef DC_ZOOM_IN_GPIO + case DC_TALLER: + DC_PUSH(DC_ZOOM_IN_GPIO); + break; +#endif +#ifdef DC_ZOOM_OUT_GPIO + case DC_WIDER: + DC_PUSH(DC_ZOOM_OUT_GPIO); + break; +#endif +#ifdef DC_POWER_GPIO + case DC_ON: + DC_PUSH(DC_POWER_GPIO); + break; +#endif +#ifdef DC_POWER_OFF_GPIO + case DC_OFF: + DC_PUSH(DC_POWER_OFF_GPIO); + dc_timer = DC_POWER_OFF_DELAY; + break; +#endif + default: + break; + } +} diff --git a/sw/airborne/firmwares/vor/vor_float_demod.h b/sw/airborne/modules/digital_cam/gpio_cam_ctrl.h similarity index 50% rename from sw/airborne/firmwares/vor/vor_float_demod.h rename to sw/airborne/modules/digital_cam/gpio_cam_ctrl.h index b78d9ff949..442dba0563 100644 --- a/sw/airborne/firmwares/vor/vor_float_demod.h +++ b/sw/airborne/modules/digital_cam/gpio_cam_ctrl.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2008 Antoine Blais, Antoine Drouin + * Copyright (C) 2010-2014 The Paparazzi Team * * This file is part of paparazzi. * @@ -17,37 +17,23 @@ * 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. - * */ -#ifndef VOR_FLOAT_DEMOD_H -#define VOR_FLOAT_DEMOD_H +/** @file modules/digital_cam/gpio_cam_ctrl.h + * @brief Digital Camera Control + * + * Provides the control of the shutter and the zoom of a digital camera + * through standard binary IOs of the board. + * + * The required initialization (dc_init()) and periodic (4Hz) process. + */ -extern void vor_float_demod_init( void); -extern void vor_float_demod_run ( float sample); +#ifndef GPIO_CAM_CTRL_H +#define GPIO_CAM_CTRL_H -extern const float vfd_te; +extern void gpio_cam_ctrl_init(void); -extern const float vfd_ref_freq; -extern float vfd_ref_phi; -extern float vfd_ref_err; -extern const float vfd_ref_alpha; - -extern const float vfd_var_freq; -extern float vfd_var_phi; -extern float vfd_var_err; -extern const float vfd_var_alpha; - -extern const float vfd_fm_freq; -extern float vfd_fm_phi; -extern float vfd_fm_err; -extern const float vfd_fm_alpha; - -extern float vfd_qdr; - - -extern float vfd_var_sig; -extern float vfd_fm_local_sig; - -#endif /* VOR_FLOAT_DEMOD_H */ +/** 4Hz Periodic */ +extern void gpio_cam_ctrl_periodic(void); +#endif // GPIO_CAM_CTRL_H diff --git a/sw/airborne/modules/digital_cam/led_cam_ctrl.c b/sw/airborne/modules/digital_cam/led_cam_ctrl.c deleted file mode 100644 index 81b701f6af..0000000000 --- a/sw/airborne/modules/digital_cam/led_cam_ctrl.c +++ /dev/null @@ -1,67 +0,0 @@ -/* - * Copyright (C) 2010 The Paparazzi Team - * - * 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. - * - */ - -#include "led_cam_ctrl.h" - -// Button Timer -uint8_t dc_timer; - - -/* Command The Camera */ -void dc_send_command(uint8_t cmd) -{ - dc_timer = DC_SHUTTER_DELAY; - switch (cmd) - { - case DC_SHOOT: - DC_PUSH(DC_SHUTTER_LED); -#ifndef DC_SHOOT_ON_BUTTON_RELEASE - dc_send_shot_position(); -#endif - break; -#ifdef DC_ZOOM_IN_LED - case DC_TALLER: - DC_PUSH(DC_ZOOM_IN_LED); - break; -#endif -#ifdef DC_ZOOM_OUT_LED - case DC_WIDER: - DC_PUSH(DC_ZOOM_OUT_LED); - break; -#endif -#ifdef DC_POWER_LED - case DC_ON: - DC_PUSH(DC_POWER_LED); - break; -#endif -#ifdef DC_POWER_OFF_LED - case DC_OFF: - DC_PUSH(DC_POWER_OFF_LED); - dc_timer = DC_POWER_OFF_DELAY; - break; -#endif - default: - break; - } -} - - diff --git a/sw/airborne/modules/digital_cam/led_cam_ctrl.h b/sw/airborne/modules/digital_cam/led_cam_ctrl.h deleted file mode 100644 index fde5cb4aa7..0000000000 --- a/sw/airborne/modules/digital_cam/led_cam_ctrl.h +++ /dev/null @@ -1,139 +0,0 @@ -/* - * Copyright (C) 2010 The Paparazzi Team - * - * 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. - * - */ - - -/** @file modules/digital_cam/led_cam_ctrl.h - * @brief Digital Camera Control - * - * Provides the control of the shutter and the zoom of a digital camera - * through standard binary IOs of the board. - * - * Configuration: - * Since the API of led.h is used, connected pins must be defined as led - * numbers (usually in the airframe file): - * @verbatim - * - * - * - * - * - * @endverbatim - * Related bank and pin must also be defined: - * @verbatim - * - * - * @endverbatim - * The required initialization (dc_init()) and periodic (4Hz) process - * - */ - -#ifndef LED_CAM_CTRL_H -#define LED_CAM_CTRL_H - -// Include Standard Camera Control Interface -#include "dc.h" - -// Include Digital IO -#include "led.h" - -extern uint8_t dc_timer; - -#ifndef DC_PUSH -#define DC_PUSH LED_ON -#endif - -#ifndef DC_RELEASE -#define DC_RELEASE LED_OFF -#endif - -#ifndef DC_SHUTTER_DELAY -#define DC_SHUTTER_DELAY 2 /* 4Hz -> 0.5s */ -#endif - -#ifndef DC_POWER_OFF_DELAY -#define DC_POWER_OFF_DELAY 3 -#endif - -#ifndef DC_SHUTTER_LED -#error DC: Please specify at least a SHUTTER LED -#endif - -static inline void led_cam_ctrl_init(void) -{ - // Call common DC init - dc_init(); - - // Do LED specific DC init - dc_timer = 0; - - DC_RELEASE(DC_SHUTTER_LED); -#ifdef DC_ZOOM_IN_LED - DC_RELEASE(DC_ZOOM_IN_LED); -#endif -#ifdef DC_ZOOM_OUT_LED - DC_RELEASE(DC_ZOOM_OUT_LED); -#endif -#ifdef DC_POWER_LED - DC_RELEASE(DC_POWER_LED); -#endif -#ifdef DC_POWER_OFF_LED - DC_RELEASE(DC_POWER_OFF_LED); -#endif -} - - -/* 4Hz Periodic */ -static inline void led_cam_ctrl_periodic( void ) -{ -#ifdef DC_SHOOT_ON_BUTTON_RELEASE - if (dc_timer==1) { - dc_send_shot_position(); - } -#endif - - if (dc_timer) { - dc_timer--; - } else { - DC_RELEASE(DC_SHUTTER_LED); -#ifdef DC_ZOOM_IN_LED - DC_RELEASE(DC_ZOOM_IN_LED); -#endif -#ifdef DC_ZOOM_OUT_LED - DC_RELEASE(DC_ZOOM_OUT_LED); -#endif -#ifdef DC_POWER_LED - DC_RELEASE(DC_POWER_LED); -#endif -#ifdef DC_POWER_OFF_LED - DC_RELEASE(DC_POWER_OFF_LED); -#endif - } - - // Common DC Periodic task - dc_periodic_4Hz(); -} - - - - - -#endif // DC_H diff --git a/sw/airborne/modules/digital_cam/servo_cam_ctrl.h b/sw/airborne/modules/digital_cam/servo_cam_ctrl.h index 99caa82862..739d6471c9 100644 --- a/sw/airborne/modules/digital_cam/servo_cam_ctrl.h +++ b/sw/airborne/modules/digital_cam/servo_cam_ctrl.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2010 The Paparazzi Team + * Copyright (C) 2010-2014 The Paparazzi Team * * This file is part of paparazzi. * @@ -17,36 +17,27 @@ * 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. - * */ - /** @file modules/digital_cam/servo_cam_ctrl.h * @brief Digital Camera Control * * Provides the control of the shutter and the zoom of a digital camera - * through standard binary IOs of the board. + * via servos. * - * Configuration: - * Since the API of led.h is used, connected pins must be defined as led - * numbers (usually in the airframe file): - * @verbatim - * - * - * - * - * @endverbatim - * Related bank and pin must also be defined: - * @verbatim - * - * - * @endverbatim - * The required initialization (dc_init()) and periodic (4Hz) process + * Configuration in airframe file (DC_SHUTTER is mandatory, others optional): + * @code{.xml] + * + * + * + * + * @endcode * + * Provides the required initialization (dc_init()) and periodic (4Hz) process. */ -#ifndef servo_cam_ctrl_H -#define servo_cam_ctrl_H +#ifndef SERVO_CAM_CTRL_H +#define SERVO_CAM_CTRL_H // Include Standard Camera Control Interface #include "dc.h" @@ -75,7 +66,7 @@ static inline void servo_cam_ctrl_init(void) #endif #ifndef DC_SHUTTER_SERVO -#error DC: Please specify at least a SHUTTER SERVO +#error DC: Please specify at least a DC_SHUTTER_SERVO #endif @@ -107,8 +98,4 @@ static inline void servo_cam_ctrl_periodic( void ) dc_periodic_4Hz(); } - - - - -#endif // DC_H +#endif // SERVO_CAM_CONTROL_H diff --git a/sw/airborne/modules/gps/gps_ubx_ucenter.c b/sw/airborne/modules/gps/gps_ubx_ucenter.c index 41313390a7..8cc26cf9c8 100644 --- a/sw/airborne/modules/gps/gps_ubx_ucenter.c +++ b/sw/airborne/modules/gps/gps_ubx_ucenter.c @@ -32,7 +32,7 @@ #include "subsystems/datalink/downlink.h" #include -#if DEBUG_GPS_UBX_UCENTER +#if PRINT_DEBUG_GPS_UBX_UCENTER #define DEBUG_PRINT(...) printf(__VA_ARGS__) #else #define DEBUG_PRINT(...) {} @@ -102,7 +102,7 @@ void gps_ubx_ucenter_periodic(void) { gps_ubx_ucenter.status = GPS_UBX_UCENTER_STATUS_CONFIG; gps_ubx_ucenter.cnt = 0; -#if DEBUG_GPS_UBX_UCENTER +#if PRINT_DEBUG_GPS_UBX_UCENTER if (gps_ubx_ucenter.baud_init > 0) { DEBUG_PRINT("Initial ublox baudrate found: %u\n", gps_ubx_ucenter.baud_init); } @@ -442,7 +442,7 @@ static bool_t gps_ubx_ucenter_configure(uint8_t nr) gps_ubx_ucenter_config_port(); break; case 1: -#if DEBUG_GPS_UBX_UCENTER +#if PRINT_DEBUG_GPS_UBX_UCENTER if (gps_ubx_ucenter.reply != GPS_UBX_UCENTER_REPLY_ACK) { DEBUG_PRINT("ublox did not acknowledge port configuration.\n"); } diff --git a/sw/airborne/modules/meteo/meteo_france_DAQ.c b/sw/airborne/modules/meteo/meteo_france_DAQ.c index 0f3549bd9f..c0eda56ec6 100644 --- a/sw/airborne/modules/meteo/meteo_france_DAQ.c +++ b/sw/airborne/modules/meteo/meteo_france_DAQ.c @@ -35,7 +35,6 @@ #include "state.h" #include "autopilot.h" -#include "generated/airframe.h" #include "subsystems/datalink/datalink.h" #include "subsystems/datalink/downlink.h" #include "subsystems/chibios-libopencm3/sdLog.h" @@ -87,7 +86,7 @@ void mf_daq_send_state(void) { void mf_daq_send_report(void) { // Send report over normal telemetry if (mf_daq.nb > 0) { - DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, mf_daq.nb, mf_daq.values); + DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 9, mf_daq.values); } // Test if log is started if (pprzLogFile.fs != NULL) { @@ -110,6 +109,7 @@ void mf_daq_send_report(void) { void parse_mf_daq_msg(void) { mf_daq.nb = DL_PAYLOAD_FLOAT_values_length(dl_buffer); if (mf_daq.nb > 0) { + if (mf_daq.nb > MF_DAQ_SIZE) mf_daq.nb = MF_DAQ_SIZE; // Store data struct directly from dl_buffer memcpy(mf_daq.values, DL_PAYLOAD_FLOAT_values(dl_buffer), mf_daq.nb * sizeof(float)); // Log on SD card diff --git a/sw/airborne/modules/meteo/meteo_france_DAQ.h b/sw/airborne/modules/meteo/meteo_france_DAQ.h index 6a852ec4fd..aa2d9da46f 100644 --- a/sw/airborne/modules/meteo/meteo_france_DAQ.h +++ b/sw/airborne/modules/meteo/meteo_france_DAQ.h @@ -36,6 +36,7 @@ #include "std.h" #include "mcu_periph/gpio.h" +#include "generated/airframe.h" #define MF_DAQ_SIZE 32 @@ -59,6 +60,7 @@ extern void parse_mf_daq_msg(void); else { gpio_clear(MF_DAQ_POWER_PORT, MF_DAQ_POWER_PIN); } \ } #else // POWER PORT and PIN undefined +INFO("MF_DAQ power pin is not defined"); #define meteo_france_DAQ_SetPower(_x) {} #endif diff --git a/sw/airborne/modules/multi/follow.c b/sw/airborne/modules/multi/follow.c new file mode 100644 index 0000000000..5adfdf6832 --- /dev/null +++ b/sw/airborne/modules/multi/follow.c @@ -0,0 +1,76 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * + * 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. + * + */ + +/** \file follow.c + * \brief Follow a certain AC ID + * + */ + +#include "multi/follow.h" +#include "generated/flight_plan.h" +#include "generated/airframe.h" + +#include "state.h" +#include "subsystems/ins/ins_int.h" +#include "navigation.h" +#include "messages.h" +#include "dl_protocol.h" + +#ifndef FOLLOW_OFFSET_X +#define FOLLOW_OFFSET_X 0.0 +#endif + +#ifndef FOLLOW_OFFSET_Y +#define FOLLOW_OFFSET_Y 0.0 +#endif + +#ifndef FOLLOW_OFFSET_Z +#define FOLLOW_OFFSET_Z 0.0 +#endif + +void follow_init( void ) { + +} + +void follow_change_wp( unsigned char* buffer ) { + struct EcefCoor_i new_pos; + struct EnuCoor_i enu; + new_pos.x = DL_REMOTE_GPS_ecef_x(buffer); + new_pos.y = DL_REMOTE_GPS_ecef_y(buffer); + new_pos.z = DL_REMOTE_GPS_ecef_z(buffer); + + // Translate to ENU + enu_of_ecef_point_i(&enu, &ins_impl.ltp_def, &new_pos); + INT32_VECT3_SCALE_2(enu, enu, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); + + // TODO: Add the angle to the north + + // Update the offsets + enu.x += POS_BFP_OF_REAL(FOLLOW_OFFSET_X); + enu.y += POS_BFP_OF_REAL(FOLLOW_OFFSET_Y); + enu.z += POS_BFP_OF_REAL(FOLLOW_OFFSET_Z); + + // TODO: Remove the angle to the north + + // Move the waypoint + INT32_VECT3_COPY(waypoints[FOLLOW_WAYPOINT_ID], enu); +} diff --git a/sw/airborne/modules/multi/follow.h b/sw/airborne/modules/multi/follow.h new file mode 100644 index 0000000000..92f3d76f26 --- /dev/null +++ b/sw/airborne/modules/multi/follow.h @@ -0,0 +1,39 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * + * 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. + * + */ + +/** \file follow.h + * \brief Follow a certain AC id + */ + +#ifndef FOLLOW_H +#define FOLLOW_H + +extern void follow_init( void ); +extern void follow_change_wp( unsigned char* buffer ); + +#define ParseRemoteGps() { \ + if (DL_REMOTE_GPS_ac_id(dl_buffer) == FOLLOW_AC_ID) { \ + follow_change_wp(dl_buffer); \ + } \ +} + +#endif // FOLLOW diff --git a/sw/airborne/modules/multi/formation.c b/sw/airborne/modules/multi/formation.c index 30d4f0ca89..c56c48b572 100644 --- a/sw/airborne/modules/multi/formation.c +++ b/sw/airborne/modules/multi/formation.c @@ -139,7 +139,7 @@ int formation_flight(void) { stateGetPositionEnu_f()->y += formation[the_acs_id[AC_ID]].north; } // set info for this AC - SetAcInfo(AC_ID, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, (*stateGetHorizontalSpeedDir_f()), stateGetPositionEnu_f()->z, (*stateGetHorizontalSpeedNorm_f()), stateGetSpeedEnu_f()->z, gps.tow); + SetAcInfo(AC_ID, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, (*stateGetHorizontalSpeedDir_f()), stateGetPositionUtm_f()->alt, (*stateGetHorizontalSpeedNorm_f()), stateGetSpeedEnu_f()->z, gps.tow); // broadcast info uint8_t ac_id = AC_ID; @@ -185,12 +185,12 @@ int formation_flight(void) { } else formation[i].status = ACTIVE; // compute control if AC is ACTIVE and around the same altitude (maybe not so usefull) - if (formation[i].status == ACTIVE && fabs(stateGetPositionEnu_f()->z - ac->alt) < form_prox && ac->alt > 0) { + if (formation[i].status == ACTIVE && fabs(stateGetPositionUtm_f()->alt - ac->alt) < form_prox && ac->alt > 0) { form_e += (ac->east + ac->gspeed*sinf(ac->course)*delta_t - stateGetPositionEnu_f()->x) - (form[i].east - form[the_acs_id[AC_ID]].east); form_n += (ac->north + ac->gspeed*cosf(ac->course)*delta_t - stateGetPositionEnu_f()->y) - (form[i].north - form[the_acs_id[AC_ID]].north); - form_a += (ac->alt - stateGetPositionEnu_f()->z) - (formation[i].alt - formation[the_acs_id[AC_ID]].alt); + form_a += (ac->alt - stateGetPositionUtm_f()->alt) - (formation[i].alt - formation[the_acs_id[AC_ID]].alt); form_speed += ac->gspeed; //form_speed_e += ac->gspeed * sinf(ac->course); //form_speed_n += ac->gspeed * cosf(ac->course); diff --git a/sw/airborne/modules/multi/potential.c b/sw/airborne/modules/multi/potential.c index d4e3006112..f1756b4d90 100644 --- a/sw/airborne/modules/multi/potential.c +++ b/sw/airborne/modules/multi/potential.c @@ -77,7 +77,7 @@ int potential_task(void) { if (de > FORCE_MAX_DIST || de < -FORCE_MAX_DIST) continue; float dn = ac->north + cha*delta_t - stateGetPositionEnu_f()->y; if (dn > FORCE_MAX_DIST || dn < -FORCE_MAX_DIST) continue; - float da = ac->alt + ac->climb*delta_t - stateGetPositionEnu_f()->z; + float da = ac->alt + ac->climb*delta_t - stateGetPositionUtm_f()->alt; if (da > FORCE_MAX_DIST || da < -FORCE_MAX_DIST) continue; float dist = sqrtf(de*de + dn*dn + da*da); if (dist == 0.) continue; diff --git a/sw/airborne/modules/multi/tcas.c b/sw/airborne/modules/multi/tcas.c index f55adf0571..819c0e0451 100644 --- a/sw/airborne/modules/multi/tcas.c +++ b/sw/airborne/modules/multi/tcas.c @@ -86,7 +86,7 @@ void tcas_init( void ) { static inline enum tcas_resolve tcas_test_direction(uint8_t id) { struct ac_info_ * ac = get_ac_info(id); - float dz = ac->alt - stateGetPositionEnu_f()->z; + float dz = ac->alt - stateGetPositionUtm_f()->alt; if (dz > tcas_alim/2) return RA_DESCEND; else if (dz < -tcas_alim/2) return RA_CLIMB; else // AC with the smallest ID descend @@ -100,7 +100,7 @@ static inline enum tcas_resolve tcas_test_direction(uint8_t id) { /* conflicts detection and monitoring */ void tcas_periodic_task_1Hz( void ) { // no TCAS under security_height - if (stateGetPositionEnu_f()->z < GROUND_ALT + SECURITY_HEIGHT) { + if (stateGetPositionUtm_f()->alt < GROUND_ALT + SECURITY_HEIGHT) { uint8_t i; for (i = 0; i < NB_ACS; i++) tcas_acs_status[i].status = TCAS_NO_ALARM; return; @@ -121,7 +121,7 @@ void tcas_periodic_task_1Hz( void ) { if (dt > TCAS_DT_MAX) continue; // lost com but keep current status float dx = the_acs[i].east - stateGetPositionEnu_f()->x; float dy = the_acs[i].north - stateGetPositionEnu_f()->y; - float dz = the_acs[i].alt - stateGetPositionEnu_f()->z; + float dz = the_acs[i].alt - stateGetPositionUtm_f()->alt; float dvx = vx - the_acs[i].gspeed * sinf(the_acs[i].course); float dvy = vy - the_acs[i].gspeed * cosf(the_acs[i].course); float dvz = stateGetSpeedEnu_f()->z - the_acs[i].climb; @@ -217,7 +217,7 @@ void tcas_periodic_task_1Hz( void ) { /* altitude control loop */ void tcas_periodic_task_4Hz( void ) { // set alt setpoint - if (stateGetPositionEnu_f()->z > GROUND_ALT + SECURITY_HEIGHT && tcas_status == TCAS_RA) { + if (stateGetPositionUtm_f()->alt > GROUND_ALT + SECURITY_HEIGHT && tcas_status == TCAS_RA) { struct ac_info_ * ac = get_ac_info(tcas_ac_RA); switch (tcas_resolve) { case RA_CLIMB : diff --git a/sw/airborne/modules/nav/nav_bungee_takeoff.c b/sw/airborne/modules/nav/nav_bungee_takeoff.c index 33488fa8a2..cdae8f0a2e 100644 --- a/sw/airborne/modules/nav/nav_bungee_takeoff.c +++ b/sw/airborne/modules/nav/nav_bungee_takeoff.c @@ -208,7 +208,7 @@ bool_t nav_bungee_takeoff_run(void) nav_route_xy(initialx,initialy,throttlePx,throttlePy); kill_throttle = 0; - if((stateGetPositionEnu_f()->z > BungeeAlt+Takeoff_Height-10) && ((*stateGetHorizontalSpeedNorm_f()) > Takeoff_Speed)) + if((stateGetPositionUtm_f()->alt > BungeeAlt+Takeoff_Height-10) && ((*stateGetHorizontalSpeedNorm_f()) > Takeoff_Speed)) { CTakeoffStatus = Finished; return FALSE; diff --git a/sw/airborne/modules/nav/nav_drop.c b/sw/airborne/modules/nav/nav_drop.c index 17e436fa60..9bd8b209cb 100644 --- a/sw/airborne/modules/nav/nav_drop.c +++ b/sw/airborne/modules/nav/nav_drop.c @@ -45,7 +45,6 @@ #ifndef MASS #define MASS 3.31e-3 #endif - #define ALPHA_M (ALPHA / MASS) #define DT 0.1 #define MAX_STEPS 100 @@ -101,7 +100,7 @@ static void integrate( uint8_t wp_target ) { /** Update the RELEASE location with the actual ground speed and altitude */ unit_t nav_drop_update_release( uint8_t wp_target ) { - nav_drop_z = stateGetPositionEnu_f()->z - waypoints[wp_target].a; + nav_drop_z = stateGetPositionUtm_f()->alt - waypoints[wp_target].a; nav_drop_x = 0.; nav_drop_y = 0.; diff --git a/sw/airborne/modules/nav/nav_spiral.c b/sw/airborne/modules/nav/nav_spiral.c index 5c3703040b..907d3e59c5 100644 --- a/sw/airborne/modules/nav/nav_spiral.c +++ b/sw/airborne/modules/nav/nav_spiral.c @@ -67,7 +67,8 @@ bool_t nav_spiral_start(uint8_t center_wp, uint8_t edge_wp, float radius_start, struct EnuCoor_f pos_enu; memcpy(&pos_enu, stateGetPositionEnu_f(), sizeof(struct EnuCoor_f)); - VECT3_DIFF(nav_spiral.trans_current, pos_enu, nav_spiral.center); + VECT2_DIFF(nav_spiral.trans_current, pos_enu, nav_spiral.center); + nav_spiral.trans_current.z = stateGetPositionUtm_f()->alt - nav_spiral.center.z; nav_spiral.dist_from_center = FLOAT_VECT3_NORM(nav_spiral.trans_current); @@ -147,7 +148,7 @@ bool_t nav_spiral_run(void) #ifdef DIGITAL_CAM if (dc_cam_tracing) { // calculating Cam angle for camera alignment - nav_spiral.trans_current.z = pos_enu.z - nav_spiral.center.z; + nav_spiral.trans_current.z = stateGetPositionUtm_f()->alt - nav_spiral.center.z; dc_cam_angle = atan(nav_spiral.radius_start/nav_spiral.trans_current.z) * 180 / M_PI; } #endif diff --git a/sw/airborne/modules/nav/nav_survey_poly_osam.c b/sw/airborne/modules/nav/nav_survey_poly_osam.c index d3dcb49d70..b805ffc91c 100644 --- a/sw/airborne/modules/nav/nav_survey_poly_osam.c +++ b/sw/airborne/modules/nav/nav_survey_poly_osam.c @@ -302,7 +302,7 @@ bool_t nav_survey_poly_osam_run(void) //follow the circle nav_circle_XY(C.x, C.y, SurveyRadius); - if(NavQdrCloseTo(SurveyCircleQdr) && NavCircleCountNoRewind() > .1 && stateGetPositionEnu_f()->z > waypoints[SurveyEntryWP].a-10) + if(NavQdrCloseTo(SurveyCircleQdr) && NavCircleCountNoRewind() > .1 && stateGetPositionUtm_f()->alt > waypoints[SurveyEntryWP].a-10) { CSurveyStatus = Sweep; nav_init_stage(); diff --git a/sw/airborne/modules/nav/nav_survey_polygon.c b/sw/airborne/modules/nav/nav_survey_polygon.c index 3597010842..bd6593326d 100644 --- a/sw/airborne/modules/nav/nav_survey_polygon.c +++ b/sw/airborne/modules/nav/nav_survey_polygon.c @@ -240,7 +240,7 @@ bool_t nav_survey_polygon_run(void) nav_circle_XY(survey.entry_center.x, survey.entry_center.y, -survey.psa_min_rad); if (NavCourseCloseTo(survey.segment_angle) && nav_approaching_xy(survey.seg_start.x, survey.seg_start.y, last_x, last_y, CARROT) - && fabs(stateGetPositionEnu_f()->z - survey.psa_altitude) <= 20) { + && fabs(stateGetPositionUtm_f()->alt - survey.psa_altitude) <= 20) { survey.stage = SEG; nav_init_stage(); #ifdef DIGITAL_CAM diff --git a/sw/airborne/modules/nav/nav_vertical_raster.c b/sw/airborne/modules/nav/nav_vertical_raster.c index c04705c5ed..63c6ec2f40 100644 --- a/sw/airborne/modules/nav/nav_vertical_raster.c +++ b/sw/airborne/modules/nav/nav_vertical_raster.c @@ -105,7 +105,7 @@ bool_t nav_vertical_raster_run(uint8_t l1, uint8_t l2, float radius, float AltSw break; case LTC2: nav_circle_XY(l2_c2.x, l2_c2.y, -radius); - if (NavQdrCloseTo(DegOfRad(qdr_out_2_2)+10) && stateGetPositionEnu_f()->z >= (waypoints[l1].a-10)) { + if (NavQdrCloseTo(DegOfRad(qdr_out_2_2)+10) && stateGetPositionUtm_f()->alt >= (waypoints[l1].a-10)) { line_status = LQC22; nav_init_stage(); } @@ -134,7 +134,7 @@ bool_t nav_vertical_raster_run(uint8_t l1, uint8_t l2, float radius, float AltSw break; case LTC1: nav_circle_XY(l1_c2.x, l1_c2.y, -radius); - if (NavQdrCloseTo(DegOfRad(qdr_out_2_2 + M_PI)+10) && stateGetPositionEnu_f()->z >= (waypoints[l1].a-5)) { + if (NavQdrCloseTo(DegOfRad(qdr_out_2_2 + M_PI)+10) && stateGetPositionUtm_f()->alt >= (waypoints[l1].a-5)) { line_status = LQC11; nav_init_stage(); } diff --git a/sw/airborne/modules/sonar/sonar_adc.c b/sw/airborne/modules/sonar/sonar_adc.c index 189cf079eb..14268aa27a 100644 --- a/sw/airborne/modules/sonar/sonar_adc.c +++ b/sw/airborne/modules/sonar/sonar_adc.c @@ -23,6 +23,7 @@ #include "modules/sonar/sonar_adc.h" #include "generated/airframe.h" #include "mcu_periph/adc.h" +#include "subsystems/abi.h" #ifdef SITL #include "state.h" #endif @@ -32,11 +33,11 @@ #include "subsystems/datalink/downlink.h" /** Sonar offset. - * Offset value in m (float) - * equals to the height when the ADC gives 0 + * Offset value in ADC + * equals to the ADC value so that height is zero */ #ifndef SONAR_OFFSET -#define SONAR_OFFSET 0. +#define SONAR_OFFSET 0 #endif /** Sonar scale. @@ -46,43 +47,39 @@ #define SONAR_SCALE 0.0166 #endif -uint16_t sonar_meas; -bool_t sonar_data_available; -float sonar_distance; -float sonar_offset; -float sonar_scale; +struct SonarAdc sonar_adc; #ifndef SITL -static struct adc_buf sonar_adc; +static struct adc_buf sonar_adc_buf; #endif void sonar_adc_init(void) { - sonar_meas = 0; - sonar_data_available = FALSE; - sonar_distance = 0; - sonar_offset = SONAR_OFFSET; - sonar_scale = SONAR_SCALE; + sonar_adc.meas = 0; + sonar_adc.offset = SONAR_OFFSET; #ifndef SITL - adc_buf_channel(ADC_CHANNEL_SONAR, &sonar_adc, DEFAULT_AV_NB_SAMPLE); + adc_buf_channel(ADC_CHANNEL_SONAR, &sonar_adc_buf, DEFAULT_AV_NB_SAMPLE); #endif } /** Read ADC value to update sonar measurement */ void sonar_adc_read(void) { + float sonar_distance; #ifndef SITL - sonar_meas = sonar_adc.sum / sonar_adc.av_nb_sample; - sonar_data_available = TRUE; - sonar_distance = ((float)sonar_meas * sonar_scale) + sonar_offset; - + sonar_adc.meas = sonar_adc_buf.sum / sonar_adc_buf.av_nb_sample; + sonar_distance = (float)(sonar_adc.meas - sonar_adc.offset) * SONAR_SCALE; #else // SITL sonar_distance = stateGetPositionEnu_f()->z; Bound(sonar_distance, 0.1f, 7.0f); #endif // SITL + // Send ABI message + AbiSendMsgAGL(AGL_SONAR_ADC_ID, &sonar_distance); + #ifdef SENSOR_SYNC_SEND_SONAR - DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &sonar_meas, &sonar_distance); + // Send Telemetry report + DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &sonar_adc.meas, &sonar_distance); #endif } diff --git a/sw/airborne/modules/sonar/sonar_adc.h b/sw/airborne/modules/sonar/sonar_adc.h index ec5a8c06cb..1bccbe0ea6 100644 --- a/sw/airborne/modules/sonar/sonar_adc.h +++ b/sw/airborne/modules/sonar/sonar_adc.h @@ -29,26 +29,14 @@ #include "std.h" -/** Raw ADC value. - */ -extern uint16_t sonar_meas; +struct SonarAdc { + uint16_t meas; ///< Raw ADC value + uint16_t offset; ///< Sonar offset in ADC +}; -/** New data available. - */ -extern bool_t sonar_data_available; - -/** Sonar distance in m. - */ -extern float sonar_distance; +extern struct SonarAdc sonar_adc; extern void sonar_adc_init(void); extern void sonar_adc_read(void); -#define SonarEvent(_handler) { \ - if (sonar_data_available) { \ - _handler(); \ - sonar_data_available = FALSE; \ - } \ -} - #endif diff --git a/sw/airborne/state.h b/sw/airborne/state.h index ee1436f1c4..6f9cd473f2 100644 --- a/sw/airborne/state.h +++ b/sw/airborne/state.h @@ -442,7 +442,7 @@ static inline void stateSetLocalOrigin_i(struct LtpDef_i* ltp_def) { /* convert to float */ ECEF_FLOAT_OF_BFP(state.ned_origin_f.ecef, state.ned_origin_i.ecef); LLA_FLOAT_OF_BFP(state.ned_origin_f.lla, state.ned_origin_i.lla); - RMAT_FLOAT_OF_BFP(state.ned_origin_f.ltp_of_ecef, state.ned_origin_i.ltp_of_ecef); + HIGH_RES_RMAT_FLOAT_OF_BFP(state.ned_origin_f.ltp_of_ecef, state.ned_origin_i.ltp_of_ecef); state.ned_origin_f.hmsl = M_OF_MM(state.ned_origin_i.hmsl); /* clear bits for all local frame representations */ diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h index f87b5298b8..b4f29738d5 100644 --- a/sw/airborne/subsystems/abi_sender_ids.h +++ b/sw/airborne/subsystems/abi_sender_ids.h @@ -76,4 +76,16 @@ #define BARO_SIM_SENDER_ID 19 #endif +/* + * IDs of AGL measurment modules that can be loaded (sonars,...) + */ +#ifndef AGL_SONAR_ADC_ID +#define AGL_SONAR_ADC_ID 1 +#endif + +#ifndef AGL_SONAR_ARDRONE2_ID +#define AGL_SONAR_ARDRONE2_ID 2 +#endif + + #endif /* ABI_SENDER_IDS_H */ diff --git a/sw/airborne/subsystems/actuators/actuators_dualpwm.h b/sw/airborne/subsystems/actuators/actuators_dualpwm.h new file mode 100644 index 0000000000..46e6c52b3e --- /dev/null +++ b/sw/airborne/subsystems/actuators/actuators_dualpwm.h @@ -0,0 +1,34 @@ +/* + * Copyright (C) 2010 The Paparazzi Team + * + * 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. + */ + +#ifndef ACTUATORS_dualpwm_H +#define ACTUATORS_dualpwm_H + +#include "subsystems/actuators/actuators_dualpwm_arch.h" + +/** Arch dependent init file. + * implemented in arch files + */ +extern void actuators_dualpwm_arch_init(void); + +#define ActuatorsDualpwmInit() actuators_dualpwm_arch_init() + +#endif /* ACTUATORS_PWM_H */ diff --git a/sw/airborne/subsystems/actuators/motor_mixing.c b/sw/airborne/subsystems/actuators/motor_mixing.c index 9cd978f250..c96812cc78 100644 --- a/sw/airborne/subsystems/actuators/motor_mixing.c +++ b/sw/airborne/subsystems/actuators/motor_mixing.c @@ -93,6 +93,7 @@ void motor_mixing_init(void) { motor_mixing.override_value[i] = MOTOR_MIXING_STOP_MOTOR; } motor_mixing.nb_failure = 0; + motor_mixing.nb_saturation = 0; } __attribute__ ((always_inline)) static inline void offset_commands(int32_t offset) { @@ -157,7 +158,11 @@ void motor_mixing_run_spinup(uint32_t counter, uint32_t max_counter) void motor_mixing_run(bool_t motors_on, bool_t override_on, pprz_t in_cmd[] ) { uint8_t i; +#if !HITL if (motors_on) { +#else + if (FALSE) { +#endif int32_t min_cmd = INT32_MAX; int32_t max_cmd = INT32_MIN; /* do the mixing in float to avoid overflows, implicitly casted back to int32_t */ @@ -186,11 +191,13 @@ void motor_mixing_run(bool_t motors_on, bool_t override_on, pprz_t in_cmd[] ) { int32_t saturation_offset = MOTOR_MIXING_MAX_MOTOR - max_cmd; BoundAbs(saturation_offset, MOTOR_MIXING_MAX_SATURATION_OFFSET); offset_commands(saturation_offset); + motor_mixing.nb_saturation++; } else if (min_cmd < MOTOR_MIXING_MIN_MOTOR) { int32_t saturation_offset = MOTOR_MIXING_MIN_MOTOR - min_cmd; BoundAbs(saturation_offset, MOTOR_MIXING_MAX_SATURATION_OFFSET); offset_commands(saturation_offset); + motor_mixing.nb_saturation++; } /* For testing motor failure */ diff --git a/sw/airborne/subsystems/actuators/motor_mixing.h b/sw/airborne/subsystems/actuators/motor_mixing.h index 3283983c3e..e62027609a 100644 --- a/sw/airborne/subsystems/actuators/motor_mixing.h +++ b/sw/airborne/subsystems/actuators/motor_mixing.h @@ -37,6 +37,7 @@ struct MotorMixing { int32_t trim[MOTOR_MIXING_NB_MOTOR]; bool_t override_enabled[MOTOR_MIXING_NB_MOTOR]; int32_t override_value[MOTOR_MIXING_NB_MOTOR]; + uint32_t nb_saturation; uint32_t nb_failure; }; diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index 2216644305..f10bcfd9e5 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -272,7 +272,7 @@ void ahrs_set_accel_gains(void) { */ ahrs_impl.accel_inv_kp = 4096 * 9.81 / (2 * ahrs_impl.accel_omega * ahrs_impl.accel_zeta * - AHRS_PROPAGATE_FREQUENCY / AHRS_PROPAGATE_FREQUENCY); + AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY); /* Complementary filter integral gain * Ki = omega^2 / AHRS_CORRECT_FREQUENCY @@ -547,8 +547,8 @@ void ahrs_update_gps(void) { #endif #if AHRS_USE_GPS_HEADING && USE_GPS - //got a 3d fix,ground speed > 0.5 m/s and course accuracy is better than 10deg - if(gps.fix == GPS_FIX_3D && + //got a 3d fix, ground speed > 5.0 m/s and course accuracy is better than 10deg + if (gps.fix == GPS_FIX_3D && gps.gspeed >= 500 && gps.cacc <= RadOfDeg(10*1e7)) { diff --git a/sw/airborne/subsystems/datalink/Makefile b/sw/airborne/subsystems/datalink/Makefile index 467f254a15..f6a3891f8e 100644 --- a/sw/airborne/subsystems/datalink/Makefile +++ b/sw/airborne/subsystems/datalink/Makefile @@ -48,6 +48,7 @@ EXPORT_FILES = \ transport.h \ datalink.h \ $(PAPARAZZI_SRC)/sw/include/std.h \ + $(PAPARAZZI_SRC)/sw/include/message_pragmas.h \ $(PAPARAZZI_HOME)/var/include/messages.h \ $(PAPARAZZI_HOME)/var/include/dl_protocol.h diff --git a/sw/airborne/subsystems/datalink/superbitrf.c b/sw/airborne/subsystems/datalink/superbitrf.c index dca6705d1d..e44cf6983d 100644 --- a/sw/airborne/subsystems/datalink/superbitrf.c +++ b/sw/airborne/subsystems/datalink/superbitrf.c @@ -28,6 +28,7 @@ #include #include "paparazzi.h" +#include "led.h" #include "mcu_periph/spi.h" #include "mcu_periph/sys_time.h" #include "mcu_periph/gpio.h" @@ -232,6 +233,10 @@ void superbitrf_event(void) { static uint8_t packet_size, tx_packet[16]; static bool_t start_transfer = TRUE; +#ifdef RADIO_CONTROL_LED + static uint32_t slowLedCpt = 0; +#endif + // Check if the cyrf6936 isn't busy and the uperbitrf is initialized if(superbitrf.cyrf6936.status != CYRF6936_IDLE) return; @@ -322,6 +327,16 @@ void superbitrf_event(void) { /* When the superbitrf is in binding mode */ case SUPERBITRF_BINDING: + +#ifdef RADIO_CONTROL_LED + slowLedCpt++; + if(slowLedCpt>100000){ + + LED_TOGGLE(RADIO_CONTROL_LED); + slowLedCpt = 0; + } +#endif + /* Switch the different states */ switch (superbitrf.state) { case 0: @@ -396,6 +411,16 @@ void superbitrf_event(void) { /* When the receiver is synchronizing with the transmitter */ case SUPERBITRF_SYNCING_A: case SUPERBITRF_SYNCING_B: + +#ifdef RADIO_CONTROL_LED + slowLedCpt++; + if(slowLedCpt>5000){ + + LED_TOGGLE(RADIO_CONTROL_LED); + slowLedCpt = 0; + } +#endif + /* Switch the different states */ switch (superbitrf.state) { case 0: @@ -499,6 +524,11 @@ void superbitrf_event(void) { /* Normal transfer mode */ case SUPERBITRF_TRANSFER: + +#ifdef RADIO_CONTROL_LED + LED_ON(RADIO_CONTROL_LED); +#endif + /* Switch the different states */ switch (superbitrf.state) { case 0: diff --git a/sw/airborne/subsystems/datalink/udp.c b/sw/airborne/subsystems/datalink/udp.c index 88ab870c1d..264a508294 100644 --- a/sw/airborne/subsystems/datalink/udp.c +++ b/sw/airborne/subsystems/datalink/udp.c @@ -67,28 +67,29 @@ void udp_receive( void ) { } //Read from the network - network_read(network, udp_read_buffer, TRANSPORT_PAYLOAD_LEN); + uint16_t read = network_read(network, udp_read_buffer, TRANSPORT_PAYLOAD_LEN); + if(read > 0) { - //Parse the packet - if(udp_read_buffer[0] == STX) { - uint8_t size = udp_read_buffer[1]-4; // minus STX, LENGTH, CK_A, CK_B - uint8_t ck_aa, ck_bb; - ck_aa = ck_bb = size+4; + //Parse the packet + if(udp_read_buffer[0] == STX) { + uint8_t size = udp_read_buffer[1]-4; // minus STX, LENGTH, CK_A, CK_B + uint8_t ck_aa, ck_bb; + ck_aa = ck_bb = size+4; - // index-offset plus 2 for STX and LENGTH - for (int i = 2; i < size+2; i++) { - dl_buffer[i-2] = udp_read_buffer[i]; - ck_aa += udp_read_buffer[i]; - ck_bb += ck_aa; - } - - // if both checksums are good, tell datalink that the message is available - if (udp_read_buffer[2+size] == ck_aa && udp_read_buffer[2+size+1] == ck_bb) { - dl_msg_available = TRUE; + // index-offset plus 2 for STX and LENGTH + for (int i = 2; i < size+2; i++) { + dl_buffer[i-2] = udp_read_buffer[i]; + ck_aa += udp_read_buffer[i]; + ck_bb += ck_aa; + } + + // if both checksums are good, tell datalink that the message is available + if (udp_read_buffer[2+size] == ck_aa && udp_read_buffer[2+size+1] == ck_bb) { + dl_msg_available = TRUE; + } } + memset(&udp_read_buffer[0], 0, sizeof(udp_read_buffer)); } - - memset(&udp_read_buffer[0], 0, sizeof(udp_read_buffer)); } diff --git a/sw/airborne/subsystems/gps/gps_datalink.c b/sw/airborne/subsystems/gps/gps_datalink.c new file mode 100644 index 0000000000..09987daeeb --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_datalink.c @@ -0,0 +1,81 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * + * 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. + */ + +/** + * @file gps_datalink.c + * @brief GPS system based on datalink + * + * This GPS parses the datalink REMOTE_GPS packet and sets the + * GPS structure to the values received. + */ + +#include "subsystems/gps.h" + +bool_t gps_available; ///< Is set to TRUE when a new REMOTE_GPS packet is received and parsed + +/** GPS initialization */ +void gps_impl_init(void) { + gps.fix = GPS_FIX_NONE; + gps_available = FALSE; + gps.gspeed = 700; // To enable course setting + gps.cacc = 0; // To enable course setting +} + +/** Parse the REMOTE_GPS datalink packet */ +void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t ecef_z, int32_t lat, int32_t lon, int32_t alt, + int32_t hmsl, int32_t ecef_xd, int32_t ecef_yd, int32_t ecef_zd, uint32_t tow, int32_t course) { + + gps.lla_pos.lat = RadOfDeg(lat); + gps.lla_pos.lon = RadOfDeg(lon); + gps.lla_pos.alt = alt; + gps.hmsl = hmsl; + + gps.ecef_pos.x = ecef_x; + gps.ecef_pos.y = ecef_y; + gps.ecef_pos.z = ecef_z; + + gps.ecef_vel.x = ecef_xd; + gps.ecef_vel.y = ecef_yd; + gps.ecef_vel.z = ecef_zd; + + gps.course = course; + gps.num_sv = numsv; + gps.tow = tow; + gps.fix = GPS_FIX_3D; + gps_available = TRUE; + +#if GPS_USE_LATLONG + // Computes from (lat, long) in the referenced UTM zone + struct LlaCoor_f lla_f; + lla_f.lat = ((float) gps.lla_pos.lat) / 1e7; + lla_f.lon = ((float) gps.lla_pos.lon) / 1e7; + struct UtmCoor_f utm_f; + utm_f.zone = nav_utm_zone0; + // convert to utm + utm_of_lla_f(&utm_f, &lla_f); + // copy results of utm conversion + gps.utm_pos.east = utm_f.east*100; + gps.utm_pos.north = utm_f.north*100; + gps.utm_pos.alt = gps.lla_pos.alt; + gps.utm_pos.zone = nav_utm_zone0; +#endif +} + diff --git a/sw/airborne/subsystems/gps/gps_datalink.h b/sw/airborne/subsystems/gps/gps_datalink.h new file mode 100644 index 0000000000..3eb1800844 --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_datalink.h @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * + * 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. + */ + +/** + * @file gps_datalink.h + * @brief GPS system based on datalink + * + * This GPS parses the datalink REMOTE_GPS packet and sets the + * GPS structure to the values received. + */ + +#ifndef GPS_DATALINK_H +#define GPS_DATALINK_H + +#include "std.h" +#define GPS_NB_CHANNELS 0 + +extern bool_t gps_available; +extern void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t ecef_z, int32_t lat, int32_t lon, int32_t alt, + int32_t hmsl, int32_t ecef_xd, int32_t ecef_yd, int32_t ecef_zd, uint32_t tow, int32_t course); + + +#define GpsEvent(_sol_available_callback) { \ + if (gps_available) { \ + gps.last_msg_ticks = sys_time.nb_sec_rem; \ + gps.last_msg_time = sys_time.nb_sec; \ + if (gps.fix == GPS_FIX_3D) { \ + gps.last_3dfix_ticks = sys_time.nb_sec_rem; \ + gps.last_3dfix_time = sys_time.nb_sec; \ + } \ + _sol_available_callback(); \ + gps_available = FALSE; \ + } \ + } + + +#endif /* GPS_DATALINK_H */ diff --git a/sw/airborne/subsystems/gps/gps_sim_hitl.c b/sw/airborne/subsystems/gps/gps_sim_hitl.c new file mode 100644 index 0000000000..87ed9082a9 --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_sim_hitl.c @@ -0,0 +1,35 @@ +/* + * Copyright (C) 2014 Sergey Krukowski + * + * 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. + */ + +/** + * @file subsystems/gps/gps_sim_hitl.c + * GPS subsystem simulation from rotorcrafts horizontal/vertical reference system + */ + +#include "subsystems/gps.h" + +bool_t gps_available; +uint32_t gps_sim_hitl_timer; + +void gps_impl_init(void) { + gps.fix = GPS_FIX_NONE; + gps_available = FALSE; +} diff --git a/sw/airborne/subsystems/gps/gps_sim_hitl.h b/sw/airborne/subsystems/gps/gps_sim_hitl.h new file mode 100644 index 0000000000..b3e6395bc4 --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_sim_hitl.h @@ -0,0 +1,83 @@ +/* + * Copyright (C) 2014 Sergey Krukowski + * + * 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. + */ + +/** + * @file subsystems/gps/gps_sim_hitl.h + * GPS subsystem simulation from rotorcrafts horizontal/vertical reference system + */ + +#ifndef GPS_SIM_HITL_H +#define GPS_SIM_HITL_H + +#include "std.h" +#include "state.h" +#include "guidance/guidance_h.h" +#include "guidance/guidance_v.h" + +extern bool_t gps_available; +extern uint32_t gps_sim_hitl_timer; + +#define GpsEvent(_sol_available_callback) { \ + if (SysTimeTimer(gps_sim_hitl_timer) > 100000) { \ + SysTimeTimerStart(gps_sim_hitl_timer); \ + gps.last_msg_ticks = sys_time.nb_sec_rem; \ + gps.last_msg_time = sys_time.nb_sec; \ + if (state.ned_initialized_i) { \ + if (!autopilot_in_flight) { \ + struct Int32Vect2 zero_vector; \ + INT_VECT2_ZERO(zero_vector); \ + gh_set_ref(zero_vector, zero_vector, zero_vector); \ + INT_VECT2_ZERO(guidance_h_pos_ref); \ + INT_VECT2_ZERO(guidance_h_speed_ref); \ + INT_VECT2_ZERO(guidance_h_accel_ref); \ + gv_set_ref(0, 0, 0); \ + guidance_v_z_ref = 0; \ + guidance_v_zd_ref = 0; \ + guidance_v_zdd_ref = 0; \ + } \ + struct NedCoor_i ned_c; \ + ned_c.x = guidance_h_pos_ref.x * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM; \ + ned_c.y = guidance_h_pos_ref.y * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM; \ + ned_c.z = guidance_v_z_ref * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM; \ + ecef_of_ned_point_i(&gps.ecef_pos, &state.ned_origin_i, &ned_c); \ + gps.lla_pos.alt = state.ned_origin_i.lla.alt - ned_c.z; \ + gps.hmsl = state.ned_origin_i.hmsl - ned_c.z; \ + ned_c.x = guidance_h_speed_ref.x * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM; \ + ned_c.y = guidance_h_speed_ref.y * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM; \ + ned_c.z = guidance_v_zd_ref * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM; \ + ecef_of_ned_vect_i(&gps.ecef_vel, &state.ned_origin_i, &ned_c); \ + gps.fix = GPS_FIX_3D; \ + gps.last_3dfix_ticks = sys_time.nb_sec_rem; \ + gps.last_3dfix_time = sys_time.nb_sec; \ + gps_available = TRUE; \ + } \ + else { \ + struct Int32Vect2 zero_vector; \ + INT_VECT2_ZERO(zero_vector); \ + gh_set_ref(zero_vector, zero_vector, zero_vector); \ + gv_set_ref(0, 0, 0); \ + } \ + _sol_available_callback(); \ + gps_available = FALSE; \ + } \ + } + +#endif /* GPS_SIM_HITL_H */ diff --git a/sw/airborne/subsystems/ins.c b/sw/airborne/subsystems/ins.c index e99e8d0b84..c60ddc05fd 100644 --- a/sw/airborne/subsystems/ins.c +++ b/sw/airborne/subsystems/ins.c @@ -64,8 +64,5 @@ void WEAK ins_reset_utm_zone(struct UtmCoor_f * utm __attribute__((unused))) {} void WEAK ins_propagate(void) {} -void WEAK ins_update_baro(void) {} - void WEAK ins_update_gps(void) {} -void WEAK ins_update_sonar(void) {} diff --git a/sw/airborne/subsystems/ins.h b/sw/airborne/subsystems/ins.h index 899eeb70dc..dbc8d500ea 100644 --- a/sw/airborne/subsystems/ins.h +++ b/sw/airborne/subsystems/ins.h @@ -86,21 +86,10 @@ extern void ins_reset_utm_zone(struct UtmCoor_f * utm); */ extern void ins_propagate(void); -/** Update INS state with barometer measurements. - * Does nothing if not implemented by specific INS algorithm. - */ -extern void ins_update_baro(void); - /** Update INS state with GPS measurements. * Reads the global #gps data struct. * Does nothing if not implemented by specific INS algorithm. */ extern void ins_update_gps(void); -/** Update INS state with sonar measurements. - * Does nothing if not implemented by specific INS algorithm. - */ -extern void ins_update_sonar(void); - - #endif /* INS_H */ diff --git a/sw/airborne/subsystems/ins/hf_float.c b/sw/airborne/subsystems/ins/hf_float.c index 27cc1c5de3..3c6d44506d 100644 --- a/sw/airborne/subsystems/ins/hf_float.c +++ b/sw/airborne/subsystems/ins/hf_float.c @@ -32,7 +32,7 @@ #include "state.h" #include "subsystems/gps.h" #include - +#include "filters/low_pass_filter.h" #include "generated/airframe.h" #ifdef SITL @@ -71,6 +71,11 @@ #define HFF_R_SPEED_MIN 1. #endif +/* low pass filter variables */ +Butterworth2LowPass_int filter_x; +Butterworth2LowPass_int filter_y; +Butterworth2LowPass_int filter_z; + /* gps measurement noise */ float Rgps_pos, Rgps_vel; @@ -100,54 +105,6 @@ float b2_hff_y_meas; /* counter for hff propagation*/ int b2_hff_ps_counter; - -/* - * accel(in body frame) buffer - */ -#define ACC_RB_MAXN 64 -struct AccBuf { - struct Int32Vect3 buf[ACC_RB_MAXN]; - int r; /* pos to read from, oldest measurement */ - int w; /* pos to write to */ - int n; /* number of elements in rb */ - int size; -}; -struct AccBuf acc_body; -struct Int32Vect3 acc_body_mean; - -void b2_hff_store_accel_body(void) { - INT32_RMAT_TRANSP_VMULT(acc_body.buf[acc_body.w], imu.body_to_imu_rmat, imu.accel); - acc_body.w = (acc_body.w + 1) < acc_body.size ? (acc_body.w + 1) : 0; - - /* once the buffer is full it always has the last acc_body.size accel measurements */ - if (acc_body.n < acc_body.size) { - acc_body.n++; - } else { - acc_body.r = (acc_body.r + 1) < acc_body.size ? (acc_body.r + 1) : 0; - } -} - -/** compute the mean of the last n accel measurements */ -static inline void b2_hff_compute_accel_body_mean(uint8_t n) { - struct Int32Vect3 sum; - int i, j; - - INT_VECT3_ZERO(sum); - - if (n > 1) { - if (n > acc_body.n) { - n = acc_body.n; - } - for (i = 1; i <= n; i++) { - j = (acc_body.w - i) > 0 ? (acc_body.w - i) : (acc_body.w - i + acc_body.size); - VECT3_ADD(sum, acc_body.buf[j]); - } - VECT3_SDIV(acc_body_mean, sum, n); - } else { - VECT3_COPY(acc_body_mean, sum); - } -} - /* * For GPS lag compensation * @@ -216,25 +173,24 @@ uint16_t b2_hff_lost_limit; uint16_t b2_hff_lost_counter; #ifdef GPS_LAG -static inline void b2_hff_get_past_accel(unsigned int back_n); -static inline void b2_hff_rb_put_state(struct HfilterFloat* source); -static inline void b2_hff_rb_drop_last(void); -static inline void b2_hff_set_state(struct HfilterFloat* dest, struct HfilterFloat* source); +static void b2_hff_get_past_accel(unsigned int back_n); +static void b2_hff_rb_put_state(struct HfilterFloat* source); +static void b2_hff_rb_drop_last(void); +static void b2_hff_set_state(struct HfilterFloat* dest, struct HfilterFloat* source); #endif +static void b2_hff_init_x(float init_x, float init_xdot); +static void b2_hff_init_y(float init_y, float init_ydot); -static inline void b2_hff_init_x(float init_x, float init_xdot); -static inline void b2_hff_init_y(float init_y, float init_ydot); +static void b2_hff_propagate_x(struct HfilterFloat* hff_work); +static void b2_hff_propagate_y(struct HfilterFloat* hff_work); -static inline void b2_hff_propagate_x(struct HfilterFloat* hff_work); -static inline void b2_hff_propagate_y(struct HfilterFloat* hff_work); +static void b2_hff_update_x(struct HfilterFloat* hff_work, float x_meas, float Rpos); +static void b2_hff_update_y(struct HfilterFloat* hff_work, float y_meas, float Rpos); -static inline void b2_hff_update_x(struct HfilterFloat* hff_work, float x_meas, float Rpos); -static inline void b2_hff_update_y(struct HfilterFloat* hff_work, float y_meas, float Rpos); - -static inline void b2_hff_update_xdot(struct HfilterFloat* hff_work, float vel, float Rvel); -static inline void b2_hff_update_ydot(struct HfilterFloat* hff_work, float vel, float Rvel); +static void b2_hff_update_xdot(struct HfilterFloat* hff_work, float vel, float Rvel); +static void b2_hff_update_ydot(struct HfilterFloat* hff_work, float vel, float Rvel); #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" @@ -277,11 +233,6 @@ void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot) { Rgps_vel = HFF_R_SPEED; b2_hff_init_x(init_x, init_xdot); b2_hff_init_y(init_y, init_ydot); - /* init buffer for mean accel calculation */ - acc_body.r = 0; - acc_body.w = 0; - acc_body.n = 0; - acc_body.size = ACC_RB_MAXN; #ifdef GPS_LAG /* init buffer for past mean accel values */ acc_buf_r = 0; @@ -319,9 +270,13 @@ void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot) { register_periodic_telemetry(DefaultPeriodic, "HFF_GPS", send_hff_gps); #endif #endif + + init_butterworth_2_low_pass_int(&filter_x, 14., (1. /AHRS_PROPAGATE_FREQUENCY), 0); + init_butterworth_2_low_pass_int(&filter_y, 14., (1. /AHRS_PROPAGATE_FREQUENCY), 0); + init_butterworth_2_low_pass_int(&filter_z, 14., (1. /AHRS_PROPAGATE_FREQUENCY), 0); } -static inline void b2_hff_init_x(float init_x, float init_xdot) { +static void b2_hff_init_x(float init_x, float init_xdot) { b2_hff_state.x = init_x; b2_hff_state.xdot = init_xdot; int i, j; @@ -330,10 +285,9 @@ static inline void b2_hff_init_x(float init_x, float init_xdot) { b2_hff_state.xP[i][j] = 0.; b2_hff_state.xP[i][i] = INIT_PXX; } - } -static inline void b2_hff_init_y(float init_y, float init_ydot) { +static void b2_hff_init_y(float init_y, float init_ydot) { b2_hff_state.y = init_y; b2_hff_state.ydot = init_ydot; int i, j; @@ -345,7 +299,7 @@ static inline void b2_hff_init_y(float init_y, float init_ydot) { } #ifdef GPS_LAG -static inline void b2_hff_store_accel_ltp(float x, float y) { +static void b2_hff_store_accel_ltp(float x, float y) { past_accel[acc_buf_w].x = x; past_accel[acc_buf_w].y = y; INC_ACC_IDX(acc_buf_w); @@ -358,7 +312,7 @@ static inline void b2_hff_store_accel_ltp(float x, float y) { } /* get the accel values from back_n steps ago */ -static inline void b2_hff_get_past_accel(unsigned int back_n) { +static void b2_hff_get_past_accel(unsigned int back_n) { int i; if (back_n > acc_buf_n) { PRINT_DBG(1, ("Cannot go back %d steps, going back only %d instead!\n", back_n, acc_buf_n)); @@ -376,7 +330,7 @@ static inline void b2_hff_get_past_accel(unsigned int back_n) { PRINT_DBG(3, ("get past accel. buf_n: %2d \tbuf_w: %2d \tback_n: %2d \ti: %2d \txdd: %f \tydd: %f\n", acc_buf_n, acc_buf_w, back_n, i, b2_hff_xdd_meas, b2_hff_ydd_meas)); } -static inline void b2_hff_rb_put_state(struct HfilterFloat* source) { +static void b2_hff_rb_put_state(struct HfilterFloat* source) { /* copy state from source into buffer */ b2_hff_set_state(b2_hff_rb_put, source); b2_hff_rb_put->lag_counter = 0; @@ -394,7 +348,7 @@ static inline void b2_hff_rb_put_state(struct HfilterFloat* source) { PRINT_DBG(2, ("put state. fill count now: %d\n", b2_hff_rb_n)); } -static inline void b2_hff_rb_drop_last(void) { +static void b2_hff_rb_drop_last(void) { if (b2_hff_rb_n > 0) { INC_RB_POINTER(b2_hff_rb_last); b2_hff_rb_n--; @@ -406,9 +360,8 @@ static inline void b2_hff_rb_drop_last(void) { PRINT_DBG(2, ("drop last state. fill count now: %d\n", b2_hff_rb_n)); } - /* copy source state to dest state */ -static inline void b2_hff_set_state(struct HfilterFloat* dest, struct HfilterFloat* source) { +static void b2_hff_set_state(struct HfilterFloat* dest, struct HfilterFloat* source) { dest->x = source->x; dest->xdot = source->xdot; dest->xdotdot = source->xdotdot; @@ -423,7 +376,7 @@ static inline void b2_hff_set_state(struct HfilterFloat* dest, struct HfilterFlo } } -static inline void b2_hff_propagate_past(struct HfilterFloat* hff_past) { +static void b2_hff_propagate_past(struct HfilterFloat* hff_past) { PRINT_DBG(1, ("enter propagate past: %d\n", hff_past->lag_counter)); /* run max MAX_PP_STEPS propagation steps */ for (int i=0; i < MAX_PP_STEPS; i++) { @@ -463,7 +416,6 @@ static inline void b2_hff_propagate_past(struct HfilterFloat* hff_past) { #endif /* GPS_LAG */ - void b2_hff_propagate(void) { if (b2_hff_lost_counter < b2_hff_lost_limit) b2_hff_lost_counter++; @@ -475,25 +427,27 @@ void b2_hff_propagate(void) { } #endif - /* store body accelerations for mean computation */ - b2_hff_store_accel_body(); + /* rotate imu accel measurement to body frame and filter */ + struct Int32Vect3 acc_meas_body; + INT32_RMAT_TRANSP_VMULT(acc_meas_body, imu.body_to_imu_rmat, imu.accel); + + struct Int32Vect3 acc_body_filtered; + acc_body_filtered.x = update_butterworth_2_low_pass_int(&filter_x, acc_meas_body.x); + acc_body_filtered.y = update_butterworth_2_low_pass_int(&filter_y, acc_meas_body.y); + acc_body_filtered.z = update_butterworth_2_low_pass_int(&filter_z, acc_meas_body.z); /* propagate current state if it is time */ if (b2_hff_ps_counter == HFF_PRESCALER) { b2_hff_ps_counter = 1; - if (b2_hff_lost_counter < b2_hff_lost_limit) { - /* compute float ltp mean acceleration */ - b2_hff_compute_accel_body_mean(HFF_PRESCALER); - struct Int32Vect3 mean_accel_ltp; + struct Int32Vect3 filtered_accel_ltp; struct Int32RMat* ltp_to_body_rmat = stateGetNedToBodyRMat_i(); - INT32_RMAT_TRANSP_VMULT(mean_accel_ltp, (*ltp_to_body_rmat), acc_body_mean); - b2_hff_xdd_meas = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.x); - b2_hff_ydd_meas = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.y); + INT32_RMAT_TRANSP_VMULT(filtered_accel_ltp, (*ltp_to_body_rmat), acc_body_filtered); + b2_hff_xdd_meas = ACCEL_FLOAT_OF_BFP(filtered_accel_ltp.x); + b2_hff_ydd_meas = ACCEL_FLOAT_OF_BFP(filtered_accel_ltp.y); #ifdef GPS_LAG b2_hff_store_accel_ltp(b2_hff_xdd_meas, b2_hff_ydd_meas); #endif - /* * propagate current state */ @@ -520,9 +474,6 @@ void b2_hff_propagate(void) { } } - - - void b2_hff_update_gps(struct FloatVect2* pos_ned, struct FloatVect2* speed_ned) { b2_hff_lost_counter = 0; @@ -569,7 +520,7 @@ void b2_hff_update_gps(struct FloatVect2* pos_ned, struct FloatVect2* speed_ned) /* apparently missed a GPS update, try next saved state */ PRINT_DBG(2, ("try next saved state\n")); b2_hff_rb_drop_last(); - b2_hff_update_gps(); + b2_hff_update_gps(pos_ned, speed_ned); } } else if (save_counter < 0) { /* ringbuffer empty -> save output filter state at next GPS validity point in time */ @@ -615,7 +566,7 @@ void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel) { Pk1 = F * Pk0 * F' + Q; */ -static inline void b2_hff_propagate_x(struct HfilterFloat* hff_work) { +static void b2_hff_propagate_x(struct HfilterFloat* hff_work) { /* update state */ hff_work->xdotdot = b2_hff_xdd_meas; hff_work->x = hff_work->x + DT_HFILTER * hff_work->xdot + DT_HFILTER*DT_HFILTER/2 * hff_work->xdotdot; @@ -632,7 +583,7 @@ static inline void b2_hff_propagate_x(struct HfilterFloat* hff_work) { hff_work->xP[1][1] = FPF11 + Qdotdot; } -static inline void b2_hff_propagate_y(struct HfilterFloat* hff_work) { +static void b2_hff_propagate_y(struct HfilterFloat* hff_work) { /* update state */ hff_work->ydotdot = b2_hff_ydd_meas; hff_work->y = hff_work->y + DT_HFILTER * hff_work->ydot + DT_HFILTER*DT_HFILTER/2 * hff_work->ydotdot; @@ -674,7 +625,7 @@ void b2_hff_update_pos (struct FloatVect2 pos, struct FloatVect2 Rpos) { b2_hff_update_y(&b2_hff_state, pos.y, Rpos.y); } -static inline void b2_hff_update_x(struct HfilterFloat* hff_work, float x_meas, float Rpos) { +static void b2_hff_update_x(struct HfilterFloat* hff_work, float x_meas, float Rpos) { b2_hff_x_meas = x_meas; const float y = x_meas - hff_work->x; @@ -696,7 +647,7 @@ static inline void b2_hff_update_x(struct HfilterFloat* hff_work, float x_meas, hff_work->xP[1][1] = P22; } -static inline void b2_hff_update_y(struct HfilterFloat* hff_work, float y_meas, float Rpos) { +static void b2_hff_update_y(struct HfilterFloat* hff_work, float y_meas, float Rpos) { b2_hff_y_meas = y_meas; const float y = y_meas - hff_work->y; @@ -719,8 +670,6 @@ static inline void b2_hff_update_y(struct HfilterFloat* hff_work, float y_meas, } - - /* * * Update velocity @@ -745,7 +694,7 @@ void b2_hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel) { b2_hff_update_ydot(&b2_hff_state, vel.y, Rvel.y); } -static inline void b2_hff_update_xdot(struct HfilterFloat* hff_work, float vel, float Rvel) { +static void b2_hff_update_xdot(struct HfilterFloat* hff_work, float vel, float Rvel) { b2_hff_xd_meas = vel; const float yd = vel - hff_work->xdot; @@ -767,7 +716,7 @@ static inline void b2_hff_update_xdot(struct HfilterFloat* hff_work, float vel, hff_work->xP[1][1] = P22; } -static inline void b2_hff_update_ydot(struct HfilterFloat* hff_work, float vel, float Rvel) { +static void b2_hff_update_ydot(struct HfilterFloat* hff_work, float vel, float Rvel) { b2_hff_yd_meas = vel; const float yd = vel - hff_work->ydot; diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c index 9a6966c081..36773d21d7 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.c +++ b/sw/airborne/subsystems/ins/ins_float_invariant.c @@ -459,15 +459,11 @@ void ahrs_update_gps(void) { } #else if (state.ned_initialized_f) { - struct NedCoor_f gps_pos_cm_ned; struct EcefCoor_f ecef_pos, ecef_vel; ECEF_FLOAT_OF_BFP(ecef_pos, gps.ecef_pos); - ned_of_ecef_point_f(&gps_pos_cm_ned, &state.ned_origin_f, &ecef_pos); - VECT3_SDIV(ins_impl.meas.pos_gps, gps_pos_cm_ned, 100.0f); - struct NedCoor_f gps_speed_cm_s_ned; + ned_of_ecef_point_f(&ins_impl.meas.pos_gps, &state.ned_origin_f, &ecef_pos); ECEF_FLOAT_OF_BFP(ecef_vel, gps.ecef_vel); - ned_of_ecef_vect_f(&gps_speed_cm_s_ned, &state.ned_origin_f, &ecef_vel); - VECT3_SDIV(ins_impl.meas.speed_gps, gps_speed_cm_s_ned, 100.0f); + ned_of_ecef_vect_f(&ins_impl.meas.speed_gps, &state.ned_origin_f, &ecef_vel); } #endif } diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough.c b/sw/airborne/subsystems/ins/ins_gps_passthrough.c index 9723fa5d7e..f0d8e16cfa 100644 --- a/sw/airborne/subsystems/ins/ins_gps_passthrough.c +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough.c @@ -22,7 +22,8 @@ /** * @file subsystems/ins/ins_gps_passthrough.c * - * Simply passes GPS position and velocity through to the state interface. + * Simply converts GPS ECEF position and velocity to NED + * and passes it through to the state interface. */ #include "subsystems/ins.h" @@ -32,57 +33,124 @@ #include "state.h" #include "subsystems/gps.h" -#include "subsystems/nav.h" -void ins_init(void) { - struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; - stateSetLocalUtmOrigin_f(&utm0); - stateSetPositionUtm_f(&utm0); +#ifndef USE_INS_NAV_INIT +#define USE_INS_NAV_INIT TRUE +PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE") +#endif - ins.status = INS_RUNNING; +#if USE_INS_NAV_INIT +#include "generated/flight_plan.h" +#endif + +struct InsGpsPassthrough { + struct LtpDef_i ltp_def; + bool_t ltp_initialized; + + /* output LTP NED */ + struct NedCoor_i ltp_pos; + struct NedCoor_i ltp_speed; + struct NedCoor_i ltp_accel; +}; + +struct InsGpsPassthrough ins_impl; + +#if PERIODIC_TELEMETRY +#include "subsystems/datalink/telemetry.h" + +static void send_ins(void) { + DOWNLINK_SEND_INS(DefaultChannel, DefaultDevice, + &ins_impl.ltp_pos.x, &ins_impl.ltp_pos.y, &ins_impl.ltp_pos.z, + &ins_impl.ltp_speed.x, &ins_impl.ltp_speed.y, &ins_impl.ltp_speed.z, + &ins_impl.ltp_accel.x, &ins_impl.ltp_accel.y, &ins_impl.ltp_accel.z); } -void ins_reset_local_origin(void) { - struct UtmCoor_f utm; -#ifdef GPS_USE_LATLONG - /* Recompute UTM coordinates in this zone */ - struct LlaCoor_f lla; - lla.lat = gps.lla_pos.lat/1e7; - lla.lon = gps.lla_pos.lon/1e7; - utm.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1; - utm_of_lla_f(&utm, &lla); -#else - utm.zone = gps.utm_pos.zone; - utm.east = gps.utm_pos.east / 100.0f; - utm.north = gps.utm_pos.north / 100.0f; +static void send_ins_z(void) { + static const float fake_baro_z = 0.0; + DOWNLINK_SEND_INS_Z(DefaultChannel, DefaultDevice, + &fake_baro_z, &ins_impl.ltp_pos.z, &ins_impl.ltp_speed.z, &ins_impl.ltp_accel.z); +} + +static void send_ins_ref(void) { + static const float fake_qfe = 0.0; + if (ins_impl.ltp_initialized) { + DOWNLINK_SEND_INS_REF(DefaultChannel, DefaultDevice, + &ins_impl.ltp_def.ecef.x, &ins_impl.ltp_def.ecef.y, &ins_impl.ltp_def.ecef.z, + &ins_impl.ltp_def.lla.lat, &ins_impl.ltp_def.lla.lon, &ins_impl.ltp_def.lla.alt, + &ins_impl.ltp_def.hmsl, &fake_qfe); + } +} #endif - // ground_alt - utm.alt = gps.hmsl / 1000.0f; - // reset state UTM ref - stateSetLocalUtmOrigin_f(&utm); + +void ins_init(void) { + +#if USE_INS_NAV_INIT + struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */ + llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0); + llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0); + /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */ + llh_nav0.alt = NAV_ALT0 + NAV_MSL0; + + struct EcefCoor_i ecef_nav0; + ecef_of_lla_i(&ecef_nav0, &llh_nav0); + + ltp_def_from_ecef_i(&ins_impl.ltp_def, &ecef_nav0); + ins_impl.ltp_def.hmsl = NAV_ALT0; + stateSetLocalOrigin_i(&ins_impl.ltp_def); + + ins_impl.ltp_initialized = TRUE; +#else + ins_impl.ltp_initialized = FALSE; +#endif + + INT32_VECT3_ZERO(ins_impl.ltp_pos); + INT32_VECT3_ZERO(ins_impl.ltp_speed); + INT32_VECT3_ZERO(ins_impl.ltp_accel); + +#if PERIODIC_TELEMETRY + register_periodic_telemetry(DefaultPeriodic, "INS", send_ins); + register_periodic_telemetry(DefaultPeriodic, "INS_Z", send_ins_z); + register_periodic_telemetry(DefaultPeriodic, "INS_REF", send_ins_ref); +#endif +} + +void ins_periodic(void) { + if (ins_impl.ltp_initialized) + ins.status = INS_RUNNING; +} + + +void ins_reset_local_origin(void) { + ltp_def_from_ecef_i(&ins_impl.ltp_def, &gps.ecef_pos); + ins_impl.ltp_def.lla.alt = gps.lla_pos.alt; + ins_impl.ltp_def.hmsl = gps.hmsl; + stateSetLocalOrigin_i(&ins_impl.ltp_def); + ins_impl.ltp_initialized = TRUE; } void ins_reset_altitude_ref(void) { - struct UtmCoor_f utm = state.utm_origin_f; - utm.alt = gps.hmsl / 1000.0f; - stateSetLocalUtmOrigin_f(&utm); + ins_impl.ltp_def.lla.alt = gps.lla_pos.alt; + ins_impl.ltp_def.hmsl = gps.hmsl; + stateSetLocalOrigin_i(&ins_impl.ltp_def); } void ins_update_gps(void) { - struct UtmCoor_f utm; - utm.east = gps.utm_pos.east / 100.0f; - utm.north = gps.utm_pos.north / 100.0f; - utm.zone = nav_utm_zone0; - utm.alt = gps.hmsl / 1000.0f; + if (gps.fix == GPS_FIX_3D) { + if (!ins_impl.ltp_initialized) { + ins_reset_local_origin(); + } - // set position - stateSetPositionUtm_f(&utm); + /* simply scale and copy pos/speed from gps */ + struct NedCoor_i gps_pos_cm_ned; + ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_impl.ltp_def, &gps.ecef_pos); + INT32_VECT3_SCALE_2(ins_impl.ltp_pos, gps_pos_cm_ned, + INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); + stateSetPositionNed_i(&ins_impl.ltp_pos); - struct NedCoor_f ned_vel = { - gps.ned_vel.x / 100.0f, - gps.ned_vel.y / 100.0f, - gps.ned_vel.z / 100.0f - }; - // set velocity - stateSetSpeedNed_f(&ned_vel); + struct NedCoor_i gps_speed_cm_s_ned; + ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_impl.ltp_def, &gps.ecef_vel); + INT32_VECT3_SCALE_2(ins_impl.ltp_speed, gps_speed_cm_s_ned, + INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); + stateSetSpeedNed_i(&ins_impl.ltp_speed); + } } diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c b/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c new file mode 100644 index 0000000000..eb209ff789 --- /dev/null +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c @@ -0,0 +1,89 @@ +/* + * Copyright (C) 2004-2012 The Paparazzi Team + * + * 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. + */ + +/** + * @file subsystems/ins/ins_gps_passthrough_utm.c + * + * Simply passes GPS UTM position and velocity through to the state interface. + * For fixedwing firmware since it sets UTM pos only. + */ + +#include "subsystems/ins.h" + +#include +#include + +#include "state.h" +#include "subsystems/gps.h" +#include "subsystems/nav.h" + +void ins_init(void) { + struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; + stateSetLocalUtmOrigin_f(&utm0); + stateSetPositionUtm_f(&utm0); + + ins.status = INS_RUNNING; +} + +void ins_reset_local_origin(void) { + struct UtmCoor_f utm; +#ifdef GPS_USE_LATLONG + /* Recompute UTM coordinates in this zone */ + struct LlaCoor_f lla; + lla.lat = gps.lla_pos.lat/1e7; + lla.lon = gps.lla_pos.lon/1e7; + utm.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1; + utm_of_lla_f(&utm, &lla); +#else + utm.zone = gps.utm_pos.zone; + utm.east = gps.utm_pos.east / 100.0f; + utm.north = gps.utm_pos.north / 100.0f; +#endif + // ground_alt + utm.alt = gps.hmsl / 1000.0f; + // reset state UTM ref + stateSetLocalUtmOrigin_f(&utm); +} + +void ins_reset_altitude_ref(void) { + struct UtmCoor_f utm = state.utm_origin_f; + utm.alt = gps.hmsl / 1000.0f; + stateSetLocalUtmOrigin_f(&utm); +} + +void ins_update_gps(void) { + struct UtmCoor_f utm; + utm.east = gps.utm_pos.east / 100.0f; + utm.north = gps.utm_pos.north / 100.0f; + utm.zone = nav_utm_zone0; + utm.alt = gps.hmsl / 1000.0f; + + // set position + stateSetPositionUtm_f(&utm); + + struct NedCoor_f ned_vel = { + gps.ned_vel.x / 100.0f, + gps.ned_vel.y / 100.0f, + gps.ned_vel.z / 100.0f + }; + // set velocity + stateSetSpeedNed_f(&ned_vel); +} diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index 8693feef2e..d07af572fd 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -58,18 +58,23 @@ #if USE_SONAR - #include "subsystems/sonar.h" - #if !USE_VFF_EXTENDED #error USE_SONAR needs USE_VFF_EXTENDED #endif +/** default sonar to use in INS */ +#ifndef INS_SONAR_ID +#define INS_SONAR_ID ABI_BROADCAST +#endif +abi_event sonar_ev; +static void sonar_cb(uint8_t sender_id, const float *distance); + #ifdef INS_SONAR_THROTTLE_THRESHOLD #include "firmwares/rotorcraft/stabilization.h" #endif #ifndef INS_SONAR_OFFSET -#define INS_SONAR_OFFSET 0 +#define INS_SONAR_OFFSET 0. #endif #define VFF_R_SONAR_0 0.1 #define VFF_R_SONAR_OF_M 0.2 @@ -79,6 +84,10 @@ PRINT_CONFIG_MSG("INS_SONAR_UPDATE_ON_AGL defaulting to FALSE") #endif +#ifndef INS_VFF_R_GPS +#define INS_VFF_R_GPS 2.0 +#endif + #endif // USE_SONAR #ifndef USE_INS_NAV_INIT @@ -147,8 +156,8 @@ void ins_init(void) { #if USE_SONAR ins_impl.update_on_agl = INS_SONAR_UPDATE_ON_AGL; - init_median_filter(&ins_impl.sonar_median); - ins_impl.sonar_offset = INS_SONAR_OFFSET; + // Bind to AGL message + AbiBindMsgAGL(INS_SONAR_ID, &sonar_ev, sonar_cb); #endif ins_impl.vf_reset = FALSE; @@ -225,25 +234,29 @@ void ins_propagate(void) { } static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) { - if (!ins_impl.baro_initialized) { + if (!ins_impl.baro_initialized && *pressure > 1e-7) { + // wait for a first positive value ins_impl.qfe = *pressure; ins_impl.baro_initialized = TRUE; } - if (ins_impl.vf_reset) { - ins_impl.vf_reset = FALSE; - ins_impl.qfe = *pressure; - vff_realign(0.); - ins_update_from_vff(); - } - else { - ins_impl.baro_z = -pprz_isa_height_of_pressure(*pressure, ins_impl.qfe); + + if (ins_impl.baro_initialized) { + if (ins_impl.vf_reset) { + ins_impl.vf_reset = FALSE; + ins_impl.qfe = *pressure; + vff_realign(0.); + ins_update_from_vff(); + } + else { + ins_impl.baro_z = -pprz_isa_height_of_pressure(*pressure, ins_impl.qfe); #if USE_VFF_EXTENDED - vff_update_baro(ins_impl.baro_z); + vff_update_baro(ins_impl.baro_z); #else - vff_update(ins_impl.baro_z); + vff_update(ins_impl.baro_z); #endif + } + ins_ned_to_state(); } - ins_ned_to_state(); } #if USE_GPS @@ -263,6 +276,10 @@ void ins_update_gps(void) { struct NedCoor_i gps_speed_cm_s_ned; ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_impl.ltp_def, &gps.ecef_vel); +#if INS_USE_GPS_ALT + vff_update_z_conf((float)gps_pos_cm_ned.z / 100.0, INS_VFF_R_GPS); +#endif + #if USE_HFF /* horizontal gps transformed to NED in meters as float */ struct FloatVect2 gps_pos_m_ned; @@ -313,38 +330,26 @@ uint8_t var_idx = 0; #if USE_SONAR -void ins_update_sonar(void) { +static void sonar_cb(uint8_t __attribute__((unused)) sender_id, const float *distance) { static float last_offset = 0.; - // new value filtered with median_filter - ins_impl.sonar_alt = update_median_filter(&ins_impl.sonar_median, sonar_meas); - float sonar = (ins_impl.sonar_alt - ins_impl.sonar_offset) * INS_SONAR_SENS; #ifdef INS_SONAR_VARIANCE_THRESHOLD /* compute variance of error between sonar and baro alt */ - float err = sonar + ins_impl.baro_z; // sonar positive up, baro positive down !!!! + float err = *distance + ins_impl.baro_z; // sonar positive up, baro positive down !!!! var_err[var_idx] = err; var_idx = (var_idx + 1) % VAR_ERR_MAX; float var = variance_float(var_err, VAR_ERR_MAX); - DOWNLINK_SEND_INS_SONAR(DefaultChannel,DefaultDevice,&err, &sonar, &var); - //DOWNLINK_SEND_INS_SONAR(DefaultChannel,DefaultDevice,&ins_impl.sonar_alt, &sonar, &var); + DOWNLINK_SEND_INS_SONAR(DefaultChannel,DefaultDevice, distance, &var); #endif /* update filter assuming a flat ground */ - if (sonar < INS_SONAR_MAX_RANGE + if (*distance < INS_SONAR_MAX_RANGE #ifdef INS_SONAR_MIN_RANGE - && sonar > INS_SONAR_MIN_RANGE + && *distance > INS_SONAR_MIN_RANGE #endif #ifdef INS_SONAR_THROTTLE_THRESHOLD && stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD #endif -#ifdef INS_SONAR_STAB_THRESHOLD - && stabilization_cmd[COMMAND_ROLL] < INS_SONAR_STAB_THRESHOLD - && stabilization_cmd[COMMAND_ROLL] > -INS_SONAR_STAB_THRESHOLD - && stabilization_cmd[COMMAND_PITCH] < INS_SONAR_STAB_THRESHOLD - && stabilization_cmd[COMMAND_PITCH] > -INS_SONAR_STAB_THRESHOLD - && stabilization_cmd[COMMAND_YAW] < INS_SONAR_STAB_THRESHOLD - && stabilization_cmd[COMMAND_YAW] > -INS_SONAR_STAB_THRESHOLD -#endif #ifdef INS_SONAR_BARO_THRESHOLD && ins_impl.baro_z > -INS_SONAR_BARO_THRESHOLD /* z down */ #endif @@ -353,7 +358,7 @@ void ins_update_sonar(void) { #endif && ins_impl.update_on_agl && ins_impl.baro_initialized) { - vff_update_alt_conf(-sonar, VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabs(sonar)); + vff_update_z_conf(-(*distance), VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabsf(*distance)); last_offset = vff.offset; } else { diff --git a/sw/airborne/subsystems/ins/ins_int.h b/sw/airborne/subsystems/ins/ins_int.h index d04ef30c6e..0dfa18eb34 100644 --- a/sw/airborne/subsystems/ins/ins_int.h +++ b/sw/airborne/subsystems/ins/ins_int.h @@ -34,10 +34,6 @@ #include "math/pprz_geodetic_int.h" #include "math/pprz_algebra_float.h" -#if USE_SONAR -#include "filters/median_filter.h" -#endif - /** Ins implementation state (fixed point) */ struct InsInt { struct LtpDef_i ltp_def; @@ -64,10 +60,7 @@ struct InsInt { bool_t baro_initialized; #if USE_SONAR - bool_t update_on_agl; /* use sonar to update agl if available */ - int32_t sonar_alt; - int32_t sonar_offset; - struct MedianFilterInt sonar_median; + bool_t update_on_agl; ///< use sonar to update agl if available #endif }; diff --git a/sw/airborne/subsystems/ins/vf_extended_float.c b/sw/airborne/subsystems/ins/vf_extended_float.c index 04555bd674..e8d4eba633 100644 --- a/sw/airborne/subsystems/ins/vf_extended_float.c +++ b/sw/airborne/subsystems/ins/vf_extended_float.c @@ -278,11 +278,11 @@ static void update_alt_conf(float z_meas, float conf) { vff.P[3][3] -= K3 * P3; } -void vff_update_alt(float z_meas) { +void vff_update_z(float z_meas) { update_alt_conf(z_meas, R_ALT); } -void vff_update_alt_conf(float z_meas, float conf) { +void vff_update_z_conf(float z_meas, float conf) { update_alt_conf(z_meas, conf); } diff --git a/sw/airborne/subsystems/ins/vf_extended_float.h b/sw/airborne/subsystems/ins/vf_extended_float.h index c8900e698c..41f5226310 100644 --- a/sw/airborne/subsystems/ins/vf_extended_float.h +++ b/sw/airborne/subsystems/ins/vf_extended_float.h @@ -52,10 +52,10 @@ extern void vff_init_zero(void); extern void vff_init(float z, float zdot, float accel_bias, float baro_offset); extern void vff_propagate(float accel); extern void vff_update_baro(float z_meas); -extern void vff_update_alt(float z_meas); +extern void vff_update_z(float z_meas); extern void vff_update_offset(float offset); extern void vff_update_baro_conf(float z_meas, float conf); -extern void vff_update_alt_conf(float z_meas, float conf); +extern void vff_update_z_conf(float z_meas, float conf); //extern void vff_update_vz_conf(float vz_meas, float conf); extern void vff_realign(float z_meas); diff --git a/sw/airborne/subsystems/nav.c b/sw/airborne/subsystems/nav.c index 435b405af5..a9ed8a50e4 100644 --- a/sw/airborne/subsystems/nav.c +++ b/sw/airborne/subsystems/nav.c @@ -109,7 +109,7 @@ void nav_init_stage( void ) { void nav_circle_XY(float x, float y, float radius) { struct EnuCoor_f* pos = stateGetPositionEnu_f(); float last_trigo_qdr = nav_circle_trigo_qdr; - nav_circle_trigo_qdr = atan2(pos->y - y, pos->x - x); + nav_circle_trigo_qdr = atan2f(pos->y - y, pos->x - x); float sign_radius = radius > 0 ? 1 : -1; if (nav_in_circle) { @@ -133,7 +133,7 @@ void nav_circle_XY(float x, float y, float radius) { (dist2_center > Square(abs_radius + dist_carrot) || dist2_center < Square(abs_radius - dist_carrot)) ? 0 : - atan((*stateGetHorizontalSpeedNorm_f())*(*stateGetHorizontalSpeedNorm_f()) / (NAV_GRAVITY*radius)); + atanf((*stateGetHorizontalSpeedNorm_f())*(*stateGetHorizontalSpeedNorm_f()) / (NAV_GRAVITY*radius)); float carrot_angle = dist_carrot / abs_radius; carrot_angle = Min(carrot_angle, M_PI/4); @@ -142,10 +142,10 @@ void nav_circle_XY(float x, float y, float radius) { horizontal_mode = HORIZONTAL_MODE_CIRCLE; float radius_carrot = abs_radius; if (nav_mode == NAV_MODE_COURSE) { - radius_carrot += (abs_radius / cos(carrot_angle) - abs_radius); + radius_carrot += (abs_radius / cosf(carrot_angle) - abs_radius); } - fly_to_xy(x+cos(alpha_carrot)*radius_carrot, - y+sin(alpha_carrot)*radius_carrot); + fly_to_xy(x+cosf(alpha_carrot)*radius_carrot, + y+sinf(alpha_carrot)*radius_carrot); nav_in_circle = TRUE; nav_circle_x = x; nav_circle_y = y; @@ -226,14 +226,14 @@ static inline bool_t nav_compute_baseleg(uint8_t wp_af, uint8_t wp_td, uint8_t w float y_0 = waypoints[wp_td].y - waypoints[wp_af].y; /* Unit vector from AF to TD */ - float d = sqrt(x_0*x_0+y_0*y_0); + float d = sqrtf(x_0*x_0+y_0*y_0); float x_1 = x_0 / d; float y_1 = y_0 / d; waypoints[wp_baseleg].x = waypoints[wp_af].x + y_1 * nav_radius; waypoints[wp_baseleg].y = waypoints[wp_af].y - x_1 * nav_radius; waypoints[wp_baseleg].a = waypoints[wp_af].a; - baseleg_out_qdr = M_PI - atan2(-y_1, -x_1); + baseleg_out_qdr = M_PI - atan2f(-y_1, -x_1); if (nav_radius < 0) baseleg_out_qdr += M_PI; @@ -247,7 +247,7 @@ static inline bool_t nav_compute_final_from_glide(uint8_t wp_af, uint8_t wp_td, float h_0 = waypoints[wp_td].a - waypoints[wp_af].a; /* Unit vector from AF to TD */ - float d = sqrt(x_0*x_0+y_0*y_0); + float d = sqrtf(x_0*x_0+y_0*y_0); float x_1 = x_0 / d; float y_1 = y_0 / d; @@ -266,8 +266,8 @@ static inline bool_t compute_TOD(uint8_t _af, uint8_t _td, uint8_t _tod, float g struct FloatVect2* wind = stateGetHorizontalWindspeed_f(); float td_af_x = WaypointX(_af) - WaypointX(_td); float td_af_y = WaypointY(_af) - WaypointY(_td); - float td_af = sqrt( td_af_x*td_af_x + td_af_y*td_af_y); - float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / glide_vspeed * (glide_airspeed - sqrt(wind->x*wind->x + wind->y*wind->y)); + float td_af = sqrtf( td_af_x*td_af_x + td_af_y*td_af_y); + float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / glide_vspeed * (glide_airspeed - sqrtf(wind->x*wind->x + wind->y*wind->y)); WaypointX(_tod) = WaypointX(_td) + td_af_x / td_af * td_tod; WaypointY(_tod) = WaypointY(_td) + td_af_y / td_af * td_tod; WaypointAlt(_tod) = WaypointAlt(_af); @@ -292,7 +292,7 @@ static inline void nav_follow(uint8_t _ac_id, float _distance, float _height) { NavVerticalAutoThrottleMode(0.); NavVerticalAltitudeMode(Max(ac->alt + _height, ground_alt+SECURITY_HEIGHT), 0.); float alpha = M_PI/2 - ac->course; - float ca = cos(alpha), sa = sin(alpha); + float ca = cosf(alpha), sa = sinf(alpha); float x = ac->east - _distance*ca; float y = ac->north - _distance*sa; fly_to_xy(x, y); @@ -314,7 +314,10 @@ float fp_pitch; /* deg */ * Computes \a dist2_to_wp and compare it to square \a carrot. * Return true if it is smaller. Else computes by scalar products if * uav has not gone past waypoint. - * Return true if it is the case. + * \a approaching_time can be negative and in this case, the UAV will + * fly after the waypoint for the given number of seconds. + * + * @return true if the position (x, y) is reached */ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time) { /** distance to waypoint in x */ @@ -322,14 +325,25 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap /** distance to waypoint in y */ float pw_y = y - stateGetPositionEnu_f()->y; - dist2_to_wp = pw_x*pw_x + pw_y *pw_y; - float min_dist = approaching_time * (*stateGetHorizontalSpeedNorm_f()); - if (dist2_to_wp < min_dist*min_dist) - return TRUE; - - float scal_prod = (x - from_x) * pw_x + (y - from_y) * pw_y; - - return (scal_prod < 0.); + if (approaching_time < 0.) { + // fly after the destination waypoint + float leg_x = x - from_x; + float leg_y = y - from_y; + float leg = sqrtf(Max(leg_x * leg_x + leg_y * leg_y, 1.)); + float exceed_dist = approaching_time * (*stateGetHorizontalSpeedNorm_f()); // negative value + float scal_prod = (leg_x * pw_x + leg_y * pw_y) / leg; + return (scal_prod < exceed_dist); + } + else { + // fly close enough of the waypoint or cross it + dist2_to_wp = pw_x*pw_x + pw_y *pw_y; + float min_dist = approaching_time * (*stateGetHorizontalSpeedNorm_f()); + if (dist2_to_wp < min_dist*min_dist) { + return TRUE; + } + float scal_prod = (x - from_x) * pw_x + (y - from_y) * pw_y; + return (scal_prod < 0.); + } } @@ -342,17 +356,17 @@ void fly_to_xy(float x, float y) { desired_x = x; desired_y = y; if (nav_mode == NAV_MODE_COURSE) { - h_ctl_course_setpoint = atan2(x - pos->x, y - pos->y); + h_ctl_course_setpoint = atan2f(x - pos->x, y - pos->y); if (h_ctl_course_setpoint < 0.) h_ctl_course_setpoint += 2 * M_PI; lateral_mode = LATERAL_MODE_COURSE; } else { - float diff = atan2(x - pos->x, y - pos->y) - (*stateGetHorizontalSpeedDir_f()); + float diff = atan2f(x - pos->x, y - pos->y) - (*stateGetHorizontalSpeedDir_f()); NormRadAngle(diff); BoundAbs(diff,M_PI/2.); - float s = sin(diff); + float s = sinf(diff); float speed = *stateGetHorizontalSpeedNorm_f(); - h_ctl_roll_setpoint = atan(2 * speed * speed * s * h_ctl_course_pgain / (CARROT * NOMINAL_AIRSPEED * 9.81) ); + h_ctl_roll_setpoint = atanf(2 * speed * speed * s * h_ctl_course_pgain / (CARROT * NOMINAL_AIRSPEED * 9.81) ); BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint); lateral_mode = LATERAL_MODE_ROLL; } @@ -366,7 +380,7 @@ void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y) { float leg_y = wp_y - last_wp_y; float leg2 = Max(leg_x * leg_x + leg_y * leg_y, 1.); nav_leg_progress = ((stateGetPositionEnu_f()->x - last_wp_x) * leg_x + (stateGetPositionEnu_f()->y - last_wp_y) * leg_y) / leg2; - nav_leg_length = sqrt(leg2); + nav_leg_length = sqrtf(leg2); /** distance of carrot (in meter) */ float carrot = CARROT * NOMINAL_AIRSPEED; @@ -548,7 +562,7 @@ void nav_eight(uint8_t target, uint8_t c1, float radius) { float target_c1_x = waypoints[c1].x - waypoints[target].x; float target_c1_y = waypoints[c1].y - waypoints[target].y; - float d = sqrt(target_c1_x*target_c1_x+target_c1_y*target_c1_y); + float d = sqrtf(target_c1_x*target_c1_x+target_c1_y*target_c1_y); d = Max(d, 1.); /* To prevent a division by zero */ /* Unit vector from target to c1 */ @@ -586,7 +600,7 @@ void nav_eight(uint8_t target, uint8_t c1, float radius) { c2.y - radius * u_x, alt }; - float qdr_out = M_PI - atan2(u_y, u_x); + float qdr_out = M_PI - atan2f(u_y, u_x); if (radius < 0) qdr_out += M_PI; @@ -670,7 +684,7 @@ void nav_oval(uint8_t p1, uint8_t p2, float radius) { float p2_p1_x = waypoints[p1].x - waypoints[p2].x; float p2_p1_y = waypoints[p1].y - waypoints[p2].y; - float d = sqrt(p2_p1_x*p2_p1_x+p2_p1_y*p2_p1_y); + float d = sqrtf(p2_p1_x*p2_p1_x+p2_p1_y*p2_p1_y); /* Unit vector from p1 to p2 */ float u_x = p2_p1_x / d; @@ -691,7 +705,7 @@ void nav_oval(uint8_t p1, uint8_t p2, float radius) { waypoints[p2].y + radius * u_x, alt }; - float qdr_out_2 = M_PI - atan2(u_y, u_x); + float qdr_out_2 = M_PI - atan2f(u_y, u_x); float qdr_out_1 = qdr_out_2 + M_PI; if (radius < 0) { qdr_out_2 += M_PI; diff --git a/sw/airborne/subsystems/navigation/common_nav.c b/sw/airborne/subsystems/navigation/common_nav.c index 103ff761e1..cd87bb558b 100644 --- a/sw/airborne/subsystems/navigation/common_nav.c +++ b/sw/airborne/subsystems/navigation/common_nav.c @@ -44,8 +44,8 @@ int32_t nav_utm_north0 = NAV_UTM_NORTH0; uint8_t nav_utm_zone0 = NAV_UTM_ZONE0; float max_dist_from_home = MAX_DIST_FROM_HOME; -/** \brief Computes square distance to the HOME waypoint potentially sets - * \a too_far_from_home +/** Computes squared distance to the HOME waypoint. + * Updates #dist2_to_home and potentially sets #too_far_from_home */ void compute_dist2_to_home(void) { struct EnuCoor_f* pos = stateGetPositionEnu_f(); @@ -80,7 +80,7 @@ unit_t nav_reset_utm_zone(void) { } /** Reset the geographic reference to the current GPS fix */ -unit_t nav_reset_reference( void ) { +unit_t nav_reset_reference(void) { /* realign INS */ ins_reset_local_origin(); @@ -97,7 +97,7 @@ unit_t nav_reset_reference( void ) { } /** Shift altitude of the waypoint according to a new ground altitude */ -unit_t nav_update_waypoints_alt( void ) { +unit_t nav_update_waypoints_alt(void) { uint8_t i; for(i = 0; i < NB_WAYPOINT; i++) { waypoints[i].a += ground_alt - previous_ground_alt; @@ -109,6 +109,12 @@ void common_nav_periodic_task_4Hz() { RunOnceEvery(4, { stage_time++; block_time++; }); } +/** Move a waypoint to given UTM coordinates. + * @param[in] wp_id Waypoint ID + * @param[in] ux UTM x (east) coordinate + * @param[in] uy UTM y (north) coordinate + * @param[in] alt Altitude above MSL. + */ void nav_move_waypoint(uint8_t wp_id, float ux, float uy, float alt) { if (wp_id < nb_waypoint) { float dx, dy; diff --git a/sw/airborne/subsystems/radio_control/sbus.c b/sw/airborne/subsystems/radio_control/sbus.c index f704fee093..7dff0b4467 100644 --- a/sw/airborne/subsystems/radio_control/sbus.c +++ b/sw/airborne/subsystems/radio_control/sbus.c @@ -70,9 +70,9 @@ struct _sbus sbus; #include "subsystems/datalink/telemetry.h" static void send_sbus(void) { - // Using PPM message for simplicity + // Using PPM message DOWNLINK_SEND_PPM(DefaultChannel, DefaultDevice, - &radio_control.frame_rate, SBUS_NB_CHANNEL, sbus.pulses); + &radio_control.frame_rate, SBUS_NB_CHANNEL, sbus.ppm); } #endif @@ -99,7 +99,7 @@ void radio_control_impl_init(void) { /** Decode the raw buffer */ -static void decode_sbus_buffer (const uint8_t *src, uint16_t *dst, bool_t *available) +static void decode_sbus_buffer (const uint8_t *src, uint16_t *dst, bool_t *available, uint16_t *dstppm) { // reset counters uint8_t byteInRawBuf = 0; @@ -124,6 +124,9 @@ static void decode_sbus_buffer (const uint8_t *src, uint16_t *dst, bool_t *avail } if (bitInChannel == SBUS_BIT_PER_CHANNEL) { bitInChannel = 0; +#if PERIODIC_TELEMETRY + dstppm[channel] = USEC_OF_RC_PPM_TICKS(dst[channel]); +#endif channel++; } } @@ -153,7 +156,7 @@ void sbus_decode_event(void) { if (sbus.idx == SBUS_BUF_LENGTH) { // Decode if last byte is the correct end byte if (rbyte == SBUS_END_BYTE) { - decode_sbus_buffer(sbus.buffer, sbus.pulses, &sbus.frame_available); + decode_sbus_buffer(sbus.buffer, sbus.pulses, &sbus.frame_available, sbus.ppm); } sbus.status = SBUS_STATUS_UNINIT; } diff --git a/sw/airborne/subsystems/radio_control/sbus.h b/sw/airborne/subsystems/radio_control/sbus.h index 3f344194a7..4cfc202551 100644 --- a/sw/airborne/subsystems/radio_control/sbus.h +++ b/sw/airborne/subsystems/radio_control/sbus.h @@ -30,11 +30,14 @@ #include "std.h" /** - * Dummy macro to use radio.h file + * Macro to use radio.h file + * + * SBUS: 0..1024..2047 (sweep 2048) + * PPM: 880..1520..2160 (sweep 1280) */ -#define RC_PPM_TICKS_OF_USEC(_v) (_v) -#define RC_PPM_SIGNED_TICKS_OF_USEC(_v) (_v) -#define USEC_OF_RC_PPM_TICKS(_v) (_v) +#define RC_PPM_TICKS_OF_USEC(_v) ((((_v) - 880) * 8) / 5) +#define RC_PPM_SIGNED_TICKS_OF_USEC(_v) (((_v) * 8) / 5) +#define USEC_OF_RC_PPM_TICKS(_v) ((((_v) * 5) / 8) + 880) /** * Generated code holding the description of a given @@ -60,6 +63,7 @@ */ struct _sbus { uint16_t pulses[SBUS_NB_CHANNEL]; ///< decoded values + uint16_t ppm[SBUS_NB_CHANNEL]; ///< decoded and converted values bool_t frame_available; ///< new frame available uint8_t buffer[SBUS_BUF_LENGTH]; ///< input buffer uint8_t idx; ///< input index diff --git a/sw/airborne/subsystems/sonar.h b/sw/airborne/subsystems/sonar.h deleted file mode 100644 index daf310400d..0000000000 --- a/sw/airborne/subsystems/sonar.h +++ /dev/null @@ -1,4 +0,0 @@ - - - -extern uint16_t sonar_meas; diff --git a/sw/cgi/configuration.cgi b/sw/cgi/configuration.cgi deleted file mode 100755 index 089040cd00..0000000000 --- a/sw/cgi/configuration.cgi +++ /dev/null @@ -1,57 +0,0 @@ -#!/usr/bin/perl - -# displays the current configuration - -use strict; -use warnings; - -use Cwd; -use CGI ':standard', '*table'; - -my $paparazzi_src; -my $paparazzi_lib; -BEGIN { - $paparazzi_src = getcwd()."/../.."; - $paparazzi_lib = $paparazzi_src."/sw/lib/perl"; -} -use lib ($paparazzi_lib); -my $paparazzi_home = $paparazzi_src; - -use Paparazzi::Environment; -Paparazzi::Environment::set_env($paparazzi_src, $paparazzi_home); - -use Paparazzi::Configuration; - -print - header(), - start_html("Paparazzi configuration"); - -my $configuration = Paparazzi::Configuration::read_current(); -print - h1 ("Paparazzi Configuration"), - h2 ("Aircrafts"); -print - start_table({border => undef}), - Tr(th(["Id", "Name", "Airframe", "Radio", "Flight plan"])); -foreach my $ac (@{$configuration->{aircrafts}}) { - print - Tr(td([$ac->{ac_id}, $ac->{name}, - a({href=>"../conf/$ac->{airframe}"}, foo($ac->{airframe}) ), - a({href=>"../conf/$ac->{radio}"}, foo($ac->{radio}) ), - a({href=>"../conf/$ac->{flight_plan}"}, foo($ac->{flight_plan}) )])); -} -print end_table(); - -print - h2 ("Ground"), - ul(li (["name : ".$configuration->{ground}->{name}, - "ivy bus : ".$configuration->{ground}->{ivy_bus} - ])), - hr(), - a({href=>"../index.html"}, "home"), - end_html(); - - -sub foo { - return ($_[0] =~ /([^\/]*)$/ ); -} diff --git a/sw/cgi/logs.cgi b/sw/cgi/logs.cgi deleted file mode 100755 index a86ebc7ba0..0000000000 --- a/sw/cgi/logs.cgi +++ /dev/null @@ -1,54 +0,0 @@ -#!/usr/bin/perl - -# displays available logs - -use strict; -use warnings; - -use Cwd; -use CGI ":standard"; -my $paparazzi_src; -my $paparazzi_lib; -BEGIN { - $paparazzi_src = getcwd()."/../.."; - $paparazzi_lib = $paparazzi_src."/sw/lib/perl"; -} -use lib ($paparazzi_lib); -my $paparazzi_home = $paparazzi_src; - -use Paparazzi::Environment; -Paparazzi::Environment::set_env($paparazzi_src, $paparazzi_home); - -use Paparazzi::Log; - -my $query = new CGI(); -print $query->header(); -print $query->start_html("Paparazzi Logs"); - -my @logs = Paparazzi::Log::get_available(); - -my $log_info = undef; -my $log_data = undef; -my @files = $query->param('file'); -if ($#files >=0 ) { - $log_info = Paparazzi::Log::read_infos($files[0]); - $log_data = Paparazzi::Log::read_data($log_info->{data_file}); -} - -print "\n

Paparazzi Logs.

\n"; -print "

Logs.

\n"; -print $query->startform(); -print $query->popup_menu('file', \@logs, $logs[0]); -print $query->submit('Action','Update'); -print $query->endform(); -if (defined $log_info) { - print $query->path_info()."
"; - Paparazzi::Log::html_print_summary($log_info, $log_data); - my $url = Paparazzi::Log::gen_activity_plot($log_info, $log_data); - print"\n"; -} -print "
\n"; -print "home
\n"; -print "
\n"; -print "
Poine.

\n"; - diff --git a/sw/ground_segment/cockpit/Makefile b/sw/ground_segment/cockpit/Makefile index 8a38286eb7..97ad612d90 100644 --- a/sw/ground_segment/cockpit/Makefile +++ b/sw/ground_segment/cockpit/Makefile @@ -49,11 +49,11 @@ all : $(MAIN) opt : $(MAIN).opt -$(MAIN) : $(CMO) $(XLIBPPRZCMA) +$(MAIN) : $(CMO) $(LIBPPRZCMA) $(XLIBPPRZCMA) @echo OL $@ $(Q)$(OCAMLC) $(OCAMLCFLAGS) $(INCLUDES) $(LIBS) $(LINKPKG) myGtkInit.cmo $(CMO) -o $@ -$(MAIN).opt : $(CMX) $(XLIBPPRZCMXA) +$(MAIN).opt : $(CMX) $(LIBPPRZCMXA) $(XLIBPPRZCMXA) @echo OOL $@ $(Q)$(OCAMLOPT) $(OCAMLCFLAGS) $(INCLUDES) $(LIBSX) -package pprz.xlib,$(LABLGTK2INIT) -linkpkg $(CMX) -o $@ @@ -119,7 +119,8 @@ actuators : actuators.c # .depend: Makefile - $(OCAMLDEP) -I $(LIBPPRZDIR) *.ml* > .depend + @echo DEPEND $@ + $(Q)$(OCAMLDEP) -I $(LIBPPRZDIR) *.ml* > .depend ifneq ($(MAKECMDGOALS),clean) -include .depend diff --git a/sw/ground_segment/cockpit/editFP.ml b/sw/ground_segment/cockpit/editFP.ml index 3a3673736f..641ccc8868 100644 --- a/sw/ground_segment/cockpit/editFP.ml +++ b/sw/ground_segment/cockpit/editFP.ml @@ -1,9 +1,7 @@ -(***************** Editing ONE (single) flight plan **************************)open Printf +(***************** Editing ONE (single) flight plan **************************) +open Printf open Latlong -module G2D = Geometry_2d - - let (//) = Filename.concat let fp_example = Env.flight_plans_path // "example.xml" let default_path_maps = Env.paparazzi_home // "data" // "maps" diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml index 2949ca0ae8..0668a56045 100644 --- a/sw/ground_segment/cockpit/live.ml +++ b/sw/ground_segment/cockpit/live.ml @@ -525,7 +525,7 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id let id = ExtXml.int_attrib block "no" in begin (* Is it a key short cut ? *) try - let key, modifiers = GtkData.AccelGroup.parse (Xml.attrib block "key") in + let key, modifiers = GtkData.AccelGroup.parse (Pprz.key_modifiers_of_string (Xml.attrib block "key")) in keys := (key, (modifiers, id)) :: !keys with _ -> () diff --git a/sw/ground_segment/cockpit/page_settings.ml b/sw/ground_segment/cockpit/page_settings.ml index 9468fba30b..063fa15239 100644 --- a/sw/ground_segment/cockpit/page_settings.ml +++ b/sw/ground_segment/cockpit/page_settings.ml @@ -58,7 +58,7 @@ let search_index = fun value array -> let add_key = fun xml do_change keys -> - let key, modifiers = GtkData.AccelGroup.parse (Xml.attrib xml "key") + let key, modifiers = GtkData.AccelGroup.parse (Pprz.key_modifiers_of_string (Xml.attrib xml "key")) and value = ExtXml.float_attrib xml "value" in keys := (key, (modifiers, fun () -> do_change value)) :: !keys diff --git a/sw/ground_segment/joystick/Makefile b/sw/ground_segment/joystick/Makefile index 9c7e84afb6..9375dd3f25 100644 --- a/sw/ground_segment/joystick/Makefile +++ b/sw/ground_segment/joystick/Makefile @@ -25,11 +25,9 @@ Q=@ include ../../Makefile.ocaml -TOOLSDIR = ../../tools CC = gcc -OCAMLINCLUDES= -I $(TOOLSDIR) PKG = -package pprz,glibivy LINKPKG = $(PKG) -linkpkg -dllpath-pkg pprz @@ -72,9 +70,9 @@ test_stick: test_sdl_stick.o @echo BUILD $@ $(Q)$(CC) -g -O2 -DSTICK_DBG $(GLIB_CFLAGS) -o $@ $^ sdl_stick.c $(GLIB_LDFLAGS) $(SDL_LDFLAGS) -input2ivy: $(TOOLSDIR)/fp_proc.cmo $(INPUT2IVY_DEPS) +input2ivy: $(LIBPPRZCMA) $(INPUT2IVY_DEPS) @echo OL $@ - $(Q)$(OCAMLC) $(OCAMLINCLUDES) -o $@ $(LINKPKG) $^ $(ML_SDL_OCAMLFLAGS) $(ML_SDL_LFLAGS) + $(Q)$(OCAMLC) -o $@ $(LINKPKG) $^ $(ML_SDL_OCAMLFLAGS) $(ML_SDL_LFLAGS) sdl_stick.so : $(SDL_STICK_DEPS) @@ -87,7 +85,7 @@ sdl_stick.so : $(SDL_STICK_DEPS) %.cmo : %.ml @echo OC $< - $(Q)$(OCAMLC) $(OCAMLINCLUDES) -c $(PKG) $< + $(Q)$(OCAMLC) -c $(PKG) $< clean: $(Q)rm -f *~ core *.o *.bak .depend test*stick *.cmo *.cmi input2ivy @@ -99,7 +97,8 @@ clean: # .depend: Makefile - $(OCAMLDEP) -I $(LIBPPRZDIR) *.ml* > .depend + @echo DEPEND $@ + $(Q)$(OCAMLDEP) -I $(LIBPPRZDIR) *.ml* > .depend ifneq ($(MAKECMDGOALS),clean) -include .depend diff --git a/sw/ground_segment/lpc21iap/Makefile b/sw/ground_segment/lpc21iap/Makefile index 19ffb61af5..a65cd0a9f6 100644 --- a/sw/ground_segment/lpc21iap/Makefile +++ b/sw/ground_segment/lpc21iap/Makefile @@ -39,7 +39,8 @@ clean: app: $(APPNAME) $(APPNAME): $(SOURCES) Makefile - $(CC) $(CFLAGS) $(SOURCES) -o $(APPNAME) -l$(LIBNAME) + @echo LD $@ + $(Q)$(CC) $(CFLAGS) $(SOURCES) -o $(APPNAME) -l$(LIBNAME) # Builds archive tar file arch: clean diff --git a/sw/ground_segment/misc/Makefile b/sw/ground_segment/misc/Makefile index b703963916..5e7b421af3 100644 --- a/sw/ground_segment/misc/Makefile +++ b/sw/ground_segment/misc/Makefile @@ -25,6 +25,7 @@ Q=@ CC = gcc +PAPARAZZI_SRC=../../.. UNAME = $(shell uname -s) ifeq ("$(UNAME)","Darwin") @@ -34,11 +35,14 @@ else LIBRARYS = -s endif +# Optitrack specific librarys and includes +NATNET_LIBRARYS = $(shell pkg-config glib-2.0 --libs) -lglibivy -lm $(shell pcre-config --libs) +INCLUDES += $(shell pkg-config glib-2.0 --cflags) -I$(PAPARAZZI_SRC)/sw/airborne/ -I$(PAPARAZZI_SRC)/sw/include/ -all: davis2ivy kestrel2ivy +all: davis2ivy kestrel2ivy natnet2ivy clean: - $(Q)rm -f *.o davis2ivy kestrel2ivy + $(Q)rm -f *.o davis2ivy kestrel2ivy natnet2ivy davis2ivy: davis2ivy.o $(Q)$(CC) -o davis2ivy davis2ivy.o $(LIBRARYS) -livy @@ -46,6 +50,9 @@ davis2ivy: davis2ivy.o kestrel2ivy: kestrel2ivy.o $(Q)$(CC) -o kestrel2ivy kestrel2ivy.o $(LIBRARYS) -livy +natnet2ivy: natnet2ivy.o $(PAPARAZZI_SRC)/sw/airborne/math/pprz_geodetic_double.o $(PAPARAZZI_SRC)/sw/airborne/fms/fms_network.o + $(Q)$(CC) -o natnet2ivy natnet2ivy.o pprz_geodetic_double.o fms_network.o $(LIBRARYS) $(NATNET_LIBRARYS) -livy + %.o : %.c $(Q)$(CC) -c -O2 -Wall $(INCLUDES) $< diff --git a/sw/ground_segment/misc/natnet2ivy.c b/sw/ground_segment/misc/natnet2ivy.c new file mode 100644 index 0000000000..a5709070e4 --- /dev/null +++ b/sw/ground_segment/misc/natnet2ivy.c @@ -0,0 +1,726 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * + * 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. + */ + + /** \file natnet2ivy.c + * \brief NatNet (GPS) to ivy forwarder + * + * This receives aircraft position information through the Optitrack system + * NatNet UDP stream and forwards it to the ivy bus. An aircraft with the gps + * subsystem "datalink" is then able to parse the GPS position and use it to + * navigate inside the Optitrack system. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "std.h" +#include "fms/fms_network.h" +#include "math/pprz_geodetic_double.h" +#include "math/pprz_algebra_double.h" + +/** Debugging options */ +uint8_t verbose = 0; +#define printf_natnet if(verbose > 1) printf +#define printf_debug if(verbose > 0) printf + +/** NatNet defaults */ +char *natnet_addr = "255.255.255.255"; +char *natnet_multicast_addr = "239.255.42.99"; +uint16_t natnet_cmd_port = 1510; +uint16_t natnet_data_port = 1511; +uint8_t natnet_major = 2; +uint8_t natnet_minor = 5; + +/** Ivy Bus default */ +#ifdef __APPLE__ +char *ivy_bus = "224.255.255.255"; +#else +char *ivy_bus = "127.255.255.255:2010"; +#endif + +/** Sample frequency and derevitive defaults */ +uint32_t freq_transmit = 30; ///< Transmitting frequency in Hz +uint16_t min_velocity_samples = 4; ///< The amount of position samples needed for a valid velocity + +/** NatNet parsing defines */ +#define MAX_PACKETSIZE 100000 +#define MAX_NAMELENGTH 256 +#define MAX_RIGIDBODIES 128 + +#define NAT_PING 0 +#define NAT_PINGRESPONSE 1 +#define NAT_REQUEST 2 +#define NAT_RESPONSE 3 +#define NAT_REQUEST_MODELDEF 4 +#define NAT_MODELDEF 5 +#define NAT_REQUEST_FRAMEOFDATA 6 +#define NAT_FRAMEOFDATA 7 +#define NAT_MESSAGESTRING 8 +#define NAT_UNRECOGNIZED_REQUEST 100 +#define UNDEFINED 999999.9999 + +/** Tracked rigid bodies */ +struct RigidBody { + int id; ///< Rigid body ID from the tracking system + float x, y, z; ///< Rigid body x, y and z coordinates in meters (note y and z are swapped) + float qx, qy, qz, qw; ///< Rigid body qx, qy, qz and qw rotations (note qy and qz are swapped) + int nMarkers; ///< Number of markers inside the rigid body (both visible and not visible) + float error; ///< Error of the position in cm + int nSamples; ///< Number of samples since last transmit + bool posSampled; ///< If the position is sampled last sampling + + double vel_x, vel_y, vel_z; ///< Sum of the (last_vel_* - current_vel_*) during nVelocitySamples + struct EcefCoor_d ecef_vel; ///< Last valid ECEF velocity in meters + int nVelocitySamples; ///< Number of velocity samples gathered + int totalVelocitySamples; ///< Total amount of velocity samples possible + int nVelocityTransmit; ///< Amount of transmits since last valid velocity transmit +}; +struct RigidBody rigidBodies[MAX_RIGIDBODIES]; ///< All rigid bodies which are tracked +uint8_t ac_ids[MAX_RIGIDBODIES]; ///< Mapping from rigid body ID to aircraft ID + +/** Natnet socket connections */ +struct FmsNetwork *natnet_data, *natnet_cmd; + +/** Tracking location LTP and angle offset from north */ +struct LtpDef_d tracking_ltp; ///< The tracking system LTP definition +double tracking_offset_angle; ///< The offset from the tracking system to the North in degrees + +/** Save the latency from natnet */ +float natnet_latency; + +/** Parse the packet from NatNet */ +void natnet_parse(unsigned char *in) { + int i,j,k; + + // Create a pointer to go trough the packet + char *ptr = (char *)in; + printf_natnet("Begin Packet\n-------\n"); + + // Message ID + int MessageID = 0; + memcpy(&MessageID, ptr, 2); ptr += 2; + printf_natnet("Message ID : %d\n", MessageID); + + // Packet size + int nBytes = 0; + memcpy(&nBytes, ptr, 2); ptr += 2; + printf_natnet("Byte count : %d\n", nBytes); + + if(MessageID == NAT_FRAMEOFDATA) // FRAME OF MOCAP DATA packet + { + // Frame number + int frameNumber = 0; memcpy(&frameNumber, ptr, 4); ptr += 4; + printf_natnet("Frame # : %d\n", frameNumber); + + // ========== MARKERSETS ========== + // Number of data sets (markersets, rigidbodies, etc) + int nMarkerSets = 0; memcpy(&nMarkerSets, ptr, 4); ptr += 4; + printf_natnet("Marker Set Count : %d\n", nMarkerSets); + + for (i=0; i < nMarkerSets; i++) + { + // Markerset name + char szName[256]; + strcpy(szName, ptr); + int nDataBytes = (int) strlen(szName) + 1; + ptr += nDataBytes; + printf_natnet("Model Name: %s\n", szName); + + // marker data + int nMarkers = 0; memcpy(&nMarkers, ptr, 4); ptr += 4; + printf_natnet("Marker Count : %d\n", nMarkers); + + for(j=0; j < nMarkers; j++) + { + float x = 0; memcpy(&x, ptr, 4); ptr += 4; + float y = 0; memcpy(&y, ptr, 4); ptr += 4; + float z = 0; memcpy(&z, ptr, 4); ptr += 4; + printf_natnet("\tMarker %d : [x=%3.2f,y=%3.2f,z=%3.2f]\n",j,x,y,z); + } + } + + // Unidentified markers + int nOtherMarkers = 0; memcpy(&nOtherMarkers, ptr, 4); ptr += 4; + printf_natnet("Unidentified Marker Count : %d\n", nOtherMarkers); + for(j=0; j < nOtherMarkers; j++) + { + float x = 0.0f; memcpy(&x, ptr, 4); ptr += 4; + float y = 0.0f; memcpy(&y, ptr, 4); ptr += 4; + float z = 0.0f; memcpy(&z, ptr, 4); ptr += 4; + printf_natnet("\tMarker %d : pos = [%3.2f,%3.2f,%3.2f]\n",j,x,y,z); + } + + // ========== RIGID BODIES ========== + // Rigid bodies + int nRigidBodies = 0; + memcpy(&nRigidBodies, ptr, 4); ptr += 4; + printf_natnet("Rigid Body Count : %d\n", nRigidBodies); + + // Check if there ie enough space for the rigid bodies + if(nRigidBodies > MAX_RIGIDBODIES) { + fprintf(stderr, "Could not sample all the rigid bodies because the amount of rigid bodies is bigger then %d (MAX_RIGIDBODIES).\r\n", MAX_RIGIDBODIES); + exit(EXIT_FAILURE); + } + + for (j=0; j < nRigidBodies; j++) + { + // rigid body pos/ori + struct RigidBody old_rigid; + memcpy(&old_rigid, &rigidBodies[j], sizeof(struct RigidBody)); + + memcpy(&rigidBodies[j].id, ptr, 4); ptr += 4; + memcpy(&rigidBodies[j].x, ptr, 4); ptr += 4; //x --> X + memcpy(&rigidBodies[j].z, ptr, 4); ptr += 4; //y --> Z + memcpy(&rigidBodies[j].y, ptr, 4); ptr += 4; //z --> Y + memcpy(&rigidBodies[j].qx, ptr, 4); ptr += 4; //qx --> QX + memcpy(&rigidBodies[j].qz, ptr, 4); ptr += 4; //qy --> QZ + memcpy(&rigidBodies[j].qy, ptr, 4); ptr += 4; //qz --> QY + memcpy(&rigidBodies[j].qw, ptr, 4); ptr += 4; //qw --> QW + printf_natnet("ID (%d) : %d\n", j, rigidBodies[j].id); + printf_natnet("pos: [%3.2f,%3.2f,%3.2f]\n", rigidBodies[j].x,rigidBodies[j].y,rigidBodies[j].z); + printf_natnet("ori: [%3.2f,%3.2f,%3.2f,%3.2f]\n", rigidBodies[j].qx,rigidBodies[j].qy,rigidBodies[j].qz,rigidBodies[j].qw); + + // Differentiate the position to get the speed (TODO: crossreference with labeled markers for occlussion) + rigidBodies[j].totalVelocitySamples++; + if(old_rigid.x != rigidBodies[j].x || old_rigid.y != rigidBodies[j].y || old_rigid.z != rigidBodies[j].z + || old_rigid.qx != rigidBodies[j].qx || old_rigid.qy != rigidBodies[j].qy || old_rigid.qz != rigidBodies[j].qz || old_rigid.qw != rigidBodies[j].qw) { + + if(old_rigid.posSampled) { + rigidBodies[j].vel_x += (rigidBodies[j].x - old_rigid.x); + rigidBodies[j].vel_y += (rigidBodies[j].y - old_rigid.y); + rigidBodies[j].vel_z += (rigidBodies[j].z - old_rigid.z); + rigidBodies[j].nVelocitySamples++; + } + + rigidBodies[j].nSamples++; + rigidBodies[j].posSampled = TRUE; + } + else + rigidBodies[j].posSampled = FALSE; + + // When marker id changed, reset the velocity + if(old_rigid.id != rigidBodies[j].id) { + rigidBodies[j].vel_x = 0; + rigidBodies[j].vel_y = 0; + rigidBodies[j].vel_z = 0; + rigidBodies[j].nSamples = 0; + rigidBodies[j].nVelocitySamples = 0; + rigidBodies[j].totalVelocitySamples = 0; + rigidBodies[j].posSampled = FALSE; + } + + // Associated marker positions + memcpy(&rigidBodies[j].nMarkers, ptr, 4); ptr += 4; + printf_natnet("Marker Count: %d\n", rigidBodies[j].nMarkers); + int nBytes = rigidBodies[j].nMarkers*3*sizeof(float); + float* markerData = (float*)malloc(nBytes); + memcpy(markerData, ptr, nBytes); + ptr += nBytes; + + if(natnet_major >= 2) + { + // Associated marker IDs + nBytes = rigidBodies[j].nMarkers*sizeof(int); + int* markerIDs = (int*)malloc(nBytes); + memcpy(markerIDs, ptr, nBytes); + ptr += nBytes; + + // Associated marker sizes + nBytes = rigidBodies[j].nMarkers*sizeof(float); + float* markerSizes = (float*)malloc(nBytes); + memcpy(markerSizes, ptr, nBytes); + ptr += nBytes; + + for(k=0; k < rigidBodies[j].nMarkers; k++) + { + printf_natnet("\tMarker %d: id=%d\tsize=%3.1f\tpos=[%3.2f,%3.2f,%3.2f]\n", k, markerIDs[k], markerSizes[k], markerData[k*3], markerData[k*3+1],markerData[k*3+2]); + } + + if(markerIDs) + free(markerIDs); + if(markerSizes) + free(markerSizes); + + } + else + { + for(k=0; k < rigidBodies[j].nMarkers; k++) + { + printf_natnet("\tMarker %d: pos = [%3.2f,%3.2f,%3.2f]\n", k, markerData[k*3], markerData[k*3+1],markerData[k*3+2]); + } + } + if(markerData) + free(markerData); + + if(natnet_major >= 2) + { + // Mean marker error + memcpy(&rigidBodies[j].error, ptr, 4); ptr += 4; + printf_natnet("Mean marker error: %3.8f\n", rigidBodies[j].error); + } + } // next rigid body + + // ========== SKELETONS ========== + // Skeletons (version 2.1 and later) + if(((natnet_major == 2) && (natnet_minor>0)) || (natnet_major>2)) + { + int nSkeletons = 0; + memcpy(&nSkeletons, ptr, 4); ptr += 4; + printf_natnet("Skeleton Count : %d\n", nSkeletons); + for (j=0; j < nSkeletons; j++) + { + // Skeleton id + int skeletonID = 0; + memcpy(&skeletonID, ptr, 4); ptr += 4; + // # of rigid bodies (bones) in skeleton + int nRigidBodies = 0; + memcpy(&nRigidBodies, ptr, 4); ptr += 4; + printf_natnet("Rigid Body Count : %d\n", nRigidBodies); + for (j=0; j < nRigidBodies; j++) + { + // Rigid body pos/ori + int ID = 0; memcpy(&ID, ptr, 4); ptr += 4; + float x = 0.0f; memcpy(&x, ptr, 4); ptr += 4; + float y = 0.0f; memcpy(&y, ptr, 4); ptr += 4; + float z = 0.0f; memcpy(&z, ptr, 4); ptr += 4; + float qx = 0; memcpy(&qx, ptr, 4); ptr += 4; + float qy = 0; memcpy(&qy, ptr, 4); ptr += 4; + float qz = 0; memcpy(&qz, ptr, 4); ptr += 4; + float qw = 0; memcpy(&qw, ptr, 4); ptr += 4; + printf_natnet("ID : %d\n", ID); + printf_natnet("pos: [%3.2f,%3.2f,%3.2f]\n", x,y,z); + printf_natnet("ori: [%3.2f,%3.2f,%3.2f,%3.2f]\n", qx,qy,qz,qw); + + // Sssociated marker positions + int nRigidMarkers = 0; memcpy(&nRigidMarkers, ptr, 4); ptr += 4; + printf_natnet("Marker Count: %d\n", nRigidMarkers); + int nBytes = nRigidMarkers*3*sizeof(float); + float* markerData = (float*)malloc(nBytes); + memcpy(markerData, ptr, nBytes); + ptr += nBytes; + + // Associated marker IDs + nBytes = nRigidMarkers*sizeof(int); + int* markerIDs = (int*)malloc(nBytes); + memcpy(markerIDs, ptr, nBytes); + ptr += nBytes; + + // Associated marker sizes + nBytes = nRigidMarkers*sizeof(float); + float* markerSizes = (float*)malloc(nBytes); + memcpy(markerSizes, ptr, nBytes); + ptr += nBytes; + + for(k=0; k < nRigidMarkers; k++) + { + printf_natnet("\tMarker %d: id=%d\tsize=%3.1f\tpos=[%3.2f,%3.2f,%3.2f]\n", k, markerIDs[k], markerSizes[k], markerData[k*3], markerData[k*3+1],markerData[k*3+2]); + } + + // Mean marker error + float fError = 0.0f; memcpy(&fError, ptr, 4); ptr += 4; + printf_natnet("Mean marker error: %3.2f\n", fError); + + // Release resources + if(markerIDs) + free(markerIDs); + if(markerSizes) + free(markerSizes); + if(markerData) + free(markerData); + } // next rigid body + } // next skeleton + } + + // ========== LABELED MARKERS ========== + // Labeled markers (version 2.3 and later) + if( ((natnet_major == 2)&&(natnet_minor>=3)) || (natnet_major>2)) + { + int nLabeledMarkers = 0; + memcpy(&nLabeledMarkers, ptr, 4); ptr += 4; + printf_natnet("Labeled Marker Count : %d\n", nLabeledMarkers); + for (j=0; j < nLabeledMarkers; j++) + { + int ID = 0; memcpy(&ID, ptr, 4); ptr += 4; + float x = 0.0f; memcpy(&x, ptr, 4); ptr += 4; + float y = 0.0f; memcpy(&y, ptr, 4); ptr += 4; + float z = 0.0f; memcpy(&z, ptr, 4); ptr += 4; + float size = 0.0f; memcpy(&size, ptr, 4); ptr += 4; + + printf_natnet("ID : %d\n", ID); + printf_natnet("pos : [%3.2f,%3.2f,%3.2f]\n", x,y,z); + printf_natnet("size: [%3.2f]\n", size); + } + } + + // Latency + natnet_latency = 0.0f; memcpy(&natnet_latency, ptr, 4); ptr += 4; + printf_natnet("latency : %3.3f\n", natnet_latency); + + // Timecode + unsigned int timecode = 0; memcpy(&timecode, ptr, 4); ptr += 4; + unsigned int timecodeSub = 0; memcpy(&timecodeSub, ptr, 4); ptr += 4; + printf_natnet("timecode : %d %d", timecode, timecodeSub); + + // End of data tag + int eod = 0; memcpy(&eod, ptr, 4); ptr += 4; + printf_natnet("End Packet\n-------------\n"); + } + else + { + printf("Error: Unrecognized packet type from Optitrack NatNet.\n"); + } +} + +/** The transmitter periodic function */ +gboolean timeout_transmit_callback(gpointer data) { + int i; + + // Loop trough all the available rigidbodies (TODO: optimize) + for(i = 0; i < MAX_RIGIDBODIES; i++) { + // Check if ID's are correct + if(rigidBodies[i].id >= MAX_RIGIDBODIES) { + fprintf(stderr, "Could not parse rigid body %d from NatNet, because ID is higher then or equal to %d (MAX_RIGIDBODIES-1).\r\n", rigidBodies[i].id, MAX_RIGIDBODIES-1); + exit(EXIT_FAILURE); + } + // Check if we want to transmit this rigid and it was sampled + if(ac_ids[rigidBodies[i].id] == 0 || rigidBodies[i].nSamples < 1) + continue; + + // Defines to make easy use of paparazzi math + struct EnuCoor_d pos, speed; + struct EcefCoor_d ecef_pos; + struct LlaCoor_d lla_pos; + struct DoubleQuat orient; + struct DoubleEulers orient_eulers; + + // Add the Optitrack angle to the x and y positions + pos.x = cos(tracking_offset_angle) * rigidBodies[i].x + sin(tracking_offset_angle) * rigidBodies[i].y; + pos.y = sin(tracking_offset_angle) * rigidBodies[i].x - cos(tracking_offset_angle) * rigidBodies[i].y; + pos.z = rigidBodies[i].z; + + // Convert the position to ecef and lla based on the Optitrack LTP + ecef_of_enu_point_d(&ecef_pos ,&tracking_ltp ,&pos); + lla_of_ecef_d(&lla_pos, &ecef_pos); + + // Check if we have enough samples to estimate the velocity + rigidBodies[i].nVelocityTransmit++; + if(rigidBodies[i].nVelocitySamples >= min_velocity_samples) { + // Calculate the derevative of the sum to get the correct velocity (1 / freq_transmit) * (samples / total_samples) + double sample_time = //((double)rigidBodies[i].nVelocitySamples / (double)rigidBodies[i].totalVelocitySamples) / + ((double)rigidBodies[i].nVelocityTransmit / (double)freq_transmit); + rigidBodies[i].vel_x = rigidBodies[i].vel_x / sample_time; + rigidBodies[i].vel_y = rigidBodies[i].vel_y / sample_time; + rigidBodies[i].vel_z = rigidBodies[i].vel_z / sample_time; + + // Add the Optitrack angle to the x and y velocities + speed.x = cos(tracking_offset_angle) * rigidBodies[i].vel_x + sin(tracking_offset_angle) * rigidBodies[i].vel_y; + speed.y = sin(tracking_offset_angle) * rigidBodies[i].vel_x - cos(tracking_offset_angle) * rigidBodies[i].vel_y; + speed.z = rigidBodies[i].vel_z; + + // Conver the speed to ecef based on the Optitrack LTP + ecef_of_enu_vect_d(&rigidBodies[i].ecef_vel ,&tracking_ltp ,&speed); + } + + // Copy the quaternions and convert to euler angles for the heading + orient.qi = rigidBodies[i].qw; + orient.qx = rigidBodies[i].qx; + orient.qy = rigidBodies[i].qy; + orient.qz = rigidBodies[i].qz; + DOUBLE_EULERS_OF_QUAT(orient_eulers, orient); + + // Calculate the heading by adding the Natnet offset angle and normalizing it + double heading = -orient_eulers.psi-tracking_offset_angle; + NormRadAngle(heading); + + printf_debug("[%d -> %d]Samples: %d\t%d\t\tTiming: %3.3f latency\n", rigidBodies[i].id, ac_ids[rigidBodies[i].id] + , rigidBodies[i].nSamples, rigidBodies[i].nVelocitySamples, natnet_latency); + printf_debug(" Heading: %f\t\tPosition: %f\t%f\t%f\t\tVelocity: %f\t%f\t%f\n", DegOfRad(heading), + rigidBodies[i].x, rigidBodies[i].y, rigidBodies[i].z, + rigidBodies[i].ecef_vel.x, rigidBodies[i].ecef_vel.y, rigidBodies[i].ecef_vel.z); + + // Transmit the REMOTE_GPS packet on the ivy bus + IvySendMsg("0 REMOTE_GPS %d %d %d %d %d %d %d %d %d %d %d %d %d %d", ac_ids[rigidBodies[i].id], + rigidBodies[i].nMarkers, //uint8 Number of markers (sv_num) + (int)(ecef_pos.x*100.0), //int32 ECEF X in CM + (int)(ecef_pos.y*100.0), //int32 ECEF Y in CM + (int)(ecef_pos.z*100.0), //int32 ECEF Z in CM + (int)(lla_pos.lat*10000000.0), //int32 LLA latitude in rad*1e7 + (int)(lla_pos.lon*10000000.0), //int32 LLA longitude in rad*1e7 + (int)(rigidBodies[i].z*1000.0), //int32 LLA altitude in mm above elipsoid + (int)(rigidBodies[i].z*1000.0), //int32 HMSL height above mean sea level in mm + (int)(rigidBodies[i].ecef_vel.x*100.0), //int32 ECEF velocity X in m/s + (int)(rigidBodies[i].ecef_vel.y*100.0), //int32 ECEF velocity Y in m/s + (int)(rigidBodies[i].ecef_vel.z*100.0), //int32 ECEF velocity Z in m/s + 0, + (int)(heading*10000000.0)); //int32 Course in rad*1e7 + + // Reset the velocity differentiator if we calculated the velocity + if(rigidBodies[i].nVelocitySamples >= min_velocity_samples) { + rigidBodies[i].vel_x = 0; + rigidBodies[i].vel_y = 0; + rigidBodies[i].vel_z = 0; + rigidBodies[i].nVelocitySamples = 0; + rigidBodies[i].totalVelocitySamples = 0; + rigidBodies[i].nVelocityTransmit = 0; + } + + rigidBodies[i].nSamples = 0; + } + + return TRUE; +} + +/** The NatNet sampler periodic function */ +static gboolean sample_data(GIOChannel *chan, GIOCondition cond, gpointer data) { + static unsigned char buffer_data[MAX_PACKETSIZE]; + static int bytes_data = 0; + + // Keep on reading until we have the whole packet + bytes_data += network_read(natnet_data, buffer_data, MAX_PACKETSIZE); + + // Parse NatNet data + if(bytes_data >= 2 && bytes_data >= buffer_data[1]) { + natnet_parse(buffer_data); + bytes_data = 0; + } + + return TRUE; +} + + +/** Print the program help */ +void print_help(char* filename) { + static const char *usage = + "Usage: %s [options]\n" + " Options :\n" + " -h, --help Display this help\n" + " -v, --verbose Verbosity level 0-2 (0)\n\n" + + " -ac Use rigid ID for GPS of ac_id (multiple possible)\n\n" + + " -multicast_addr NatNet server multicast address (239.255.42.99)\n" + " -server NatNet server IP address (255.255.255.255)\n" + " -version NatNet server version (2.5)\n" + " -data_port NatNet server data socket UDP port (1510)\n" + " -cmd_port NatNet server command socket UDP port (1511)\n\n" + + " -ecef ECEF coordinates of the tracking system\n" + " -lla Latitude, longitude and altitude of the tracking system\n" + " -offset_angle Tracking system angle offset compared to the North in degrees\n\n" + + " -tf Transmit frequency to the ivy bus in hertz (60)\n" + " -vel_samples Minimum amount of samples for the velocity differentiator (4)\n\n" + + " -ivy_bus Ivy bus address and port (127.255.255.255:2010)\n"; + fprintf(stderr, usage, filename); +} + +/** Check the amount of arguments */ +void check_argcount(int argc, char** argv, int i, int expected) { + if(i+expected >= argc) { + fprintf(stderr, "Option %s expected %d arguments\r\n\r\n", argv[i], expected); + print_help(argv[0]); + exit(0); + } +} + +/** Parse the options from the commandline */ +static void parse_options(int argc, char** argv) { + int i, count_ac = 0; + for(i = 1; i < argc; ++i) { + + // Print help + if(strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-h") == 0) { + print_help(argv[0]); + exit(0); + } + // Set the verbosity level + if(strcmp(argv[i], "--verbosity") == 0 || strcmp(argv[i], "-v") == 0) { + check_argcount(argc, argv, i, 1); + + verbose = atoi(argv[++i]); + } + + // Set an rigid body to ivy ac_id + else if(strcmp(argv[i], "-ac") == 0) { + check_argcount(argc, argv, i, 2); + + int rigid_id = atoi(argv[++i]); + uint8_t ac_id = atoi(argv[++i]); + + if(rigid_id >= MAX_RIGIDBODIES) { + fprintf(stderr, "Rigid body ID must be less then %d (MAX_RIGIDBODIES)\n\n", MAX_RIGIDBODIES); + print_help(argv[0]); + exit(EXIT_FAILURE); + } + ac_ids[rigid_id] = ac_id; + count_ac++; + } + + // Set the NatNet multicast address + else if(strcmp(argv[i], "-multicast_addr") == 0) { + check_argcount(argc, argv, i, 1); + + natnet_multicast_addr = argv[++i]; + } + // Set the NatNet server ip address + else if(strcmp(argv[i], "-server") == 0) { + check_argcount(argc, argv, i, 1); + + natnet_addr = argv[++i]; + } + // Set the NatNet server version + else if(strcmp(argv[i], "-version") == 0) { + check_argcount(argc, argv, i, 1); + + float version = atof(argv[++i]); + natnet_major = (uint8_t)version; + natnet_minor = (uint8_t)(version * 10.0) % 10; + } + // Set the NatNet server data port + else if(strcmp(argv[i], "-data_port") == 0) { + check_argcount(argc, argv, i, 1); + + natnet_data_port = atoi(argv[++i]); + } + // Set the NatNet server command port + else if(strcmp(argv[i], "-cmd_port") == 0) { + check_argcount(argc, argv, i, 1); + + natnet_cmd_port = atoi(argv[++i]); + } + + // Set the Tracking system position in ECEF + else if (strcmp(argv[i], "-ecef") == 0) { + check_argcount(argc, argv, i, 3); + + struct EcefCoor_d tracking_ecef; + tracking_ecef.x = atof(argv[++i]); + tracking_ecef.y = atof(argv[++i]); + tracking_ecef.z = atof(argv[++i]); + ltp_def_from_ecef_d(&tracking_ltp, &tracking_ecef); + } + // Set the tracking system position in LLA + else if (strcmp(argv[i], "-lla") == 0) { + check_argcount(argc, argv, i, 3); + + struct EcefCoor_d tracking_ecef; + struct LlaCoor_d tracking_lla; + tracking_lla.lat = atof(argv[++i]); + tracking_lla.lon = atof(argv[++i]); + tracking_lla.alt = atof(argv[++i]); + ecef_of_lla_d(&tracking_ecef, &tracking_lla); + ltp_def_from_ecef_d(&tracking_ltp, &tracking_ecef); + } + // Set the tracking system offset angle in degrees + else if(strcmp(argv[i], "-offset_angle") == 0) { + check_argcount(argc, argv, i, 1); + + tracking_offset_angle = atof(argv[++i]); + } + + // Set the transmit frequency + else if(strcmp(argv[i], "-tf") == 0) { + check_argcount(argc, argv, i, 1); + + freq_transmit = atoi(argv[++i]); + } + // Set the minimum amount of velocity samples for the differentiator + else if(strcmp(argv[i], "-vel_samples") == 0) { + check_argcount(argc, argv, i, 1); + + min_velocity_samples = atoi(argv[++i]); + } + + // Set the ivy bus + else if(strcmp(argv[i], "-ivy_bus") == 0) { + check_argcount(argc, argv, i, 1); + + ivy_bus = argv[++i]; + } + + // Unknown option + else { + fprintf(stderr, "Unknown option %s\r\n\r\n", argv[i]); + print_help(argv[0]); + exit(0); + } + } + + // Check if at least one aircraft is set + if(count_ac < 1) { + fprintf(stderr, "You must specify at least one aircraft (-ac )\n\n"); + print_help(argv[0]); + exit(EXIT_FAILURE); + } +} + +int main(int argc, char** argv) +{ + // Set the default tracking system position and angle + struct EcefCoor_d tracking_ecef; + tracking_ecef.x = 3924304; + tracking_ecef.y = 300360; + tracking_ecef.z = 5002162; + tracking_offset_angle = 123.0 / 57.6; + ltp_def_from_ecef_d(&tracking_ltp, &tracking_ecef); + + // Parse the options from cmdline + parse_options(argc, argv); + printf_debug("Tracking system Latitude: %f Longitude: %f Offset to North: %f degrees\n", DegOfRad(tracking_ltp.lla.lat), DegOfRad(tracking_ltp.lla.lon), DegOfRad(tracking_offset_angle)); + + // Create the network connections + printf_debug("Starting NatNet listening (multicast address: %s, data port: %d, version: %d.%d)\n", natnet_multicast_addr, natnet_data_port, natnet_major, natnet_minor); + natnet_data = network_new("", -1, natnet_data_port, 0); // Only receiving + network_subscribe_multicast(natnet_data, natnet_multicast_addr); + network_set_recvbuf(natnet_data, 0x100000); // 1MB + + printf_debug("Starting NatNet command socket (server address: %s, command port: %d)\n", natnet_addr, natnet_cmd_port); + natnet_cmd = network_new(natnet_addr, natnet_cmd_port, 0, 1); + network_set_recvbuf(natnet_cmd, 0x100000); // 1MB + + // Create the Ivy Client + GMainLoop *ml = g_main_loop_new(NULL, FALSE); + IvyInit("natnet2ivy", "natnet2ivy READY", 0, 0, 0, 0); + IvyStart(ivy_bus); + + // Create the main timers + printf_debug("Starting transmitting and sampling timeouts (transmitting frequency: %dHz, minimum velocity samples: %d)\n", + freq_transmit, min_velocity_samples); + g_timeout_add(1000/freq_transmit, timeout_transmit_callback, NULL); + + GIOChannel *sk = g_io_channel_unix_new(natnet_data->socket_in); + g_io_add_watch(sk, G_IO_IN | G_IO_NVAL | G_IO_HUP, + sample_data, NULL); + + // Run the main loop + g_main_loop_run(ml); + + return 0; +} diff --git a/sw/ground_segment/multimon/Makefile b/sw/ground_segment/multimon/Makefile index 6f2fb1675f..03f60be811 100644 --- a/sw/ground_segment/multimon/Makefile +++ b/sw/ground_segment/multimon/Makefile @@ -106,13 +106,16 @@ $(BINDIR)/multimon: $(OBJ_L2) $(OBJ_L1) $(OBJ_MISC) $(Q)$(CC) $^ $(LDFLAGS) $(LDFLAGSX) -o $@ $(BINDIR)/gen: $(OBJ_GEN) - $(CC) $^ $(LDFLAGS) -o $@ + @echo LD $@ + $(Q)$(CC) $^ $(LDFLAGS) -o $@ $(BINDIR)/mkcostab: $(BINDIR)/mkcostab.o - $(CC) $^ $(LDFLAGS) -o $@ + @echo LD $@ + $(Q)$(CC) $^ $(LDFLAGS) -o $@ costabi.c costabf.c: $(BINDIR)/mkcostab - $(BINDIR)/mkcostab + @echo EXEC $< + $(Q)$(BINDIR)/mkcostab libtest: pprzlib.o demodml.c demod.ml test.ml @@ -124,10 +127,12 @@ hdlc_test : multimon.cma test_gen_hdlc.ml hdlc.cmo : hdlc.cmi %.cmo : %.ml - $(OCAMLC) -c $< + @echo OC $< + $(Q)$(OCAMLC) -c $< %.cmi : %.mli - $(OCAMLC) $< + @echo OC $< + $(Q)$(OCAMLC) $< clean: $(Q)rm -fr *.cm* mkcostab .depend diff --git a/sw/ground_segment/tmtc/Makefile b/sw/ground_segment/tmtc/Makefile index 77a51e6cd6..6aa0368644 100644 --- a/sw/ground_segment/tmtc/Makefile +++ b/sw/ground_segment/tmtc/Makefile @@ -41,71 +41,66 @@ SERVERCMO = server_globals.cmo aircraft.cmo wind.cmo airprox.cmo kml.cmo fw_serv SERVERCMX = $(SERVERCMO:.cmo=.cmx) -all: link server messages settings dia diadec $(VAR)/boa.conf ivy_tcp_aircraft ivy_tcp_controller broadcaster ivy2udp ivy_serial_bridge +all: link server messages settings dia diadec ivy_tcp_aircraft ivy_tcp_controller broadcaster ivy2udp ivy_serial_bridge app_server opt: server.opt clean: - $(Q)rm -f link server messages settings dia diadec *.bak *~ core *.o .depend *.opt *.out *.cm* ivy_tcp_aircraft ivy_tcp_controller broadcaster ivy2udp ivy_serial_bridge gpsd2ivy c_ivy_client_example_1 c_ivy_client_example_2 c_ivy_client_example_3 + $(Q)rm -f link server messages settings dia diadec *.bak *~ core *.o .depend *.opt *.out *.cm* ivy_tcp_aircraft ivy_tcp_controller broadcaster ivy2udp ivy_serial_bridge app_server gpsd2ivy c_ivy_client_example_1 c_ivy_client_example_2 c_ivy_client_example_3 -$(VAR)/boa.conf :$(CONF)/boa.conf - mkdir -p $(VAR) - sed 's|PAPARAZZI_HOME|$(PAPARAZZI_HOME)|' < $< > $@ - - -messages : messages.cmo +messages : messages.cmo $(LIBPPRZCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) gtkInit.cmo $< -settings : settings.cmo ../cockpit/page_settings.cmo +settings : settings.cmo ../cockpit/page_settings.cmo $(LIBPPRZCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(XLINKPKG) gtkInit.cmo -I ../cockpit gtk_save_settings.cmo saveSettings.cmo page_settings.cmo $< -server : $(SERVERCMO) +server : $(SERVERCMO) $(LIBPPRZCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $(SERVERCMO) -server.opt : $(SERVERCMX) +server.opt : $(SERVERCMX) $(LIBPPRZCMXA) @echo OOL $@ $(Q)$(OCAMLOPT) $(INCLUDES) -o $@ -package glibivy,pprz -linkpkg $(SERVERCMX) -link : link.cmo $(LIBMULTIMONCMA) +link : link.cmo $(LIBMULTIMONCMA) $(LIBPPRZCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $(LIBMULTIMONDLL) $< -ivy_tcp_aircraft : ivy_tcp_aircraft.cmo $(LIBMULTIMONCMA) +ivy_tcp_aircraft : ivy_tcp_aircraft.cmo $(LIBMULTIMONCMA) $(LIBPPRZCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $(LIBMULTIMONDLL) $< -ivy_tcp_controller : ivy_tcp_controller.cmo $(LIBMULTIMONCMA) +ivy_tcp_controller : ivy_tcp_controller.cmo $(LIBMULTIMONCMA) $(LIBPPRZCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $(LIBMULTIMONDLL) $< -broadcaster : broadcaster.cmo $(LIBMULTIMONCMA) +broadcaster : broadcaster.cmo $(LIBMULTIMONCMA) $(LIBPPRZCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $(LIBMULTIMONDLL) $< -ivy2udp : ivy2udp.cmo +ivy2udp : ivy2udp.cmo $(LIBPPRZCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $< -dia : dia.cmo $(LIBMULTIMONCMA) +dia : dia.cmo $(LIBMULTIMONCMA) $(LIBPPRZCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $(LIBMULTIMONDLL) $< -diadec : diadec.cmo $(LIBMULTIMONCMA) +diadec : diadec.cmo $(LIBMULTIMONCMA) $(LIBPPRZCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $(LIBMULTIMONDLL) $< -150m : 150m.cmo +150m : 150m.cmo $(LIBPPRZCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) gtkInit.cmo $< @@ -138,6 +133,10 @@ ifeq ("$(UNAME)","Darwin") C_INCLUDES = $(shell if test -d /opt/paparazzi/include; then echo "-I/opt/paparazzi/include"; elif test -d /opt/local/include; then echo "-I/opt/local/include"; fi) endif +app_server: app_server.c + @echo OL $@ + $(Q)$(CC) $(GLIB_CFLAGS) $(shell pkg-config libxml-2.0 gio-2.0 ivy-glib --cflags) -o $@ $^ -lm -lz $(shell pkg-config libxml-2.0 gio-2.0 ivy-glib libpcre --libs) + gpsd2ivy: gpsd2ivy.c $(CC) $(GLIB_CFLAGS) $(C_LIBRARYS) $(C_INCLUDES) -o $@ $< $(GLIB_LDFLAGS) -lgps @@ -151,7 +150,8 @@ c_ivy_client_example_3: c_ivy_client_example_3.c $(CC) $(GTK_CFLAGS) $(C_LIBRARYS) $(C_INCLUDES) -o $@ $< $(GTK_LDFLAGS) ivy_serial_bridge: ivy_serial_bridge.c - $(CC) $(GTK_CFLAGS) $(C_LIBRARYS) $(C_INCLUDES) -o $@ $< $(GTK_LDFLAGS) + @echo OL $@ + $(Q)$(CC) $(GTK_CFLAGS) $(C_LIBRARYS) $(C_INCLUDES) -o $@ $< $(GTK_LDFLAGS) .PHONY: all opt clean @@ -161,7 +161,8 @@ ivy_serial_bridge: ivy_serial_bridge.c # .depend: Makefile - $(OCAMLDEP) -I $(LIBPPRZDIR) -I ../multimon *.ml* > .depend + @echo DEPEND $@ + $(Q)$(OCAMLDEP) -I $(LIBPPRZDIR) -I ../multimon *.ml* > .depend ifneq ($(MAKECMDGOALS),clean) -include .depend diff --git a/sw/ground_segment/tmtc/app_server.c b/sw/ground_segment/tmtc/app_server.c new file mode 100644 index 0000000000..e703dbc1a3 --- /dev/null +++ b/sw/ground_segment/tmtc/app_server.c @@ -0,0 +1,812 @@ +/* + * Copyright (C) 2014 + * + * Created by: Savas Sen - ENAC UAV Lab + * + * 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. + * + */ + +/* + * Server application for remote devices such as Android. + * + * It forwards paparazzi ground class messages to remote app + * and receives commands from allowed clients to control the aircraft + * + * Several clients can be connected at the same time with full or restricted access + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// assuming local path from sw/ground_segment/tmtc +char defaultPprzFolder[] = "../../.."; + +char defaultAppPass[] = "1234"; //4 char password to control ac's over app "pass ground stg stg stg.. +char* AppPass; + +#define BUFLEN 2048 +#define MAXCLIENT 20 +#define MAXIPLEN 50 +#define MAXNAMELENGTH 500 +#define MAXDEVICENUMB 255 + +#define MAXWPNUMB 50 //NUMBER OF WP PER AC (MAX) +#define MAXWPNUMB 50 //NUMBER OF WP PER AC (MAX) +#define MAXWPNAMELEN 50 //WP NAME LENGTH (MAX) + +// Default TCP port (listen clients commands) +int tcp_port = 5010; +// Default UDP port (broadcast data to clients) +int udp_port = 5005; + +#ifdef __APPLE__ +char defaultIvyBus[] = "224.255.255.255:2010"; +#else +char defaultIvyBus[] = "127.255.255.255:2010"; +#endif +char* IvyBus; + +char ivybuffer[BUFLEN]; + +// verbose flag +int verbose = 0; + +//Block structure +typedef struct { + //Will we need any more block data? + char Bl_Name[MAXWPNAMELEN]; +} bl_data; + +//Waypoint structure +typedef struct { + //Will we need any more waypoint data? + char Wp_Name[MAXWPNAMELEN]; +} wp_data; + +//Aircraft Data Structure +typedef struct { + int device_id; + char name[MAXNAMELENGTH]; + char type[MAXNAMELENGTH]; + char color[MAXNAMELENGTH]; + char flight_plan_path[MAXNAMELENGTH]; + char airframe_path[MAXNAMELENGTH]; + char settings_path[MAXNAMELENGTH]; + int dl_launch_ind; + int kill_thr_ind; + wp_data AcWp[MAXWPNUMB]; + bl_data AcBl[MAXWPNUMB]; +} device_names; + +device_names DevNames[MAXDEVICENUMB]; + +//Connected Clients Data Structure +typedef struct { + int used ; + int client_port; //do we need that?? + char client_ip[MAXIPLEN]; +} client_data; + +client_data ConnectedClients[MAXCLIENT]; //Holds all status of devices + + +//Remove Client from UdpBroadcast list +void remove_client(char* RemClientIpAd) { + //remove client from client list + int i=0; + for (; i < MAXCLIENT; i++) { + //Search thru the client list + + if ( (ConnectedClients[i].used > 0) ) { + //Client list is being used + if ( g_strcmp0(RemClientIpAd , ConnectedClients[i].client_ip) == 0 ) { + //record found clean it!! + ConnectedClients[i].client_ip[0]='\0'; + ConnectedClients[i].used=0; + if (verbose) { + printf("App Server: Client removed from client list %s\n", RemClientIpAd); + fflush(stdout); + } + return; + } + } + } + //no record found! + if (verbose) { + printf("App Server: No record found to be removed from client list\n"); + fflush(stdout); + } +} + +//Record client (if new) +void add_client(char* ClientIpAd) { + /* Check if client exists. If exists return else record it */ + int i; + for (i = 0; i < MAXCLIENT; i++) { + //Search thru the client list + if ( (ConnectedClients[i].used > 0) ) { + //Client list is being used + if ( g_strcmp0(ClientIpAd , ConnectedClients[i].client_ip) == 0 ) { + //Already in client list return + return; + } + continue; + } + //no record found! + + //record new client ip + g_stpcpy(ConnectedClients[i].client_ip,ClientIpAd); + // + ConnectedClients[i].used = 1; + if (verbose) { + printf("App Server: New client added to client list %s\n", ConnectedClients[i].client_ip); + fflush(stdout); + } + break; + } +} + +//Get aircraft name and color +int get_ac_data(char* InStr, char* RetBuf) { + const char delimiters[] = " "; + int AcID; + char StrBuf[BUFLEN]; + + //Make writable copy + strcpy(StrBuf, InStr); + strtok(StrBuf, delimiters); + + //Get id from command string + AcID = atoi(strtok(NULL, delimiters)); + //Get & create return string + if ( AcID > 0 ) { + //Dont search it, it is thereeee :) + sprintf(RetBuf, "AppServer ACd %d %s %s %s %d %d\n", AcID, + DevNames[AcID].name, + DevNames[AcID].type, + DevNames[AcID].color, + DevNames[AcID].dl_launch_ind, + DevNames[AcID].kill_thr_ind); + } + return AcID; +} + +//Get aircraft name waypoint names,ids +int get_wp_data(char* InStr, char* RetBuf) { + //Input command + const char delimiters[] = " "; + int AcID; + char StrBuf[BUFLEN]; + + //Make writable copy + strcpy(StrBuf, InStr); + strtok(StrBuf, delimiters); + + //Get id from command string + AcID = atoi(strtok(NULL, delimiters)); + //Get & create return string + if ( AcID > 0 ) { + //create wp data string + char WpStr[BUFLEN] = ""; + int i = 1; + while ( strlen(DevNames[AcID].AcWp[i].Wp_Name) > 0 ) { + //Read & add wp data to return string + sprintf(WpStr, "%s%d,%s ", WpStr, i, DevNames[AcID].AcWp[i].Wp_Name); + i++; + } + sprintf(RetBuf, "AppServer WPs %d %s\n", AcID, WpStr); + } + return AcID; +} + +//Get block names,ids +int get_bl_data(char* InStr, char* RetBuf) { + //Input command + const char delimiters[] = " "; + int AcID; + char StrBuf[BUFLEN]; + + //Make writable copy + strcpy(StrBuf, InStr); + strtok(StrBuf, delimiters); + + //Get id from command string + AcID = atoi(strtok (NULL, delimiters)); + //Get & create return string + if ( AcID > 0 ) { + //create wp data string + char BlStr[BUFLEN] =""; + int i=1; + while ( strlen(DevNames[AcID].AcBl[i].Bl_Name) > 0 ) { + sprintf(BlStr, "%s%s", BlStr, DevNames[AcID].AcBl[i].Bl_Name); + i++; + } + sprintf(RetBuf, "AppServer BLs %d %s\n", AcID, BlStr); + } + return AcID; +} + +//Bfoadcast ivy msgs to clients +void broadcast_to_clients () { + + int i; + for (i = 0; i < MAXCLIENT; i++) { + if (ConnectedClients[i].used > 0) { + + //Send data + GInetAddress *udpAddress; + GSocketAddress *udpSocketAddress; + GSocket *udpSocket; + + udpAddress = g_inet_address_new_from_string(ConnectedClients[i].client_ip); + + udpSocketAddress = g_inet_socket_address_new(udpAddress, udp_port); + + udpSocket = g_socket_new(G_SOCKET_FAMILY_IPV4, G_SOCKET_TYPE_DATAGRAM, 17, NULL); + + if (g_socket_connect(udpSocket, udpSocketAddress, NULL, NULL) == FALSE) { + printf("App Server: stg wrong with socket connect\n"); + fflush(stdout); + } + if (g_socket_send(udpSocket, ivybuffer, strlen(ivybuffer) , NULL, NULL) < 0) { + printf("App Server: stg wrong with send func\n"); + fflush(stdout); + } + if (g_socket_close(udpSocket, NULL) == FALSE) { + printf("App Server: stg wrong with socket close\n"); + fflush(stdout); + } + //Unref objects + g_object_unref(udpAddress); + g_object_unref(udpSocketAddress); + g_object_unref(udpSocket); + } + } +} + +//Read tcp requests of connected clients +gboolean network_read(GIOChannel *source, GIOCondition cond, gpointer data) { + + GString *s = g_string_new(NULL); + GError *error = NULL; + GIOStatus ret = g_io_channel_read_line_string(source, s, NULL, &error); + + if (ret == G_IO_STATUS_ERROR) { + //unref connection + g_object_unref (data); + g_string_free (s,TRUE); + return FALSE; + } + else{ + //Read request command + gchar *RecString; + RecString=s->str; + //check client password + if ((strncmp(RecString, AppPass, strlen(AppPass))) == 0) { + //Password ok can send command + GString *incs = g_string_new(s->str); + incs = g_string_erase(s,0,(strlen(AppPass)+1)); + IvySendMsg("%s",incs->str); + if (verbose) { + printf("App Server: Command passed to ivy.. %s\n",incs->str); + fflush(stdout); + } + } + //AC data request. (Ignore client password) + else if ((strncmp(RecString, "getac ", strlen("getac "))) == 0) { + //AC data request + char AcData[BUFLEN]; + //Read ac data + if (get_ac_data(RecString, AcData)) { + //Send requested data to client + GOutputStream * ostream = g_io_stream_get_output_stream (data); + g_output_stream_write(ostream, AcData, strlen(AcData), NULL, &error); + } + } + //Waypoint data request (Ignore client password) + else if ((strncmp(RecString, "getwp ", strlen("getwp "))) == 0) { + char AcData[BUFLEN]; + //Read wp data of ac + if (get_wp_data(RecString, AcData)) { + //Send requested data to client + GOutputStream * ostream = g_io_stream_get_output_stream (data); + g_output_stream_write(ostream, AcData, strlen(AcData), NULL, &error); + } + } + //Waypoint data request (Ignore client password) + else if ((strncmp(RecString, "getbl ", strlen("getbl "))) == 0) { + char AcData[BUFLEN]; + //Read block data of AC + if (get_bl_data(RecString, AcData)) { + //Send requested data to client + GOutputStream * ostream = g_io_stream_get_output_stream (data); + g_output_stream_write(ostream, AcData, strlen(AcData), NULL, &error); + } + } + + //If client sends removeme command, remove it from broadcast list + else if ((strncmp( RecString, "removeme", strlen("removeme"))) == 0) { + GSocketAddress *sockaddr = g_socket_connection_get_remote_address(data, NULL); + GInetAddress *addr = g_inet_socket_address_get_address(G_INET_SOCKET_ADDRESS(sockaddr)); + //Read sender ip + if (verbose) { + printf("App Server: need to remove %s\n", g_inet_address_to_string(addr)); + fflush(stdout); + } + //Remove client + remove_client(g_inet_address_to_string(addr)); + //Free objects + g_string_free(s, TRUE); + g_object_unref(sockaddr); + g_object_unref(addr); + g_object_unref(data); + //Return false to stop listening this socket + return FALSE; + } + else { + //Unknown command + if (verbose) { + printf("App Server: Client send an unknown command: %s\n",RecString); + fflush(stdout); + } + } + } + + if (ret == G_IO_STATUS_EOF) { + //Client disconnected + if (verbose) { + printf("App Server: Client disconnected without saying 'bye':(\n"); + fflush(stdout); + } + g_string_free(s, TRUE); + //Unref the socket and return false to allow the client to reconnect + g_object_unref(data); + return FALSE; + } + //None above.. Keep listening the socket + g_string_free(s, TRUE); + return TRUE; +} + +//New tcp conection +gboolean new_connection(GSocketService *service, GSocketConnection *connection, GObject *source_object, gpointer user_data) { + + //New client connected + GSocketAddress *sockaddr = g_socket_connection_get_remote_address(connection, NULL); + GInetAddress *addr = g_inet_socket_address_get_address(G_INET_SOCKET_ADDRESS(sockaddr)); + guint16 port = g_inet_socket_address_get_port(G_INET_SOCKET_ADDRESS(sockaddr)); + + if (verbose) { + printf("App Server: New Connection from %s:%d\n", g_inet_address_to_string(addr), port); + fflush(stdout); + } + + //Record client (if new) + add_client(g_inet_address_to_string(addr)); + + g_object_ref (connection); + GSocket *socket = g_socket_connection_get_socket(connection); + + gint fd = g_socket_get_fd(socket); + GIOChannel *channel = g_io_channel_unix_new(fd); + g_io_add_watch(channel, G_IO_IN, (GIOFunc) network_read, connection); + return TRUE; +} + +//Ivy msg function +void Ivy_All_Msgs(IvyClientPtr app, void *user_data, int argc, char *argv[]){ + //Ivy msg received broadcast to clients.. + sprintf(ivybuffer, "%s", argv[0]); + broadcast_to_clients(); +} + +//Parse AC flight plan xml (block & waypoint names +void parse_ac_fp(int DevNameIndex, char *filename) { + xmlTextReaderPtr reader; + int ret; + + reader = xmlReaderForFile(filename, NULL, XML_PARSE_NOWARNING | XML_PARSE_NOERROR); /* Dont validate with the DTD */ + + xmlChar *name, *value; + if (reader != NULL) { + ret = xmlTextReaderRead(reader); + + int wp_queue=1; + int bl_queue=1; + while (ret == 1) { + name = xmlTextReaderName(reader); + if (name == NULL) { + name = xmlStrdup(BAD_CAST "--"); + } + + //read waypoint names + if (xmlStrEqual(name, (const xmlChar *)"waypoint")) { + //waypoint node read name attr. + xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"name"); + value = xmlTextReaderValue(reader); + //copy it to DevNames[] structure + strcpy(DevNames[DevNameIndex].AcWp[wp_queue].Wp_Name, (char *) value); + wp_queue++; + } + + if (xmlStrEqual(name, (const xmlChar *)"block")) { + //Read block names + if ( xmlTextReaderAttributeCount(reader) >= 1 ) { + xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"name"); + value = xmlTextReaderValue(reader); + strcpy(DevNames[DevNameIndex].AcBl[bl_queue].Bl_Name, (char *) value); + bl_queue++; + } + } + + ret = xmlTextReaderRead(reader); + } + + xmlFreeTextReader(reader); + if (ret != 0) { + if (verbose) { + printf("App Server: failed to parse %s\n", filename); + fflush(stdout); + } + } + } + else + { + if (verbose) { + printf("App Server: Unable to open %s\n", filename); + fflush(stdout); + } + } + +} + +//check firmware type +//return true if "acceptable" type (e.g. fixedwing or rotorcraft) +int check_firmware_type (xmlChar* firmware) { + return (xmlStrEqual(firmware, (const xmlChar *)"fixedwing") + || xmlStrEqual(firmware, (const xmlChar *)"rotorcraft")); +} + +void parse_ac_af(int DevNameIndex, char *filename) { + xmlTextReaderPtr reader; + int ret; + + reader = xmlReaderForFile(filename, NULL, XML_PARSE_NOWARNING | XML_PARSE_NOERROR); /* Dont validate with the DTD */ + + xmlChar *name, *value; + if (reader != NULL) { + ret = xmlTextReaderRead(reader); + + while (ret == 1) { + name = xmlTextReaderName(reader); + + if (name == NULL) { + name = xmlStrdup(BAD_CAST "--"); + } + + if (xmlStrEqual(name, (const xmlChar *)"firmware")) { + xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"name"); + value = xmlTextReaderValue(reader); + //check if firmware name is accaptable + if (check_firmware_type(value)>0) { + strcpy(DevNames[DevNameIndex].type, (char *) value); + } + + } + + if (xmlStrEqual(name, (const xmlChar *)"define")) { + xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"name"); + value = xmlTextReaderValue(reader); + + if (xmlStrEqual(value, (const xmlChar *)"AC_ICON")) { + xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"value"); + value = xmlTextReaderValue(reader); + strcpy(DevNames[DevNameIndex].type, (char *) value); + return; + } + } + + ret = xmlTextReaderRead(reader); + } + + xmlFreeTextReader(reader); + if (ret != 0) { + if (verbose) { + printf("App Server: failed to parse %s\n", filename); + fflush(stdout); + } + } + } else { + if (verbose) { + printf("App Server: Unable to open %s\n", filename); + fflush(stdout); + } + } + +} + +//Parse dl values +void parse_dl_settings(int DevNameIndex, char *filename) { + + xmlTextReaderPtr reader; + int ret; + + reader = xmlReaderForFile(filename, NULL, XML_PARSE_NOWARNING | XML_PARSE_NOERROR); /* Dont validate with the DTD */ + + xmlChar *name, *value; + if (reader != NULL) { + ret = xmlTextReaderRead(reader); + + int valind = 0; + while (ret == 1) { + name = xmlTextReaderName(reader); + if (name == NULL) { + name = xmlStrdup(BAD_CAST "--"); + } + + if (xmlStrEqual(name, (const xmlChar *)"dl_setting")) { + xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"var"); + + value = xmlTextReaderValue(reader); + if (xmlStrEqual(value, (const xmlChar *)"launch")) { + DevNames[DevNameIndex].dl_launch_ind=valind; + } + + if (xmlStrEqual(value, (const xmlChar *)"kill_throttle")) { + DevNames[DevNameIndex].kill_thr_ind=valind; + } + xmlTextReaderNext(reader); + valind+=1; + } + ret = xmlTextReaderRead(reader); + } + xmlFreeTextReader(reader); + if (ret != 0) { + if (verbose) { + printf("App Server: %s : failed to parse\n", filename); + fflush(stdout); + } + } + } + else { + if (verbose) { + printf("App Server: Unable to open %s\n", filename); + fflush(stdout); + } + } +} + +//Parse ac data from conf.xml +void parse_ac_data (char *PprzFolder) { + xmlTextReaderPtr reader; + int ret; + + //Create full file path + char xmlFileName[BUFLEN]; + strcpy(xmlFileName, PprzFolder); + strcat(xmlFileName, "/conf/conf.xml"); + + reader = xmlReaderForFile(xmlFileName, NULL, XML_PARSE_NOWARNING | XML_PARSE_NOERROR); /* Dont validate with the DTD */ + + xmlChar *AcName, *AcInd, *FpPath, *AcColor, *name, *AfPath; + if (reader != NULL) { + ret = xmlTextReaderRead(reader); + int AcId; + + while (ret == 1) { + name = xmlTextReaderName(reader); + if (name == NULL) { + name = xmlStrdup(BAD_CAST "--"); + } + //read waypoint names + + if (xmlStrEqual(name, (const xmlChar *)"aircraft")) { + + xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"ac_id"); + AcInd = xmlTextReaderValue(reader); + + xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"name"); + AcName = xmlTextReaderValue(reader); + + xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"airframe"); + AfPath = xmlTextReaderValue(reader); + + xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"flight_plan"); + FpPath = xmlTextReaderValue(reader); + + xmlTextReaderMoveToAttribute(reader,(const xmlChar *)"gui_color"); + AcColor = xmlTextReaderValue(reader); + + //Get Device Id + AcId = atoi(((char *) AcInd)); + + //Save Device Name + strcpy(DevNames[AcId].name, ((char *) AcName)); + + //Save color + strcpy(DevNames[AcId].color, (char *) AcColor); + + //Save Flight Plan Path + strcpy(DevNames[AcId].flight_plan_path, "/conf/"); + strcat(DevNames[AcId].flight_plan_path , (char *) FpPath); + + //Save airframe Path + strcpy(DevNames[AcId].airframe_path, "/conf/"); + strcat(DevNames[AcId].airframe_path , (char *) AfPath); + + //Save Settings Path + sprintf(DevNames[AcId].settings_path, "/var/%s/settings.xml", (char *) AcName); + + //parse flight plan file for waypoint and block names + char FlightPlanPath[BUFLEN]; + strcpy(FlightPlanPath, PprzFolder); + strcat(FlightPlanPath, DevNames[AcId].flight_plan_path); + parse_ac_fp(AcId, FlightPlanPath); + + //parse airframe file + char AirframePath[BUFLEN]; + strcpy(AirframePath, PprzFolder); + strcat(AirframePath, DevNames[AcId].airframe_path); + parse_ac_af(AcId, AirframePath); + + //parse dl_settings for launch & kill throttle + char SettingsPath[BUFLEN]; + strcpy(SettingsPath, PprzFolder); + strcat(SettingsPath, DevNames[AcId].settings_path); + parse_dl_settings(AcId, SettingsPath); + } + + ret = xmlTextReaderRead(reader); + } + + xmlFreeTextReader(reader); + if (ret != 0) { + if (verbose) { + printf("App Server: failed to parse %s\n", xmlFileName); + fflush(stdout); + } + } + } + else{ + if (verbose) { + printf("App Server: Unable to open %s\n", xmlFileName); + fflush(stdout); + } + } + + return; +} // end of XMLParseDoc function + +// Print help message +void print_help() { + printf("Usage: app_server [options]\n"); + printf(" Options :\n"); + printf(" -t \tfor receiving devices commands (default: %d)\n", tcp_port); + printf(" -u \tfor sending AC data (default: %d)\n", udp_port); + printf(" -b \tdefault is %s\n", defaultIvyBus); + printf(" -p \tpassword for connection with control capabilities (default is %s)\n", defaultAppPass); + printf(" -v\tverbose\n"); + printf(" -h --help show this help\n"); +} + +int main(int argc, char **argv) { + + // default password + AppPass = defaultAppPass; + + // try environment variable first, set to default if failed + IvyBus = getenv("IVYBUS"); + if (IvyBus == NULL) IvyBus = defaultIvyBus; + + // Look for paparazzi folder (PAPARAZZI_HOME or assume local path by default) + char* PprzFolder = getenv("PAPARAZZI_HOME"); + if (PprzFolder == NULL) PprzFolder = defaultPprzFolder; + + // Parse options + int i; + for (i = 1; i < argc; ++i) { + if (strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-h") == 0) { + print_help(); + exit(0); + } + else if (strcmp(argv[i], "-t") == 0) { + tcp_port = atoi(argv[++i]); + } + else if (strcmp(argv[i], "-u") == 0) { + udp_port = atoi(argv[++i]); + } + else if (strcmp(argv[i], "-b") == 0) { + IvyBus = argv[++i]; + } + else if (strcmp(argv[i], "-p") == 0) { + AppPass = argv[++i]; + } + else if (strcmp(argv[i], "-v") == 0) { + verbose = 1; + } + else { + printf("App Server: Unknown option\n"); + print_help(); + exit(0); + } + } + + if (verbose) { + printf("### Paparazzi App Server ###\n"); + printf("Using Paparazzi Folder : %s\n", PprzFolder); + printf("Server listen port (TCP) : %d\n", tcp_port); + printf("Server broadcast port (UDP) : %d\n", udp_port); + printf("Control Pass : %s\n", AppPass); + printf("Ivy Bus : %s\n", IvyBus); + fflush(stdout); + } + + //Parse conf.xml + parse_ac_data(PprzFolder); + + //Create tcp listener +#if !GLIB_CHECK_VERSION (2, 35, 1) + // init GLib type system (only for older version) + g_type_init(); +#endif + GSocketService *service = g_socket_service_new(); + + GInetAddress *address = g_inet_address_new_any(G_SOCKET_FAMILY_IPV6); //G_SOCKET_FAMILY_IPV4 could be used + GSocketAddress *socket_address = g_inet_socket_address_new(address, tcp_port); + //Add listener + g_socket_listener_add_address(G_SOCKET_LISTENER(service), socket_address, G_SOCKET_TYPE_STREAM, + G_SOCKET_PROTOCOL_TCP, NULL, NULL, NULL); + + g_object_unref(socket_address); + g_object_unref(address); + g_socket_service_start(service); + + //Connect listening signal + g_signal_connect(service, "incoming", G_CALLBACK(new_connection), NULL); + + //Here comes the ivy bindings + IvyInit ("PPRZ_App_Server", "Papparazzi App Server Ready", NULL, NULL, NULL, NULL); + + IvyBindMsg(Ivy_All_Msgs, NULL, "(^ground .*)"); + IvyBindMsg(Ivy_All_Msgs, NULL, "(^\\S* AIRSPEED (\\S*) (\\S*) (\\S*) (\\S*))"); + IvyStart(IvyBus); + + GMainLoop *loop = g_main_loop_new(NULL, FALSE); + + if (verbose) { + printf("Starting App Server\n"); + fflush(stdout); + } + + g_main_loop_run(loop); + + if (verbose) { + printf("Stoping App Server\n"); + fflush(stdout); + } + return 0; +} + diff --git a/sw/ground_segment/tmtc/boa b/sw/ground_segment/tmtc/boa deleted file mode 100755 index 75446c3443..0000000000 --- a/sw/ground_segment/tmtc/boa +++ /dev/null @@ -1,6 +0,0 @@ -#! /bin/sh -if test -z "$PAPARAZZI_SRC"; then - PAPARAZZI_SRC=/usr/share/paparazzi -fi -mkdir -p $PAPARAZZI_HOME/var/logs -/usr/sbin/boa -d -f $PAPARAZZI_HOME/var/boa.conf diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml index 4a637d3fc3..2de1e85ef8 100644 --- a/sw/ground_segment/tmtc/server.ml +++ b/sw/ground_segment/tmtc/server.ml @@ -201,7 +201,7 @@ let send_dl_values = fun a -> for i = 0 to a.nb_dl_setting_values - 1 do csv := sprintf "%s%f," !csv a.dl_setting_values.(i) done; - let vs = ["ac_id", Pprz.String a.id; "values", Pprz.String !csv] in + let vs = ["ac_id", Pprz.String a.id; "values", Pprz.String ("|"^ !csv ^"|")] in Ground_Pprz.message_send my_id "DL_VALUES" vs let send_svsinfo = fun a -> @@ -223,7 +223,7 @@ let send_svsinfo = fun a -> concat azim a.svinfo.(i).azim; concat age a.svinfo.(i).age done; - let f = fun s r -> (s, Pprz.String !r) in + let f = fun s r -> (s, Pprz.String ("|"^ !r ^"|")) in let vs = ["ac_id", Pprz.String a.id; "pacc", Pprz.Int a.gps_Pacc; f "svid" svid; f "flags" flags; f "qi" qi; f "msg_age" age; diff --git a/sw/lib/ocaml/Makefile b/sw/lib/ocaml/Makefile index d0872d3788..9b0f9e53c1 100644 --- a/sw/lib/ocaml/Makefile +++ b/sw/lib/ocaml/Makefile @@ -55,7 +55,7 @@ PKGCOMMON=xml-light,netclient,glibivy,lablgtk2 XINCLUDES= XPKGCOMMON=xml-light,glibivy,$(LABLGTK2GNOMECANVAS),lablgtk2.glade -SRC = 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 convert.o ubx.ml pprz.ml xbee.ml logpprz.ml xmlCom.ml os_calls.ml editAirframe.ml defivybus.ml +SRC = 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 convert.o ubx.ml pprz.ml xbee.ml logpprz.ml xmlCom.ml os_calls.ml editAirframe.ml defivybus.ml fp_proc.ml CMO = $(SRC:.ml=.cmo) CMX = $(SRC:.ml=.cmx) @@ -187,7 +187,8 @@ clean : # .depend: Makefile $(GEN_DEP) - $(OCAMLDEP) *.ml* > .depend + @echo DEPEND $@ + $(Q)$(OCAMLDEP) *.ml* > .depend ifneq ($(MAKECMDGOALS),clean) -include .depend diff --git a/sw/lib/ocaml/env.ml b/sw/lib/ocaml/env.ml index cf98beaed2..1aeeacadf0 100644 --- a/sw/lib/ocaml/env.ml +++ b/sw/lib/ocaml/env.ml @@ -51,7 +51,7 @@ let gconf_file = paparazzi_home // "conf" // "%gconf.xml" let gcs_icons_path = paparazzi_home // "data" // "pictures" // "gcs_icons" -let dump_fp = paparazzi_src // "sw" // "tools" // "gen_flight_plan.out -dump" +let dump_fp = paparazzi_src // "sw" // "tools" // "generators" // "gen_flight_plan.out -dump" let expand_ac_xml = fun ?(raise_exception = true) ac_conf -> let prefix = fun s -> sprintf "%s/conf/%s" paparazzi_home s in @@ -80,6 +80,7 @@ let expand_ac_xml = fun ?(raise_exception = true) ac_conf -> 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 *) diff --git a/sw/tools/fp_proc.ml b/sw/lib/ocaml/fp_proc.ml similarity index 100% rename from sw/tools/fp_proc.ml rename to sw/lib/ocaml/fp_proc.ml diff --git a/sw/tools/fp_proc.mli b/sw/lib/ocaml/fp_proc.mli similarity index 100% rename from sw/tools/fp_proc.mli rename to sw/lib/ocaml/fp_proc.mli diff --git a/sw/lib/ocaml/geometry_2d.ml b/sw/lib/ocaml/geometry_2d.ml index eb0513cfcb..3ae3e65fa1 100644 --- a/sw/lib/ocaml/geometry_2d.ml +++ b/sw/lib/ocaml/geometry_2d.ml @@ -298,7 +298,7 @@ let test_on_hl t = (test_in_segment t)||(t=T_OUT_SEG_PT4)||(t=T_OUT_SEG_PT2) let crossing_seg_seg a b c d = match crossing_point a (vect_make a b) c (vect_make c d) with None -> false - | Some (type1, type2, _pt) -> (test_in_segment type1)&&(test_in_segment type2) + | Some (type1, type2, _pt) -> (test_in_segment type1) && (test_in_segment type2) (* ============================================================================= *) (* = Teste l'intersection d'un segment (a,b) et d'une demi-droite (c,v) = *) @@ -346,7 +346,7 @@ let poly_is_closed poly = (* = Ferme un polygone [A; B; C; D] -> [A; B; C; D; A] = *) (* ============================================================================= *) let poly_close poly = - if poly = [] or poly_is_closed poly then poly else poly@[List.hd poly] + if poly = [] || poly_is_closed poly then poly else poly@[List.hd poly] (* ============================================================================= *) (* = Ferme un polygone [A; B; C; D] -> [|A; B; C; D; A; B|] = *) @@ -393,10 +393,10 @@ let convex_hull poly = in let plus_proche a b c d = - (d=a) or ((not(c=a)) & - ((du_meme_cote b c d a) or ( + (d=a) || ((not(c=a)) && + ((du_meme_cote b c d a) || ( let u = vect_make b c and v = vect_make b d in - (det u v)=0.0 & (abs_float(u.x2D)+.abs_float(u.y2D)> + (det u v)=0.0 && (abs_float(u.x2D)+.abs_float(u.y2D)> abs_float(v.x2D)+.abs_float(v.y2D))))) in @@ -412,7 +412,7 @@ let convex_hull poly = | [] -> raise Exit in - let p a b = a.x2Db.y2D) in + let p a b = a.x2Db.y2D) in let l2=poly in let debut,_=extract_mini p l2 in let rec itere a o liste sol = @@ -640,7 +640,7 @@ let in_tesselation poly = let a = vertices.(n1) and b = vertices.(n2) in (* AAA if is_in_cone a b && is_in_cone b a then begin *) - if is_in_cone a b or is_in_cone b a then begin + if is_in_cone a b || is_in_cone b a then begin let rec f l = match l with c::reste -> diff --git a/sw/lib/ocaml/mapCanvas.ml b/sw/lib/ocaml/mapCanvas.ml index eb2a4ffce3..b550e984de 100644 --- a/sw/lib/ocaml/mapCanvas.ml +++ b/sw/lib/ocaml/mapCanvas.ml @@ -258,6 +258,7 @@ object (self) initializer ( spin_button#set_adjustment adj; + spin_button#set_value 1.; (* this should be done by set_adjustment but seems to fail on ubuntu 13.10 (at least) *) utc_time#hide (); diff --git a/sw/lib/ocaml/mapWaypoints.ml b/sw/lib/ocaml/mapWaypoints.ml index adc7231e0b..bd2f8dd45e 100644 --- a/sw/lib/ocaml/mapWaypoints.ml +++ b/sw/lib/ocaml/mapWaypoints.ml @@ -143,8 +143,9 @@ object (self) ~value:alt ~lower:(-100.) ~upper:10000. ~step_incr:1. ~page_incr:10.0 ~page_size:0. () in ea#set_adjustment adj; + ea#set_value alt; (* this should be done by set_adjustment but seems to fail on ubuntu 13.10 (at least) *) - let agl = alt -. float (try Srtm.of_wgs84 wgs84 with _ -> 0) in + let agl = alt -. float (try Srtm.of_wgs84 initial_wgs84 with _ -> 0) in let agl_lab = GMisc.label ~text:(sprintf " AGL: %4.0fm" agl) ~packing:ha#add () in let plus10= GButton.button ~label:"+10" ~packing:ha#add () in let change_alt = fun x -> diff --git a/sw/lib/ocaml/ocaml_tools.ml b/sw/lib/ocaml/ocaml_tools.ml index 6d801905c2..43579b9921 100644 --- a/sw/lib/ocaml/ocaml_tools.ml +++ b/sw/lib/ocaml/ocaml_tools.ml @@ -25,8 +25,8 @@ let pi = 3.14159265358979323846;; let open_compress file = - if Filename.check_suffix file "gz" or Filename.check_suffix file "Z" or - Filename.check_suffix file "zip" or Filename.check_suffix file "ZIP" then + if Filename.check_suffix file "gz" || Filename.check_suffix file "Z" || + Filename.check_suffix file "zip" || Filename.check_suffix file "ZIP" then Unix.open_process_in ("gunzip -c "^file) else if Filename.check_suffix file "bz2" then Unix.open_process_in ("bunzip2 -c "^file) diff --git a/sw/lib/ocaml/pprz.ml b/sw/lib/ocaml/pprz.ml index f2de6e2ff8..b6af3c1fa6 100644 --- a/sw/lib/ocaml/pprz.ml +++ b/sw/lib/ocaml/pprz.ml @@ -160,7 +160,7 @@ let rec string_of_value = function | Int64 x -> Int64.to_string x | Char c -> String.make 1 c | String s -> s - | Array a -> String.concat separator (Array.to_list (Array.map string_of_value a)) + | Array a -> "|"^(String.concat separator (Array.to_list (Array.map string_of_value a)))^"|" let magic = fun x -> (Obj.magic x:('a,'b,'c) Pervasives.format) @@ -241,6 +241,17 @@ let alt_unit_coef_of_xml = fun ?auto xml -> in coef +let key_modifiers_of_string = fun key -> + let key_split = Str.split (Str.regexp "\\+") key in + let keys = List.map (fun k -> + match k with + | "Ctrl" -> "" + | "Alt" -> "" + | "Shift" -> "" + | "Meta" -> "" + | x -> x + ) key_split in + String.concat "" keys let pipe_regexp = Str.regexp "|" let field_of_xml = fun xml -> @@ -650,8 +661,21 @@ module MessagesOfXml(Class:CLASS_Xml) = struct let space = Str.regexp "[ \t]+" + let array_sep = Str.regexp "|" let values_of_string = fun s -> - match Str.split space s with + (* split arguments and arrays *) + let array_split = Str.full_split array_sep s in + let rec loop = fun fields -> + match fields with + | [] -> [] + | (Str.Delim "|")::((Str.Text l)::[Str.Delim "|"]) -> [l] + | (Str.Delim "|")::((Str.Text l)::((Str.Delim "|")::xs)) -> [l] @ (loop xs) + | [Str.Text x] -> Str.split space x + | (Str.Text x)::xs -> (Str.split space x) @ (loop xs) + | (Str.Delim _)::_ -> failwith "Pprz.values_of_string: incorrect array delimiter" + in + let msg_split = loop array_split in + match msg_split with msg_name::args -> begin try diff --git a/sw/lib/ocaml/pprz.mli b/sw/lib/ocaml/pprz.mli index 4181035fb0..0f47a307d2 100644 --- a/sw/lib/ocaml/pprz.mli +++ b/sw/lib/ocaml/pprz.mli @@ -109,6 +109,12 @@ val alt_unit_coef_of_xml : ?auto:string -> Xml.xml -> string (** Return coef for alternate unit *) +val key_modifiers_of_string : string -> string +(** Convert key modifiers from Qt style (without '<' or '>', separated with '+') + * to GTK style. + * Supported modifiers are Alt, Ctrl, Shift and Meta + *) + exception Unknown_msg_name of string * string (** [Unknown_msg_name (name, class_name)] Raised if message [name] is not found in class [class_name]. *) diff --git a/sw/logalizer/Makefile b/sw/logalizer/Makefile index 2dad1b104d..36c45ed2c8 100644 --- a/sw/logalizer/Makefile +++ b/sw/logalizer/Makefile @@ -32,23 +32,23 @@ XLINKPKG = $(XPKG) -linkpkg -dllpath-pkg pprz.xlib all: play plotter plot sd2log plotprofile openlog2tlm -play : log_file.cmo play_core.cmo play.cmo +play : log_file.cmo play_core.cmo play.cmo $(LIBPPRZCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) gtkInit.cmo $^ -play-nox : play_core.cmo play-nox.cmo +play-nox : play_core.cmo play-nox.cmo $(LIBPPRZCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $^ -plotter : plotter.cmo +plotter : plotter.cmo $(LIBPPRZCMA) $(XLIBPPRZCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(XLINKPKG) gtkInit.cmo $^ -plot : log_file.cmo gtk_export.cmo export.cmo plot.cmo +plot : log_file.cmo gtk_export.cmo export.cmo plot.cmo $(LIBPPRZCMA) $(XLIBPPRZCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(XLINKPKG) gtkInit.cmo $^ -sd2log : sd2log.cmo +sd2log : sd2log.cmo $(LIBPPRZCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $^ @@ -200,7 +200,8 @@ clean: # .depend: Makefile - $(OCAMLDEP) -I $(LIBPPRZDIR) *.ml* > .depend + @echo DEPEND $@ + $(Q)$(OCAMLDEP) -I $(LIBPPRZDIR) *.ml* > .depend ifneq ($(MAKECMDGOALS),clean) -include .depend diff --git a/sw/logalizer/export.ml b/sw/logalizer/export.ml index ae3e95be1d..1ff7431e41 100644 --- a/sw/logalizer/export.ml +++ b/sw/logalizer/export.ml @@ -274,8 +274,18 @@ let popup = fun ?(no_gui = false) xml log_filename data -> (** Render the columns *) display_columns w#treeview_messages model; + (** Build list of message name *) + let msg_names = Hashtbl.create 30 in + List.iter (fun (_, name, _) -> if not (Hashtbl.mem msg_names name) then Hashtbl.add msg_names name ()) data; (** Fill the colums *) let xml_class = ExtXml.child ~select:(fun c -> ExtXml.attrib c "name" = class_name) xml "class" in + (** Filter xml message *) + let xml_class = Xml.Element ( + class_name, [], + List.filter (fun xml -> + let name = Xml.attrib xml "name" in + Hashtbl.mem msg_names name) (Xml.children xml_class) + ) in let prefs = read_preferences () in fill_data w#treeview_messages model xml_class prefs; diff --git a/sw/logalizer/play_core.ml b/sw/logalizer/play_core.ml index c6268c50ed..8a9d891d34 100644 --- a/sw/logalizer/play_core.ml +++ b/sw/logalizer/play_core.ml @@ -29,7 +29,7 @@ module Tm_Pprz = Pprz.Messages(struct let name = "telemetry" end) let (//) = Filename.concat let replay_dir = Env.paparazzi_home // "var" // "replay" -let dump_fp = Env.paparazzi_src // "sw" // "tools" // "gen_flight_plan.out -dump" +let dump_fp = Env.paparazzi_src // "sw" // "tools" // "generators" // "gen_flight_plan.out -dump" let log = ref [||] diff --git a/sw/simulator/Makefile b/sw/simulator/Makefile index dc1f876449..37198beead 100644 --- a/sw/simulator/Makefile +++ b/sw/simulator/Makefile @@ -56,19 +56,19 @@ fg.so : fg.o @echo BUILD $@ $(Q)$(CC) -shared -o $@ $^ -simhitl : fg.so $(SIMHCMO) simhitl.cmo +simhitl : fg.so $(SIMHCMO) simhitl.cmo $(LIBPPRZCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) gtkInit.cmo $< -sitl.cma : fg.o $(SIMSCMO) +sitl.cma : fg.o $(SIMSCMO) $(LIBPPRZCMA) @echo OL $@ $(Q)$(OCAMLMKLIB) -o sitl $^ -sitl.cmxa : $(SIMSCMX) +sitl.cmxa : $(SIMSCMX) $(LIBPPRZCMXA) @echo OC $@ $(Q)$(OCAMLOPT) -o $@ -a $^ -gaia : gaia.cmo +gaia : gaia.cmo $(LIBPPRZCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) gtkInit.cmo $< @@ -103,7 +103,8 @@ clean : # .depend: Makefile - $(OCAMLDEP) -I $(LIBPPRZDIR) *.ml* > .depend + @echo DEPEND $@ + $(Q)$(OCAMLDEP) -I $(LIBPPRZDIR) *.ml* > .depend ifneq ($(MAKECMDGOALS),clean) -include .depend diff --git a/sw/simulator/nps/nps_fdm_jsbsim.c b/sw/simulator/nps/nps_fdm_jsbsim.c index 1e20c5c376..b831f18a2f 100644 --- a/sw/simulator/nps/nps_fdm_jsbsim.c +++ b/sw/simulator/nps/nps_fdm_jsbsim.c @@ -94,6 +94,9 @@ static FGFDMExec* FDMExec; static struct LtpDef_d ltpdef; +// Offset between ecef in geodetic and geocentric coordinates +static struct EcefCoor_d offset; + /// The largest distance between vehicle CG and contact point double vehicle_radius_max; @@ -110,6 +113,8 @@ void nps_fdm_init(double dt) { fdm.nan_count = 0; + VECT3_ASSIGN(offset, 0., 0., 0.); + init_jsbsim(dt); FDMExec->RunIC(); @@ -391,6 +396,7 @@ static void init_jsbsim(double dt) { IC->SetLatitudeDegIC(DegOfRad(gc_lat)); IC->SetLongitudeDegIC(NAV_LON0 / 1e7); + IC->SetAltitudeASLFtIC(FeetOfMeters(GROUND_ALT + 2.0)); IC->SetTerrainElevationFtIC(FeetOfMeters(GROUND_ALT)); IC->SetPsiDegIC(QFU); @@ -402,6 +408,16 @@ static void init_jsbsim(double dt) { cerr << "Initialization from flight plan unsuccessful" << endl; exit(-1); } + + // compute offset between geocentric and geodetic ecef + struct LlaCoor_d lla0 = { RadOfDeg(NAV_LON0 / 1e7), gd_lat, (double)(NAV_ALT0+NAV_MSL0)/1000. }; + ecef_of_lla_d(&offset, &lla0); + struct EcefCoor_d ecef0 = { + MetersOfFeet(FDMExec->GetPropagate()->GetLocation().Entry(1)), + MetersOfFeet(FDMExec->GetPropagate()->GetLocation().Entry(2)), + MetersOfFeet(FDMExec->GetPropagate()->GetLocation().Entry(3)) + }; + VECT3_DIFF(offset, offset, ecef0); } // calculate vehicle max radius in m @@ -465,6 +481,7 @@ static void jsbsimloc_to_loc(EcefCoor_d* fdm_location, const FGLocation* jsb_loc fdm_location->y = MetersOfFeet(jsb_location->Entry(2)); fdm_location->z = MetersOfFeet(jsb_location->Entry(3)); + VECT3_ADD(*fdm_location, offset); } /** diff --git a/sw/supervision/Makefile b/sw/supervision/Makefile index 374c645e19..49634ee0e4 100644 --- a/sw/supervision/Makefile +++ b/sw/supervision/Makefile @@ -85,7 +85,8 @@ clean: # .depend: Makefile - $(OCAMLDEP) -I $(LIBPPRZDIR) *.ml* > .depend + @echo DEPEND $@ + $(Q)$(OCAMLDEP) -I $(LIBPPRZDIR) *.ml* > .depend ifneq ($(MAKECMDGOALS),clean) -include .depend diff --git a/sw/tools/Makefile b/sw/tools/Makefile index b3ed28bbe7..1565800749 100644 --- a/sw/tools/Makefile +++ b/sw/tools/Makefile @@ -22,28 +22,20 @@ # Quiet compilation Q=@ +CC = gcc + include ../Makefile.ocaml INCLUDES = PKG = -package pprz LINKPKG = $(PKG) -linkpkg -dllpath-pkg pprz -all: gen_common.cmo gen_aircraft.out gen_airframe.out gen_messages2.out gen_messages.out gen_ubx.out gen_mtk.out gen_flight_plan.out gen_radio.out gen_periodic.out gen_settings.out gen_xsens.out gen_modules.out gen_autopilot.out gen_abi.out find_free_msg_id.out gen_srtm.out mergelogs +all: find_free_msg_id.out mergelogs -FP_CMO = fp_proc.cmo gen_flight_plan.cmo -ABS_FP = $(FP_CMO:%=$$PAPARAZZI_SRC/sw/tools/%) -gen_flight_plan.out : $(FP_CMO) $(LIBPPRZCMA) - @echo OL $@ - $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $^ - -gen_srtm.out : gen_srtm.ml $(LIBPPRZCMA) - @echo OL $@ - $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $< - -%.out : %.ml gen_common.cmo $(LIBPPRZCMA) +%.out : %.ml $(LIBPPRZCMA) @echo OL $< - $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) gen_common.cmo $< + $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $< %.cmo : %.ml @echo OC $< @@ -54,10 +46,11 @@ gen_srtm.out : gen_srtm.ml $(LIBPPRZCMA) $(Q)$(OCAMLC) $(INCLUDES) $(PKG) -c $< mergelogs: mergelogs.c - gcc mergelogs.c -o mergelogs + @echo LD $@ + $(Q)$(CC) mergelogs.c -o mergelogs clean: - $(Q)rm -f *.cm* *.out *~ .depend fp_parser.ml fp_parser.mli mergelogs + $(Q)rm -f *.cm* *.out *~ .depend mergelogs .PHONY: all clean @@ -66,7 +59,8 @@ clean: # .depend: Makefile - $(OCAMLDEP) -I $(LIBPPRZDIR) *.ml *.mli > .depend + @echo DEPEND $@ + $(Q)$(OCAMLDEP) -I $(LIBPPRZDIR) *.ml *.mli > .depend ifneq ($(MAKECMDGOALS),clean) -include .depend diff --git a/sw/tools/generators/Makefile b/sw/tools/generators/Makefile new file mode 100644 index 0000000000..dcc5a43ff8 --- /dev/null +++ b/sw/tools/generators/Makefile @@ -0,0 +1,68 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# Copyright (C) 2003-2014 The Paparazzi Team +# +# 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. + +# Quiet compilation +Q=@ + +include ../../Makefile.ocaml + +INCLUDES = +PKG = -package pprz +LINKPKG = $(PKG) -linkpkg -dllpath-pkg pprz + +all: gen_common.cmo gen_aircraft.out gen_airframe.out gen_messages2.out gen_messages.out gen_ubx.out gen_mtk.out gen_flight_plan.out gen_radio.out gen_periodic.out gen_settings.out gen_xsens.out gen_modules.out gen_autopilot.out gen_abi.out gen_srtm.out + +gen_flight_plan.out : gen_flight_plan.cmo $(LIBPPRZCMA) + @echo OL $@ + $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $^ + +gen_srtm.out : gen_srtm.ml $(LIBPPRZCMA) + @echo OL $@ + $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $< + +%.out : %.ml gen_common.cmo $(LIBPPRZCMA) + @echo OL $< + $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) gen_common.cmo $< + +%.cmo : %.ml + @echo OC $< + $(Q)$(OCAMLC) $(INCLUDES) $(PKG) -c $< + +%.cmi : %.mli + @echo OC $< + $(Q)$(OCAMLC) $(INCLUDES) $(PKG) -c $< + +clean: + $(Q)rm -f *.cm* *.out *~ .depend + +.PHONY: all clean + +# +# Dependencies +# + +.depend: Makefile + @echo DEPEND $@ + $(Q)$(OCAMLDEP) -I $(LIBPPRZDIR) *.ml *.mli > .depend + +ifneq ($(MAKECMDGOALS),clean) +-include .depend +endif diff --git a/sw/tools/gen_abi.ml b/sw/tools/generators/gen_abi.ml similarity index 100% rename from sw/tools/gen_abi.ml rename to sw/tools/generators/gen_abi.ml diff --git a/sw/tools/gen_aircraft.ml b/sw/tools/generators/gen_aircraft.ml similarity index 100% rename from sw/tools/gen_aircraft.ml rename to sw/tools/generators/gen_aircraft.ml diff --git a/sw/tools/gen_airframe.ml b/sw/tools/generators/gen_airframe.ml similarity index 100% rename from sw/tools/gen_airframe.ml rename to sw/tools/generators/gen_airframe.ml diff --git a/sw/tools/gen_autopilot.ml b/sw/tools/generators/gen_autopilot.ml similarity index 100% rename from sw/tools/gen_autopilot.ml rename to sw/tools/generators/gen_autopilot.ml diff --git a/sw/tools/gen_common.ml b/sw/tools/generators/gen_common.ml similarity index 100% rename from sw/tools/gen_common.ml rename to sw/tools/generators/gen_common.ml diff --git a/sw/tools/gen_common.mli b/sw/tools/generators/gen_common.mli similarity index 100% rename from sw/tools/gen_common.mli rename to sw/tools/generators/gen_common.mli diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/generators/gen_flight_plan.ml similarity index 98% rename from sw/tools/gen_flight_plan.ml rename to sw/tools/generators/gen_flight_plan.ml index d0f466edf2..3cd7146a82 100644 --- a/sw/tools/gen_flight_plan.ml +++ b/sw/tools/generators/gen_flight_plan.ml @@ -363,7 +363,14 @@ let rec print_stage = fun index_of_waypoints x -> lprintf "waypoints[0].y = %s;\n" (parsed_attrib x "y"); "0" in - let at = try ExtXml.attrib x "approaching_time" with _ -> "CARROT" in + let at = try Some (ExtXml.attrib x "approaching_time") with _ -> None in + let et = try Some (ExtXml.attrib x "exceeding_time") with _ -> None in + let at = match at, et with + | Some a, None -> a + | None, Some e -> "-"^e + | None, None -> "CARROT" + | _, _ -> failwith "Error: 'approaching_time' and 'exceeding_time' attributes are not compatible" + in let last_wp = try get_index_waypoint (ExtXml.attrib x "from") index_of_waypoints diff --git a/sw/tools/gen_messages.ml b/sw/tools/generators/gen_messages.ml similarity index 100% rename from sw/tools/gen_messages.ml rename to sw/tools/generators/gen_messages.ml diff --git a/sw/tools/gen_messages2.ml b/sw/tools/generators/gen_messages2.ml similarity index 100% rename from sw/tools/gen_messages2.ml rename to sw/tools/generators/gen_messages2.ml diff --git a/sw/tools/gen_modules.ml b/sw/tools/generators/gen_modules.ml similarity index 100% rename from sw/tools/gen_modules.ml rename to sw/tools/generators/gen_modules.ml diff --git a/sw/tools/gen_mtk.ml b/sw/tools/generators/gen_mtk.ml similarity index 100% rename from sw/tools/gen_mtk.ml rename to sw/tools/generators/gen_mtk.ml diff --git a/sw/tools/gen_periodic.ml b/sw/tools/generators/gen_periodic.ml similarity index 100% rename from sw/tools/gen_periodic.ml rename to sw/tools/generators/gen_periodic.ml diff --git a/sw/tools/gen_radio.ml b/sw/tools/generators/gen_radio.ml similarity index 100% rename from sw/tools/gen_radio.ml rename to sw/tools/generators/gen_radio.ml diff --git a/sw/tools/gen_settings.ml b/sw/tools/generators/gen_settings.ml similarity index 100% rename from sw/tools/gen_settings.ml rename to sw/tools/generators/gen_settings.ml diff --git a/sw/tools/gen_srtm.ml b/sw/tools/generators/gen_srtm.ml similarity index 100% rename from sw/tools/gen_srtm.ml rename to sw/tools/generators/gen_srtm.ml diff --git a/sw/tools/gen_ubx.ml b/sw/tools/generators/gen_ubx.ml similarity index 100% rename from sw/tools/gen_ubx.ml rename to sw/tools/generators/gen_ubx.ml diff --git a/sw/tools/gen_xsens.ml b/sw/tools/generators/gen_xsens.ml similarity index 100% rename from sw/tools/gen_xsens.ml rename to sw/tools/generators/gen_xsens.ml