diff --git a/.gitmodules b/.gitmodules index 4c2d20aa4d..4a2ea3a539 100644 --- a/.gitmodules +++ b/.gitmodules @@ -19,3 +19,6 @@ [submodule "sw/ext/mavlink"] path = sw/ext/mavlink url = https://github.com/paparazzi/mavlink.git +[submodule "sw/ext/pprzlink"] + path = sw/ext/pprzlink + url = https://github.com/paparazzi/pprzlink.git diff --git a/Makefile b/Makefile index 2a43d6eff5..2897ef0821 100644 --- a/Makefile +++ b/Makefile @@ -57,11 +57,10 @@ endif # define some paths # LIB=sw/lib -STATICINCLUDE =$(PAPARAZZI_HOME)/var/include +STATICINCLUDE=$(PAPARAZZI_HOME)/var/include CONF=$(PAPARAZZI_SRC)/conf AIRBORNE=sw/airborne SIMULATOR=sw/simulator -MULTIMON=sw/ground_segment/multimon COCKPIT=sw/ground_segment/cockpit TMTC=sw/ground_segment/tmtc GENERATORS=$(PAPARAZZI_SRC)/sw/tools/generators @@ -82,7 +81,7 @@ SUBDIRS = $(PPRZCENTER) $(MISC) $(LOGALIZER) # # xml files used as input for header generation # -MESSAGES_XML = $(CONF)/messages.xml +CUSTOM_MESSAGES_XML = $(CONF)/messages.xml ABI_XML = $(CONF)/abi.xml UBX_XML = $(CONF)/ubx.xml MTK_XML = $(CONF)/mtk.xml @@ -91,20 +90,17 @@ XSENS_XML = $(CONF)/xsens_MTi-G.xml # # generated header files # -MESSAGES_H=$(STATICINCLUDE)/messages.h -MESSAGES2_H=$(STATICINCLUDE)/messages2.h +PPRZLINK_DIR=sw/ext/pprzlink +PPRZLINK_INSTALL=$(PAPARAZZI_HOME)/var/lib/ocaml +MESSAGES_INSTALL=$(PAPARAZZI_HOME)/var UBX_PROTOCOL_H=$(STATICINCLUDE)/ubx_protocol.h MTK_PROTOCOL_H=$(STATICINCLUDE)/mtk_protocol.h XSENS_PROTOCOL_H=$(STATICINCLUDE)/xsens_protocol.h -DL_PROTOCOL_H=$(STATICINCLUDE)/dl_protocol.h -DL_PROTOCOL2_H=$(STATICINCLUDE)/dl_protocol2.h ABI_MESSAGES_H=$(STATICINCLUDE)/abi_messages.h -INTERMCU_MSG_H=$(STATICINCLUDE)/intermcu_msg.h MAVLINK_DIR=$(STATICINCLUDE)/mavlink/ MAVLINK_PROTOCOL_H=$(MAVLINK_DIR)protocol.h -GEN_HEADERS = $(MESSAGES_H) $(UBX_PROTOCOL_H) $(MTK_PROTOCOL_H) $(XSENS_PROTOCOL_H) $(DL_PROTOCOL_H) $(ABI_MESSAGES_H) $(INTERMCU_MSG_H) $(MAVLINK_PROTOCOL_H) - +GEN_HEADERS = $(UBX_PROTOCOL_H) $(MTK_PROTOCOL_H) $(XSENS_PROTOCOL_H) $(ABI_MESSAGES_H) $(MAVLINK_PROTOCOL_H) all: ground_segment ext lpctools @@ -139,11 +135,12 @@ ground_segment.opt: ground_segment cockpit.opt tmtc.opt static: cockpit tmtc generators sim_static joystick static_h -libpprz: _save_build_version - $(MAKE) -C $(LIB)/ocaml +libpprzlink: + $(MAKE) -C $(EXT) pprzlink.update + $(Q)Q=$(Q) DESTDIR=$(PPRZLINK_INSTALL) $(MAKE) -C $(PPRZLINK_DIR) libpprzlink-install -multimon: - $(MAKE) -C $(MULTIMON) +libpprz: libpprzlink _save_build_version + $(MAKE) -C $(LIB)/ocaml cockpit: libpprz $(MAKE) -C $(COCKPIT) @@ -151,10 +148,10 @@ cockpit: libpprz cockpit.opt: libpprz $(MAKE) -C $(COCKPIT) opt -tmtc: libpprz cockpit multimon +tmtc: libpprz cockpit $(MAKE) -C $(TMTC) -tmtc.opt: libpprz cockpit.opt multimon +tmtc.opt: libpprz cockpit.opt $(MAKE) -C $(TMTC) opt generators: libpprz @@ -184,24 +181,19 @@ $(PPRZCENTER): libpprz $(LOGALIZER): libpprz +static_h: pprzlink_protocol $(GEN_HEADERS) -static_h: $(GEN_HEADERS) - -$(MESSAGES_H) : $(MESSAGES_XML) generators +pprzlink_protocol : $(Q)test -d $(STATICINCLUDE) || mkdir -p $(STATICINCLUDE) - @echo GENERATE $@ - $(eval $@_TMP := $(shell $(MKTEMP))) - $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) $(GENERATORS)/gen_messages.out $< telemetry > $($@_TMP) - $(Q)mv $($@_TMP) $@ - $(Q)chmod a+r $@ + $(Q)test -d $(STATICLIB) || mkdir -p $(STATICLIB) +ifeq ("$(wildcard $(CUSTOM_MESSAGES_XML))","") + @echo GENERATE $@ with default messages + $(Q)Q=$(Q) MESSAGES_INSTALL=$(MESSAGES_INSTALL) VALIDATE_XML=FALSE $(MAKE) -C $(PPRZLINK_DIR) pymessages +else + @echo GENERATE $@ with custome messages from $(CUSTOM_MESSAGES_XML) + $(Q)Q=$(Q) MESSAGES_XML=$(CUSTOM_MESSAGES_XML) MESSAGES_INSTALL=$(MESSAGES_INSTALL) $(MAKE) -C $(PPRZLINK_DIR) pymessages +endif -$(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) $(GENERATORS)/gen_messages2.out $< telemetry > $($@_TMP) - $(Q)mv $($@_TMP) $@ - $(Q)chmod a+r $@ $(UBX_PROTOCOL_H) : $(UBX_XML) generators @echo GENERATE $@ @@ -224,20 +216,6 @@ $(XSENS_PROTOCOL_H) : $(XSENS_XML) generators $(Q)mv $($@_TMP) $@ $(Q)chmod a+r $@ -$(DL_PROTOCOL_H) : $(MESSAGES_XML) generators - @echo GENERATE $@ - $(eval $@_TMP := $(shell $(MKTEMP))) - $(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) generators - @echo GENERATE $@ - $(eval $@_TMP := $(shell $(MKTEMP))) - $(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) generators @echo GENERATE $@ $(eval $@_TMP := $(shell $(MKTEMP))) @@ -245,14 +223,6 @@ $(ABI_MESSAGES_H) : $(ABI_XML) generators $(Q)mv $($@_TMP) $@ $(Q)chmod a+r $@ -$(INTERMCU_MSG_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) $(GENERATORS)/gen_messages.out $< intermcu > $($@_TMP) - $(Q)mv $($@_TMP) $@ - $(Q)chmod a+r $@ - $(MAVLINK_PROTOCOL_H) : $(Q)make -C $(PAPARAZZI_HOME)/sw/ext mavlink @@ -298,6 +268,7 @@ dox: clean: $(Q)rm -fr dox build-stamp configure-stamp conf/%gconf.xml paparazzi $(Q)rm -f $(GEN_HEADERS) + $(Q)MESSAGES_INSTALL=$(MESSAGES_INSTALL) $(MAKE) -C $(PPRZLINK_DIR) uninstall $(Q)rm -fr $(MAVLINK_DIR) $(Q)find . -mindepth 2 -name Makefile -a ! -path "./sw/ext/*" -exec sh -c 'echo "Cleaning {}"; $(MAKE) -C `dirname {}` $@' \; $(Q)$(MAKE) -C $(EXT) clean @@ -349,7 +320,7 @@ test_sim: all prove tests/sim .PHONY: all print_build_version _print_building _save_build_version update_google_version init dox ground_segment ground_segment.opt \ -subdirs $(SUBDIRS) conf ext libpprz multimon cockpit cockpit.opt tmtc tmtc.opt generators\ +subdirs $(SUBDIRS) conf ext libpprz cockpit cockpit.opt tmtc tmtc.opt generators\ static sim_static lpctools commands \ clean cleanspaces ab_clean dist_clean distclean dist_clean_irreversible \ test test_examples test_math test_sim test_all_confs diff --git a/Makefile.ac b/Makefile.ac index 5e05dfba78..4644b38ab0 100644 --- a/Makefile.ac +++ b/Makefile.ac @@ -28,9 +28,10 @@ include conf/Makefile.local # main directory where the generated files and compilation results for an aircraft are stored AIRCRAFT_BUILD_DIR = $(PAPARAZZI_HOME)/var/aircrafts/$(AIRCRAFT) CONF=$(PAPARAZZI_HOME)/conf +VAR=$(PAPARAZZI_HOME)/var CONF_XML ?= $(CONF)/conf.xml AIRBORNE=sw/airborne -MESSAGES_XML = $(CONF)/messages.xml +MESSAGES_XML = $(VAR)/messages.xml # make sure the TARGET variable is set if needed for current make target ifneq (,$(findstring $(MAKECMDGOALS),all_ac_h radio_ac_h flight_plan_ac_h)) diff --git a/conf/Makefile.sim b/conf/Makefile.sim index c0687777e2..0f78bf8817 100644 --- a/conf/Makefile.sim +++ b/conf/Makefile.sim @@ -33,7 +33,7 @@ CC = gcc SIMDIR = $(PAPARAZZI_SRC)/sw/simulator CAMLINCLUDES = -I $(LIBPPRZDIR) -I $(SIMDIR) -I $(OBJDIR) PKG = -package glibivy,pprz -LINKPKG = $(PKG) -linkpkg -dllpath-pkg pprz +LINKPKG = $(PKG) -linkpkg -dllpath-pkg pprz,pprzlink SIMSITLML = $(OBJDIR)/simsitl.ml SITLCMA = $(SIMDIR)/sitl.cma OPT ?= 2 diff --git a/conf/messages.dtd b/conf/abi.dtd similarity index 75% rename from conf/messages.dtd rename to conf/abi.dtd index 2e6072565f..f7d3822707 100644 --- a/conf/messages.dtd +++ b/conf/abi.dtd @@ -10,7 +10,6 @@ @@ -19,10 +18,6 @@ diff --git a/conf/abi.xml b/conf/abi.xml index ac16c25163..28765f31d5 100644 --- a/conf/abi.xml +++ b/conf/abi.xml @@ -1,5 +1,5 @@ - + @@ -72,6 +72,12 @@ + + + + + + diff --git a/conf/AGGIEAIR/aggieair_conf.xml b/conf/airframes/AGGIEAIR/aggieair_conf.xml similarity index 100% rename from conf/AGGIEAIR/aggieair_conf.xml rename to conf/airframes/AGGIEAIR/aggieair_conf.xml diff --git a/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml b/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml index be86c838b9..f9ff144034 100644 --- a/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml +++ b/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml @@ -159,13 +159,7 @@
-
- -
- - - - +
@@ -183,8 +177,6 @@
- -
diff --git a/conf/CDW/cdw_conf.xml b/conf/airframes/CDW/cdw_conf.xml similarity index 100% rename from conf/CDW/cdw_conf.xml rename to conf/airframes/CDW/cdw_conf.xml diff --git a/conf/CDW/cdw_twoseas_conf.xml b/conf/airframes/CDW/cdw_twoseas_conf.xml similarity index 100% rename from conf/CDW/cdw_twoseas_conf.xml rename to conf/airframes/CDW/cdw_twoseas_conf.xml diff --git a/conf/ESDEN/esden_conf.xml b/conf/airframes/ESDEN/esden_conf.xml similarity index 100% rename from conf/ESDEN/esden_conf.xml rename to conf/airframes/ESDEN/esden_conf.xml diff --git a/conf/FLIXR/flixr_conf.xml b/conf/airframes/FLIXR/flixr_conf.xml similarity index 100% rename from conf/FLIXR/flixr_conf.xml rename to conf/airframes/FLIXR/flixr_conf.xml diff --git a/conf/HOOPERFLY/hooperfly_conf.xml b/conf/airframes/HOOPERFLY/hooperfly_conf.xml similarity index 100% rename from conf/HOOPERFLY/hooperfly_conf.xml rename to conf/airframes/HOOPERFLY/hooperfly_conf.xml diff --git a/conf/HOOPERFLY/hooperfly_control_panel.xml b/conf/airframes/HOOPERFLY/hooperfly_control_panel.xml similarity index 100% rename from conf/HOOPERFLY/hooperfly_control_panel.xml rename to conf/airframes/HOOPERFLY/hooperfly_control_panel.xml diff --git a/conf/HOOPERFLY/hooperfly_dot_bashrc b/conf/airframes/HOOPERFLY/hooperfly_dot_bashrc similarity index 100% rename from conf/HOOPERFLY/hooperfly_dot_bashrc rename to conf/airframes/HOOPERFLY/hooperfly_dot_bashrc diff --git a/conf/HOOPERFLY/hooperfly_slurp.sh b/conf/airframes/HOOPERFLY/hooperfly_slurp.sh similarity index 100% rename from conf/HOOPERFLY/hooperfly_slurp.sh rename to conf/airframes/HOOPERFLY/hooperfly_slurp.sh diff --git a/conf/LS/ls_conf.xml b/conf/airframes/LS/ls_conf.xml similarity index 100% rename from conf/LS/ls_conf.xml rename to conf/airframes/LS/ls_conf.xml diff --git a/conf/airframes/LS/ls_ladybird_lisa_s_bluegiga_small_gps_messages.xml b/conf/airframes/LS/ls_ladybird_lisa_s_bluegiga_small_gps_messages.xml index 99c55f2732..298fd77b8e 100644 --- a/conf/airframes/LS/ls_ladybird_lisa_s_bluegiga_small_gps_messages.xml +++ b/conf/airframes/LS/ls_ladybird_lisa_s_bluegiga_small_gps_messages.xml @@ -173,13 +173,6 @@
-
- - - - -
-
diff --git a/conf/airframes/LS/ls_quadrotor_bebop_small_gps_messages.xml b/conf/airframes/LS/ls_quadrotor_bebop_small_gps_messages.xml index b5d7e19075..0732035bde 100644 --- a/conf/airframes/LS/ls_quadrotor_bebop_small_gps_messages.xml +++ b/conf/airframes/LS/ls_quadrotor_bebop_small_gps_messages.xml @@ -102,13 +102,6 @@
-
- - - - -
-
diff --git a/conf/OPENUAS/openuas_conf.xml b/conf/airframes/OPENUAS/openuas_conf.xml similarity index 100% rename from conf/OPENUAS/openuas_conf.xml rename to conf/airframes/OPENUAS/openuas_conf.xml diff --git a/conf/OPENUAS/openuas_control_panel.xml b/conf/airframes/OPENUAS/openuas_control_panel.xml similarity index 100% rename from conf/OPENUAS/openuas_control_panel.xml rename to conf/airframes/OPENUAS/openuas_control_panel.xml diff --git a/conf/OPENUAS/openuas_obc2014_conf.xml b/conf/airframes/OPENUAS/openuas_obc2014_conf.xml similarity index 100% rename from conf/OPENUAS/openuas_obc2014_conf.xml rename to conf/airframes/OPENUAS/openuas_obc2014_conf.xml diff --git a/conf/OPENUAS/openuas_obc2014_control_panel.xml b/conf/airframes/OPENUAS/openuas_obc2014_control_panel.xml similarity index 100% rename from conf/OPENUAS/openuas_obc2014_control_panel.xml rename to conf/airframes/OPENUAS/openuas_obc2014_control_panel.xml diff --git a/conf/TUDELFT/tudelft_KM_conf.xml b/conf/airframes/TUDELFT/tudelft_KM_conf.xml similarity index 100% rename from conf/TUDELFT/tudelft_KM_conf.xml rename to conf/airframes/TUDELFT/tudelft_KM_conf.xml diff --git a/conf/TUDELFT/tudelft_KM_control_panel.xml b/conf/airframes/TUDELFT/tudelft_KM_control_panel.xml similarity index 100% rename from conf/TUDELFT/tudelft_KM_control_panel.xml rename to conf/airframes/TUDELFT/tudelft_KM_control_panel.xml diff --git a/conf/airframes/TUDELFT/tudelft_bebop_flip.xml b/conf/airframes/TUDELFT/tudelft_bebop_flip.xml index eaf7c191d2..cc0ba70a00 100644 --- a/conf/airframes/TUDELFT/tudelft_bebop_flip.xml +++ b/conf/airframes/TUDELFT/tudelft_bebop_flip.xml @@ -21,7 +21,6 @@ - diff --git a/conf/TUDELFT/tudelft_conf.xml b/conf/airframes/TUDELFT/tudelft_conf.xml similarity index 100% rename from conf/TUDELFT/tudelft_conf.xml rename to conf/airframes/TUDELFT/tudelft_conf.xml diff --git a/conf/TUDELFT/tudelft_control_panel.xml b/conf/airframes/TUDELFT/tudelft_control_panel.xml similarity index 100% rename from conf/TUDELFT/tudelft_control_panel.xml rename to conf/airframes/TUDELFT/tudelft_control_panel.xml diff --git a/conf/TUDELFT/tudelft_imav_2013_conf.xml b/conf/airframes/TUDELFT/tudelft_imav_2013_conf.xml similarity index 100% rename from conf/TUDELFT/tudelft_imav_2013_conf.xml rename to conf/airframes/TUDELFT/tudelft_imav_2013_conf.xml diff --git a/conf/TUDELFT/tudelft_mavlink_conf.xml b/conf/airframes/TUDELFT/tudelft_mavlink_conf.xml similarity index 100% rename from conf/TUDELFT/tudelft_mavlink_conf.xml rename to conf/airframes/TUDELFT/tudelft_mavlink_conf.xml diff --git a/conf/TUDELFT/tudelft_rm_conf.xml b/conf/airframes/TUDELFT/tudelft_rm_conf.xml similarity index 100% rename from conf/TUDELFT/tudelft_rm_conf.xml rename to conf/airframes/TUDELFT/tudelft_rm_conf.xml diff --git a/conf/TUDELFT/tudelft_rm_control_panel.xml b/conf/airframes/TUDELFT/tudelft_rm_control_panel.xml similarity index 100% rename from conf/TUDELFT/tudelft_rm_control_panel.xml rename to conf/airframes/TUDELFT/tudelft_rm_control_panel.xml diff --git a/conf/firmwares/setup.makefile b/conf/firmwares/setup.makefile index 7cf27a2132..1458cc015d 100644 --- a/conf/firmwares/setup.makefile +++ b/conf/firmwares/setup.makefile @@ -110,7 +110,7 @@ setup_actuators.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) SETUP_ACTUATORS_MODEM_PORT_LOWER=$(shell echo $(MODEM_PORT) | tr A-Z a-z) setup_actuators.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=$(SETUP_ACTUATORS_MODEM_PORT_LOWER) -DPPRZ_UART=$(SETUP_ACTUATORS_MODEM_PORT_LOWER) setup_actuators.CFLAGS += -DDOWNLINK_TRANSPORT=pprz_tp -DDATALINK=PPRZ -setup_actuators.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c +setup_actuators.srcs += subsystems/datalink/downlink.c $(PAPARAZZI_HOME)/var/share/pprzlink/src/pprz_transport.c setup_actuators.srcs += subsystems/actuators.c setup_actuators.srcs += $(SRC_FIRMWARE)/setup_actuators.c diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile index 47f742a092..c270e832e0 100644 --- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile +++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile @@ -196,7 +196,7 @@ sim.CFLAGS += -DSITL sim.srcs += $(SRC_ARCH)/sim_ap.c sim.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_TRANSPORT=ivy_tp -DDOWNLINK_DEVICE=ivy_tp -sim.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c subsystems/datalink/ivy_transport.c subsystems/datalink/telemetry.c $(SRC_FIRMWARE)/ap_downlink.c $(SRC_FIRMWARE)/fbw_downlink.c +sim.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(PAPARAZZI_HOME)/var/share/pprzlink/src/ivy_transport.c subsystems/datalink/telemetry.c $(SRC_FIRMWARE)/ap_downlink.c $(SRC_FIRMWARE)/fbw_downlink.c sim.srcs += $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_adc_generic.c diff --git a/conf/firmwares/subsystems/fixedwing/gps_ublox_utm.makefile b/conf/firmwares/subsystems/fixedwing/gps_ublox_utm.makefile index 31da09264a..c7fd2abd9e 100644 --- a/conf/firmwares/subsystems/fixedwing/gps_ublox_utm.makefile +++ b/conf/firmwares/subsystems/fixedwing/gps_ublox_utm.makefile @@ -1,27 +1,3 @@ -# Hey Emacs, this is a -*- makefile -*- -# UBlox LEA 4P +include $(CFG_SHARED)/gps_ublox.makefile -GPS_LED ?= none -UBX_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z) - -ap.CFLAGS += -DUSE_GPS -DUBX -ap.CFLAGS += -DGPS_LINK=$(UBX_GPS_PORT_LOWER) -ap.CFLAGS += -DUSE_$(GPS_PORT) -ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD) - -ifneq ($(GPS_LED),none) - ap.CFLAGS += -DGPS_LED=$(GPS_LED) -endif - -ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\" -ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_ubx.c - -$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c - -sim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" -sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c - -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 +$(info Please replace with ) diff --git a/conf/firmwares/subsystems/rotorcraft/intermcu_uart.makefile b/conf/firmwares/subsystems/rotorcraft/intermcu_uart.makefile index ced590ff50..6c4e1763d1 100644 --- a/conf/firmwares/subsystems/rotorcraft/intermcu_uart.makefile +++ b/conf/firmwares/subsystems/rotorcraft/intermcu_uart.makefile @@ -7,7 +7,7 @@ ifeq ($(TARGET),fbw) INTERMCU_PORT_LOWER = $(shell echo $(INTERMCU_PORT) | tr A-Z a-z) fbw.CFLAGS += -DINTERMCU_LINK=$(INTERMCU_PORT_LOWER) -DUSE_$(INTERMCU_PORT) -D$(INTERMCU_PORT)_BAUD=B230400 fbw.CFLAGS += -DINTER_MCU_FBW - fbw.srcs += subsystems/datalink/pprz_transport.c + fbw.srcs += $(PAPARAZZI_HOME)/var/share/pprzlink/src/pprz_transport.c fbw.srcs += subsystems/intermcu/intermcu_fbw.c else INTERMCU_PORT ?= UART3 @@ -18,6 +18,6 @@ else $(TARGET).CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED) ap.srcs += subsystems/intermcu/intermcu_ap.c - ap.srcs += subsystems/datalink/pprz_transport.c + ap.srcs += (PAPARAZZI_HOME)/var/share/pprzlink/src/pprz_transport.c $(TARGET).srcs += subsystems/radio_control.c endif diff --git a/conf/firmwares/subsystems/fixedwing/gps_piksi.makefile b/conf/firmwares/subsystems/shared/gps_piksi.makefile similarity index 100% rename from conf/firmwares/subsystems/fixedwing/gps_piksi.makefile rename to conf/firmwares/subsystems/shared/gps_piksi.makefile diff --git a/conf/firmwares/subsystems/shared/gps_ublox.makefile b/conf/firmwares/subsystems/shared/gps_ublox.makefile index b739a9ada7..d18fc98df7 100644 --- a/conf/firmwares/subsystems/shared/gps_ublox.makefile +++ b/conf/firmwares/subsystems/shared/gps_ublox.makefile @@ -1,5 +1,5 @@ # Hey Emacs, this is a -*- makefile -*- -# UBlox LEA 5H +# UBlox LEA GPS_LED ?= none UBX_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z) diff --git a/conf/firmwares/subsystems/shared/imu_apogee_mpu9150.makefile b/conf/firmwares/subsystems/shared/imu_apogee_mpu9150.makefile new file mode 100644 index 0000000000..1e4dcecb88 --- /dev/null +++ b/conf/firmwares/subsystems/shared/imu_apogee_mpu9150.makefile @@ -0,0 +1,16 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# Apogee IMU +# + +include $(CFG_SHARED)/imu_apogee.makefile + +IMU_APOGEE_MPU9150_CFLAGS = -DAPOGEE_USE_MPU9150 +IMU_APOGEE_MPU9150_SRCS = peripherals/ak8975.c + +# add it for all targets except sim, fbw and nps +ifeq (,$(findstring $(TARGET),sim fbw nps)) +$(TARGET).CFLAGS += $(IMU_APOGEE_MPU9150_CFLAGS) +$(TARGET).srcs += $(IMU_APOGEE_MPU9150_SRCS) +endif + diff --git a/conf/firmwares/subsystems/shared/sdlog.makefile b/conf/firmwares/subsystems/shared/sdlog.makefile index 62ee4f683e..d4437b46cb 100644 --- a/conf/firmwares/subsystems/shared/sdlog.makefile +++ b/conf/firmwares/subsystems/shared/sdlog.makefile @@ -1,7 +1,7 @@ # Hey Emacs, this is a -*- makefile -*- sdlog_CFLAGS = -DDOWNLINK -DUSE_PPRZLOG -sdlog_srcs = subsystems/datalink/downlink.c subsystems/datalink/pprzlog_transport.c +sdlog_srcs = subsystems/datalink/downlink.c $(PAPARAZZI_HOME)/var/share/pprzlink/src/pprzlog_transport.c ap.CFLAGS += $(sdlog_CFLAGS) ap.srcs += $(sdlog_srcs) diff --git a/conf/firmwares/subsystems/shared/telemetry_bluegiga.makefile b/conf/firmwares/subsystems/shared/telemetry_bluegiga.makefile index 2df1682330..a5613864a4 100644 --- a/conf/firmwares/subsystems/shared/telemetry_bluegiga.makefile +++ b/conf/firmwares/subsystems/shared/telemetry_bluegiga.makefile @@ -32,4 +32,4 @@ ap.CFLAGS += -DMODEM_LED=$(MODEM_LED) endif ap.srcs += $(SRC_SUBSYSTEMS)/datalink/downlink.c $(SRC_SUBSYSTEMS)/datalink/bluegiga.c -ap.srcs += $(SRC_SUBSYSTEMS)/datalink/pprz_transport.c $(SRC_SUBSYSTEMS)/datalink/telemetry.c +ap.srcs += $(PAPARAZZI_HOME)/var/share/pprzlink/src/pprz_transport.c $(SRC_SUBSYSTEMS)/datalink/telemetry.c diff --git a/conf/firmwares/subsystems/shared/telemetry_ivy.makefile b/conf/firmwares/subsystems/shared/telemetry_ivy.makefile index c8c0dc8600..aeb34dbfb0 100644 --- a/conf/firmwares/subsystems/shared/telemetry_ivy.makefile +++ b/conf/firmwares/subsystems/shared/telemetry_ivy.makefile @@ -1,3 +1,3 @@ $(TARGET).CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_TRANSPORT=ivy_tp -DDOWNLINK_DEVICE=ivy_tp -$(TARGET).srcs += subsystems/datalink/ivy_transport.c +$(TARGET).srcs += $(PAPARAZZI_HOME)/var/share/pprzlink/src/ivy_transport.c $(TARGET).srcs += subsystems/datalink/downlink.c subsystems/datalink/telemetry.c diff --git a/conf/firmwares/subsystems/shared/telemetry_superbitrf.makefile b/conf/firmwares/subsystems/shared/telemetry_superbitrf.makefile index fce4da75ad..ed8d664826 100644 --- a/conf/firmwares/subsystems/shared/telemetry_superbitrf.makefile +++ b/conf/firmwares/subsystems/shared/telemetry_superbitrf.makefile @@ -7,4 +7,4 @@ $(TARGET).CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=superbitrf $(TARGET).CFLAGS += -DDOWNLINK_TRANSPORT=pprz_tp -DDATALINK=SUPERBITRF $(TARGET).srcs += peripherals/cyrf6936.c -$(TARGET).srcs += subsystems/datalink/downlink.c subsystems/datalink/superbitrf.c subsystems/datalink/pprz_transport.c subsystems/datalink/telemetry.c +$(TARGET).srcs += subsystems/datalink/downlink.c subsystems/datalink/superbitrf.c $(PAPARAZZI_HOME)/var/share/pprzlink/src/pprz_transport.c subsystems/datalink/telemetry.c diff --git a/conf/firmwares/subsystems/shared/telemetry_transparent.makefile b/conf/firmwares/subsystems/shared/telemetry_transparent.makefile index 30ba2ea1d0..77854f5e9e 100644 --- a/conf/firmwares/subsystems/shared/telemetry_transparent.makefile +++ b/conf/firmwares/subsystems/shared/telemetry_transparent.makefile @@ -13,5 +13,5 @@ $(TARGET).CFLAGS += -D$(PPRZ_MODEM_PORT_UPPER)_BAUD=$(MODEM_BAUD) $(TARGET).CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=$(PPRZ_MODEM_PORT_LOWER) -DPPRZ_UART=$(PPRZ_MODEM_PORT_LOWER) $(TARGET).CFLAGS += -DDOWNLINK_TRANSPORT=pprz_tp -DDATALINK=PPRZ -$(TARGET).srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c subsystems/datalink/telemetry.c +$(TARGET).srcs += subsystems/datalink/downlink.c $(PAPARAZZI_HOME)/var/share/pprzlink/src/pprz_transport.c subsystems/datalink/telemetry.c diff --git a/conf/firmwares/subsystems/shared/telemetry_transparent_udp.makefile b/conf/firmwares/subsystems/shared/telemetry_transparent_udp.makefile index c49d2f82f9..a803cfb0d4 100644 --- a/conf/firmwares/subsystems/shared/telemetry_transparent_udp.makefile +++ b/conf/firmwares/subsystems/shared/telemetry_transparent_udp.makefile @@ -19,5 +19,5 @@ TELEM_CFLAGS += -DDOWNLINK_TRANSPORT=pprz_tp -DDATALINK=PPRZ $(TARGET).CFLAGS += $(MODEM_CFLAGS) $(TELEM_CFLAGS) -$(TARGET).srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c subsystems/datalink/telemetry.c +$(TARGET).srcs += subsystems/datalink/downlink.c $(PAPARAZZI_HOME)/var/share/pprzlink/src/pprz_transport.c subsystems/datalink/telemetry.c diff --git a/conf/firmwares/subsystems/shared/telemetry_transparent_usb.makefile b/conf/firmwares/subsystems/shared/telemetry_transparent_usb.makefile index c37c574730..bd56a9a294 100644 --- a/conf/firmwares/subsystems/shared/telemetry_transparent_usb.makefile +++ b/conf/firmwares/subsystems/shared/telemetry_transparent_usb.makefile @@ -4,7 +4,7 @@ $(TARGET).CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=usb_serial -DPPRZ_UART=usb_serial $(TARGET).CFLAGS += -DDOWNLINK_TRANSPORT=pprz_tp -DDATALINK=PPRZ -DUSE_USB_SERIAL $(TARGET).CFLAGS += -DPERIODIC_TELEMETRY -$(TARGET).srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c subsystems/datalink/telemetry.c +$(TARGET).srcs += subsystems/datalink/downlink.c $(PAPARAZZI_HOME)/var/share/pprzlink/src/pprz_transport.c subsystems/datalink/telemetry.c ifeq ($(ARCH), lpc21) $(TARGET).srcs += $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c diff --git a/conf/firmwares/subsystems/shared/telemetry_w5100.makefile b/conf/firmwares/subsystems/shared/telemetry_w5100.makefile index 59901cba91..11f4b1ff58 100644 --- a/conf/firmwares/subsystems/shared/telemetry_w5100.makefile +++ b/conf/firmwares/subsystems/shared/telemetry_w5100.makefile @@ -10,7 +10,7 @@ W5100_MULTICAST_PORT ?= "1234" $(TARGET).CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=chip0 $(TARGET).CFLAGS += -DDOWNLINK_TRANSPORT=pprz_tp -DDATALINK=W5100 $(TARGET).CFLAGS += -DW5100_IP=$(W5100_IP) -DW5100_SUBNET=$(W5100_SUBNET) -DW5100_MULTICAST_IP=$(W5100_MULTICAST_IP) -DW5100_MULTICAST_PORT=$(W5100_MULTICAST_PORT) -$(TARGET).srcs += subsystems/datalink/downlink.c subsystems/datalink/w5100.c subsystems/datalink/pprz_transport.c subsystems/datalink/telemetry.c +$(TARGET).srcs += subsystems/datalink/downlink.c subsystems/datalink/w5100.c $(PAPARAZZI_HOME)/var/share/pprzlink/src/pprz_transport.c subsystems/datalink/telemetry.c ifeq ($(ARCH), lpc21) # only an issue of setting the DRDY pin in w5100.c, which is stm32 specific diff --git a/conf/firmwares/subsystems/shared/telemetry_xbee_api.makefile b/conf/firmwares/subsystems/shared/telemetry_xbee_api.makefile index 01f39a9a22..7f422aaca9 100644 --- a/conf/firmwares/subsystems/shared/telemetry_xbee_api.makefile +++ b/conf/firmwares/subsystems/shared/telemetry_xbee_api.makefile @@ -14,4 +14,4 @@ $(TARGET).CFLAGS += -D$(XBEE_MODEM_PORT_UPPER)_BAUD=$(MODEM_BAUD) -DXBEE_BAUD=$( $(TARGET).CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=$(XBEE_MODEM_PORT_LOWER) -DXBEE_UART=$(XBEE_MODEM_PORT_LOWER) $(TARGET).CFLAGS += -DDOWNLINK_TRANSPORT=xbee_tp -DDATALINK=XBEE -$(TARGET).srcs += subsystems/datalink/downlink.c subsystems/datalink/xbee.c subsystems/datalink/telemetry.c +$(TARGET).srcs += subsystems/datalink/downlink.c $(PAPARAZZI_HOME)/var/share/pprzlink/src/xbee_transport.c subsystems/datalink/telemetry.c diff --git a/conf/firmwares/test_progs.makefile b/conf/firmwares/test_progs.makefile index c2409f2852..bb815daa46 100644 --- a/conf/firmwares/test_progs.makefile +++ b/conf/firmwares/test_progs.makefile @@ -70,7 +70,7 @@ endif # pprz downlink/datalink COMMON_TELEMETRY_CFLAGS = -DDOWNLINK -DDOWNLINK_TRANSPORT=pprz_tp -DDATALINK=PPRZ -COMMON_TELEMETRY_SRCS = subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c +COMMON_TELEMETRY_SRCS = subsystems/datalink/downlink.c $(PAPARAZZI_HOME)/var/share/pprzlink/src/pprz_transport.c # check if we are using UDP ifneq (,$(findstring UDP, $(MODEM_DEV))) diff --git a/conf/messages.xml b/conf/messages.xml deleted file mode 100644 index 321011a511..0000000000 --- a/conf/messages.xml +++ /dev/null @@ -1,2914 +0,0 @@ - - - - - - - - version encoded as: MAJOR * 10000 + MINOR * 100 + PATCH - version description as string from paparazzi_version - - - - alive/heartbeat message containing the MD5sum of the aircraft configuration - - - - - Answer to PING datalink message, to measure latencies - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Altitude above geoid (MSL) - norm of 2d ground speed in cm/s - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Datalink status reported by an aircraft for the ground - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Waypoint with id wp_id has been updated/moved to the specified UTM coordinates. - - - - - Height above Mean Sea Level (geoid) - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Height above Mean Sea Level (geoid) - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - altitude above WGS84 reference ellipsoid - Height above Mean Sea Level (geoid) - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - altitude above WGS84 reference ellipsoid - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gedetic latitude - Longitude - altitude above WGS84 reference ellipsoid - Height above Mean Sea Level (geoid) - Euler angle around x-axis (roll) - Euler angle around y-axis (pitch) - Euler angle around z-axis (yaw) - Course over ground (CW/north) - horizontal ground speed - GPS time of week - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - altitude above WGS84 reference ellipsoid - height above mean sea level (geoid) - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - static pressure - differential pressure - air temperature - barometric pressure adjusted to sea level - barometric altitude above mean sea level - Equivalent Air Speed (or Calibrated Air Speed at low speed/altitude) - True Air Speed (when P, T and P_diff are available) - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Height above Mean Sea Level (geoid) - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - altitude above geoid (MSL) - - - - - - - - - altitude above geoid (MSL) - - - - - - - - - altitude above geoid (MSL) - - - - - - - - - - altitude above geoid (MSL) - - - - - - - - - - - - altitude above geoid (MSL) - - - - - - - - - - - altitude above geoid (MSL) - - - - - - - - - - - - - - - - - altitude above geoid (MSL) - - - - - - - - - - - - - - - - - - altitude above geoid (MSL) - - - - - - - - - - - - altitude above geoid (MSL) - - - - - - - - - - - altitude above geoid (MSL) - - - - - - - - - - - - - - - - - - - Set vehicle position or velocity in NED. - Frame can be specified with the bits 0-3 in the flags field: - 0x0: LOCAL_NED - 0x1: LOCAL_OFFSET_NED - 0x2: BODY_NED - 0x3: BODY_OFFSET_NED - Bits 4-7 in the flag field can be used to enable velocity control on specifc fields: - bit 4: x as velocity - bit 5: y as velocity - bit 6: z as velocity - bit 7: yaw as rate - - - bits 0-3: frame, bits 4-7: use as velocity - X position/velocity in NED - Y position/velocity in NED - Z position/velocity in NED (negative altitude) - yaw/rate setpoint - - - - - - - - - - - - - - - - - - - - - - - - - - - - bits 31-22 x position in cm : bits 21-12 y position in cm : bits 11-2 z position in cm : bits 1 and 0 are free - bits 31-22 speed x in cm/s : bits 21-12 speed y in cm/s : bits 11-2 heading in rad*1e2 : bits 1 and 0 are free - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Datalink status reported by Server for the GCS - Combines DATLINK_REPORT (telemetry class) and LINK_REPORT (ground class) - - - - - - - - - - - - - - - - - Report a telemetry error - - - - - - Encapsulated a telemetry class message (when using redundant link) - - - - - - - Encapsulated a datalink class message (when using redundant link) - - - - - - - Datalink status reported by Link for the Server - - - - - - - - - - - - - - - - - - - altitude above WGS84 reference ellipsoid - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/conf/modules/airspeed_ets.xml b/conf/modules/airspeed_ets.xml index 2f6dfcff9e..51cb506ba4 100644 --- a/conf/modules/airspeed_ets.xml +++ b/conf/modules/airspeed_ets.xml @@ -25,6 +25,7 @@ +
diff --git a/conf/modules/humid_sht.xml b/conf/modules/humid_sht.xml index 3c0824d9ba..0fbe7cc44a 100644 --- a/conf/modules/humid_sht.xml +++ b/conf/modules/humid_sht.xml @@ -8,6 +8,7 @@ +
diff --git a/conf/modules/logger_sd_spi_direct.xml b/conf/modules/logger_sd_spi_direct.xml index 47d640e870..10fd580ea6 100644 --- a/conf/modules/logger_sd_spi_direct.xml +++ b/conf/modules/logger_sd_spi_direct.xml @@ -54,7 +54,7 @@ Do not use start/stop functionality of the module, the module is not intended to ap.CFLAGS += -DLOGGER_LED=$(LOGGER_LED) endif - ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprzlog_transport.c + ap.srcs += subsystems/datalink/downlink.c $(PAPARAZZI_HOME)/var/share/pprzlink/src/pprzlog_transport.c include $(CFG_SHARED)/spi_master.makefile diff --git a/conf/modules/temp_temod.xml b/conf/modules/temp_temod.xml index f6cfe7f625..f80f626a86 100644 --- a/conf/modules/temp_temod.xml +++ b/conf/modules/temp_temod.xml @@ -5,6 +5,7 @@ Hygrosens TEMOD-I2C-Rx temperature sensor +
@@ -14,12 +15,12 @@ - TEMOD_DEV ?= i2c0 - TEMOD_DEV_LOWER=$(shell echo $(TEMOD_DEV) | tr A-Z a-z) - TEMOD_DEV_UPPER=$(shell echo $(TEMOD_DEV) | tr a-z A-Z) + TEMOD_I2C_DEV ?= i2c0 + TEMOD_I2C_DEV_LOWER=$(shell echo $(TEMOD_I2C_DEV) | tr A-Z a-z) + TEMOD_I2C_DEV_UPPER=$(shell echo $(TEMOD_I2C_DEV) | tr a-z A-Z) - - + + diff --git a/conf/units.xml b/conf/units.xml deleted file mode 100644 index 98086c0d0e..0000000000 --- a/conf/units.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/sw/Makefile.ocaml b/sw/Makefile.ocaml index 4201f9ee79..7248b7fbd5 100644 --- a/sw/Makefile.ocaml +++ b/sw/Makefile.ocaml @@ -34,13 +34,17 @@ OCAMLC = ocamlfind ocamlc OCAMLOPT = ocamlfind ocamlopt OCAMLDEP = ocamlfind ocamldep OCAMLMKLIB = ocamlmklib +OCAMLLEX=ocamllex +OCAMLYACC=ocamlyacc +OCAMLLIBDIR=$(shell $(OCAMLC) -where) +LIBPPRZLINKDIR = $(PAPARAZZI_HOME)/var/lib/ocaml LIBPPRZDIR = $(PAPARAZZI_SRC)/sw/lib/ocaml LIBPPRZCMA = $(LIBPPRZDIR)/lib-pprz.cma LIBPPRZCMXA = $(LIBPPRZCMA:.cma=.cmxa) XLIBPPRZCMA = $(LIBPPRZDIR)/xlib-pprz.cma XLIBPPRZCMXA = $(XLIBPPRZCMA:.cma=.cmxa) -OCAMLDLL = -dllpath $(LIBPPRZDIR) -OCAMLXDLL = -dllpath $(LIBPPRZDIR) +OCAMLDLL = -dllpath $(LIBPPRZDIR),$(LIBPPRZLINKDIR) +OCAMLXDLL = -dllpath $(LIBPPRZDIR),$(LIBPPRZLINKDIR) -OCAMLPATH:=$(shell echo $(LIBPPRZDIR):$(OCAMLPATH)) +OCAMLPATH:=$(shell echo $(LIBPPRZLINKDIR):$(LIBPPRZDIR):$(OCAMLPATH)) export OCAMLPATH diff --git a/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.h b/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.h index f695de20e9..4cbf94d925 100644 --- a/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.h +++ b/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.h @@ -32,7 +32,7 @@ #define SPI_SLAVE_HS_ARCH_H #include "std.h" -#include "mcu_periph/link_device.h" +#include "pprzlink/pprzlink_device.h" struct spi_slave_hs { /** Generic device interface */ diff --git a/sw/airborne/arch/sim/sim_gps.c b/sw/airborne/arch/sim/sim_gps.c index 39199eb6e6..e99c70fe40 100644 --- a/sw/airborne/arch/sim/sim_gps.c +++ b/sw/airborne/arch/sim/sim_gps.c @@ -39,7 +39,7 @@ value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, valu //TODO set alt above ellipsoid, use hmsl for now lla_f.alt = Double_val(a); LLA_BFP_OF_REAL(gps.lla_pos, lla_f); - SetBit(gps.valid_fields, GPS_VALID_POS_UTM_BIT); + SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); gps.utm_pos.east = Int_val(x); gps.utm_pos.north = Int_val(y); diff --git a/sw/airborne/boards/apogee/imu_apogee.c b/sw/airborne/boards/apogee/imu_apogee.c index 12052bdab0..19b6fdfe1c 100644 --- a/sw/airborne/boards/apogee/imu_apogee.c +++ b/sw/airborne/boards/apogee/imu_apogee.c @@ -35,7 +35,7 @@ // Downlink #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #ifndef IMU_APOGEE_CHAN_X @@ -66,11 +66,33 @@ PRINT_CONFIG_VAR(APOGEE_GYRO_RANGE) #endif PRINT_CONFIG_VAR(APOGEE_ACCEL_RANGE) +#if APOGEE_USE_MPU9150 +/** Normal frequency with the default settings + * + * the mag read function should be called at around 50 Hz + */ +#ifndef APOGEE_MAG_FREQ +#define APOGEE_MAG_FREQ 50 +#endif +PRINT_CONFIG_VAR(APOGEE_MAG_FREQ) +/** Mag periodic prescaler + */ +#define MAG_PRESCALER Max(1,((PERIODIC_FREQUENCY)/APOGEE_MAG_FREQ)) +PRINT_CONFIG_VAR(MAG_PRESCALER) + +// mag config will be done later in bypass mode +bool_t configure_mag_slave(Mpu60x0ConfigSet mpu_set, void *mpu); +bool_t configure_mag_slave(Mpu60x0ConfigSet mpu_set __attribute__((unused)), void *mpu __attribute__((unused))) +{ + return TRUE; +} + +#endif + struct ImuApogee imu_apogee; // baro config will be done later in bypass mode bool_t configure_baro_slave(Mpu60x0ConfigSet mpu_set, void *mpu); - bool_t configure_baro_slave(Mpu60x0ConfigSet mpu_set __attribute__((unused)), void *mpu __attribute__((unused))) { return TRUE; @@ -90,6 +112,12 @@ void imu_impl_init(void) imu_apogee.mpu.config.nb_slaves = 1; imu_apogee.mpu.config.slaves[0].configure = &configure_baro_slave; imu_apogee.mpu.config.i2c_bypass = TRUE; +#if APOGEE_USE_MPU9150 + // if using MPU9150, internal mag needs to be configured + ak8975_init(&imu_apogee.ak, &(IMU_APOGEE_I2C_DEV), AK8975_I2C_SLV_ADDR); + imu_apogee.mpu.config.nb_slaves = 2; + imu_apogee.mpu.config.slaves[1].configure = &configure_mag_slave; +#endif } void imu_periodic(void) @@ -97,6 +125,11 @@ void imu_periodic(void) // Start reading the latest gyroscope data mpu60x0_i2c_periodic(&imu_apogee.mpu); +#if APOGEE_USE_MPU9150 + // Start reading internal mag if available + RunOnceEvery(MAG_PRESCALER, ak8975_periodic(&imu_apogee.ak)); +#endif + //RunOnceEvery(10,imu_apogee_downlink_raw()); } @@ -134,5 +167,20 @@ void imu_apogee_event(void) AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro); AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel); } + +#if APOGEE_USE_MPU9150 + ak8975_event(&imu_apogee.ak); + if (imu_apogee.ak.data_available) { + struct Int32Vect3 mag = { + (int32_t)( imu_apogee.ak.data.value[IMU_APOGEE_CHAN_Y]), + (int32_t)(-imu_apogee.ak.data.value[IMU_APOGEE_CHAN_X]), + (int32_t)( imu_apogee.ak.data.value[IMU_APOGEE_CHAN_Z]) + }; + VECT3_COPY(imu.mag_unscaled, mag); + imu_apogee.ak.data_available = FALSE; + imu_scale_mag(&imu); + AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag); + } +#endif } diff --git a/sw/airborne/boards/apogee/imu_apogee.h b/sw/airborne/boards/apogee/imu_apogee.h index 04a1c75e16..c68c1e1ed4 100644 --- a/sw/airborne/boards/apogee/imu_apogee.h +++ b/sw/airborne/boards/apogee/imu_apogee.h @@ -33,6 +33,9 @@ #include "std.h" #include "generated/airframe.h" #include "subsystems/imu.h" +#if APOGEE_USE_MPU9150 +#include "peripherals/ak8975.h" +#endif #include "peripherals/mpu60x0_i2c.h" @@ -96,6 +99,9 @@ struct ImuApogee { struct Mpu60x0_I2c mpu; +#if APOGEE_USE_MPU9150 + struct Ak8975 ak; +#endif }; extern struct ImuApogee imu_apogee; diff --git a/sw/airborne/boards/baro_board_ms5611_i2c.c b/sw/airborne/boards/baro_board_ms5611_i2c.c index 8e55b850a2..b7d2225483 100644 --- a/sw/airborne/boards/baro_board_ms5611_i2c.c +++ b/sw/airborne/boards/baro_board_ms5611_i2c.c @@ -36,7 +36,7 @@ #include "subsystems/abi.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #ifdef BARO_PERIODIC_FREQUENCY diff --git a/sw/airborne/boards/baro_board_ms5611_spi.c b/sw/airborne/boards/baro_board_ms5611_spi.c index 0185d0d600..c0486c8946 100644 --- a/sw/airborne/boards/baro_board_ms5611_spi.c +++ b/sw/airborne/boards/baro_board_ms5611_spi.c @@ -36,7 +36,7 @@ #include "subsystems/abi.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #ifdef BARO_PERIODIC_FREQUENCY diff --git a/sw/airborne/boards/elle0_common.h b/sw/airborne/boards/elle0_common.h index fd9527ab1a..702d4a6d61 100644 --- a/sw/airborne/boards/elle0_common.h +++ b/sw/airborne/boards/elle0_common.h @@ -262,55 +262,42 @@ * these directly map to the index number of the 4 adc channels defined above * 4th (index 3) is used for bat monitoring by default */ + +#ifndef USE_ADC_1 +#define USE_ADC_1 1 +#endif + #if USE_ADC_1 -#define AD1_1_CHANNEL 13 +#define AD1_1_CHANNEL 14 #define ADC_1 AD1_1 #define ADC_1_GPIO_PORT GPIOC -#define ADC_1_GPIO_PIN GPIO3 +#define ADC_1_GPIO_PIN GPIO4 #endif #if USE_ADC_2 -#define AD1_2_CHANNEL 10 +#define AD1_2_CHANNEL 15 #define ADC_2 AD1_2 #define ADC_2_GPIO_PORT GPIOC -#define ADC_2_GPIO_PIN GPIO0 +#define ADC_2_GPIO_PIN GPIO5 #endif #if USE_ADC_3 -#define AD1_3_CHANNEL 11 +#define AD1_3_CHANNEL 0 #define ADC_3 AD1_3 -#define ADC_3_GPIO_PORT GPIOC -#define ADC_3_GPIO_PIN GPIO1 +#define ADC_3_GPIO_PORT GPIOA +#define ADC_3_GPIO_PIN GPIO0 #endif #if USE_ADC_4 -#define AD2_1_CHANNEL 15 -#define ADC_4 AD2_1 -#define ADC_4_GPIO_PORT GPIOC -#define ADC_4_GPIO_PIN GPIO5 -#endif - -// Internal ADC for battery enabled by default -#ifndef USE_ADC_5 -#define USE_ADC_5 1 -#endif -#if USE_ADC_5 -#define AD1_4_CHANNEL 14 -#define ADC_5 AD1_4 -#define ADC_5_GPIO_PORT GPIOC -#define ADC_5_GPIO_PIN GPIO4 -#endif - -#if USE_ADC_6 -#define AD2_2_CHANNEL 12 -#define ADC_6 AD2_2 -#define ADC_6_GPIO_PORT GPIOC -#define ADC_6_GPIO_PIN GPIO2 +#define AD1_4_CHANNEL 1 +#define ADC_4 AD1_4 +#define ADC_4_GPIO_PORT GPIOA +#define ADC_4_GPIO_PIN GPIO1 #endif /* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/ #ifndef ADC_CHANNEL_VSUPPLY -#define ADC_CHANNEL_VSUPPLY ADC_5 +#define ADC_CHANNEL_VSUPPLY ADC_1 #endif #define DefaultVoltageOfAdc(adc) (0.0045*adc) diff --git a/sw/airborne/boards/hbmini/imu_hbmini.c b/sw/airborne/boards/hbmini/imu_hbmini.c index d3d3b1098c..e2886d29d8 100644 --- a/sw/airborne/boards/hbmini/imu_hbmini.c +++ b/sw/airborne/boards/hbmini/imu_hbmini.c @@ -38,7 +38,7 @@ // Downlink #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" diff --git a/sw/airborne/boards/krooz/imu_krooz.c b/sw/airborne/boards/krooz/imu_krooz.c index e81400faaf..ff4a49f48e 100644 --- a/sw/airborne/boards/krooz/imu_krooz.c +++ b/sw/airborne/boards/krooz/imu_krooz.c @@ -38,7 +38,7 @@ // Downlink #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #if !defined KROOZ_LOWPASS_FILTER && !defined KROOZ_SMPLRT_DIV diff --git a/sw/airborne/boards/navgo/imu_navgo.c b/sw/airborne/boards/navgo/imu_navgo.c index a628daaf9e..c516703b0f 100644 --- a/sw/airborne/boards/navgo/imu_navgo.c +++ b/sw/airborne/boards/navgo/imu_navgo.c @@ -38,7 +38,7 @@ // Downlink #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" diff --git a/sw/airborne/boards/umarim/imu_umarim.c b/sw/airborne/boards/umarim/imu_umarim.c index ad9ff9e7ae..5006a02d7b 100644 --- a/sw/airborne/boards/umarim/imu_umarim.c +++ b/sw/airborne/boards/umarim/imu_umarim.c @@ -37,7 +37,7 @@ // Downlink #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" diff --git a/sw/airborne/firmwares/fixedwing/autopilot.h b/sw/airborne/firmwares/fixedwing/autopilot.h index 29a7d5a409..bc823b8cd6 100644 --- a/sw/airborne/firmwares/fixedwing/autopilot.h +++ b/sw/airborne/firmwares/fixedwing/autopilot.h @@ -165,7 +165,7 @@ static inline void autopilot_ClearSettings(float clear) } #if DOWNLINK -#include "subsystems/datalink/transport.h" +#include "pprzlink/pprzlink_transport.h" extern void send_autopilot_version(struct transport_tx *trans, struct link_device *dev); #endif diff --git a/sw/airborne/firmwares/motor_bench/main_motor_bench.c b/sw/airborne/firmwares/motor_bench/main_motor_bench.c index cf6369ca86..011407ed5a 100644 --- a/sw/airborne/firmwares/motor_bench/main_motor_bench.c +++ b/sw/airborne/firmwares/motor_bench/main_motor_bench.c @@ -13,7 +13,7 @@ #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include "generated/settings.h" diff --git a/sw/airborne/firmwares/motor_bench/main_turntable.c b/sw/airborne/firmwares/motor_bench/main_turntable.c index 28fe92aa33..86d28b29de 100644 --- a/sw/airborne/firmwares/motor_bench/main_turntable.c +++ b/sw/airborne/firmwares/motor_bench/main_turntable.c @@ -6,7 +6,7 @@ #include "mcu_periph/uart.h" #include "mcu_arch.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include "armVIC.h" diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h index a6da8c521b..7e1ddeeace 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot.h @@ -179,7 +179,7 @@ static inline void autopilot_ClearSettings(float clear) } #if DOWNLINK -#include "subsystems/datalink/transport.h" +#include "pprzlink/pprzlink_transport.h" extern void send_autopilot_version(struct transport_tx *trans, struct link_device *dev); #endif diff --git a/sw/airborne/firmwares/rotorcraft/datalink.c b/sw/airborne/firmwares/rotorcraft/datalink.c index 539bd80030..ce96099794 100644 --- a/sw/airborne/firmwares/rotorcraft/datalink.c +++ b/sw/airborne/firmwares/rotorcraft/datalink.c @@ -34,8 +34,8 @@ #include "generated/settings.h" #include "subsystems/datalink/downlink.h" -#include "messages.h" -#include "dl_protocol.h" +#include "pprzlink/messages.h" +#include "pprzlink/dl_protocol.h" #include "mcu_periph/uart.h" #if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK @@ -90,7 +90,7 @@ void dl_parse_msg(void) } break; -#if defined USE_NAVIGATION +#ifdef USE_NAVIGATION case DL_BLOCK : { if (DL_BLOCK_ac_id(dl_buffer) != AC_ID) { break; } nav_goto_block(DL_BLOCK_block_id(dl_buffer)); @@ -138,21 +138,21 @@ void dl_parse_msg(void) } break; #endif // RADIO_CONTROL_TYPE_DATALINK -#if defined GPS_DATALINK -#ifdef GPS_USE_DATALINK_SMALL - case DL_REMOTE_GPS_SMALL : +#if USE_GPS +#ifdef GPS_DATALINK + case DL_REMOTE_GPS_SMALL : { // Check if the GPS is for this AC - if (DL_REMOTE_GPS_SMALL_ac_id(dl_buffer) != AC_ID) { - break; - } + if (DL_REMOTE_GPS_SMALL_ac_id(dl_buffer) != AC_ID) { break; } parse_gps_datalink_small( DL_REMOTE_GPS_SMALL_numsv(dl_buffer), DL_REMOTE_GPS_SMALL_pos_xyz(dl_buffer), - DL_REMOTE_GPS_SMALL_speed_xy(dl_buffer)); - break; -#endif - case DL_REMOTE_GPS : + DL_REMOTE_GPS_SMALL_speed_xyz(dl_buffer), + DL_REMOTE_GPS_SMALL_heading(dl_buffer)); + } + break; + + case DL_REMOTE_GPS : { // Check if the GPS is for this AC if (DL_REMOTE_GPS_ac_id(dl_buffer) != AC_ID) { break; } @@ -171,10 +171,11 @@ void dl_parse_msg(void) DL_REMOTE_GPS_ecef_zd(dl_buffer), DL_REMOTE_GPS_tow(dl_buffer), DL_REMOTE_GPS_course(dl_buffer)); - break; -#endif -#if USE_GPS - case DL_GPS_INJECT : + } + break; +#endif // GPS_DATALINK + + case DL_GPS_INJECT : { // Check if the GPS is for this AC if (DL_GPS_INJECT_ac_id(dl_buffer) != AC_ID) { break; } @@ -183,9 +184,10 @@ void dl_parse_msg(void) DL_GPS_INJECT_packet_id(dl_buffer), DL_GPS_INJECT_data_length(dl_buffer), DL_GPS_INJECT_data(dl_buffer) - ); - break; -#endif + ); + } + break; +#endif // USE_GPS case DL_GUIDED_SETPOINT_NED: if (DL_GUIDED_SETPOINT_NED_ac_id(dl_buffer) != AC_ID) { break; } diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index 57b22a6fb1..eeeb26f462 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -45,7 +45,7 @@ #include "math/pprz_algebra_int.h" #include "subsystems/datalink/downlink.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "mcu_periph/uart.h" struct EnuCoor_i navigation_target; diff --git a/sw/airborne/firmwares/tutorial/main_demo4.c b/sw/airborne/firmwares/tutorial/main_demo4.c index cc8af43436..f573fe8893 100644 --- a/sw/airborne/firmwares/tutorial/main_demo4.c +++ b/sw/airborne/firmwares/tutorial/main_demo4.c @@ -4,7 +4,7 @@ #include "led.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" static inline void main_init(void); diff --git a/sw/airborne/firmwares/tutorial/main_demo5.c b/sw/airborne/firmwares/tutorial/main_demo5.c index cb020add05..ea56b03cab 100644 --- a/sw/airborne/firmwares/tutorial/main_demo5.c +++ b/sw/airborne/firmwares/tutorial/main_demo5.c @@ -4,7 +4,7 @@ #include "led.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" static inline void main_init(void); diff --git a/sw/airborne/firmwares/tutorial/main_demo6.c b/sw/airborne/firmwares/tutorial/main_demo6.c index c1739caec9..c9ee9696ff 100644 --- a/sw/airborne/firmwares/tutorial/main_demo6.c +++ b/sw/airborne/firmwares/tutorial/main_demo6.c @@ -4,7 +4,7 @@ #include "led.h" #include "mcu_periph/usb_serial.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" static inline void main_init(void); diff --git a/sw/airborne/firmwares/wind_tunnel/main.c b/sw/airborne/firmwares/wind_tunnel/main.c index 26a866c552..ee1d7606ea 100644 --- a/sw/airborne/firmwares/wind_tunnel/main.c +++ b/sw/airborne/firmwares/wind_tunnel/main.c @@ -4,12 +4,12 @@ #include "led.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include "subsystems/datalink/datalink.h" #include "generated/settings.h" -#include "dl_protocol.h" +#include "pprzlink/dl_protocol.h" #include "wt_servo.h" diff --git a/sw/airborne/firmwares/wind_tunnel/main_mb.c b/sw/airborne/firmwares/wind_tunnel/main_mb.c index e7efcc4f53..48cd7172a2 100644 --- a/sw/airborne/firmwares/wind_tunnel/main_mb.c +++ b/sw/airborne/firmwares/wind_tunnel/main_mb.c @@ -4,12 +4,12 @@ #include "led.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include "subsystems/datalink/datalink.h" #include "generated/settings.h" -#include "dl_protocol.h" +#include "pprzlink/dl_protocol.h" #include "i2c.h" #include "mb_twi_controller_mkk.h" diff --git a/sw/airborne/mcu_periph/link_device.h b/sw/airborne/mcu_periph/link_device.h deleted file mode 100644 index 68bd498daa..0000000000 --- a/sw/airborne/mcu_periph/link_device.h +++ /dev/null @@ -1,58 +0,0 @@ -/* - * Copyright (C) 2014 Gautier Hattenberger - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, see - * . - * - */ - -/** \file mcu_periph/link_device.h - * generic device header - */ - -#ifndef LINK_DEVICE_H -#define LINK_DEVICE_H - -#include - -/** Function pointers definition - * - * they are used to cast the real functions with the correct type - * to store in the device structure - */ -typedef int (*check_free_space_t)(void *, uint8_t); -typedef void (*put_byte_t)(void *, uint8_t); -typedef void (*send_message_t)(void *); -typedef int (*char_available_t)(void *); -typedef uint8_t (*get_byte_t)(void *); - -/** Device structure - */ -struct link_device { - check_free_space_t check_free_space; ///< check if transmit buffer is not full - put_byte_t put_byte; ///< put one byte - send_message_t send_message; ///< send completed buffer - char_available_t char_available; ///< check if a new character is available - get_byte_t get_byte; ///< get a new char - void *periph; ///< pointer to parent implementation - - uint16_t nb_msgs; ///< The number of messages send - uint8_t nb_ovrn; ///< The number of overruns - uint32_t nb_bytes; ///< The number of bytes send -}; - -#endif // LINK_DEVICE_H - diff --git a/sw/airborne/mcu_periph/uart.c b/sw/airborne/mcu_periph/uart.c index c2125bd445..bbb98f4246 100644 --- a/sw/airborne/mcu_periph/uart.c +++ b/sw/airborne/mcu_periph/uart.c @@ -246,6 +246,7 @@ void uart_periph_init(struct uart_periph *p) p->device.send_message = (send_message_t)null_function; p->device.char_available = (char_available_t)uart_char_available; p->device.get_byte = (get_byte_t)uart_getch; + p->device.set_baudrate = (set_baudrate_t)uart_periph_set_baudrate; #if PERIODIC_TELEMETRY // the first to register do it for the others diff --git a/sw/airborne/mcu_periph/uart.h b/sw/airborne/mcu_periph/uart.h index c7b1c5c897..22be5f5800 100644 --- a/sw/airborne/mcu_periph/uart.h +++ b/sw/airborne/mcu_periph/uart.h @@ -29,7 +29,7 @@ #define MCU_PERIPH_UART_H #include "mcu_periph/uart_arch.h" -#include "mcu_periph/link_device.h" +#include "pprzlink/pprzlink_device.h" #include "std.h" #ifndef UART_RX_BUFFER_SIZE diff --git a/sw/airborne/mcu_periph/udp.h b/sw/airborne/mcu_periph/udp.h index 775cab5d5f..1d33f03325 100644 --- a/sw/airborne/mcu_periph/udp.h +++ b/sw/airborne/mcu_periph/udp.h @@ -30,7 +30,7 @@ #include "std.h" #include "mcu_periph/udp_arch.h" -#include "mcu_periph/link_device.h" +#include "pprzlink/pprzlink_device.h" #define UDP_RX_BUFFER_SIZE 256 #define UDP_TX_BUFFER_SIZE 256 diff --git a/sw/airborne/mcu_periph/usb_serial.h b/sw/airborne/mcu_periph/usb_serial.h index 3d7ab5eb27..85126dc0f1 100644 --- a/sw/airborne/mcu_periph/usb_serial.h +++ b/sw/airborne/mcu_periph/usb_serial.h @@ -30,7 +30,7 @@ #include #include "std.h" -#include "mcu_periph/link_device.h" +#include "pprzlink/pprzlink_device.h" struct usb_serial_periph { /** Generic device interface */ diff --git a/sw/airborne/modules/adcs/adc_generic.c b/sw/airborne/modules/adcs/adc_generic.c index 318de1995e..5546f10eca 100644 --- a/sw/airborne/modules/adcs/adc_generic.c +++ b/sw/airborne/modules/adcs/adc_generic.c @@ -1,7 +1,7 @@ #include "adc_generic.h" #include "mcu_periph/adc.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include BOARD_CONFIG diff --git a/sw/airborne/modules/adcs/max11040.c b/sw/airborne/modules/adcs/max11040.c index b714c359cc..a8c509f598 100644 --- a/sw/airborne/modules/adcs/max11040.c +++ b/sw/airborne/modules/adcs/max11040.c @@ -27,7 +27,7 @@ #include "led.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include "max11040.h" #include "adcs/max11040_hw.h" diff --git a/sw/airborne/modules/benchmark/flight_benchmark.c b/sw/airborne/modules/benchmark/flight_benchmark.c index 7529c8153c..05b931d805 100644 --- a/sw/airborne/modules/benchmark/flight_benchmark.c +++ b/sw/airborne/modules/benchmark/flight_benchmark.c @@ -8,7 +8,7 @@ #include "firmwares/fixedwing/guidance/guidance_v.h" #include "state.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include "mcu_periph/uart.h" #include "generated/airframe.h" diff --git a/sw/airborne/modules/calibration/send_imu_mag_current.c b/sw/airborne/modules/calibration/send_imu_mag_current.c index 38ef498e05..dc3df49cdb 100644 --- a/sw/airborne/modules/calibration/send_imu_mag_current.c +++ b/sw/airborne/modules/calibration/send_imu_mag_current.c @@ -27,7 +27,7 @@ #include "subsystems/imu.h" #include "subsystems/electrical.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "mcu_periph/uart.h" #include "subsystems/datalink/downlink.h" diff --git a/sw/airborne/modules/config/config_mkk_v2.c b/sw/airborne/modules/config/config_mkk_v2.c index a5742b4b83..97f214408e 100644 --- a/sw/airborne/modules/config/config_mkk_v2.c +++ b/sw/airborne/modules/config/config_mkk_v2.c @@ -59,7 +59,7 @@ void config_mkk_v2_periodic_read_status(void) } #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" diff --git a/sw/airborne/modules/core/sys_mon.c b/sw/airborne/modules/core/sys_mon.c index 18742e7a2e..cdf68ee1a7 100644 --- a/sw/airborne/modules/core/sys_mon.c +++ b/sw/airborne/modules/core/sys_mon.c @@ -60,7 +60,7 @@ void init_sysmon(void) } #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" void periodic_report_sysmon(void) diff --git a/sw/airborne/modules/datalink/extra_pprz_dl.h b/sw/airborne/modules/datalink/extra_pprz_dl.h index 386866f619..051d5e5c92 100644 --- a/sw/airborne/modules/datalink/extra_pprz_dl.h +++ b/sw/airborne/modules/datalink/extra_pprz_dl.h @@ -29,7 +29,7 @@ #define EXTRA_PPRZ_DL_H #include "subsystems/datalink/datalink.h" -#include "subsystems/datalink/pprz_transport.h" +#include "pprzlink/pprz_transport.h" /* PPRZ transport structure */ extern struct pprz_transport extra_pprz_tp; diff --git a/sw/airborne/modules/datalink/mavlink_decoder.c b/sw/airborne/modules/datalink/mavlink_decoder.c index 3d86c5ad17..1fa08902b8 100644 --- a/sw/airborne/modules/datalink/mavlink_decoder.c +++ b/sw/airborne/modules/datalink/mavlink_decoder.c @@ -34,7 +34,7 @@ uint8_t mavlink_crc_extra[256] = {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, #if MAVLINK_DECODER_DEBUG #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" void mavlink_send_debug(struct mavlink_transport *t) diff --git a/sw/airborne/modules/datalink/mavlink_decoder.h b/sw/airborne/modules/datalink/mavlink_decoder.h index ee0970d256..d93d482d26 100644 --- a/sw/airborne/modules/datalink/mavlink_decoder.h +++ b/sw/airborne/modules/datalink/mavlink_decoder.h @@ -29,7 +29,7 @@ #define MAVLINK_DECODER_H #include "std.h" -#include "subsystems/datalink/transport.h" +#include "pprzlink/pprzlink_transport.h" #include "mcu_periph/uart.h" /* MAVLINK Transport diff --git a/sw/airborne/modules/datalink/xtend_rssi.c b/sw/airborne/modules/datalink/xtend_rssi.c index 93ffadb79f..44b7093117 100644 --- a/sw/airborne/modules/datalink/xtend_rssi.c +++ b/sw/airborne/modules/datalink/xtend_rssi.c @@ -33,7 +33,7 @@ #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" //from Digi XTend manual diff --git a/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.c b/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.c index ed97b64aa6..3d396e6fc9 100644 --- a/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.c +++ b/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.c @@ -34,7 +34,7 @@ #include "led.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" // In I2C mode we can not inline this function: diff --git a/sw/airborne/modules/digital_cam/dc.c b/sw/airborne/modules/digital_cam/dc.c index 597170738d..470b87d989 100644 --- a/sw/airborne/modules/digital_cam/dc.c +++ b/sw/airborne/modules/digital_cam/dc.c @@ -88,7 +88,7 @@ float dc_autoshoot_period; uint16_t dc_photo_nr = 0; #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include "state.h" #include "subsystems/gps.h" diff --git a/sw/airborne/modules/digital_cam/hackhd.c b/sw/airborne/modules/digital_cam/hackhd.c index 565f9834ca..cd37c2dd42 100644 --- a/sw/airborne/modules/digital_cam/hackhd.c +++ b/sw/airborne/modules/digital_cam/hackhd.c @@ -83,7 +83,7 @@ static inline uint16_t pin_of_gpio(uint32_t __attribute__((unused)) port, uint16 #if HACKHD_SYNC_SEND #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include "state.h" #include "subsystems/gps.h" diff --git a/sw/airborne/modules/digital_cam/sim_i2c_cam_ctrl.c b/sw/airborne/modules/digital_cam/sim_i2c_cam_ctrl.c index 1525b3f590..b91f095aa4 100644 --- a/sw/airborne/modules/digital_cam/sim_i2c_cam_ctrl.c +++ b/sw/airborne/modules/digital_cam/sim_i2c_cam_ctrl.c @@ -31,7 +31,7 @@ #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include "state.h" diff --git a/sw/airborne/modules/energy/MPPT.c b/sw/airborne/modules/energy/MPPT.c index 4112331b07..1305e13472 100644 --- a/sw/airborne/modules/energy/MPPT.c +++ b/sw/airborne/modules/energy/MPPT.c @@ -54,7 +54,7 @@ struct i2c_transaction mppt_trans; #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" uint8_t MPPT_mode; diff --git a/sw/airborne/modules/energy/sim_MPPT.c b/sw/airborne/modules/energy/sim_MPPT.c index 1b0e6a82be..321e3ca78e 100644 --- a/sw/airborne/modules/energy/sim_MPPT.c +++ b/sw/airborne/modules/energy/sim_MPPT.c @@ -22,7 +22,7 @@ #include "modules/energy/MPPT.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" diff --git a/sw/airborne/modules/enose/enose.c b/sw/airborne/modules/enose/enose.c index 509689729e..47bb5d1bc8 100644 --- a/sw/airborne/modules/enose/enose.c +++ b/sw/airborne/modules/enose/enose.c @@ -47,7 +47,7 @@ void enose_set_heat(uint8_t no_sensor, uint8_t value) #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" void enose_periodic(void) diff --git a/sw/airborne/modules/gps/gps_ubx_ucenter.h b/sw/airborne/modules/gps/gps_ubx_ucenter.h index a21d61928d..0730adf452 100644 --- a/sw/airborne/modules/gps/gps_ubx_ucenter.h +++ b/sw/airborne/modules/gps/gps_ubx_ucenter.h @@ -29,7 +29,7 @@ #define GPS_UBX_UCENTER_H #include "std.h" -#include "mcu_periph/link_device.h" +#include "pprzlink/pprzlink_device.h" /** U-Center Variables */ #define GPS_UBX_UCENTER_CONFIG_STEPS 19 diff --git a/sw/airborne/modules/gumstix_interface/qr_code_spi_link.c b/sw/airborne/modules/gumstix_interface/qr_code_spi_link.c index 9a4fdc30ae..47081b186e 100644 --- a/sw/airborne/modules/gumstix_interface/qr_code_spi_link.c +++ b/sw/airborne/modules/gumstix_interface/qr_code_spi_link.c @@ -26,7 +26,7 @@ #include "mcu_periph/spi.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" //struct qr_code_spi_link_data qr_code_spi_link_data; diff --git a/sw/airborne/modules/ins/ahrs_chimu_spi.c b/sw/airborne/modules/ins/ahrs_chimu_spi.c index 9d4f196cca..18ed9222d2 100644 --- a/sw/airborne/modules/ins/ahrs_chimu_spi.c +++ b/sw/airborne/modules/ins/ahrs_chimu_spi.c @@ -20,7 +20,7 @@ #if CHIMU_DOWNLINK_IMMEDIATE #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #endif diff --git a/sw/airborne/modules/ins/ahrs_chimu_uart.c b/sw/airborne/modules/ins/ahrs_chimu_uart.c index 7d5d83f6df..913c28cefa 100644 --- a/sw/airborne/modules/ins/ahrs_chimu_uart.c +++ b/sw/airborne/modules/ins/ahrs_chimu_uart.c @@ -16,7 +16,7 @@ #if CHIMU_DOWNLINK_IMMEDIATE #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #endif diff --git a/sw/airborne/modules/ins/alt_filter.c b/sw/airborne/modules/ins/alt_filter.c index bafb13e244..88479aca5e 100644 --- a/sw/airborne/modules/ins/alt_filter.c +++ b/sw/airborne/modules/ins/alt_filter.c @@ -25,7 +25,7 @@ #include "modules/sensors/baro_ets.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" TypeKalman alt_filter; diff --git a/sw/airborne/modules/ins/ins_arduimu.c b/sw/airborne/modules/ins/ins_arduimu.c index 043a88f257..42f06aaa25 100644 --- a/sw/airborne/modules/ins/ins_arduimu.c +++ b/sw/airborne/modules/ins/ins_arduimu.c @@ -31,7 +31,7 @@ int32_t GPS_Data[14]; #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" struct i2c_transaction ardu_gps_trans; diff --git a/sw/airborne/modules/ins/ins_arduimu_basic.c b/sw/airborne/modules/ins/ins_arduimu_basic.c index 9303dfd4b4..f9e319fa1f 100644 --- a/sw/airborne/modules/ins/ins_arduimu_basic.c +++ b/sw/airborne/modules/ins/ins_arduimu_basic.c @@ -50,7 +50,7 @@ #ifdef ARDUIMU_SYNC_SEND #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #endif diff --git a/sw/airborne/modules/ins/ins_module.h b/sw/airborne/modules/ins/ins_module.h index 3256e26626..bfb01dfad0 100644 --- a/sw/airborne/modules/ins/ins_module.h +++ b/sw/airborne/modules/ins/ins_module.h @@ -71,7 +71,7 @@ void handle_ins_msg(void); void parse_ins_msg(void); void parse_ins_buffer(uint8_t); -#include "mcu_periph/link_device.h" +#include "pprzlink/pprzlink_device.h" #define InsLinkDevice (&((INS_LINK).device)) diff --git a/sw/airborne/modules/ins/ins_vn100.c b/sw/airborne/modules/ins/ins_vn100.c index f35c2c5e78..15bf8cbce6 100644 --- a/sw/airborne/modules/ins/ins_vn100.c +++ b/sw/airborne/modules/ins/ins_vn100.c @@ -33,7 +33,7 @@ // for telemetry report #include "mcu_periph/uart.h" #include "subsystems/datalink/downlink.h" -#include "messages.h" +#include "pprzlink/messages.h" #ifndef INS_YAW_NEUTRAL_DEFAULT #define INS_YAW_NEUTRAL_DEFAULT 0. @@ -305,7 +305,7 @@ static inline void parse_ins_msg(void) } #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" extern void vn100_report_task(void) diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index 852c2c3ea7..5c3d3bc9d3 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -33,7 +33,7 @@ #include "generated/airframe.h" #include "mcu_periph/sys_time.h" -#include "messages.h" +#include "pprzlink/messages.h" #if USE_GPS_XSENS #if !USE_GPS @@ -178,7 +178,7 @@ uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD]; // FIXME Debugging Only #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" diff --git a/sw/airborne/modules/ins/ins_xsens700.c b/sw/airborne/modules/ins/ins_xsens700.c index 7e62ba91b4..44f10dd576 100644 --- a/sw/airborne/modules/ins/ins_xsens700.c +++ b/sw/airborne/modules/ins/ins_xsens700.c @@ -35,7 +35,7 @@ #include "mcu_periph/sys_time.h" #include "subsystems/datalink/downlink.h" -#include "messages.h" +#include "pprzlink/messages.h" #if USE_GPS_XSENS #if !USE_GPS @@ -126,7 +126,7 @@ uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD]; // FIXME Debugging Only #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" diff --git a/sw/airborne/modules/loggers/flight_recorder.c b/sw/airborne/modules/loggers/flight_recorder.c index 2fc52e977b..013b9e9e25 100644 --- a/sw/airborne/modules/loggers/flight_recorder.c +++ b/sw/airborne/modules/loggers/flight_recorder.c @@ -28,7 +28,7 @@ #include "modules/loggers/flight_recorder.h" #include "subsystems/datalink/telemetry.h" -#include "subsystems/datalink/pprzlog_transport.h" +#include "pprzlink/pprzlog_transport.h" #if FLIGHTRECORDER_SDLOG #include "sdLog.h" diff --git a/sw/airborne/modules/loggers/openlog.c b/sw/airborne/modules/loggers/openlog.c index 2e51ac5c95..27c9f506fd 100644 --- a/sw/airborne/modules/loggers/openlog.c +++ b/sw/airborne/modules/loggers/openlog.c @@ -29,7 +29,7 @@ */ #include "openlog.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include "mcu_periph/sys_time.h" diff --git a/sw/airborne/modules/loggers/sdlogger_spi_direct.c b/sw/airborne/modules/loggers/sdlogger_spi_direct.c index 099a32a27b..8e12c7e2a5 100644 --- a/sw/airborne/modules/loggers/sdlogger_spi_direct.c +++ b/sw/airborne/modules/loggers/sdlogger_spi_direct.c @@ -30,7 +30,7 @@ #define PERIODIC_C_LOGGER #include "modules/loggers/sdlogger_spi_direct.h" -#include "subsystems/datalink/pprzlog_transport.h" +#include "pprzlink/pprzlog_transport.h" #include "subsystems/datalink/telemetry.h" #include "subsystems/radio_control.h" #include "led.h" diff --git a/sw/airborne/modules/loggers/sdlogger_spi_direct.h b/sw/airborne/modules/loggers/sdlogger_spi_direct.h index f5bb79c6ef..2d55fa744e 100644 --- a/sw/airborne/modules/loggers/sdlogger_spi_direct.h +++ b/sw/airborne/modules/loggers/sdlogger_spi_direct.h @@ -28,7 +28,7 @@ #define SDLOGGER_BUFFER_SIZE 128 -#include "mcu_periph/link_device.h" +#include "pprzlink/pprzlink_device.h" #include "peripherals/sdcard_spi.h" enum SDLoggerStatus { diff --git a/sw/airborne/modules/meteo/charge_sens.c b/sw/airborne/modules/meteo/charge_sens.c index d9c969bbc6..d548be6baf 100644 --- a/sw/airborne/modules/meteo/charge_sens.c +++ b/sw/airborne/modules/meteo/charge_sens.c @@ -28,7 +28,7 @@ #include "modules/meteo/charge_sens.h" #include "mcu_periph/i2c.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" diff --git a/sw/airborne/modules/meteo/dust_gp2y.c b/sw/airborne/modules/meteo/dust_gp2y.c index ea6a191c16..ee60752e1f 100644 --- a/sw/airborne/modules/meteo/dust_gp2y.c +++ b/sw/airborne/modules/meteo/dust_gp2y.c @@ -32,7 +32,7 @@ #include "mcu_periph/i2c.h" #include "mcu_periph/sys_time.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" uint8_t dust_gp2y_status; diff --git a/sw/airborne/modules/meteo/geiger_counter.c b/sw/airborne/modules/meteo/geiger_counter.c index 2563a0fa56..cb3298864d 100644 --- a/sw/airborne/modules/meteo/geiger_counter.c +++ b/sw/airborne/modules/meteo/geiger_counter.c @@ -28,7 +28,7 @@ #include "modules/meteo/geiger_counter.h" #include "mcu_periph/i2c.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" diff --git a/sw/airborne/modules/meteo/humid_dpicco.c b/sw/airborne/modules/meteo/humid_dpicco.c index 2109358198..51608bb13f 100644 --- a/sw/airborne/modules/meteo/humid_dpicco.c +++ b/sw/airborne/modules/meteo/humid_dpicco.c @@ -33,7 +33,7 @@ #include "mcu_periph/i2c.h" #include "led.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" diff --git a/sw/airborne/modules/meteo/humid_hih.c b/sw/airborne/modules/meteo/humid_hih.c index eb10ae662d..c84d3e6b5d 100644 --- a/sw/airborne/modules/meteo/humid_hih.c +++ b/sw/airborne/modules/meteo/humid_hih.c @@ -33,7 +33,7 @@ #include "modules/meteo/humid_sht.h" #include "mcu_periph/adc.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" diff --git a/sw/airborne/modules/meteo/humid_htm_b71.c b/sw/airborne/modules/meteo/humid_htm_b71.c index 68dfcd2a77..1bfffc822a 100644 --- a/sw/airborne/modules/meteo/humid_htm_b71.c +++ b/sw/airborne/modules/meteo/humid_htm_b71.c @@ -33,7 +33,7 @@ #include "mcu_periph/sys_time.h" #include "mcu_periph/i2c.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" diff --git a/sw/airborne/modules/meteo/humid_pcap01.c b/sw/airborne/modules/meteo/humid_pcap01.c index 22373602d6..570d8e4d7a 100644 --- a/sw/airborne/modules/meteo/humid_pcap01.c +++ b/sw/airborne/modules/meteo/humid_pcap01.c @@ -32,7 +32,7 @@ #include "mcu_periph/sys_time.h" #include "mcu_periph/i2c.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include "modules/meteo/humid_pcap01.h" #ifdef PCAP01_LOAD_FIRMWARE diff --git a/sw/airborne/modules/meteo/humid_sht.c b/sw/airborne/modules/meteo/humid_sht.c index ba77043e91..d13bfd87b2 100644 --- a/sw/airborne/modules/meteo/humid_sht.c +++ b/sw/airborne/modules/meteo/humid_sht.c @@ -31,10 +31,18 @@ #include "std.h" #include "mcu_periph/gpio.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include "humid_sht.h" +// sd-log +#if SHT_SDLOG +#include "sdLog.h" +#include "subsystems/chibios-libopencm3/chibios_sdlog.h" +#include "subsystems/gps.h" +bool_t log_sht_started; +#endif + //#include "led.h" #define noACK 0 @@ -299,6 +307,11 @@ void humid_sht_init(void) humid_sht_available = FALSE; humid_sht_status = SHT_IDLE; + +#if SHT_SDLOG + log_sht_started = FALSE; +#endif + } void humid_sht_periodic(void) @@ -338,6 +351,21 @@ void humid_sht_periodic(void) humid_sht_status = SHT_MEASURING_HUMID; DOWNLINK_SEND_SHT_STATUS(DefaultChannel, DefaultDevice, &humidsht, &tempsht, &fhumidsht, &ftempsht); humid_sht_available = FALSE; + +#if SHT_SDLOG + if (pprzLogFile != -1) { + if (!log_sht_started) { + sdLogWriteLog(pprzLogFile, "SHT75: Humid(pct) Temp(degC) H(usec) GPS_fix TOW(ms) Week Lat(1e7rad) Lon(1e7rad) HMSL(mm) gpseed(cm/s) course(1e7rad) climb(cm/s)\n"); + log_sht_started = TRUE; + } + sdLogWriteLog(pprzLogFile, "sht75: %9.4f %9.4f %d %d %d %d %d %d %d %d %d\n", + fhumidsht, ftempsht, + gps.fix, gps.tow, gps.week, + gps.lla_pos.lat, gps.lla_pos.lon, gps.hmsl, + gps.gspeed, gps.course, -gps.ned_vel.z); + } +#endif + } } } diff --git a/sw/airborne/modules/meteo/humid_sht_i2c.c b/sw/airborne/modules/meteo/humid_sht_i2c.c index b56074210a..23757a82d2 100644 --- a/sw/airborne/modules/meteo/humid_sht_i2c.c +++ b/sw/airborne/modules/meteo/humid_sht_i2c.c @@ -30,7 +30,7 @@ #include "mcu_periph/i2c.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" diff --git a/sw/airborne/modules/meteo/ir_mlx.c b/sw/airborne/modules/meteo/ir_mlx.c index fecfad91a0..1dabd53d2f 100644 --- a/sw/airborne/modules/meteo/ir_mlx.c +++ b/sw/airborne/modules/meteo/ir_mlx.c @@ -33,7 +33,7 @@ #include "mcu_periph/i2c.h" #include "led.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" diff --git a/sw/airborne/modules/meteo/light_solar.c b/sw/airborne/modules/meteo/light_solar.c index 69a2cb5355..d03c4ed07a 100644 --- a/sw/airborne/modules/meteo/light_solar.c +++ b/sw/airborne/modules/meteo/light_solar.c @@ -29,7 +29,7 @@ #include "mcu_periph/adc.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include "modules/meteo/light_solar.h" diff --git a/sw/airborne/modules/meteo/light_temt.c b/sw/airborne/modules/meteo/light_temt.c index 2b9914465d..4de52db2a5 100644 --- a/sw/airborne/modules/meteo/light_temt.c +++ b/sw/airborne/modules/meteo/light_temt.c @@ -30,7 +30,7 @@ #include "modules/meteo/light_temt.h" #include "mcu_periph/adc.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #ifndef ADC_CHANNEL_LIGHT_TEMT diff --git a/sw/airborne/modules/meteo/meteo_stick.c b/sw/airborne/modules/meteo/meteo_stick.c index 2d44673ab3..ff0a836950 100644 --- a/sw/airborne/modules/meteo/meteo_stick.c +++ b/sw/airborne/modules/meteo/meteo_stick.c @@ -188,7 +188,7 @@ bool_t log_ptu_started; #if SEND_MS #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include "subsystems/gps.h" diff --git a/sw/airborne/modules/meteo/mf_ptu.c b/sw/airborne/modules/meteo/mf_ptu.c index 6660fa09db..45ebadb974 100644 --- a/sw/airborne/modules/meteo/mf_ptu.c +++ b/sw/airborne/modules/meteo/mf_ptu.c @@ -69,7 +69,7 @@ bool_t log_ptu_started; #if SEND_PTU #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include "subsystems/gps.h" #endif diff --git a/sw/airborne/modules/meteo/temp_lm75.c b/sw/airborne/modules/meteo/temp_lm75.c index 2318f0176c..208674a109 100644 --- a/sw/airborne/modules/meteo/temp_lm75.c +++ b/sw/airborne/modules/meteo/temp_lm75.c @@ -32,7 +32,7 @@ #include "mcu_periph/i2c.h" #include "led.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" diff --git a/sw/airborne/modules/meteo/temp_tcouple_adc.c b/sw/airborne/modules/meteo/temp_tcouple_adc.c index 5a845fb62a..5e672564f9 100644 --- a/sw/airborne/modules/meteo/temp_tcouple_adc.c +++ b/sw/airborne/modules/meteo/temp_tcouple_adc.c @@ -30,7 +30,7 @@ #include "mcu_periph/adc.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include "modules/meteo/temp_tcouple_adc.h" diff --git a/sw/airborne/modules/meteo/temp_temod.c b/sw/airborne/modules/meteo/temp_temod.c index e45251260f..5ff5ab3088 100644 --- a/sw/airborne/modules/meteo/temp_temod.c +++ b/sw/airborne/modules/meteo/temp_temod.c @@ -30,9 +30,17 @@ #include "mcu_periph/i2c.h" #include "led.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" +// sd-log +#if TEMP_TEMOD_SDLOG +#include "sdLog.h" +#include "subsystems/chibios-libopencm3/chibios_sdlog.h" +#include "subsystems/gps.h" +bool_t log_temod_started; +#endif + float ftmd_temperature; struct i2c_transaction tmd_trans; @@ -50,11 +58,16 @@ struct i2c_transaction tmd_trans; void temod_init(void) { tmd_trans.status = I2CTransDone; + +#if TEMP_TEMOD_SDLOG + log_temod_started = FALSE; +#endif } void temod_periodic(void) { i2c_receive(&TEMOD_I2C_DEV, &tmd_trans, TEMOD_SLAVE_ADDR, 2); + } void temod_event(void) @@ -72,6 +85,24 @@ void temod_event(void) DOWNLINK_SEND_TMP_STATUS(DefaultChannel, DefaultDevice, &tmd_temperature, &ftmd_temperature); tmd_trans.status = I2CTransDone; + + +#if TEMP_TEMOD_SDLOG + if (pprzLogFile != -1) { + if (!log_temod_started) { + sdLogWriteLog(pprzLogFile, "TEMOD: Temp(degC) H(usec) GPS_fix TOW(ms) Week Lat(1e7rad) Lon(1e7rad) HMSL(mm) gpseed(cm/s) course(1e7rad) climb(cm/s)\n"); + log_temod_started = TRUE; + } + else { + sdLogWriteLog(pprzLogFile, "temod: %9.4f %d %d %d %d %d %d %d %d %d\n", + ftmd_temperature, + gps.fix, gps.tow, gps.week, + gps.lla_pos.lat, gps.lla_pos.lon, gps.hmsl, + gps.gspeed, gps.course, -gps.ned_vel.z); + } + } +#endif + } } diff --git a/sw/airborne/modules/meteo/temp_tmp102.c b/sw/airborne/modules/meteo/temp_tmp102.c index 7c7b5d438c..2a98612b59 100644 --- a/sw/airborne/modules/meteo/temp_tmp102.c +++ b/sw/airborne/modules/meteo/temp_tmp102.c @@ -32,7 +32,7 @@ #include "mcu_periph/i2c.h" #include "led.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" diff --git a/sw/airborne/modules/meteo/wind_gfi.c b/sw/airborne/modules/meteo/wind_gfi.c index 002944ded7..a5240b4b24 100644 --- a/sw/airborne/modules/meteo/wind_gfi.c +++ b/sw/airborne/modules/meteo/wind_gfi.c @@ -32,7 +32,7 @@ #include "mcu_periph/i2c.h" #include "led.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" struct i2c_transaction pcf_trans; diff --git a/sw/airborne/modules/meteo/windturbine.c b/sw/airborne/modules/meteo/windturbine.c index 0c28e96fe8..c7d8c7a5a1 100644 --- a/sw/airborne/modules/meteo/windturbine.c +++ b/sw/airborne/modules/meteo/windturbine.c @@ -34,7 +34,7 @@ #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" diff --git a/sw/airborne/modules/multi/follow.c b/sw/airborne/modules/multi/follow.c index d92ceee567..9d355083e2 100644 --- a/sw/airborne/modules/multi/follow.c +++ b/sw/airborne/modules/multi/follow.c @@ -32,8 +32,8 @@ #include "subsystems/navigation/waypoints.h" #include "state.h" -#include "messages.h" -#include "dl_protocol.h" +#include "pprzlink/messages.h" +#include "pprzlink/dl_protocol.h" #ifndef FOLLOW_OFFSET_X #define FOLLOW_OFFSET_X 0.0 diff --git a/sw/airborne/modules/multi/formation.c b/sw/airborne/modules/multi/formation.c index 94a3da7841..fdadf30d5b 100644 --- a/sw/airborne/modules/multi/formation.c +++ b/sw/airborne/modules/multi/formation.c @@ -16,7 +16,7 @@ #include "subsystems/gps.h" #include "generated/flight_plan.h" #include "generated/airframe.h" -#include "dl_protocol.h" +#include "pprzlink/dl_protocol.h" #include diff --git a/sw/airborne/modules/multi/potential.c b/sw/airborne/modules/multi/potential.c index f968d1ddc9..de7b15d14e 100644 --- a/sw/airborne/modules/multi/potential.c +++ b/sw/airborne/modules/multi/potential.c @@ -6,7 +6,7 @@ #include #include "subsystems/datalink/downlink.h" -#include "dl_protocol.h" +#include "pprzlink/dl_protocol.h" #include "potential.h" #include "state.h" diff --git a/sw/airborne/modules/multi/tcas.c b/sw/airborne/modules/multi/tcas.c index 4c23707915..36f0890924 100644 --- a/sw/airborne/modules/multi/tcas.c +++ b/sw/airborne/modules/multi/tcas.c @@ -32,7 +32,7 @@ #include "subsystems/gps.h" #include "generated/flight_plan.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" float tcas_alt_setpoint; diff --git a/sw/airborne/modules/nav/nav_catapult.c b/sw/airborne/modules/nav/nav_catapult.c index 3373787f0f..bf01285cd8 100644 --- a/sw/airborne/modules/nav/nav_catapult.c +++ b/sw/airborne/modules/nav/nav_catapult.c @@ -47,7 +47,7 @@ #include "subsystems/imu.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/datalink.h" diff --git a/sw/airborne/modules/nav/nav_survey_poly_rotorcraft.c b/sw/airborne/modules/nav/nav_survey_poly_rotorcraft.c index ca18804d9e..301e2b75c1 100644 --- a/sw/airborne/modules/nav/nav_survey_poly_rotorcraft.c +++ b/sw/airborne/modules/nav/nav_survey_poly_rotorcraft.c @@ -27,7 +27,7 @@ /* #include #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" */ diff --git a/sw/airborne/modules/nav/nav_survey_rectangle_rotorcraft.c b/sw/airborne/modules/nav/nav_survey_rectangle_rotorcraft.c index 1745c7ac44..ef47a6823a 100644 --- a/sw/airborne/modules/nav/nav_survey_rectangle_rotorcraft.c +++ b/sw/airborne/modules/nav/nav_survey_rectangle_rotorcraft.c @@ -39,7 +39,7 @@ #endif #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #if PERIODIC_TELEMETRY diff --git a/sw/airborne/modules/optical_flow/px4flow.c b/sw/airborne/modules/optical_flow/px4flow.c index f50f8e6659..56c863328c 100644 --- a/sw/airborne/modules/optical_flow/px4flow.c +++ b/sw/airborne/modules/optical_flow/px4flow.c @@ -75,7 +75,7 @@ void px4flow_init(void) // Messages #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" /** Downlink message for debug diff --git a/sw/airborne/modules/sensors/airspeed_amsys.c b/sw/airborne/modules/sensors/airspeed_amsys.c index e9dc36b3c2..3782153982 100644 --- a/sw/airborne/modules/sensors/airspeed_amsys.c +++ b/sw/airborne/modules/sensors/airspeed_amsys.c @@ -27,7 +27,7 @@ #include "state.h" #include "mcu_periph/i2c.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include //#include diff --git a/sw/airborne/modules/sensors/airspeed_ets.c b/sw/airborne/modules/sensors/airspeed_ets.c index 748fce7df8..ec5c3d0682 100644 --- a/sw/airborne/modules/sensors/airspeed_ets.c +++ b/sw/airborne/modules/sensors/airspeed_ets.c @@ -44,7 +44,7 @@ #include "mcu_periph/i2c.h" #include "mcu_periph/uart.h" #include "mcu_periph/sys_time.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include @@ -83,6 +83,13 @@ PRINT_CONFIG_VAR(AIRSPEED_ETS_I2C_DEV) #endif PRINT_CONFIG_VAR(AIRSPEED_ETS_START_DELAY) +#if AIRSPEED_ETS_SDLOG +#include "sdLog.h" +#include "subsystems/chibios-libopencm3/chibios_sdlog.h" +#include "subsystems/gps.h" +bool_t log_airspeed_ets_started; +#endif + // Global variables uint16_t airspeed_ets_raw; @@ -123,6 +130,10 @@ void airspeed_ets_init(void) airspeed_ets_delay_done = FALSE; SysTimeTimerStart(airspeed_ets_delay_time); + +#if AIRSPEED_ETS_SDLOG + log_airspeed_ets_started = FALSE; +#endif } void airspeed_ets_read_periodic(void) @@ -221,6 +232,23 @@ void airspeed_ets_read_event(void) airspeed_ets = 0.0; } + +#if AIRSPEED_ETS_SDLOG + if (pprzLogFile != -1) { + if (!log_airspeed_ets_started) { + sdLogWriteLog(pprzLogFile, "AIRSPEED_ETS: raw offset airspeed(m/s) GPS_fix TOW(ms) Week Lat(1e7rad) Lon(1e7rad) HMSL(mm) gpseed(cm/s) course(1e7rad) climb(cm/s)\n"); + log_airspeed_ets_started = TRUE; + } + sdLogWriteLog(pprzLogFile, "airspeed_ets: %d %d %8.4f %d %d %d %d %d %d %d %d %d\n", + airspeed_ets_raw, airspeed_ets_offset, airspeed_ets, + gps.fix, gps.tow, gps.week, + gps.lla_pos.lat, gps.lla_pos.lon, gps.hmsl, + gps.gspeed, gps.course, -gps.ned_vel.z); + } +#endif + + + // Transaction has been read airspeed_ets_i2c_trans.status = I2CTransDone; } diff --git a/sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c b/sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c index 33d756a48b..576c0407fc 100644 --- a/sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c +++ b/sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c @@ -32,7 +32,7 @@ #include "subsystems/abi.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #if PERIODIC_TELEMETRY diff --git a/sw/airborne/modules/sensors/airspeed_otf.c b/sw/airborne/modules/sensors/airspeed_otf.c index 5888dc7d65..f8ba178356 100644 --- a/sw/airborne/modules/sensors/airspeed_otf.c +++ b/sw/airborne/modules/sensors/airspeed_otf.c @@ -32,7 +32,7 @@ #include #include #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include "met_module.h" diff --git a/sw/airborne/modules/sensors/alt_srf08.c b/sw/airborne/modules/sensors/alt_srf08.c index 048043ced1..b950db614c 100644 --- a/sw/airborne/modules/sensors/alt_srf08.c +++ b/sw/airborne/modules/sensors/alt_srf08.c @@ -30,7 +30,7 @@ #include "mcu_periph/i2c.h" #include "alt_srf08.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include "led.h" diff --git a/sw/airborne/modules/sensors/aoa_adc.c b/sw/airborne/modules/sensors/aoa_adc.c index f5760ecacb..8f9c75cd45 100644 --- a/sw/airborne/modules/sensors/aoa_adc.c +++ b/sw/airborne/modules/sensors/aoa_adc.c @@ -34,7 +34,7 @@ // Messages #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" /// Default offset value (assuming 0 AOA is in the middle of the range) diff --git a/sw/airborne/modules/sensors/aoa_pwm.c b/sw/airborne/modules/sensors/aoa_pwm.c index 0eb071537d..ae11c4c6c5 100644 --- a/sw/airborne/modules/sensors/aoa_pwm.c +++ b/sw/airborne/modules/sensors/aoa_pwm.c @@ -29,7 +29,7 @@ #include "modules/sensors/aoa_pwm.h" #include "mcu_periph/pwm_input.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include "generated/airframe.h" diff --git a/sw/airborne/modules/sensors/baro_amsys.c b/sw/airborne/modules/sensors/baro_amsys.c index 89b65822ef..28e6123990 100644 --- a/sw/airborne/modules/sensors/baro_amsys.c +++ b/sw/airborne/modules/sensors/baro_amsys.c @@ -32,7 +32,7 @@ //Messages #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" //#include "gps.h" diff --git a/sw/airborne/modules/sensors/baro_bmp.c b/sw/airborne/modules/sensors/baro_bmp.c index 844c275099..eaa2d147cd 100644 --- a/sw/airborne/modules/sensors/baro_bmp.c +++ b/sw/airborne/modules/sensors/baro_bmp.c @@ -37,7 +37,7 @@ #include "led.h" #include "mcu_periph/uart.h" #include "subsystems/abi.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" diff --git a/sw/airborne/modules/sensors/baro_ets.c b/sw/airborne/modules/sensors/baro_ets.c index d672ac2799..a5d667c2e1 100644 --- a/sw/airborne/modules/sensors/baro_ets.c +++ b/sw/airborne/modules/sensors/baro_ets.c @@ -51,7 +51,7 @@ #ifdef BARO_ETS_SYNC_SEND #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #endif //BARO_ETS_SYNC_SEND diff --git a/sw/airborne/modules/sensors/baro_hca.c b/sw/airborne/modules/sensors/baro_hca.c index d802b257b0..4c36172083 100644 --- a/sw/airborne/modules/sensors/baro_hca.c +++ b/sw/airborne/modules/sensors/baro_hca.c @@ -27,7 +27,7 @@ //Messages #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" diff --git a/sw/airborne/modules/sensors/baro_mpl3115.c b/sw/airborne/modules/sensors/baro_mpl3115.c index fd2be6f12d..b8c35c54ec 100644 --- a/sw/airborne/modules/sensors/baro_mpl3115.c +++ b/sw/airborne/modules/sensors/baro_mpl3115.c @@ -32,7 +32,7 @@ //Messages #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.c b/sw/airborne/modules/sensors/baro_ms5611_i2c.c index bc830a9000..2aeca5c956 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_i2c.c +++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.c @@ -33,7 +33,7 @@ #include "mcu_periph/sys_time.h" #include "subsystems/abi.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" diff --git a/sw/airborne/modules/sensors/baro_ms5611_spi.c b/sw/airborne/modules/sensors/baro_ms5611_spi.c index b043f4d8fb..768c8aa4d9 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_spi.c +++ b/sw/airborne/modules/sensors/baro_ms5611_spi.c @@ -33,7 +33,7 @@ #include "mcu_periph/sys_time.h" #include "subsystems/abi.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #ifndef MS5611_SPI_DEV diff --git a/sw/airborne/modules/sensors/baro_scp.c b/sw/airborne/modules/sensors/baro_scp.c index ef00a66a4f..05a819dc52 100644 --- a/sw/airborne/modules/sensors/baro_scp.c +++ b/sw/airborne/modules/sensors/baro_scp.c @@ -5,7 +5,7 @@ #include "subsystems/abi.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include "mcu_periph/spi.h" diff --git a/sw/airborne/modules/sensors/baro_scp_i2c.c b/sw/airborne/modules/sensors/baro_scp_i2c.c index 49b5e3397c..520edcb0b9 100644 --- a/sw/airborne/modules/sensors/baro_scp_i2c.c +++ b/sw/airborne/modules/sensors/baro_scp_i2c.c @@ -13,7 +13,7 @@ #include "led.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #ifndef SENSOR_SYNC_SEND diff --git a/sw/airborne/modules/sensors/imu_aspirin2.c b/sw/airborne/modules/sensors/imu_aspirin2.c index 25fa7ae465..da5d39ddb3 100644 --- a/sw/airborne/modules/sensors/imu_aspirin2.c +++ b/sw/airborne/modules/sensors/imu_aspirin2.c @@ -26,7 +26,7 @@ // Downlink #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" diff --git a/sw/airborne/modules/sensors/mag_hmc5843.c b/sw/airborne/modules/sensors/mag_hmc5843.c index 2ed2f7c403..a47f5830b6 100644 --- a/sw/airborne/modules/sensors/mag_hmc5843.c +++ b/sw/airborne/modules/sensors/mag_hmc5843.c @@ -19,7 +19,7 @@ */ #include "mcu_periph/i2c.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include diff --git a/sw/airborne/modules/sensors/mag_hmc58xx.c b/sw/airborne/modules/sensors/mag_hmc58xx.c index 36569b2054..047bbaf972 100644 --- a/sw/airborne/modules/sensors/mag_hmc58xx.c +++ b/sw/airborne/modules/sensors/mag_hmc58xx.c @@ -27,7 +27,7 @@ #include "modules/sensors/mag_hmc58xx.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #if MODULE_HMC58XX_UPDATE_AHRS diff --git a/sw/airborne/modules/sensors/mag_micromag_fw.c b/sw/airborne/modules/sensors/mag_micromag_fw.c index 7492334ec4..f6f6a44387 100644 --- a/sw/airborne/modules/sensors/mag_micromag_fw.c +++ b/sw/airborne/modules/sensors/mag_micromag_fw.c @@ -3,7 +3,7 @@ #include "led.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" volatile uint8_t micromag_status; diff --git a/sw/airborne/modules/sensors/temp_adc.c b/sw/airborne/modules/sensors/temp_adc.c index 67db03d4f7..4b477d5160 100644 --- a/sw/airborne/modules/sensors/temp_adc.c +++ b/sw/airborne/modules/sensors/temp_adc.c @@ -27,7 +27,7 @@ #include "modules/sensors/temp_adc.h" #include "mcu_periph/adc.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include BOARD_CONFIG diff --git a/sw/airborne/modules/sensors/trigger_ext.c b/sw/airborne/modules/sensors/trigger_ext.c index fa9a84535c..b8581429c3 100644 --- a/sw/airborne/modules/sensors/trigger_ext.c +++ b/sw/airborne/modules/sensors/trigger_ext.c @@ -34,7 +34,7 @@ #include "subsystems/gps.h" #include "mcu_periph/sys_time.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" diff --git a/sw/airborne/modules/sonar/sonar_adc.c b/sw/airborne/modules/sonar/sonar_adc.c index 028db175a1..cbb21a0778 100644 --- a/sw/airborne/modules/sonar/sonar_adc.c +++ b/sw/airborne/modules/sonar/sonar_adc.c @@ -29,7 +29,7 @@ #endif #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" /** Sonar offset. diff --git a/sw/airborne/modules/stereocam/droplet/stereocam_droplet.c b/sw/airborne/modules/stereocam/droplet/stereocam_droplet.c index 1196770a3f..c958d71d09 100644 --- a/sw/airborne/modules/stereocam/droplet/stereocam_droplet.c +++ b/sw/airborne/modules/stereocam/droplet/stereocam_droplet.c @@ -56,7 +56,7 @@ struct link_device *xdev = STEREO_PORT; #ifndef DOWNLINK_DEVICE #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE #endif -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include "led.h" diff --git a/sw/airborne/modules/stereocam/nav_line_avoid/avoid_navigation.c b/sw/airborne/modules/stereocam/nav_line_avoid/avoid_navigation.c index 00087a4fab..35104332cf 100644 --- a/sw/airborne/modules/stereocam/nav_line_avoid/avoid_navigation.c +++ b/sw/airborne/modules/stereocam/nav_line_avoid/avoid_navigation.c @@ -44,7 +44,7 @@ #ifndef DOWNLINK_DEVICE #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE #endif -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include "led.h" diff --git a/sw/airborne/modules/stereocam/stereoprotocol.h b/sw/airborne/modules/stereocam/stereoprotocol.h index 588f3ca647..4a75f0406f 100644 --- a/sw/airborne/modules/stereocam/stereoprotocol.h +++ b/sw/airborne/modules/stereocam/stereoprotocol.h @@ -28,7 +28,7 @@ #define SW_AIRBORNE_MODULES_STEREO_CAM_STEREOPROTOCOL_H_ #include -#include "mcu_periph/link_device.h" +#include "pprzlink/pprzlink_device.h" struct MsgProperties { uint16_t positionImageStart; diff --git a/sw/airborne/peripherals/ak8975.c b/sw/airborne/peripherals/ak8975.c new file mode 100644 index 0000000000..4a2f972c8d --- /dev/null +++ b/sw/airborne/peripherals/ak8975.c @@ -0,0 +1,197 @@ +/* + * Copyright (C) 2015 Xavier Paris, Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file peripherals/ak8975.c + * + * Driver for the AKM AK8975 magnetometer. + * + */ + +#include "peripherals/ak8975.h" +#include "mcu_periph/sys_time.h" + +#define AK8975_MEAS_TIME_MS 9 + +// Internal calibration coeff +// Currently fetched at startup but not used after +// Only relying on general IMU mag calibration +static float calibration_values[3] = { 0, 0, 0 }; + +static float __attribute__((unused)) get_ajusted_value(const int16_t val, const uint8_t axis) +{ + const float H = (float) val; + const float corr_factor = calibration_values[axis]; + const float Hadj = corr_factor * H; + + return Hadj; +} + +void ak8975_init(struct Ak8975 *ak, struct i2c_periph *i2c_p, uint8_t addr) +{ + /* set i2c_peripheral */ + ak->i2c_p = i2c_p; + /* set i2c address */ + ak->i2c_trans.slave_addr = addr; + + ak->i2c_trans.status = I2CTransDone; + + ak->initialized = FALSE; + ak->status = AK_STATUS_IDLE; + ak->init_status = AK_CONF_UNINIT; + ak->data_available = FALSE; +} + +// Configure +void ak8975_configure(struct Ak8975 *ak) +{ + // Only configure when not busy + if (ak->i2c_trans.status != I2CTransSuccess && ak->i2c_trans.status != I2CTransFailed + && ak->i2c_trans.status != I2CTransDone) { + return; + } + + // Only when succesfull continue with next + if (ak->i2c_trans.status == I2CTransSuccess) { + ak->init_status++; + } + + ak->i2c_trans.status = I2CTransDone; + switch (ak->init_status) { + + case AK_CONF_UNINIT: + // Set AK8975 in fuse ROM access mode to read ADC calibration data + ak->i2c_trans.buf[0] = AK8975_REG_CNTL_ADDR; + ak->i2c_trans.buf[1] = AK8975_MODE_FUSE_ACCESS; + i2c_transmit(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 2); + break; + + case AK_REQ_CALIBRATION: + // Request AK8975 for ADC calibration data + ak->i2c_trans.buf[0] = AK8975_REG_ASASX; + i2c_transceive(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 1, 3); + break; + + case AK_DISABLE_ACCESS_CALIBRATION: + // Read config + for (uint8_t i =0; i<=2; i++) + calibration_values[i] = + ((((float)(ak->i2c_trans.buf[i]) - 128.0f)*0.5f)/128.0f)+1.0f; + + // Set AK8975 in power-down mode to stop read calibration data + ak->i2c_trans.buf[0] = AK8975_REG_CNTL_ADDR; + ak->i2c_trans.buf[1] = AK8975_MODE_POWER_DOWN; + i2c_transmit(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 2); + break; + + case AK_CONF_REQUESTED: + ak->initialized = TRUE; + break; + + default: + break; + } +} + +void ak8975_read(struct Ak8975 *ak) +{ + if (ak->status != AK_STATUS_IDLE) { + return; + } + + // Send single measurement request + ak->i2c_trans.buf[0] = AK8975_REG_CNTL_ADDR; + ak->i2c_trans.buf[1] = AK8975_MODE_SINGLE_MEAS; + i2c_transmit(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 2); + ak->last_meas_time = get_sys_time_msec(); + ak->status = AK_STATUS_MEAS; +} + +// Get raw value +#define RawFromBuf(_buf,_idx) ((int16_t)(_buf[_idx] | (_buf[_idx+1] << 8))) +// Raw is actually a 14 bits signed value +#define Int16FromRaw(_raw) ( (_raw & 0x1FFF) > 0xFFF ? (_raw & 0x1FFF) - 0x2000 : (_raw & 0x0FFF) ) +void ak8975_event(struct Ak8975 *ak) +{ + if (!ak->initialized) { + return; + } + + switch (ak->status) { + + case AK_STATUS_MEAS: + // Send a read data command if measurement time is done (9ms max) + if (ak->i2c_trans.status == I2CTransSuccess && + (get_sys_time_msec() - ak->last_meas_time >= AK8975_MEAS_TIME_MS)) { + ak->i2c_trans.buf[0] = AK8975_REG_ST1_ADDR; + i2c_transceive(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 1, 8); + ak->status++; + } + break; + + case AK_STATUS_READ: + if (ak->i2c_trans.status == I2CTransSuccess) { + // Mag data : + // Status 1 + // 1 byte + // Measures : + // 2 bytes + // 2 bytes + // 2 bytes + // Status 2 + // 1 byte + + // Read status and error bytes + const bool_t dr = ak->i2c_trans.buf[0] & 0x01; // data ready + const bool_t de = ak->i2c_trans.buf[7] & 0x04; // data error + const bool_t mo = ak->i2c_trans.buf[7] & 0x08; // mag overflow + if (de || !dr) { + // read error or data not ready, keep reading + break; + } + if (mo) { + // overflow, back to idle + ak->status = AK_STATUS_IDLE; + } + // Copy the data + int16_t val; + val = RawFromBuf(ak->i2c_trans.buf, 1); + ak->data.vect.x = Int16FromRaw(val); + val = RawFromBuf(ak->i2c_trans.buf, 3); + ak->data.vect.y = Int16FromRaw(val); + val = RawFromBuf(ak->i2c_trans.buf, 5); + ak->data.vect.z = Int16FromRaw(val); + ak->data_available = TRUE; + // End reading, back to idle + ak->status = AK_STATUS_IDLE; + break; + } + break; + + default: + if (ak->i2c_trans.status == I2CTransSuccess || ak->i2c_trans.status == I2CTransFailed) { + // Goto idle + ak->i2c_trans.status = I2CTransDone; + ak->status = AK_STATUS_IDLE; + } + break; + } +} + diff --git a/sw/airborne/peripherals/ak8975.h b/sw/airborne/peripherals/ak8975.h new file mode 100644 index 0000000000..bf06c8fcc5 --- /dev/null +++ b/sw/airborne/peripherals/ak8975.h @@ -0,0 +1,92 @@ +/* + * Copyright (C) 2015 Xavier Paris, Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file peripherals/ak8975.h + * + * Driver for the AKM AK8975 magnetometer. + */ + +#ifndef AK8975_H +#define AK8975_H + +#include "std.h" +#include "mcu_periph/i2c.h" +#include "math/pprz_algebra_int.h" + +/* Address and register definitions */ +#define AK8975_I2C_SLV_ADDR (0x0C<<1) +#define AK8975_REG_ST1_ADDR 0x02 +#define AK8975_REG_CNTL_ADDR 0x0A +#define AK8975_REG_ASASX 0x10 +#define AK8975_MODE_FUSE_ACCESS 0b00001111 +#define AK8975_MODE_POWER_DOWN 0b00000000 +#define AK8975_MODE_SINGLE_MEAS 0b00000001 + +/** config status states */ +enum Ak8975ConfStatus { + AK_CONF_UNINIT, + AK_REQ_CALIBRATION, + AK_DISABLE_ACCESS_CALIBRATION, + AK_CONF_REQUESTED +}; + +/** Normal status states */ +enum Ak8975Status { + AK_STATUS_IDLE, + AK_STATUS_MEAS, + AK_STATUS_READ, + AK_STATUS_DONE +}; + +struct Ak8975 { + struct i2c_periph *i2c_p; ///< peripheral used for communcation + struct i2c_transaction i2c_trans; ///< i2c transaction used for communication with the ak8936 + bool_t initialized; ///< config done flag + + enum Ak8975Status status; ///< main status + enum Ak8975ConfStatus init_status; ///< init status + uint32_t last_meas_time; ///< last measurement time in ms + + volatile bool_t data_available; ///< data ready flag + union { + struct Int16Vect3 vect; ///< data vector in mag coordinate system + int16_t value[3]; ///< data values accessible by channel index + } data; +}; + + +// Functions +extern void ak8975_init(struct Ak8975 *ak, struct i2c_periph *i2c_p, uint8_t addr); +extern void ak8975_configure(struct Ak8975 *ak); +extern void ak8975_event(struct Ak8975 *ak); +extern void ak8975_read(struct Ak8975 *ak); + +/// convenience function: read or start configuration if not already initialized +static inline void ak8975_periodic(struct Ak8975 *ak) +{ + if (ak->initialized) { + ak8975_read(ak); + } else { + ak8975_configure(ak); + } +} + +#endif /* AK8975_H */ diff --git a/sw/airborne/peripherals/cyrf6936.c b/sw/airborne/peripherals/cyrf6936.c index 49d3217cb4..ef7593bf27 100644 --- a/sw/airborne/peripherals/cyrf6936.c +++ b/sw/airborne/peripherals/cyrf6936.c @@ -31,7 +31,7 @@ #include "subsystems/radio_control.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" /* Static functions used in the different statuses */ diff --git a/sw/airborne/peripherals/mpu60x0.c b/sw/airborne/peripherals/mpu60x0.c index 70cf0e0233..4bfb5ffdff 100644 --- a/sw/airborne/peripherals/mpu60x0.c +++ b/sw/airborne/peripherals/mpu60x0.c @@ -44,6 +44,7 @@ void mpu60x0_set_default_config(struct Mpu60x0Config *c) */ c->nb_bytes = 15; c->nb_slaves = 0; + c->nb_slave_init = 0; c->i2c_bypass = FALSE; } diff --git a/sw/airborne/peripherals/mpu60x0.h b/sw/airborne/peripherals/mpu60x0.h index 419b5b59e1..693068ad3c 100644 --- a/sw/airborne/peripherals/mpu60x0.h +++ b/sw/airborne/peripherals/mpu60x0.h @@ -46,6 +46,11 @@ /// Default clock: PLL with X gyro reference #define MPU60X0_DEFAULT_CLK_SEL 1 +// Default number of I2C slaves +#ifndef MPU60X0_I2C_NB_SLAVES +#define MPU60X0_I2C_NB_SLAVES 5 +#endif + enum Mpu60x0ConfStatus { MPU60X0_CONF_UNINIT, MPU60X0_CONF_RESET, @@ -87,7 +92,8 @@ struct Mpu60x0Config { bool_t i2c_bypass; uint8_t nb_slaves; ///< number of used I2C slaves - struct Mpu60x0I2cSlave slaves[5]; ///< I2C slaves + uint8_t nb_slave_init; ///< number of already configured/initialized slaves + struct Mpu60x0I2cSlave slaves[MPU60X0_I2C_NB_SLAVES]; ///< I2C slaves enum Mpu60x0MstClk i2c_mst_clk; ///< MPU I2C master clock speed uint8_t i2c_mst_delay; ///< MPU I2C slaves delayed sample rate }; diff --git a/sw/airborne/peripherals/mpu60x0_i2c.c b/sw/airborne/peripherals/mpu60x0_i2c.c index 7157ea10e3..1a403896cc 100644 --- a/sw/airborne/peripherals/mpu60x0_i2c.c +++ b/sw/airborne/peripherals/mpu60x0_i2c.c @@ -130,7 +130,7 @@ void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu) } } -/** @todo: only one slave so far. */ +/** configure the registered I2C slaves */ bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void *mpu) { struct Mpu60x0_I2c *mpu_i2c = (struct Mpu60x0_I2c *)(mpu); @@ -150,8 +150,15 @@ bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void *mpu) mpu_i2c->slave_init_status++; break; case MPU60X0_I2C_CONF_SLAVES_CONFIGURE: - /* configure each slave. TODO: currently only one */ - if (mpu_i2c->config.slaves[0].configure(mpu_set, mpu)) { + /* configure each slave until all nb_slaves are done */ + if (mpu_i2c->config.nb_slave_init < mpu_i2c->config.nb_slaves && mpu_i2c->config.nb_slave_init < MPU60X0_I2C_NB_SLAVES) { + // proceed to next slave if configure for current one returns true + if (mpu_i2c->config.slaves[mpu_i2c->config.nb_slave_init].configure(mpu_set, mpu)) { + mpu_i2c->config.nb_slave_init++; + } + } + else { + /* all slave devies configured, continue MPU side configuration of I2C slave stuff */ mpu_i2c->slave_init_status++; } break; diff --git a/sw/airborne/peripherals/mpu60x0_spi.c b/sw/airborne/peripherals/mpu60x0_spi.c index 5b53f42b9d..b85885f069 100644 --- a/sw/airborne/peripherals/mpu60x0_spi.c +++ b/sw/airborne/peripherals/mpu60x0_spi.c @@ -148,7 +148,7 @@ void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu) } } -/** @todo: only one slave so far. */ +/** configure the registered I2C slaves */ bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void *mpu) { struct Mpu60x0_Spi *mpu_spi = (struct Mpu60x0_Spi *)(mpu); @@ -175,8 +175,15 @@ bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void *mpu) mpu_spi->slave_init_status++; break; case MPU60X0_SPI_CONF_SLAVES_CONFIGURE: - /* configure first slave, only one slave supported so far */ - if (mpu_spi->config.slaves[0].configure(mpu_set, mpu)) { + /* configure each slave until all nb_slaves are done */ + if (mpu_spi->config.nb_slave_init < mpu_spi->config.nb_slaves && mpu_spi->config.nb_slave_init < MPU60X0_I2C_NB_SLAVES) { + // proceed to next slave if configure for current one returns true + if (mpu_spi->config.slaves[mpu_spi->config.nb_slave_init].configure(mpu_set, mpu)) { + mpu_spi->config.nb_slave_init++; + } + } + else { + /* all slave devies configured, continue MPU side configuration of I2C slave stuff */ mpu_spi->slave_init_status++; } break; diff --git a/sw/airborne/peripherals/mpu9250.c b/sw/airborne/peripherals/mpu9250.c index a2326bbc2e..a65f65ca62 100644 --- a/sw/airborne/peripherals/mpu9250.c +++ b/sw/airborne/peripherals/mpu9250.c @@ -45,6 +45,7 @@ void mpu9250_set_default_config(struct Mpu9250Config *c) */ c->nb_bytes = 15; c->nb_slaves = 0; + c->nb_slave_init = 0; c->i2c_bypass = FALSE; } diff --git a/sw/airborne/peripherals/mpu9250.h b/sw/airborne/peripherals/mpu9250.h index 81ca095cd5..fd8daa8057 100644 --- a/sw/airborne/peripherals/mpu9250.h +++ b/sw/airborne/peripherals/mpu9250.h @@ -47,6 +47,11 @@ /// Default clock: PLL with X gyro reference #define MPU9250_DEFAULT_CLK_SEL 1 +// Default number of I2C slaves +#ifndef MPU9250_I2C_NB_SLAVES +#define MPU9250_I2C_NB_SLAVES 5 +#endif + enum Mpu9250ConfStatus { MPU9250_CONF_UNINIT, MPU9250_CONF_RESET, @@ -90,7 +95,8 @@ struct Mpu9250Config { bool_t i2c_bypass; uint8_t nb_slaves; ///< number of used I2C slaves - struct Mpu9250I2cSlave slaves[5]; ///< I2C slaves + uint8_t nb_slave_init; ///< number of already configured/initialized slaves + struct Mpu9250I2cSlave slaves[MPU9250_I2C_NB_SLAVES]; ///< I2C slaves enum Mpu9250MstClk i2c_mst_clk; ///< MPU I2C master clock speed uint8_t i2c_mst_delay; ///< MPU I2C slaves delayed sample rate }; diff --git a/sw/airborne/peripherals/mpu9250_i2c.c b/sw/airborne/peripherals/mpu9250_i2c.c index 158ebdc22e..9cd52538a3 100644 --- a/sw/airborne/peripherals/mpu9250_i2c.c +++ b/sw/airborne/peripherals/mpu9250_i2c.c @@ -163,7 +163,7 @@ bool_t imu_mpu9250_configure_mag_slave(Mpu9250ConfigSet mpu_set __attribute__((u } } -/** @todo: only one slave so far. */ +/** configure the registered I2C slaves */ bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void *mpu) { struct Mpu9250_I2c *mpu_i2c = (struct Mpu9250_I2c *)(mpu); @@ -183,8 +183,15 @@ bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void *mpu) mpu_i2c->slave_init_status++; break; case MPU9250_I2C_CONF_SLAVES_CONFIGURE: - /* configure each slave. TODO: currently only one */ - if (mpu_i2c->config.slaves[0].configure(mpu_set, mpu)) { + /* configure each slave until all nb_slaves are done */ + if (mpu_i2c->config.nb_slave_init < mpu_i2c->config.nb_slaves && mpu_i2c->config.nb_slave_init < MPU9250_I2C_NB_SLAVES) { + // proceed to next slave if configure for current one returns true + if (mpu_i2c->config.slaves[mpu_i2c->config.nb_slave_init].configure(mpu_set, mpu)) { + mpu_i2c->config.nb_slave_init++; + } + } + else { + /* all slave devies configured, continue MPU side configuration of I2C slave stuff */ mpu_i2c->slave_init_status++; } break; diff --git a/sw/airborne/peripherals/mpu9250_spi.c b/sw/airborne/peripherals/mpu9250_spi.c index 6af6631f6b..ac6ed5667f 100644 --- a/sw/airborne/peripherals/mpu9250_spi.c +++ b/sw/airborne/peripherals/mpu9250_spi.c @@ -145,7 +145,7 @@ void mpu9250_spi_event(struct Mpu9250_Spi *mpu) } } -/** @todo: only one slave so far. */ +/** configure the registered I2C slaves */ bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void *mpu) { struct Mpu9250_Spi *mpu_spi = (struct Mpu9250_Spi *)(mpu); @@ -172,8 +172,15 @@ bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void *mpu) mpu_spi->slave_init_status++; break; case MPU9250_SPI_CONF_SLAVES_CONFIGURE: - /* configure first slave, only one slave supported so far */ - if (mpu_spi->config.slaves[0].configure(mpu_set, mpu)) { + /* configure each slave until all nb_slaves are done */ + if (mpu_spi->config.nb_slave_init < mpu_spi->config.nb_slaves && mpu_spi->config.nb_slave_init < MPU9250_I2C_NB_SLAVES) { + // proceed to next slave if configure for current one returns true + if (mpu_spi->config.slaves[mpu_spi->config.nb_slave_init].configure(mpu_set, mpu)) { + mpu_spi->config.nb_slave_init++; + } + } + else { + /* all slave devies configured, continue MPU side configuration of I2C slave stuff */ mpu_spi->slave_init_status++; } break; diff --git a/sw/airborne/pprz_debug.h b/sw/airborne/pprz_debug.h index a1c2294337..97c04baa7e 100644 --- a/sw/airborne/pprz_debug.h +++ b/sw/airborne/pprz_debug.h @@ -30,7 +30,7 @@ #include "std.h" #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" extern uint8_t pprz_debug_mod; diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h index 0750518d7b..29254371dd 100644 --- a/sw/airborne/subsystems/abi_sender_ids.h +++ b/sw/airborne/subsystems/abi_sender_ids.h @@ -230,5 +230,12 @@ #define PX4FLOW_VELOCITY_ID 17 #endif +/* + * IDs of RSSI measurements (message 13) + */ +#ifndef RSSI_BLUEGIGA_ID +#define RSSI_BLUEGIGA_ID 1 +#endif + #endif /* ABI_SENDER_IDS_H */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c index 72e34b8ea5..1ac9197932 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c @@ -45,7 +45,7 @@ #if FLOAT_DCM_SEND_DEBUG // FIXME Debugging Only #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #endif diff --git a/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.h b/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.h index b5b278b740..55808d79cb 100644 --- a/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.h +++ b/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.h @@ -30,7 +30,7 @@ #include "ff.h" #include "subsystems/chibios-libopencm3/sdLog.h" -#include "mcu_periph/link_device.h" +#include "pprzlink/pprzlink_device.h" /* what to be done : diff --git a/sw/airborne/subsystems/datalink/audio_telemetry.h b/sw/airborne/subsystems/datalink/audio_telemetry.h deleted file mode 100644 index d83ff0f872..0000000000 --- a/sw/airborne/subsystems/datalink/audio_telemetry.h +++ /dev/null @@ -1,75 +0,0 @@ -/* - * Paparazzi telemetry via audio channel modem functions - * - * Copyright (C) 2003 Pascal Brisset, 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 AUDIO_TELEMETRY_H -#define AUDIO_TELEMETRY_H - -#include "inttypes.h" - -extern uint8_t audio_telemetry_nb_ovrn; - -#include "generated/airframe.h" -#include "audio_telemetry_hw.h" - - -#define TX_BUF_SIZE 255 -extern uint8_t tx_head; -extern volatile uint8_t tx_tail; -extern uint8_t tx_buf[ TX_BUF_SIZE ]; - -extern uint8_t tx_byte; -extern uint8_t tx_byte_idx; - - -#define AudioTelemetrySendMessage() AUDIO_TELEMETRY_CHECK_RUNNING() - -#if TX_BUF_SIZE == 256 -#define UPDATE_HEAD() { \ - tx_head++; \ - } -#else -#define UPDATE_HEAD() { \ - tx_head++; \ - if (tx_head >= TX_BUF_SIZE) tx_head = 0; \ - } -#endif - -#define AudioTelemetryCheckFreeSpace(_space) (tx_head>=tx_tail? _space < (TX_BUF_SIZE - (tx_head - tx_tail)) : _space < (tx_tail - tx_head)) - -#define AudioTelemetryPut1Byte(_byte) { \ - tx_buf[tx_head] = _byte; \ - UPDATE_HEAD(); \ - } - -#define AUDIO_TELEMETRY_LOAD_NEXT_BYTE() { \ - tx_byte = tx_buf[tx_tail]; \ - tx_byte_idx = 0; \ - tx_tail++; \ - if( tx_tail >= TX_BUF_SIZE ) \ - tx_tail = 0; \ - } - -#define AudioTelemetryTransmit(_x) Audio_TelemetryPut1Byte(_x) - -#endif /* AUDIO_TELEMETRY_H */ diff --git a/sw/airborne/subsystems/datalink/bluegiga.c b/sw/airborne/subsystems/datalink/bluegiga.c index 8985c5b498..82e83e41e9 100644 --- a/sw/airborne/subsystems/datalink/bluegiga.c +++ b/sw/airborne/subsystems/datalink/bluegiga.c @@ -30,9 +30,19 @@ #include "mcu_periph/gpio.h" #include "mcu_periph/spi.h" +#ifdef MODEM_LED +#include "led.h" +#endif + +#include "subsystems/abi.h" + // for memset #include +//#include "subsystems/datalink/telemetry_common.h" +//#include "subsystems/datalink/telemetry.h" +#include "generated/periodic_telemetry.h" + #ifndef BLUEGIGA_SPI_DEV #error "bluegiga: must define a BLUEGIGA_SPI_DEV" #endif @@ -46,14 +56,18 @@ #define BLUEGIGA_DRDY_GPIO_PIN SUPERBITRF_DRDY_PIN #endif +#define TxStrengthOfSender(x) (x[1]) +#define RssiOfSender(x) (x[2]) +#define Pprz_StxOfMsg(x) (x[3]) +#define SenderIdOfMsg(x) (x[5]) + enum BlueGigaStatus coms_status; struct bluegiga_periph bluegiga_p; struct spi_transaction bluegiga_spi; -signed char bluegiga_rssi[256]; // values initialized with 127 -unsigned char telemetry_copy[20]; +uint8_t broadcast_msg[20]; -void bluegiga_load_tx(struct bluegiga_periph *p); +void bluegiga_load_tx(struct bluegiga_periph *p, struct spi_transaction *trans); void bluegiga_transmit(struct bluegiga_periph *p, uint8_t data); void bluegiga_receive(struct spi_transaction *trans); @@ -80,6 +94,8 @@ static int dev_char_available(struct bluegiga_periph *p) { return bluegiga_ch_available(p); } + +// note, need to run dev_char_available first static uint8_t dev_get_byte(struct bluegiga_periph *p) { uint8_t ret = p->rx_buf[p->rx_extract_idx]; @@ -115,7 +131,7 @@ static void send_bluegiga(struct transport_tx *trans, struct link_device *dev) if (now_ts > last_ts) { uint32_t rate = 1000 * bluegiga_p.bytes_recvd_since_last / (now_ts - last_ts); - pprz_msg_send_BLUEGIGA(trans, dev, AC_ID, &rate, 20, telemetry_copy); + pprz_msg_send_BLUEGIGA(trans, dev, AC_ID, &rate); bluegiga_p.bytes_recvd_since_last = 0; last_ts = now_ts; @@ -140,7 +156,7 @@ void bluegiga_init(struct bluegiga_periph *p) bluegiga_spi.cpha = SPICphaEdge2; bluegiga_spi.dss = SPIDss8bit; bluegiga_spi.bitorder = SPIMSBFirst; - bluegiga_spi.cdiv = SPIDiv64; + bluegiga_spi.cdiv = SPIDiv256; bluegiga_spi.after_cb = (SPICallback) trans_cb; // Configure generic link device @@ -157,18 +173,14 @@ void bluegiga_init(struct bluegiga_periph *p) p->tx_insert_idx = 0; p->tx_extract_idx = 0; - for (int i = 0; i < bluegiga_spi.input_length; i++) { - p->work_rx[i] = 0; - } - for (int i = 0; i < bluegiga_spi.output_length; i++) { - p->work_tx[i] = 0; - } - for (int i = 0; i < 255; i++) { - bluegiga_rssi[i] = 127; - } + memset(p->work_rx, 0, bluegiga_spi.input_length); + memset(p->work_tx, 0, bluegiga_spi.output_length); + + memset(broadcast_msg, 0, 19); p->bytes_recvd_since_last = 0; p->end_of_msg = p->tx_insert_idx; + p->connected = 0; // set DRDY interrupt pin for spi master triggered on falling edge gpio_setup_output(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN); @@ -194,10 +206,11 @@ void bluegiga_transmit(struct bluegiga_periph *p, uint8_t data) } /* Load waiting data into tx peripheral buffer */ -void bluegiga_load_tx(struct bluegiga_periph *p) +void bluegiga_load_tx(struct bluegiga_periph *p, struct spi_transaction *trans) { + uint8_t packet_len; // check data available in buffer to send - uint8_t packet_len = ((p->end_of_msg - p->tx_extract_idx + BLUEGIGA_BUFFER_SIZE) % BLUEGIGA_BUFFER_SIZE); + packet_len = ((p->end_of_msg - p->tx_extract_idx + BLUEGIGA_BUFFER_SIZE) % BLUEGIGA_BUFFER_SIZE); if (packet_len > 19) { packet_len = 19; } @@ -214,7 +227,7 @@ void bluegiga_load_tx(struct bluegiga_periph *p) bluegiga_increment_buf(&p->tx_extract_idx, packet_len); // clear unused bytes - for (i = packet_len + 1; i < bluegiga_spi.output_length; i++) { + for (i = packet_len + 1; i < trans->output_length; i++) { p->work_tx[i] = 0; } @@ -235,97 +248,104 @@ void bluegiga_receive(struct spi_transaction *trans) for (uint8_t i = 0; i < trans->output_length; i++) { trans->output_buf[i] = 0; } + } else if (coms_status == BLUEGIGA_SENDING_BROADCAST) { + // sending second half of broadcast message + for (uint8_t i = 0; i < broadcast_msg[0]; i++) { + trans->output_buf[i] = broadcast_msg[i]; + } + coms_status = BLUEGIGA_SENDING; + return; } /* - * 0xff communication lost with ground station - * 0xfe RSSI value from broadcaster - * 0xfd Change in broadcast mode - * 0xfc Receive all recorded RSSI - * <=20 Data package from ground station + * >235 data package from broadcast mode + * 0x50 communication lost with ground station + * 0x51 interrupt handled + * <20 data package from connection */ uint8_t packet_len = 0; uint8_t read_offset = 0; switch (trans->input_buf[0]) { - case 0xff: // communication lost with ground station -#ifdef MODEM_LED - LED_OFF(MODEM_LED); -#endif - coms_status = BLUEGIGA_UNINIT; - gpio_set(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN); // Reset interrupt pin - break; - case 0xfe: // RSSI value from broadcaster - bluegiga_rssi[trans->input_buf[1]] = trans->input_buf[2]; - packet_len = trans->input_buf[3]; - read_offset = 4; - break; - case 0xfd: // Change in broadcast mode - gpio_set(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN); // Reset interrupt pin - - // fetch scan status - if (trans->input_buf[1] == 1) { - coms_status = BLUEGIGA_BROADCASTING; + case 0x50: // communication status changed + bluegiga_p.connected = trans->input_buf[1]; + if (bluegiga_p.connected) { + //telemetry_mode_Main = TELEMETRY_PROCESS_Main; } else { - coms_status = BLUEGIGA_UNINIT; + //telemetry_mode_Main = NB_TELEMETRY_MODES; // send no periodic telemetry } + coms_status = BLUEGIGA_IDLE; break; - case 0xfc: // Receive all recorded RSSI - for (uint8_t i = 0; i < trans->input_buf[1]; i++) { - bluegiga_rssi[trans->input_buf[2] + i] = trans->input_buf[3 + i]; - } + case 0x51: // Interrupt handled + gpio_set(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN); // Reset interrupt pin break; default: - packet_len = trans->input_buf[0]; // length of transmitted message - read_offset = 1; + coms_status = BLUEGIGA_IDLE; + // compute length of transmitted message + if (trans->input_buf[0] < trans->input_length) { // normal connection mode + packet_len = trans->input_buf[0]; + read_offset = 1; + } else if (trans->input_buf[0] > 0xff - trans->input_length) { // broadcast mode + packet_len = 0xff - trans->input_buf[0]; + + int8_t tx_strength = TxStrengthOfSender(trans->input_buf); + int8_t rssi = RssiOfSender(trans->input_buf); + uint8_t ac_id = SenderIdOfMsg(trans->input_buf); + + if (Pprz_StxOfMsg(trans->input_buf) == PPRZ_STX) { + AbiSendMsgRSSI(RSSI_BLUEGIGA_ID, ac_id, tx_strength, rssi); + } + + read_offset = 3; + } } // handle incoming datalink message - if (packet_len > 0 && packet_len <= trans->input_length) { + if (packet_len > 0 && packet_len <= trans->input_length - read_offset) { +#ifdef MODEM_LED + LED_TOGGLE(MODEM_LED); +#endif // Handle received message for (uint8_t i = 0; i < packet_len; i++) { bluegiga_p.rx_buf[(bluegiga_p.rx_insert_idx + i) % BLUEGIGA_BUFFER_SIZE] = trans->input_buf[i + read_offset]; } bluegiga_increment_buf(&bluegiga_p.rx_insert_idx, packet_len); bluegiga_p.bytes_recvd_since_last += packet_len; - coms_status = BLUEGIGA_IDLE; - - for (uint8_t i = 0; i < trans->input_length; i++) { - telemetry_copy[i] = trans->input_buf[i]; - } - } else { - coms_status = BLUEGIGA_IDLE; } // load next message to be sent into work buffer, needs to be loaded before calling spi_slave_register - bluegiga_load_tx(&bluegiga_p); + bluegiga_load_tx(&bluegiga_p, trans); // register spi slave read for next transaction - spi_slave_register(&(BLUEGIGA_SPI_DEV), &bluegiga_spi); + spi_slave_register(&(BLUEGIGA_SPI_DEV), trans); } } -/* command bluetooth to switch to active scan mode to get rssi values from neighbouring drones */ -void bluegiga_scan(struct bluegiga_periph *p) +/* Send data for broadcast message to the bluegiga module + * maximum size of message is 22 bytes + */ +void bluegiga_broadcast_msg(struct bluegiga_periph *p, char *msg, uint8_t msg_len) { + if (msg_len == 0 || msg_len > 22) { + return; + } - memset(p->work_tx, 0, 20); - p->work_tx[0] = 0xfd; // change broadcast mode header + uint8_t max_length = 20; + p->work_tx[0] = msg_len; - coms_status = BLUEGIGA_SENDING; - - // trigger bluegiga to read direct command - gpio_clear(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN); // set interrupt -} - -/* Request list of all recorded RSSI */ -void bluegiga_request_all_rssi(struct bluegiga_periph *p) -{ - - memset(p->work_tx, 0, 20); - p->work_tx[0] = 0xfc; - - coms_status = BLUEGIGA_SENDING; + if (msg_len < max_length) { + for (uint8_t i = 0; i < msg_len; i++) { + p->work_tx[i + 1] = msg[i]; + } + coms_status = BLUEGIGA_SENDING; + } else { + for (uint8_t i = 0; i < max_length - 1; i++) { + p->work_tx[i + 1] = msg[i]; + } + + memcpy(broadcast_msg, msg + max_length - 1, msg_len - (max_length - 1)); + coms_status = BLUEGIGA_SENDING_BROADCAST; + } // trigger bluegiga to read direct command gpio_clear(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN); // set interrupt diff --git a/sw/airborne/subsystems/datalink/bluegiga.h b/sw/airborne/subsystems/datalink/bluegiga.h index f7770abd33..52ac935593 100644 --- a/sw/airborne/subsystems/datalink/bluegiga.h +++ b/sw/airborne/subsystems/datalink/bluegiga.h @@ -27,14 +27,15 @@ #ifndef BLUEGIGA_DATA_LINK_H #define BLUEGIGA_DATA_LINK_H -#include "mcu_periph/link_device.h" +#include "pprzlink/pprzlink_device.h" +#include "subsystems/datalink/datalink.h" /* The different statuses the communication can be in */ enum BlueGigaStatus { BLUEGIGA_UNINIT, /**< The com isn't initialized */ BLUEGIGA_IDLE, /**< The com is in idle */ BLUEGIGA_SENDING, /**< The com is sending */ - BLUEGIGA_BROADCASTING /**< The com is switched from data link to rssi scanning */ + BLUEGIGA_SENDING_BROADCAST /**< The com is switched from data link to rssi scanning */ }; #ifndef BLUEGIGA_BUFFER_SIZE @@ -63,6 +64,7 @@ struct bluegiga_periph { /* some administrative variables */ uint32_t bytes_recvd_since_last; uint8_t end_of_msg; + uint8_t connected; }; @@ -75,38 +77,6 @@ void bluegiga_increment_buf(uint8_t *buf_idx, uint8_t len); void bluegiga_init(struct bluegiga_periph *p); void bluegiga_scan(struct bluegiga_periph *p); -void bluegiga_request_all_rssi(struct bluegiga_periph *p); - -// BLUEGIGA is using pprz_transport -// FIXME it should not appear here, this will be fixed with the rx improvements some day... -// BLUEGIGA needs a specific read_buffer function -#include "subsystems/datalink/pprz_transport.h" -#include "led.h" -static inline void bluegiga_read_buffer(struct bluegiga_periph *p, struct pprz_transport *t) -{ - do { - uint8_t c = 0; - do { - parse_pprz(t, p->rx_buf[(p->rx_extract_idx + c++) % BLUEGIGA_BUFFER_SIZE]); - } while (((p->rx_extract_idx + c) % BLUEGIGA_BUFFER_SIZE != p->rx_insert_idx) - && !(t->trans_rx.msg_received)); - // reached end of circular read buffer or message received - // if received, decode and advance - if (t->trans_rx.msg_received) { -#ifdef MODEM_LED - LED_TOGGLE(MODEM_LED); -#endif - pprz_parse_payload(t); - t->trans_rx.msg_received = FALSE; - } - bluegiga_increment_buf(&p->rx_extract_idx, c); - } while (bluegiga_ch_available(p)); // continue till all messages read -} - -// transmit previous data in buffer and parse data received -#define BlueGigaCheckAndParse(_dev,_trans) { \ - if (bluegiga_ch_available(&(_dev))) \ - bluegiga_read_buffer(&(_dev), &(_trans)); \ - } +void bluegiga_broadcast_msg(struct bluegiga_periph *p, char *msg, uint8_t msg_len); #endif /* BLUEGIGA_DATA_LINK_H */ diff --git a/sw/airborne/subsystems/datalink/datalink.h b/sw/airborne/subsystems/datalink/datalink.h index 23bbbb9171..2d4aa1ec6e 100644 --- a/sw/airborne/subsystems/datalink/datalink.h +++ b/sw/airborne/subsystems/datalink/datalink.h @@ -40,7 +40,7 @@ #endif #include "std.h" -#include "dl_protocol.h" +#include "pprzlink/dl_protocol.h" /** Datalink kinds */ #define PPRZ 1 @@ -68,6 +68,15 @@ EXTERN void dl_parse_msg(void); EXTERN bool_t datalink_enabled; #endif +/** Convenience macro to fill dl_buffer */ +#define DatalinkFillDlBuffer(_buf, _len) { \ + uint8_t _i = 0; \ + for (_i = 0; _i < _len; _i++) { \ + dl_buffer[_i] = _buf[_i]; \ + } \ + dl_msg_available = TRUE; \ +} + /** Check for new message and parse */ static inline void DlCheckAndParse(void) { @@ -89,14 +98,14 @@ static inline void DlCheckAndParse(void) #if defined DATALINK && DATALINK == PPRZ #define DatalinkEvent() { \ - PprzCheckAndParse(PPRZ_UART, pprz_tp); \ + pprz_check_and_parse(&(PPRZ_UART).device, &pprz_tp, dl_buffer, &dl_msg_available); \ DlCheckAndParse(); \ } #elif defined DATALINK && DATALINK == XBEE #define DatalinkEvent() { \ - XBeeCheckAndParse(XBEE_UART, xbee_tp); \ + xbee_check_and_parse(&(XBEE_UART).device, &xbee_tp, dl_buffer, (uint8_t*)(&dl_msg_available)); \ DlCheckAndParse(); \ } @@ -117,7 +126,7 @@ static inline void DlCheckAndParse(void) #elif defined DATALINK && DATALINK == BLUEGIGA #define DatalinkEvent() { \ - BlueGigaCheckAndParse(DOWNLINK_DEVICE, pprz_tp); \ + pprz_check_and_parse(&(DOWNLINK_DEVICE).device, &pprz_tp, dl_buffer, (uint8_t*)(&dl_msg_available)); \ DlCheckAndParse(); \ } diff --git a/sw/airborne/subsystems/datalink/downlink.c b/sw/airborne/subsystems/datalink/downlink.c index efcafa3470..dec200c9d9 100644 --- a/sw/airborne/subsystems/datalink/downlink.c +++ b/sw/airborne/subsystems/datalink/downlink.c @@ -27,10 +27,28 @@ #include "subsystems/datalink/downlink.h" +#include "generated/airframe.h" // AC_ID is required + +#if (defined DATALINK) || PERIODIC_TELEMETRY +#include "subsystems/datalink/datalink.h" +#endif + +#if defined SITL && !USE_NPS +struct ivy_transport ivy_tp; +#endif + +#if DATALINK == PPRZ || DATALINK == SUPERBITRF || DATALINK == W5100 || DATALINK == BLUEGIGA +struct pprz_transport pprz_tp; +#endif +#if DATALINK == XBEE +struct xbee_transport xbee_tp; +#endif +#if USE_PPRZLOG +struct pprzlog_transport pprzlog_tp; +#endif #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -#include "subsystems/datalink/datalink.h" #include "mcu_periph/sys_time.h" static uint32_t last_down_nb_bytes = 0; // previous number of bytes sent @@ -76,7 +94,13 @@ void downlink_init(void) pprz_transport_init(&pprz_tp); #endif #if DATALINK == XBEE - xbee_init(); +#ifndef XBEE_TYPE +#define XBEE_TYPE XBEE_24 +#endif +#ifndef XBEE_INIT +#define XBEE_INIT "" +#endif + xbee_transport_init(&xbee_tp, &((DefaultDevice).device), AC_ID, XBEE_TYPE, sys_time_usleep, XBEE_INIT); #endif #if DATALINK == W5100 w5100_init(); @@ -88,11 +112,11 @@ void downlink_init(void) #endif #if USE_PPRZLOG - pprzlog_transport_init(); + pprzlog_transport_init(&pprzlog_tp, get_sys_time_usec); #endif #if SITL && !USE_NPS - ivy_transport_init(); + ivy_transport_init(&ivy_tp); #endif #if PERIODIC_TELEMETRY diff --git a/sw/airborne/subsystems/datalink/downlink.h b/sw/airborne/subsystems/datalink/downlink.h index aeb2044464..f2aed5f62b 100644 --- a/sw/airborne/subsystems/datalink/downlink.h +++ b/sw/airborne/subsystems/datalink/downlink.h @@ -30,31 +30,45 @@ #include -#ifndef PPRZ_DATALINK_EXPORT - +#include "generated/airframe.h" #include "generated/modules.h" -#include "messages.h" -#include "generated/airframe.h" // AC_ID is required +#include "pprzlink/messages.h" +#include "subsystems/datalink/datalink.h" + #if defined SITL && !USE_NPS /** Software In The Loop simulation uses IVY bus directly as the transport layer */ -#include "ivy_transport.h" +#include "pprzlink/ivy_transport.h" +extern struct ivy_transport ivy_tp; #else /** SITL */ -#include "subsystems/datalink/pprz_transport.h" -#include "subsystems/datalink/pprzlog_transport.h" -#include "subsystems/datalink/xbee.h" +#if DATALINK == PPRZ || DATALINK == SUPERBITRF || DATALINK == W5100 || DATALINK == BLUEGIGA +#include "pprzlink/pprz_transport.h" +extern struct pprz_transport pprz_tp; +#endif + +#if USE_PPRZLOG +#include "pprzlink/pprzlog_transport.h" +extern struct pprzlog_transport pprzlog_tp; +#endif + +#if DATALINK == XBEE +#include "pprzlink/xbee_transport.h" +extern struct xbee_transport xbee_tp; +#endif + +#if DATALINK == W5100 #include "subsystems/datalink/w5100.h" +#endif + #if DATALINK == BLUEGIGA #include "subsystems/datalink/bluegiga.h" #endif + #if USE_SUPERBITRF #include "subsystems/datalink/superbitrf.h" #endif -#if USE_AUDIO_TELEMETRY -#include "subsystems/datalink/audio_telemetry.h" -#endif #if USE_USB_SERIAL #include "mcu_periph/usb_serial.h" #endif @@ -65,15 +79,6 @@ #endif /** !SITL */ -#else /* PPRZ_DATALINK_EXPORT defined */ - -#include "messages.h" -#include "pprz_transport.h" -#ifndef AC_ID -#define AC_ID 0 -#endif - -#endif #ifndef DefaultChannel #define DefaultChannel DOWNLINK_TRANSPORT diff --git a/sw/airborne/subsystems/datalink/downlink_transport.h b/sw/airborne/subsystems/datalink/downlink_transport.h deleted file mode 100644 index e532af245a..0000000000 --- a/sw/airborne/subsystems/datalink/downlink_transport.h +++ /dev/null @@ -1,63 +0,0 @@ -/* - * Copyright (C) 2003-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 downlink_transport.h - * \brief Interface for downlink transport implementation - * - */ - -#ifndef DOWNLINK_TRANSPORT_H -#define DOWNLINK_TRANSPORT_H - -#include - -enum DownlinkDataType { - DL_TYPE_ARRAY_LENGTH = 1, - DL_TYPE_UINT8, - DL_TYPE_INT8, - DL_TYPE_UINT16, - DL_TYPE_INT16, - DL_TYPE_UINT32, - DL_TYPE_INT32, - DL_TYPE_UINT64, - DL_TYPE_INT64, - DL_TYPE_FLOAT, - DL_TYPE_DOUBLE, - DL_TYPE_TIMESTAMP, -}; - -struct DownlinkTransport { - uint8_t (*SizeOf)(void *impl, uint8_t size); - int (*CheckFreeSpace)(void *impl, uint8_t size); - - void (*PutBytes)(void *impl, enum DownlinkDataType data_type, uint8_t len, const void *bytes); - - void (*StartMessage)(void *impl, char *name, uint8_t msg_id, uint8_t payload_len); - void (*EndMessage)(void *impl); - void (*Overrun)(void *impl); - void (*CountBytes)(void *impl, uint8_t len); - void (*Periodic)(void *impl); - - void *impl; -}; - -#endif /* DOWNLINK_TRANSPORT_H */ diff --git a/sw/airborne/subsystems/datalink/ivy_transport.c b/sw/airborne/subsystems/datalink/ivy_transport.c deleted file mode 100644 index 30931c45d4..0000000000 --- a/sw/airborne/subsystems/datalink/ivy_transport.c +++ /dev/null @@ -1,204 +0,0 @@ -/* - * Copyright (C) 2003 Pascal Brisset, Antoine Drouin - * Copyright (C) 2014 Gautier Hattenberger - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, see - * . - * - */ - -/** - * @file subsystems/datalink/ivy_transport.c - * - * Building Paparazzi frames over IVY. - * - */ - -#include "std.h" -#include "subsystems/datalink/ivy_transport.h" -#include "subsystems/datalink/downlink.h" -#include "subsystems/datalink/transport.h" - -#include -#include - -struct ivy_transport ivy_tp; - -static void put_bytes(struct ivy_transport *trans, struct link_device *dev __attribute__((unused)), - enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), - uint8_t len, const void *bytes) -{ - const uint8_t *b = (const uint8_t *) bytes; - - // Start delimiter "quote" for char arrays (strings) - if (format == DL_FORMAT_ARRAY && type == DL_TYPE_CHAR) { - trans->ivy_p += sprintf(trans->ivy_p, "\""); - } - - int i = 0; - while (i < len) { - // print data with correct type - switch (type) { - case DL_TYPE_CHAR: - trans->ivy_p += sprintf(trans->ivy_p, "%c", (char)(*((char *)(b + i)))); - i++; - break; - case DL_TYPE_UINT8: - trans->ivy_p += sprintf(trans->ivy_p, "%u", b[i]); - i++; - break; - case DL_TYPE_UINT16: - trans->ivy_p += sprintf(trans->ivy_p, "%u", (uint16_t)(*((uint16_t *)(b + i)))); - i += 2; - break; - case DL_TYPE_UINT32: - case DL_TYPE_TIMESTAMP: - trans->ivy_p += sprintf(trans->ivy_p, "%u", (uint32_t)(*((uint32_t *)(b + i)))); - i += 4; - break; - case DL_TYPE_UINT64: -#if __WORDSIZE == 64 - trans->ivy_p += sprintf(trans->ivy_p, "%lu", (uint64_t)(*((uint64_t *)(b + i)))); -#else - trans->ivy_p += sprintf(trans->ivy_p, "%llu", (uint64_t)(*((uint64_t *)(b + i)))); -#endif - i += 8; - break; - case DL_TYPE_INT8: - trans->ivy_p += sprintf(trans->ivy_p, "%d", (int8_t)(*((int8_t *)(b + i)))); - i++; - break; - case DL_TYPE_INT16: - trans->ivy_p += sprintf(trans->ivy_p, "%d", (int16_t)(*((int16_t *)(b + i)))); - i += 2; - break; - case DL_TYPE_INT32: - trans->ivy_p += sprintf(trans->ivy_p, "%d", (int32_t)(*((int32_t *)(b + i)))); - i += 4; - break; - case DL_TYPE_INT64: -#if __WORDSIZE == 64 - trans->ivy_p += sprintf(trans->ivy_p, "%ld", (uint64_t)(*((uint64_t *)(b + i)))); -#else - trans->ivy_p += sprintf(trans->ivy_p, "%lld", (uint64_t)(*((uint64_t *)(b + i)))); -#endif - i += 8; - break; - case DL_TYPE_FLOAT: - trans->ivy_p += sprintf(trans->ivy_p, "%f", (float)(*((float *)(b + i)))); - i += 4; - break; - case DL_TYPE_DOUBLE: - trans->ivy_p += sprintf(trans->ivy_p, "%f", (double)(*((double *)(b + i)))); - i += 8; - break; - case DL_TYPE_ARRAY_LENGTH: - default: - // Don't print array length but increment index - i++; - break; - } - // Coma delimiter for array, no delimiter for char array (string), space otherwise - if (format == DL_FORMAT_ARRAY) { - if (type != DL_TYPE_CHAR) { - trans->ivy_p += sprintf(trans->ivy_p, ","); - } - } else { - trans->ivy_p += sprintf(trans->ivy_p, " "); - } - } - - // space end delimiter for arrays, additionally un-quote char arrays (strings) - if (format == DL_FORMAT_ARRAY) { - if (type == DL_TYPE_CHAR) { - trans->ivy_p += sprintf(trans->ivy_p, "\" "); - } else { - trans->ivy_p += sprintf(trans->ivy_p, " "); - } - } -} - -static void put_named_byte(struct ivy_transport *trans, struct link_device *dev __attribute__((unused)), - enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), - uint8_t byte __attribute__((unused)), const char *name __attribute__((unused))) -{ - trans->ivy_p += sprintf(trans->ivy_p, "%s ", name); -} - -static uint8_t size_of(struct ivy_transport *trans __attribute__((unused)), uint8_t len) -{ - return len; -} - -static void start_message(struct ivy_transport *trans, struct link_device *dev __attribute__((unused)), - uint8_t payload_len __attribute__((unused))) -{ - trans->ivy_p = trans->ivy_buf; -} - -static void end_message(struct ivy_transport *trans, struct link_device *dev) -{ - *(--trans->ivy_p) = '\0'; - if (trans->ivy_dl_enabled) { - IvySendMsg("%s", trans->ivy_buf); - dev->nb_msgs++; - } -} - -static void overrun(struct ivy_transport *trans __attribute__((unused)), - struct link_device *dev) -{ - dev->nb_ovrn++; -} - -static void count_bytes(struct ivy_transport *trans __attribute__((unused)), - struct link_device *dev, uint8_t bytes) -{ - dev->nb_bytes += bytes; -} - -static int check_available_space(struct ivy_transport *trans __attribute__((unused)), - struct link_device *dev __attribute__((unused)), uint8_t bytes __attribute__((unused))) -{ - return TRUE; -} - -static int check_free_space(struct ivy_transport *p __attribute__((unused)), uint8_t len __attribute__((unused))) { return TRUE; } -static void transmit(struct ivy_transport *p __attribute__((unused)), uint8_t byte __attribute__((unused))) {} -static void send_message(struct ivy_transport *p __attribute__((unused))) {} -static int null_function(struct ivy_transport *p __attribute__((unused))) { return 0; } - -void ivy_transport_init(void) -{ - ivy_tp.ivy_p = ivy_tp.ivy_buf; - ivy_tp.ivy_dl_enabled = TRUE; - - ivy_tp.trans_tx.size_of = (size_of_t) size_of; - ivy_tp.trans_tx.check_available_space = (check_available_space_t) check_available_space; - ivy_tp.trans_tx.put_bytes = (put_bytes_t) put_bytes; - ivy_tp.trans_tx.put_named_byte = (put_named_byte_t) put_named_byte; - ivy_tp.trans_tx.start_message = (start_message_t) start_message; - ivy_tp.trans_tx.end_message = (end_message_t) end_message; - ivy_tp.trans_tx.overrun = (overrun_t) overrun; - ivy_tp.trans_tx.count_bytes = (count_bytes_t) count_bytes; - ivy_tp.trans_tx.impl = (void *)(&ivy_tp); - ivy_tp.device.check_free_space = (check_free_space_t) check_free_space; - ivy_tp.device.put_byte = (put_byte_t) transmit; - ivy_tp.device.send_message = (send_message_t) send_message; - ivy_tp.device.char_available = (char_available_t) null_function; - ivy_tp.device.get_byte = (get_byte_t) null_function; - ivy_tp.device.periph = (void *)(&ivy_tp); -} diff --git a/sw/airborne/subsystems/datalink/ivy_transport.h b/sw/airborne/subsystems/datalink/ivy_transport.h deleted file mode 100644 index cc1dd7be8f..0000000000 --- a/sw/airborne/subsystems/datalink/ivy_transport.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * Copyright (C) 2003 Pascal Brisset, Antoine Drouin - * Copyright (C) 2014 Gautier Hattenberger - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, see - * . - * - */ - -/** - * @file subsystems/datalink/ivy_transport.h - * - * Building Paparazzi frames over IVY. - * - */ - -#ifndef IVY_TRANSPORT_H -#define IVY_TRANSPORT_H - -#include "subsystems/datalink/transport.h" -#include "mcu_periph/link_device.h" - -// IVY transport -struct ivy_transport { - char ivy_buf[256]; - char *ivy_p; - int ivy_dl_enabled; - // generic transmission interface - struct transport_tx trans_tx; - // generic (dummy) device - struct link_device device; -}; - -extern struct ivy_transport ivy_tp; - -// Init function -extern void ivy_transport_init(void); - -#endif // IVY_TRANSPORT_H - diff --git a/sw/airborne/subsystems/datalink/pprz_transport.c b/sw/airborne/subsystems/datalink/pprz_transport.c deleted file mode 100644 index dc6b7f91bc..0000000000 --- a/sw/airborne/subsystems/datalink/pprz_transport.c +++ /dev/null @@ -1,131 +0,0 @@ -/* - * Copyright (C) 2006 Pascal Brisset, Antoine Drouin - * Copyright (C) 2014 Gautier Hattenberger - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, see - * . - * - */ - -/** - * @file subsystems/datalink/pprz_transport.c - * - * Building and parsing Paparazzi frames. - * - * Pprz frame: - * - * |STX|length|... payload=(length-4) bytes ...|Checksum A|Checksum B| - * - * where checksum is computed over length and payload: - * @code - * ck_A = ck_B = length - * for each byte b in payload - * ck_A += b; - * ck_b += ck_A; - * @endcode - */ - -#include -#include "subsystems/datalink/downlink.h" -#ifndef PPRZ_DATALINK_EXPORT -#include "subsystems/datalink/pprz_transport.h" -#else /* PPRZ_DATALINK_EXPORT defined */ -#include "pprz_transport.h" -#endif - -struct pprz_transport pprz_tp; - -static void put_1byte(struct pprz_transport *trans, struct link_device *dev, const uint8_t byte) -{ - trans->ck_a_tx += byte; - trans->ck_b_tx += trans->ck_a_tx; - dev->put_byte(dev->periph, byte); -} - -static void put_bytes(struct pprz_transport *trans, struct link_device *dev, - enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), - uint8_t len, const void *bytes) -{ - const uint8_t *b = (const uint8_t *) bytes; - int i; - for (i = 0; i < len; i++) { - put_1byte(trans, dev, b[i]); - } -} - -static void put_named_byte(struct pprz_transport *trans, struct link_device *dev, - enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), - uint8_t byte, const char *name __attribute__((unused))) -{ - put_1byte(trans, dev, byte); -} - -static uint8_t size_of(struct pprz_transport *trans __attribute__((unused)), uint8_t len) -{ - // message length: payload + protocol overhead (STX + len + ck_a + ck_b = 4) - return len + 4; -} - -static void start_message(struct pprz_transport *trans, struct link_device *dev, uint8_t payload_len) -{ - dev->put_byte(dev->periph, STX); - const uint8_t msg_len = size_of(trans, payload_len); - dev->put_byte(dev->periph, msg_len); - trans->ck_a_tx = msg_len; - trans->ck_b_tx = msg_len; - dev->nb_msgs++; -} - -static void end_message(struct pprz_transport *trans, struct link_device *dev) -{ - dev->put_byte(dev->periph, trans->ck_a_tx); - dev->put_byte(dev->periph, trans->ck_b_tx); - dev->send_message(dev->periph); -} - -static void overrun(struct pprz_transport *trans __attribute__((unused)), - struct link_device *dev) -{ - dev->nb_ovrn++; -} - -static void count_bytes(struct pprz_transport *trans __attribute__((unused)), - struct link_device *dev, uint8_t bytes) -{ - dev->nb_bytes += bytes; -} - -static int check_available_space(struct pprz_transport *trans __attribute__((unused)), struct link_device *dev, - uint8_t bytes) -{ - return dev->check_free_space(dev->periph, bytes); -} - -void pprz_transport_init(struct pprz_transport *t) -{ - t->status = UNINIT; - t->trans_rx.msg_received = FALSE; - t->trans_tx.size_of = (size_of_t) size_of; - t->trans_tx.check_available_space = (check_available_space_t) check_available_space; - t->trans_tx.put_bytes = (put_bytes_t) put_bytes; - t->trans_tx.put_named_byte = (put_named_byte_t) put_named_byte; - t->trans_tx.start_message = (start_message_t) start_message; - t->trans_tx.end_message = (end_message_t) end_message; - t->trans_tx.overrun = (overrun_t) overrun; - t->trans_tx.count_bytes = (count_bytes_t) count_bytes; - t->trans_tx.impl = (void *)(t); -} - diff --git a/sw/airborne/subsystems/datalink/pprz_transport.h b/sw/airborne/subsystems/datalink/pprz_transport.h deleted file mode 100644 index 1ddb9f7724..0000000000 --- a/sw/airborne/subsystems/datalink/pprz_transport.h +++ /dev/null @@ -1,159 +0,0 @@ -/* - * Copyright (C) 2003 Pascal Brisset, Antoine Drouin - * Copyright (C) 2014 Gautier Hattenberger - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, see - * . - * - */ - -/** - * @file subsystems/datalink/pprz_transport.h - * - * Building and parsing Paparazzi frames. - * - * Pprz frame: - * - * |STX|length|... payload=(length-4) bytes ...|Checksum A|Checksum B| - * - * where checksum is computed over length and payload: - * @code - * ck_A = ck_B = length - * for each byte b in payload - * ck_A += b; - * ck_b += ck_A; - * @endcode - */ - -#ifndef PPRZ_TRANSPORT_H -#define PPRZ_TRANSPORT_H - -#include -#include "std.h" -#ifndef PPRZ_DATALINK_EXPORT -#include "subsystems/datalink/datalink.h" -#include "subsystems/datalink/transport.h" -#else /* PPRZ_DATALINK_EXPORT defined */ -#include "datalink.h" -#include "transport.h" -#endif - -/* PPRZ Transport - */ - -#define STX 0x99 - -// PPRZ parsing state machine -#define UNINIT 0 -#define GOT_STX 1 -#define GOT_LENGTH 2 -#define GOT_PAYLOAD 3 -#define GOT_CRC1 4 - -struct pprz_transport { - // generic reception interface - struct transport_rx trans_rx; - // specific pprz transport_rx variables - uint8_t status; - uint8_t payload_idx; - uint8_t ck_a_rx, ck_b_rx; - // generic transmission interface - struct transport_tx trans_tx; - // specific pprz transport_tx variables - uint8_t ck_a_tx, ck_b_tx; -}; - -extern struct pprz_transport pprz_tp; - -// Init function -extern void pprz_transport_init(struct pprz_transport *t); - -static inline void parse_pprz(struct pprz_transport *t, uint8_t c) -{ - switch (t->status) { - case UNINIT: - if (c == STX) { - t->status++; - } - break; - case GOT_STX: - if (t->trans_rx.msg_received) { - t->trans_rx.ovrn++; - goto error; - } - t->trans_rx.payload_len = c - 4; /* Counting STX, LENGTH and CRC1 and CRC2 */ - t->ck_a_rx = t->ck_b_rx = c; - t->status++; - t->payload_idx = 0; - break; - case GOT_LENGTH: - t->trans_rx.payload[t->payload_idx] = c; - t->ck_a_rx += c; t->ck_b_rx += t->ck_a_rx; - t->payload_idx++; - if (t->payload_idx == t->trans_rx.payload_len) { - t->status++; - } - break; - case GOT_PAYLOAD: - if (c != t->ck_a_rx) { - goto error; - } - t->status++; - break; - case GOT_CRC1: - if (c != t->ck_b_rx) { - goto error; - } - t->trans_rx.msg_received = TRUE; - goto restart; - default: - goto error; - } - return; -error: - t->trans_rx.error++; -restart: - t->status = UNINIT; - return; -} - -static inline void pprz_parse_payload(struct pprz_transport *t) -{ - uint8_t i; - for (i = 0; i < t->trans_rx.payload_len; i++) { - dl_buffer[i] = t->trans_rx.payload[i]; - } - dl_msg_available = TRUE; -} - - -#define PprzCheckAndParse(_dev, _trans) pprz_check_and_parse(&(_dev).device, &(_trans)) - -static inline void pprz_check_and_parse(struct link_device *dev, struct pprz_transport *trans) -{ - if (dev->char_available(dev->periph)) { - while (dev->char_available(dev->periph) && !trans->trans_rx.msg_received) { - parse_pprz(trans, dev->get_byte(dev->periph)); - } - if (trans->trans_rx.msg_received) { - pprz_parse_payload(trans); - trans->trans_rx.msg_received = FALSE; - } - } -} - -#endif /* PPRZ_TRANSPORT_H */ - diff --git a/sw/airborne/subsystems/datalink/pprzlog_transport.c b/sw/airborne/subsystems/datalink/pprzlog_transport.c deleted file mode 100644 index 0a47cdff2c..0000000000 --- a/sw/airborne/subsystems/datalink/pprzlog_transport.c +++ /dev/null @@ -1,125 +0,0 @@ -/* - * Copyright (C) 2014 Gautier Hattenberger - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, see - * . - * - */ - -/** - * @file subsystems/datalink/pprzlog_transport.c - * - * Building and Paparazzi frames with timestamp for data logger. - * - * LOG-message: ABCDEFGHxxxxxxxI - * A PPRZ_STX (0x99) - * B LENGTH (H->H) - * C SOURCE (0=uart0, 1=uart1, 2=i2c0, ...) - * D TIMESTAMP_LSB (100 microsec raster) - * E TIMESTAMP - * F TIMESTAMP - * G TIMESTAMP_MSB - * H PPRZ_DATA - * 0 SENDER_ID - * 1 MSG_ID - * 2 MSG_PAYLOAD - * . DATA (messages.xml) - * I CHECKSUM (sum[B->H]) - * - */ - -#include -#include "subsystems/datalink/pprzlog_transport.h" - -struct pprzlog_transport pprzlog_tp; - -#define STX_LOG 0x99 - -static void put_1byte(struct pprzlog_transport *trans, struct link_device *dev, const uint8_t byte) -{ - trans->ck += byte; - dev->put_byte(dev->periph, byte); -} - -static void put_bytes(struct pprzlog_transport *trans, struct link_device *dev, - enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), - uint8_t len, const void *bytes) -{ - const uint8_t *b = (const uint8_t *) bytes; - int i; - for (i = 0; i < len; i++) { - put_1byte(trans, dev, b[i]); - } -} - -static void put_named_byte(struct pprzlog_transport *trans, struct link_device *dev, - enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), - uint8_t byte, const char *name __attribute__((unused))) -{ - put_1byte(trans, dev, byte); -} - -static uint8_t size_of(struct pprzlog_transport *trans __attribute__((unused)), uint8_t len) -{ - return len; -} - -static void start_message(struct pprzlog_transport *trans, struct link_device *dev, uint8_t payload_len) -{ - dev->put_byte(dev->periph, STX_LOG); - const uint8_t msg_len = size_of(trans, payload_len); - trans->ck = 0; - put_1byte(trans, dev, msg_len); - put_1byte(trans, dev, 0); // TODO use correct source ID - uint32_t ts = get_sys_time_usec() / 100; - put_bytes(trans, dev, DL_TYPE_TIMESTAMP, DL_FORMAT_SCALAR, 4, (uint8_t *)(&ts)); -} - -static void end_message(struct pprzlog_transport *trans, struct link_device *dev) -{ - dev->put_byte(dev->periph, trans->ck); - dev->send_message(dev->periph); -} - -static void overrun(struct pprzlog_transport *trans __attribute__((unused)), - struct link_device *dev __attribute__((unused))) -{ -} - -static void count_bytes(struct pprzlog_transport *trans __attribute__((unused)), - struct link_device *dev __attribute__((unused)), uint8_t bytes __attribute__((unused))) -{ -} - -static int check_available_space(struct pprzlog_transport *trans __attribute__((unused)), struct link_device *dev, - uint8_t bytes) -{ - return dev->check_free_space(dev->periph, bytes); -} - -void pprzlog_transport_init(void) -{ - pprzlog_tp.trans_tx.size_of = (size_of_t) size_of; - pprzlog_tp.trans_tx.check_available_space = (check_available_space_t) check_available_space; - pprzlog_tp.trans_tx.put_bytes = (put_bytes_t) put_bytes; - pprzlog_tp.trans_tx.put_named_byte = (put_named_byte_t) put_named_byte; - pprzlog_tp.trans_tx.start_message = (start_message_t) start_message; - pprzlog_tp.trans_tx.end_message = (end_message_t) end_message; - pprzlog_tp.trans_tx.overrun = (overrun_t) overrun; - pprzlog_tp.trans_tx.count_bytes = (count_bytes_t) count_bytes; - pprzlog_tp.trans_tx.impl = (void *)(&pprzlog_tp); -} - diff --git a/sw/airborne/subsystems/datalink/pprzlog_transport.h b/sw/airborne/subsystems/datalink/pprzlog_transport.h deleted file mode 100644 index 31e85c1328..0000000000 --- a/sw/airborne/subsystems/datalink/pprzlog_transport.h +++ /dev/null @@ -1,48 +0,0 @@ -/* - * Copyright (C) 2014 Gautier Hattenberger - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, see - * . - * - */ - -/** - * @file subsystems/datalink/pprzlog_transport.h - * - * Protocol for on-board data logger with timestamp - * - */ - -#ifndef PPRZLOG_TRANSPORT_H -#define PPRZLOG_TRANSPORT_H - -#include "mcu_periph/sys_time.h" -#include "subsystems/datalink/transport.h" - -struct pprzlog_transport { - // generic transmission interface - struct transport_tx trans_tx; - // specific pprz transport_tx variables - uint8_t ck; -}; - -extern struct pprzlog_transport pprzlog_tp; - -// Init function -extern void pprzlog_transport_init(void); - -#endif - diff --git a/sw/airborne/subsystems/datalink/superbitrf.c b/sw/airborne/subsystems/datalink/superbitrf.c index e44d9c49d1..f994a3b748 100644 --- a/sw/airborne/subsystems/datalink/superbitrf.c +++ b/sw/airborne/subsystems/datalink/superbitrf.c @@ -863,7 +863,7 @@ static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, ui // When we have a full message if (superbitrf.rx_transport.trans_rx.msg_received) { - pprz_parse_payload(&superbitrf.rx_transport); + DatalinkFillDlBuffer(superbitrf.rx_transport.trans_rx.payload, superbitrf.rx_transport.trans_rx.payload_len); superbitrf.rx_transport.trans_rx.msg_received = FALSE; } } @@ -923,7 +923,7 @@ static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, ui // When we have a full message if (superbitrf.rx_transport.trans_rx.msg_received) { - pprz_parse_payload(&superbitrf.rx_transport); + DatalinkFillDlBuffer(superbitrf.rx_transport.trans_rx.payload, superbitrf.rx_transport.trans_rx.payload_len); superbitrf.rx_transport.trans_rx.msg_received = FALSE; } } @@ -1009,7 +1009,7 @@ static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, ui // When we have a full message if (superbitrf.rx_transport.trans_rx.msg_received) { - pprz_parse_payload(&superbitrf.rx_transport); + DatalinkFillDlBuffer(superbitrf.rx_transport.trans_rx.payload, superbitrf.rx_transport.trans_rx.payload_len); superbitrf.rx_transport.trans_rx.msg_received = FALSE; } } diff --git a/sw/airborne/subsystems/datalink/superbitrf.h b/sw/airborne/subsystems/datalink/superbitrf.h index fd99e325a4..9d194a6497 100644 --- a/sw/airborne/subsystems/datalink/superbitrf.h +++ b/sw/airborne/subsystems/datalink/superbitrf.h @@ -29,9 +29,9 @@ #include "mcu_periph/gpio.h" #include "peripherals/cyrf6936.h" -#include "mcu_periph/link_device.h" +#include "pprzlink/pprzlink_device.h" #include "subsystems/datalink/datalink.h" -#include "subsystems/datalink/pprz_transport.h" +#include "pprzlink/pprz_transport.h" /* The timings in microseconds */ #define SUPERBITRF_BIND_RECV_TIME 10000 /**< The time to wait for a bind packet on a channel in microseconds */ diff --git a/sw/airborne/subsystems/datalink/telemetry_common.h b/sw/airborne/subsystems/datalink/telemetry_common.h index ce0d1aba1a..3b6a01189e 100644 --- a/sw/airborne/subsystems/datalink/telemetry_common.h +++ b/sw/airborne/subsystems/datalink/telemetry_common.h @@ -31,9 +31,9 @@ #include #include "std.h" -#include "mcu_periph/link_device.h" -#include "subsystems/datalink/transport.h" -#include "messages.h" +#include "pprzlink/pprzlink_device.h" +#include "pprzlink/pprzlink_transport.h" +#include "pprzlink/messages.h" /** Telemetry callback definition */ diff --git a/sw/airborne/subsystems/datalink/transport.h b/sw/airborne/subsystems/datalink/transport.h deleted file mode 100644 index a1701954a2..0000000000 --- a/sw/airborne/subsystems/datalink/transport.h +++ /dev/null @@ -1,102 +0,0 @@ -/* - * Copyright (C) 2011-2014 Gautier Hattenberger - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, see - * . - * - */ - -/** \file subsystems/datalink/transport.h - * generic transport header - */ - -#ifndef TRANSPORT_H -#define TRANSPORT_H - -#include -#include "mcu_periph/link_device.h" -#include "std.h" - -#ifndef TRANSPORT_PAYLOAD_LEN -#define TRANSPORT_PAYLOAD_LEN 256 -#endif - -/** Generic reception transport header - */ -struct transport_rx { - uint8_t payload[TRANSPORT_PAYLOAD_LEN]; ///< payload buffer - volatile uint8_t payload_len; ///< payload buffer length - volatile bool_t msg_received; ///< message received flag - uint8_t ovrn, error; ///< overrun and error flags -}; - -/** Data type - */ -enum TransportDataType { - DL_TYPE_ARRAY_LENGTH, - DL_TYPE_CHAR, - DL_TYPE_UINT8, - DL_TYPE_INT8, - DL_TYPE_UINT16, - DL_TYPE_INT16, - DL_TYPE_UINT32, - DL_TYPE_INT32, - DL_TYPE_UINT64, - DL_TYPE_INT64, - DL_TYPE_FLOAT, - DL_TYPE_DOUBLE, - DL_TYPE_TIMESTAMP -}; - -/** Data format (scalar or array) - */ -enum TransportDataFormat { - DL_FORMAT_SCALAR, - DL_FORMAT_ARRAY -}; - -/** Function pointers definition - * - * they are used to cast the real functions with the correct type - * to store in the transport structure - */ -typedef uint8_t (*size_of_t)(void *, uint8_t); -typedef int (*check_available_space_t)(void *, struct link_device *, uint8_t); -typedef void (*put_bytes_t)(void *, struct link_device *, enum TransportDataType, enum TransportDataFormat, uint8_t, - const void *); -typedef void (*put_named_byte_t)(void *, struct link_device *, enum TransportDataType, enum TransportDataFormat, - uint8_t, const char *); -typedef void (*start_message_t)(void *, struct link_device *, uint8_t); -typedef void (*end_message_t)(void *, struct link_device *); -typedef void (*overrun_t)(void *, struct link_device *); -typedef void (*count_bytes_t)(void *, struct link_device *, uint8_t); - -/** Generic transmission transport header - */ -struct transport_tx { - size_of_t size_of; ///< get size of payload with transport header and trailer - check_available_space_t check_available_space; ///< check if transmit buffer is not full - put_bytes_t put_bytes; ///< send bytes - put_named_byte_t put_named_byte; ///< send a single byte or its name - start_message_t start_message; ///< transport header - end_message_t end_message; ///< transport trailer - overrun_t overrun; ///< overrun - count_bytes_t count_bytes; ///< count bytes to send - void *impl; ///< pointer to parent implementation -}; - -#endif /* TRANSPORT_H */ - diff --git a/sw/airborne/subsystems/datalink/uart_print.h b/sw/airborne/subsystems/datalink/uart_print.h index 97f16513dd..23ba84823f 100644 --- a/sw/airborne/subsystems/datalink/uart_print.h +++ b/sw/airborne/subsystems/datalink/uart_print.h @@ -25,7 +25,7 @@ #include "mcu_periph/uart.h" #include "mcu_periph/usb_serial.h" -#include "mcu_periph/link_device.h" +#include "pprzlink/pprzlink_device.h" #define _PrintString(out_fun, s) { \ uint8_t i = 0; \ diff --git a/sw/airborne/subsystems/datalink/w5100.h b/sw/airborne/subsystems/datalink/w5100.h index e5b3cbc87d..33cb150a13 100644 --- a/sw/airborne/subsystems/datalink/w5100.h +++ b/sw/airborne/subsystems/datalink/w5100.h @@ -28,7 +28,8 @@ #ifndef W5100_H #define W5100_H -#include "mcu_periph/link_device.h" +#include "pprzlink/pprzlink_device.h" +#include "subsystems/datalink/datalink.h" #include "generated/airframe.h" #define W5100_RX_BUFFER_SIZE 80 @@ -74,7 +75,7 @@ bool_t w5100_ch_available(void); // W5100 is using pprz_transport // FIXME it should not appear here, this will be fixed with the rx improvements some day... // W5100 needs a specific read_buffer function -#include "subsystems/datalink/pprz_transport.h" +#include "pprzlink/pprz_transport.h" static inline void w5100_read_buffer(struct pprz_transport *t) { @@ -93,7 +94,7 @@ static inline void w5100_check_and_parse(struct link_device *dev, struct pprz_tr if (dev->char_available(dev->periph)) { w5100_read_buffer(trans); if (trans->trans_rx.msg_received) { - pprz_parse_payload(trans); + DatalinkFillDlBuffer(trans->trans_rx.payload, trans->trans_rx.payload_len); trans->trans_rx.msg_received = FALSE; } } diff --git a/sw/airborne/subsystems/datalink/xbee.c b/sw/airborne/subsystems/datalink/xbee.c deleted file mode 100644 index 66aae3e0d4..0000000000 --- a/sw/airborne/subsystems/datalink/xbee.c +++ /dev/null @@ -1,230 +0,0 @@ -/* - * Copyright (C) 2006 Pascal Brisset, Antoine Drouin - * Copyright (C) 2014 Gautier Hattenberger - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, see - * . - * - */ - -/** - * @file subsystems/datalink/xbee.c - * Maxstream XBee serial input and output - */ - -#include "mcu_periph/sys_time.h" -#include "subsystems/datalink/uart_print.h" -#include "subsystems/datalink/xbee.h" -#include "subsystems/datalink/downlink.h" - -/** Ground station address */ -#define GROUND_STATION_ADDR 0x100 -/** Aircraft address */ -#define XBEE_MY_ADDR AC_ID - -/** Constants for the API protocol */ -#define TX_OPTIONS 0x00 -#define NO_FRAME_ID 0 -#define XBEE_API_OVERHEAD 5 /* start + len_msb + len_lsb + API_id + checksum */ - -struct xbee_transport xbee_tp; - -#define AT_COMMAND_SEQUENCE "+++" -#define AT_SET_MY "ATMY" -#define AT_AP_MODE "ATAP1\r" -#define AT_EXIT "ATCN\r" - -/** Xbee protocol implementation */ - -static void put_1byte(struct xbee_transport *trans, struct link_device *dev, const uint8_t byte) -{ - trans->cs_tx += byte; - dev->put_byte(dev->periph, byte); -} - -static void put_bytes(struct xbee_transport *trans, struct link_device *dev, - enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), - uint8_t len, const void *bytes) -{ - const uint8_t *b = (const uint8_t *) bytes; - int i; - for (i = 0; i < len; i++) { - put_1byte(trans, dev, b[i]); - } -} - -static void put_named_byte(struct xbee_transport *trans, struct link_device *dev, - enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), - uint8_t byte, const char *name __attribute__((unused))) -{ - put_1byte(trans, dev, byte); -} - -static uint8_t size_of(struct xbee_transport *trans __attribute__((unused)), uint8_t len) -{ - // message length: payload + API overhead + XBEE TX overhead (868 or 2.4) - return len + XBEE_API_OVERHEAD + XBEE_TX_OVERHEAD; -} - -static void start_message(struct xbee_transport *trans, struct link_device *dev, uint8_t payload_len) -{ - dev->put_byte(dev->periph, XBEE_START); - const uint16_t len = payload_len + XBEE_API_OVERHEAD; - dev->put_byte(dev->periph, (len >> 8)); - dev->put_byte(dev->periph, (len & 0xff)); - trans->cs_tx = 0; - const uint8_t header[] = XBEE_TX_HEADER; - put_bytes(trans, dev, DL_TYPE_UINT8, DL_FORMAT_SCALAR, XBEE_TX_OVERHEAD + 1, header); - dev->nb_msgs++; -} - -static void end_message(struct xbee_transport *trans, struct link_device *dev) -{ - trans->cs_tx = 0xff - trans->cs_tx; - dev->put_byte(dev->periph, trans->cs_tx); - dev->send_message(dev->periph); -} - -static void overrun(struct xbee_transport *trans __attribute__((unused)), - struct link_device *dev) -{ - dev->nb_ovrn++; -} - -static void count_bytes(struct xbee_transport *trans __attribute__((unused)), - struct link_device *dev, uint8_t bytes) -{ - dev->nb_bytes += bytes; -} - -static int check_available_space(struct xbee_transport *trans __attribute__((unused)), struct link_device *dev, - uint8_t bytes) -{ - return dev->check_free_space(dev->periph, bytes); -} - -static uint8_t xbee_text_reply_is_ok(struct link_device *dev) -{ - char c[2]; - int count = 0; - - while (dev->char_available(dev->periph)) { - char cc = dev->get_byte(dev->periph); - if (count < 2) { - c[count] = cc; - } - count++; - } - - if ((count > 2) && (c[0] == 'O') && (c[1] == 'K')) { - return TRUE; - } - - return FALSE; -} - -static uint8_t xbee_try_to_enter_api(struct link_device *dev) -{ - - /** Switching to AT mode (FIXME: busy waiting) */ - print_string(dev, AT_COMMAND_SEQUENCE); - - /** - busy wait 1.25s */ - sys_time_usleep(1250000); - - return xbee_text_reply_is_ok(dev); -} - - -#if XBEE_BAUD == B9600 -#define XBEE_BAUD_ALTERNATE B57600 -#define XBEE_ATBD_CODE "ATBD3\rATWR\r" -#pragma message "Experimental: XBEE-API@9k6 auto-baudrate 57k6 -> 9k6 (stop ground link for correct operation)" -#elif XBEE_BAUD == B57600 -#define XBEE_BAUD_ALTERNATE B9600 -#define XBEE_ATBD_CODE "ATBD6\rATWR\r" -#pragma message "Experimental: XBEE-API@57k6 auto-baudrate 9k6 -> 57k6 (stop ground link for correct operation)" -#else -#pragma message "XBEE-API Non default baudrate: auto-baud disabled." -#endif - - -void xbee_init(void) -{ - xbee_tp.status = XBEE_UNINIT; - xbee_tp.trans_rx.msg_received = FALSE; - xbee_tp.trans_tx.size_of = (size_of_t) size_of; - xbee_tp.trans_tx.check_available_space = (check_available_space_t) check_available_space; - xbee_tp.trans_tx.put_bytes = (put_bytes_t) put_bytes; - xbee_tp.trans_tx.put_named_byte = (put_named_byte_t) put_named_byte; - xbee_tp.trans_tx.start_message = (start_message_t) start_message; - xbee_tp.trans_tx.end_message = (end_message_t) end_message; - xbee_tp.trans_tx.overrun = (overrun_t) overrun; - xbee_tp.trans_tx.count_bytes = (count_bytes_t) count_bytes; - xbee_tp.trans_tx.impl = (void *)(&xbee_tp); - - struct link_device *dev = &((XBEE_UART).device); - - // Empty buffer before init process - while (dev->char_available(dev->periph)) { - dev->get_byte(dev->periph); - } - -#ifndef NO_XBEE_API_INIT - /** - busy wait 1.25s */ - sys_time_usleep(1250000); - - if (! xbee_try_to_enter_api(dev)) { -#ifdef XBEE_BAUD_ALTERNATE - - // Badly configured... try the alternate baudrate: - uart_periph_set_baudrate(&(XBEE_UART), - XBEE_BAUD_ALTERNATE); // FIXME add set_baudrate to generic device, assuming uart for now - if (xbee_try_to_enter_api(dev)) { - // The alternate baudrate worked, - print_string(dev, XBEE_ATBD_CODE); - } else { - // Complete failure, none of the 2 baudrates result in any reply - // TODO: set LED? - - // Set the default baudrate, just in case everything is right - uart_periph_set_baudrate(&(XBEE_UART), XBEE_BAUD); // FIXME add set_baudrate to generic device, assuming uart for now - print_string(dev, "\r"); - } - -#endif - // Continue changing settings until the EXIT is issued. - } - - /** Setting my address */ - print_string(dev, AT_SET_MY); - uint16_t addr = XBEE_MY_ADDR; - print_hex16(dev, addr); - print_string(dev, "\r"); - - print_string(dev, AT_AP_MODE); - -#ifdef XBEE_INIT - print_string(dev, XBEE_INIT); -#endif - - /** Switching back to normal mode */ - print_string(dev, AT_EXIT); - - uart_periph_set_baudrate(&(XBEE_UART), XBEE_BAUD); // FIXME add set_baudrate to generic device, assuming uart for now - -#endif -} diff --git a/sw/airborne/subsystems/datalink/xbee.h b/sw/airborne/subsystems/datalink/xbee.h deleted file mode 100644 index 7b35568b40..0000000000 --- a/sw/airborne/subsystems/datalink/xbee.h +++ /dev/null @@ -1,152 +0,0 @@ -/* - * Copyright (C) 2006 Pascal Brisset, Antoine Drouin - * Copyright (C) 2014 Gautier Hattenberger - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, see - * . - * - */ - -/** - * @file subsystems/datalink/xbee.h - * Maxstream XBee serial input and output - */ - -#ifndef XBEE_H -#define XBEE_H - -#include "subsystems/datalink/datalink.h" -#include "generated/airframe.h" -#include "subsystems/datalink/transport.h" - -#ifdef XBEE868 -#include "subsystems/datalink/xbee868.h" -#else /* Not 868 */ -#include "subsystems/datalink/xbee24.h" -#endif - -#define XBEE_START 0x7e - -/** Initialisation in API mode and setting of the local address - * FIXME: busy wait */ -void xbee_init(void); - -/** Status of the API packet receiver automata */ -#define XBEE_UNINIT 0 -#define XBEE_GOT_START 1 -#define XBEE_GOT_LENGTH_MSB 2 -#define XBEE_GOT_LENGTH_LSB 3 -#define XBEE_GOT_PAYLOAD 4 - -struct xbee_transport { - // generic reception interface - struct transport_rx trans_rx; - // specific xbee transport variables - uint8_t status; - uint8_t payload_idx; - uint8_t cs_rx; - uint8_t rssi; - // generic transmission interface - struct transport_tx trans_tx; - // specific pprz transport_tx variables - uint8_t cs_tx; -}; - -extern struct xbee_transport xbee_tp; - -/** Parsing a XBee API frame */ -static inline void parse_xbee(struct xbee_transport *t, uint8_t c) -{ - switch (t->status) { - case XBEE_UNINIT: - if (c == XBEE_START) { - t->status++; - } - break; - case XBEE_GOT_START: - if (t->trans_rx.msg_received) { - t->trans_rx.ovrn++; - goto error; - } - t->trans_rx.payload_len = c << 8; - t->status++; - break; - case XBEE_GOT_LENGTH_MSB: - t->trans_rx.payload_len |= c; - t->status++; - t->payload_idx = 0; - t->cs_rx = 0; - break; - case XBEE_GOT_LENGTH_LSB: - t->trans_rx.payload[t->payload_idx] = c; - t->cs_rx += c; - t->payload_idx++; - if (t->payload_idx == t->trans_rx.payload_len) { - t->status++; - } - break; - case XBEE_GOT_PAYLOAD: - if (c + t->cs_rx != 0xff) { - goto error; - } - t->trans_rx.msg_received = TRUE; - goto restart; - break; - default: - goto error; - } - return; -error: - t->trans_rx.error++; -restart: - t->status = XBEE_UNINIT; - return; -} - -/** Parsing a frame data and copy the payload to the datalink buffer */ -static inline void xbee_parse_payload(struct xbee_transport *t) -{ - switch (t->trans_rx.payload[0]) { - case XBEE_RX_ID: - case XBEE_TX_ID: /* Useful if A/C is connected to the PC with a cable */ - XbeeGetRSSI(t->trans_rx.payload); - uint8_t i; - for (i = XBEE_RFDATA_OFFSET; i < t->trans_rx.payload_len; i++) { - dl_buffer[i - XBEE_RFDATA_OFFSET] = t->trans_rx.payload[i]; - } - dl_msg_available = TRUE; - break; - default: - return; - } -} - -#define XBeeCheckAndParse(_dev, _trans) xbee_check_and_parse(&(_dev).device, &(_trans)) - -static inline void xbee_check_and_parse(struct link_device *dev, struct xbee_transport *trans) -{ - if (dev->char_available(dev->periph)) { - while (dev->char_available(dev->periph) && !trans->trans_rx.msg_received) { - parse_xbee(trans, dev->get_byte(dev->periph)); - } - if (trans->trans_rx.msg_received) { - xbee_parse_payload(trans); - trans->trans_rx.msg_received = FALSE; - } - } -} - -#endif /* XBEE_H */ diff --git a/sw/airborne/subsystems/datalink/xbee24.h b/sw/airborne/subsystems/datalink/xbee24.h deleted file mode 100644 index dbe947f5b6..0000000000 --- a/sw/airborne/subsystems/datalink/xbee24.h +++ /dev/null @@ -1,46 +0,0 @@ -/* - * Copyright (C) 2009 ENAC, Pascal Brisset - * Copyright (C) 2014 Gautier Hattenberger - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, see - * . - * - */ - -/** - * @file subsystems/datalink/xbee24.h - * Configuration for 2.4GHz "series 1" and 900MHz modules - */ - -#ifndef XBEE24_H -#define XBEE24_H - -#define XBEE_TX_ID 0x01 /* 16 bits address */ -#define XBEE_RX_ID 0x81 /* 16 bits address */ -#define XBEE_RFDATA_OFFSET 5 - -#define XBEE_TX_OVERHEAD 4 -#define XBEE_TX_HEADER { \ - XBEE_TX_ID, \ - NO_FRAME_ID, \ - (GROUND_STATION_ADDR >> 8), \ - (GROUND_STATION_ADDR & 0xff), \ - TX_OPTIONS \ - } - -#define XbeeGetRSSI(_payload) { xbee_tp.rssi = _payload[3]; } - -#endif // XBEE24_H diff --git a/sw/airborne/subsystems/datalink/xbee868.h b/sw/airborne/subsystems/datalink/xbee868.h deleted file mode 100644 index d77ef559c0..0000000000 --- a/sw/airborne/subsystems/datalink/xbee868.h +++ /dev/null @@ -1,55 +0,0 @@ -/* - * Copyright (C) 2009 ENAC, Pascal Brisset - * Copyright (C) 2014 Gautier Hattenberger - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, see - * . - * - */ - -/** - * @file subsystems/datalink/xbee868.h - * Configuration for 868MHz modules - */ - -#ifndef XBEE868_H -#define XBEE868_H - -#define XBEE_TX_ID 0x10 -#define XBEE_RX_ID 0x90 -#define XBEE_RFDATA_OFFSET 12 - -#define XBEE_TX_OVERHEAD 13 -#define XBEE_TX_HEADER { \ - XBEE_TX_ID, \ - NO_FRAME_ID, \ - 0x00, \ - 0x00, \ - 0x00, \ - 0x00, \ - 0x00, \ - 0x00, \ - (GROUND_STATION_ADDR >> 8), \ - (GROUND_STATION_ADDR & 0xff), \ - 0xff, \ - 0xfe, \ - 0x00, \ - TX_OPTIONS \ - } - -#define XbeeGetRSSI(_payload) {} - -#endif // XBEE868_H diff --git a/sw/airborne/subsystems/gps/gps_datalink.c b/sw/airborne/subsystems/gps/gps_datalink.c index 1b6f4c3545..788a3569c8 100644 --- a/sw/airborne/subsystems/gps/gps_datalink.c +++ b/sw/airborne/subsystems/gps/gps_datalink.c @@ -27,35 +27,16 @@ * GPS structure to the values received. */ +#include "generated/flight_plan.h" // reference lla NAV_XXX0 + #include "subsystems/gps.h" #include "subsystems/abi.h" -// #include - -#if GPS_USE_DATALINK_SMALL -#ifndef GPS_LOCAL_ECEF_ORIGIN_X -#error Local x coordinate in ECEF of the remote GPS required -#endif - -#ifndef GPS_LOCAL_ECEF_ORIGIN_Y -#error Local y coordinate in ECEF of the remote GPS required -#endif - -#ifndef GPS_LOCAL_ECEF_ORIGIN_Z -#error Local z coordinate in ECEF of the remote GPS required -#endif - -struct EcefCoor_i tracking_ecef; - -struct LtpDef_i tracking_ltp; +// #include +struct LtpDef_i ltp_def; struct EnuCoor_i enu_pos, enu_speed; -struct EcefCoor_i ecef_pos, ecef_vel; - -struct LlaCoor_i lla_pos; -#endif - bool_t gps_available; ///< Is set to TRUE when a new REMOTE_GPS packet is received and parsed /** GPS initialization */ @@ -66,70 +47,67 @@ void gps_impl_init(void) gps.gspeed = 700; // To enable course setting gps.cacc = 0; // To enable course setting -#if GPS_USE_DATALINK_SMALL - tracking_ecef.x = GPS_LOCAL_ECEF_ORIGIN_X; - tracking_ecef.y = GPS_LOCAL_ECEF_ORIGIN_Y; - tracking_ecef.z = GPS_LOCAL_ECEF_ORIGIN_Z; + struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */ + llh_nav0.lat = NAV_LAT0; + llh_nav0.lon = NAV_LON0; + /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */ + llh_nav0.alt = NAV_ALT0 + NAV_MSL0; - ltp_def_from_ecef_i(&tracking_ltp, &tracking_ecef); -#endif + struct EcefCoor_i ecef_nav0; + ecef_of_lla_i(&ecef_nav0, &llh_nav0); + ltp_def_from_ecef_i(<p_def, &ecef_nav0); } -#ifdef GPS_USE_DATALINK_SMALL // Parse the REMOTE_GPS_SMALL datalink packet -void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xy) +void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xyz, int16_t heading) { - // Position in ENU coordinates - enu_pos.x = (int32_t)((pos_xyz >> 22) & 0x3FF); // bits 31-22 x position in cm - if (enu_pos.x & 0x200) { - enu_pos.x |= 0xFFFFFC00; // fix for twos complements + enu_pos.x = (int32_t)((pos_xyz >> 21) & 0x7FF); // bits 31-21 x position in cm + if (enu_pos.x & 0x400) { + enu_pos.x |= 0xFFFFF800; // sign extend for twos complements } - enu_pos.y = (int32_t)((pos_xyz >> 12) & 0x3FF); // bits 21-12 y position in cm - if (enu_pos.y & 0x200) { - enu_pos.y |= 0xFFFFFC00; // fix for twos complements + enu_pos.y = (int32_t)((pos_xyz >> 10) & 0x7FF); // bits 20-10 y position in cm + if (enu_pos.y & 0x400) { + enu_pos.y |= 0xFFFFF800; // sign extend for twos complements } - enu_pos.z = (int32_t)(pos_xyz >> 2 & 0x3FF); // bits 11-2 z position in cm - // bits 1 and 0 are free - - // printf("ENU Pos: %u (%d, %d, %d)\n", pos_xyz, enu_pos.x, enu_pos.y, enu_pos.z); + enu_pos.z = (int32_t)(pos_xyz & 0x3FF); // bits 9-0 z position in cm // Convert the ENU coordinates to ECEF - ecef_of_enu_point_i(&ecef_pos, &tracking_ltp, &enu_pos); - gps.ecef_pos = ecef_pos; + ecef_of_enu_point_i(&gps.ecef_pos, <p_def, &enu_pos); + SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT); - lla_of_ecef_i(&lla_pos, &ecef_pos); - gps.lla_pos = lla_pos; + lla_of_ecef_i(&gps.lla_pos, &gps.ecef_pos); SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); - enu_speed.x = (int32_t)((speed_xy >> 22) & 0x3FF); // bits 31-22 speed x in cm/s - if (enu_speed.x & 0x200) { - enu_speed.x |= 0xFFFFFC00; // fix for twos complements + enu_speed.x = (int32_t)((speed_xyz >> 21) & 0x7FF); // bits 31-21 speed x in cm/s + if (enu_speed.x & 0x400) { + enu_speed.x |= 0xFFFFF800; // sign extend for twos complements } - enu_speed.y = (int32_t)((speed_xy >> 12) & 0x3FF); // bits 21-12 speed y in cm/s - if (enu_speed.y & 0x200) { - enu_speed.y |= 0xFFFFFC00; // fix for twos complements + enu_speed.y = (int32_t)((speed_xyz >> 10) & 0x7FF); // bits 20-10 speed y in cm/s + if (enu_speed.y & 0x400) { + enu_speed.y |= 0xFFFFF800; // sign extend for twos complements + } + enu_speed.z = (int32_t)((speed_xyz) & 0x3FF); // bits 9-0 speed z in cm/s + if (enu_speed.z & 0x200) { + enu_speed.z |= 0xFFFFFC00; // sign extend for twos complements } - enu_speed.z = 0; - // printf("ENU Speed: %u (%d, %d, %d)\n", speed_xy, enu_speed.x, enu_speed.y, enu_speed.z); - - ecef_of_enu_vect_i(&gps.ecef_vel , &tracking_ltp , &enu_speed); + ecef_of_enu_vect_i(&gps.ecef_vel , <p_def , &enu_speed); SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT); - gps.hmsl = tracking_ltp.hmsl + enu_pos.z * 10; // TODO: try to compensate for the loss in accuracy + gps.ned_vel.x = enu_speed.y; + gps.ned_vel.y = enu_speed.x; + gps.ned_vel.z = -enu_speed.z; + SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT); + + gps.hmsl = ltp_def.hmsl + enu_pos.z * 10; SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); - gps.course = (int32_t)((speed_xy >> 2) & 0x3FF); // bits 11-2 heading in rad*1e2 - if (gps.course & 0x200) { - gps.course |= 0xFFFFFC00; // fix for twos complements - } - // printf("Heading: %d\n", gps.course); - gps.course *= 1e5; + gps.course = ((int32_t)heading) * 1e3; SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); gps.num_sv = num_sv; - gps.tow = 0; // set time-of-weak to 0 + gps.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); gps.fix = GPS_FIX_3D; // set 3D fix to true gps_available = TRUE; // set GPS available to true @@ -143,7 +121,6 @@ void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_x } AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps); } -#endif /** 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, @@ -168,10 +145,20 @@ void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t e gps.ecef_vel.z = ecef_zd; SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT); + gps.ned_vel.x = enu_speed.y; + gps.ned_vel.y = enu_speed.x; + gps.ned_vel.z = -enu_speed.z; + SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT); + gps.course = course; SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); + gps.num_sv = numsv; - gps.tow = tow; + if (tow == 0) { + gps.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); //tow; + } else { + gps.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); //tow; + } gps.fix = GPS_FIX_3D; gps_available = TRUE; diff --git a/sw/airborne/subsystems/gps/gps_datalink.h b/sw/airborne/subsystems/gps/gps_datalink.h index 504ba49f9c..3d82d80277 100644 --- a/sw/airborne/subsystems/gps/gps_datalink.h +++ b/sw/airborne/subsystems/gps/gps_datalink.h @@ -35,9 +35,9 @@ #define GPS_NB_CHANNELS 0 extern bool_t gps_available; -#ifdef GPS_USE_DATALINK_SMALL -void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xy); -#endif + +extern void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xyz, int16_t heading); + 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, diff --git a/sw/airborne/subsystems/gps/gps_mtk.c b/sw/airborne/subsystems/gps/gps_mtk.c index 1ff03545d6..d5b35dc5bf 100644 --- a/sw/airborne/subsystems/gps/gps_mtk.c +++ b/sw/airborne/subsystems/gps/gps_mtk.c @@ -397,7 +397,7 @@ restart: */ #ifdef GPS_CONFIGURE -#include "mcu_periph/link_device.h" +#include "pprzlink/pprzlink_device.h" static void MtkSend_CFG(char *dat) { diff --git a/sw/airborne/subsystems/gps/gps_mtk.h b/sw/airborne/subsystems/gps/gps_mtk.h index dd78486d42..26163c94b4 100644 --- a/sw/airborne/subsystems/gps/gps_mtk.h +++ b/sw/airborne/subsystems/gps/gps_mtk.h @@ -54,7 +54,7 @@ extern struct GpsMtk gps_mtk; /* * This part is used by the autopilot to read data from a uart */ -#include "mcu_periph/link_device.h" +#include "pprzlink/pprzlink_device.h" #ifdef GPS_CONFIGURE extern bool_t gps_configuring; diff --git a/sw/airborne/subsystems/gps/gps_nmea.h b/sw/airborne/subsystems/gps/gps_nmea.h index 34426d3b70..9e9003a558 100644 --- a/sw/airborne/subsystems/gps/gps_nmea.h +++ b/sw/airborne/subsystems/gps/gps_nmea.h @@ -55,7 +55,7 @@ extern struct GpsNmea gps_nmea; */ /** The function to be called when a characted from the device is available */ -#include "mcu_periph/link_device.h" +#include "pprzlink/pprzlink_device.h" extern void nmea_configure(void); extern void nmea_parse_char(uint8_t c); diff --git a/sw/airborne/subsystems/gps/gps_sirf.h b/sw/airborne/subsystems/gps/gps_sirf.h index 575ff5fd7a..d8929120e9 100644 --- a/sw/airborne/subsystems/gps/gps_sirf.h +++ b/sw/airborne/subsystems/gps/gps_sirf.h @@ -127,7 +127,7 @@ struct sirf_msg_41 { /* * This part is used by the autopilot to read data from a uart */ -#include "mcu_periph/link_device.h" +#include "pprzlink/pprzlink_device.h" #include "mcu_periph/uart.h" extern void sirf_parse_char(uint8_t c); diff --git a/sw/airborne/subsystems/gps/gps_skytraq.h b/sw/airborne/subsystems/gps/gps_skytraq.h index 5895cf58e4..8e51e2da13 100644 --- a/sw/airborne/subsystems/gps/gps_skytraq.h +++ b/sw/airborne/subsystems/gps/gps_skytraq.h @@ -58,7 +58,7 @@ extern struct GpsSkytraq gps_skytraq; /* * This part is used by the autopilot to read data from a uart */ -#include "mcu_periph/link_device.h" +#include "pprzlink/pprzlink_device.h" extern void gps_skytraq_read_message(void); extern void gps_skytraq_parse(uint8_t c); diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h index 8be5296f00..27cc9a8ea2 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.h +++ b/sw/airborne/subsystems/gps/gps_ubx.h @@ -80,7 +80,7 @@ extern struct GpsUbxRaw gps_ubx_raw; /* * This part is used by the autopilot to read data from a uart */ -#include "mcu_periph/link_device.h" +#include "pprzlink/pprzlink_device.h" extern void ubx_header(struct link_device *dev, uint8_t nav_id, uint8_t msg_id, uint16_t len); extern void ubx_trailer(struct link_device *dev); diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c index 870e1ecc83..77143cd4ea 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.c +++ b/sw/airborne/subsystems/ins/ins_alt_float.c @@ -49,6 +49,10 @@ #warning Please remove the obsolete ALT_KALMAN and ALT_KALMAN_ENABLED defines from your airframe file. #endif +#ifndef USE_INS_NAV_INIT +#define USE_INS_NAV_INIT TRUE +PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE") +#endif struct InsAltFloat ins_altf; @@ -87,11 +91,15 @@ void ins_alt_float_update_gps(struct GpsState *gps_s); void ins_alt_float_init(void) { - +#if USE_INS_NAV_INIT struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, ground_alt, nav_utm_zone0 }; stateSetLocalUtmOrigin_f(&utm0); + ins_altf.origin_initialized = TRUE; stateSetPositionUtm_f(&utm0); +#else + ins_altf.origin_initialized = FALSE; +#endif // set initial body to imu to 0 struct Int32Eulers b2i0 = { 0, 0, 0 }; @@ -122,6 +130,8 @@ void ins_reset_local_origin(void) // reset state UTM ref stateSetLocalUtmOrigin_f(&utm); + ins_altf.origin_initialized = TRUE; + // reset filter flag ins_altf.reset_alt_ref = TRUE; } @@ -186,6 +196,14 @@ void ins_alt_float_update_baro(float pressure __attribute__((unused))) void ins_alt_float_update_gps(struct GpsState *gps_s) { #if USE_GPS + if (gps_s->fix < GPS_FIX_3D) { + return; + } + + if (!ins_altf.origin_initialized) { + ins_reset_local_origin(); + } + struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0); #if !USE_BAROMETER diff --git a/sw/airborne/subsystems/ins/ins_alt_float.h b/sw/airborne/subsystems/ins/ins_alt_float.h index 7ced685cb2..5f0c147882 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.h +++ b/sw/airborne/subsystems/ins/ins_alt_float.h @@ -39,6 +39,7 @@ struct InsAltFloat { float alt_dot; ///< estimated vertical speed in m/s (positive-up) bool_t reset_alt_ref; ///< flag to request reset of altitude reference to current alt + bool_t origin_initialized; ///< TRUE if UTM origin was initialized #if USE_BAROMETER float qfe; diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index 7081e20635..3fbad1560f 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -94,10 +94,16 @@ PRINT_CONFIG_MSG("INS_SONAR_UPDATE_ON_AGL defaulting to FALSE") #endif // USE_SONAR +#if USE_GPS #ifndef INS_VFF_R_GPS #define INS_VFF_R_GPS 2.0 #endif +#ifndef INS_VFF_VZ_R_GPS +#define INS_VFF_VZ_R_GPS 2.0 +#endif +#endif // USE_GPS + /** maximum number of propagation steps without any updates in between */ #ifndef INS_MAX_PROPAGATION_STEPS #define INS_MAX_PROPAGATION_STEPS 200 @@ -375,6 +381,9 @@ void ins_int_update_gps(struct GpsState *gps_s) #if INS_USE_GPS_ALT vff_update_z_conf(((float)gps_pos_cm_ned.z) / 100.0, INS_VFF_R_GPS); #endif +#if INS_USE_GPS_ALT_SPEED + vff_update_vz_conf(((float)gps_speed_cm_s_ned.z) / 100.0, INS_VFF_VZ_R_GPS); +#endif #if USE_HFF /* horizontal gps transformed to NED in meters as float */ diff --git a/sw/airborne/subsystems/ins/vf_extended_float.c b/sw/airborne/subsystems/ins/vf_extended_float.c index efc600c159..bcc271d3f2 100644 --- a/sw/airborne/subsystems/ins/vf_extended_float.c +++ b/sw/airborne/subsystems/ins/vf_extended_float.c @@ -43,7 +43,7 @@ PRINT_CONFIG_VAR(DEBUG_VFF_EXTENDED) #if DEBUG_VFF_EXTENDED #include "mcu_periph/uart.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #endif diff --git a/sw/airborne/subsystems/intermcu/intermcu_ap.c b/sw/airborne/subsystems/intermcu/intermcu_ap.c index 59b83ea1b5..e9862cef18 100644 --- a/sw/airborne/subsystems/intermcu/intermcu_ap.c +++ b/sw/airborne/subsystems/intermcu/intermcu_ap.c @@ -25,9 +25,9 @@ */ #include "intermcu_ap.h" -#include "intermcu_msg.h" +#include "pprzlink/intermcu_msg.h" #include "subsystems/radio_control.h" -#include "subsystems/datalink/pprz_transport.h" +#include "pprzlink/pprz_transport.h" #include "mcu_periph/uart.h" #if COMMANDS_NB > 8 diff --git a/sw/airborne/subsystems/intermcu/intermcu_fbw.c b/sw/airborne/subsystems/intermcu/intermcu_fbw.c index ee2747b49c..f97a34badb 100644 --- a/sw/airborne/subsystems/intermcu/intermcu_fbw.c +++ b/sw/airborne/subsystems/intermcu/intermcu_fbw.c @@ -25,10 +25,10 @@ */ #include "intermcu_fbw.h" -#include "intermcu_msg.h" +#include "pprzlink/intermcu_msg.h" #include "subsystems/radio_control.h" #include "mcu_periph/uart.h" -#include "subsystems/datalink/pprz_transport.h" +#include "pprzlink/pprz_transport.h" #if RADIO_CONTROL_NB_CHANNEL > 8 #undef RADIO_CONTROL_NB_CHANNEL diff --git a/sw/airborne/test/subsystems/test_ahrs.c b/sw/airborne/test/subsystems/test_ahrs.c index a8a433fcf8..80c6fca42b 100644 --- a/sw/airborne/test/subsystems/test_ahrs.c +++ b/sw/airborne/test/subsystems/test_ahrs.c @@ -38,7 +38,7 @@ #include "mcu_periph/sys_time.h" #include "led.h" #include "mcu_periph/i2c.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include "subsystems/datalink/telemetry.h" diff --git a/sw/airborne/test/subsystems/test_imu.c b/sw/airborne/test/subsystems/test_imu.c index a3bcada60f..e4a33c5b93 100644 --- a/sw/airborne/test/subsystems/test_imu.c +++ b/sw/airborne/test/subsystems/test_imu.c @@ -32,7 +32,7 @@ #include "mcu_periph/sys_time.h" #include "led.h" #include "mcu_periph/i2c.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" #include "subsystems/imu.h" diff --git a/sw/ext/Makefile b/sw/ext/Makefile index e2b6c43be1..a3daba3fe9 100644 --- a/sw/ext/Makefile +++ b/sw/ext/Makefile @@ -77,6 +77,11 @@ mavlink.build: libsbp: libsbp.update +pprzlink: pprzlink.update pprzlink.build + +pprzlink.build: + $(Q)$(MAKE) -C pprzlink + clean: $(Q)if [ -f libopencm3/Makefile ]; then \ $(MAKE) -C $(EXT_DIR)/libopencm3 clean; \ diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink new file mode 160000 index 0000000000..b13dd7a0b6 --- /dev/null +++ b/sw/ext/pprzlink @@ -0,0 +1 @@ +Subproject commit b13dd7a0b64ab571fa05bcdbf31e6d959fc3533e diff --git a/sw/ground_segment/cockpit/Makefile b/sw/ground_segment/cockpit/Makefile index 15a92792db..b3563b8603 100644 --- a/sw/ground_segment/cockpit/Makefile +++ b/sw/ground_segment/cockpit/Makefile @@ -31,9 +31,9 @@ INCLUDES= LIBS= LIBSX=$(LIBS:.cma=.cmxa) -INCLUDES= -I ../multimon +INCLUDES= PKG = -package pprz.xlib -LINKPKG = $(PKG) -linkpkg -dllpath-pkg pprz.xlib +LINKPKG = $(PKG) -linkpkg -dllpath-pkg pprz.xlib,pprzlink LABLGTK2INIT = $(shell ocamlfind query -p-format lablgtk2.init 2>/dev/null) ifeq ($(LABLGTK2INIT),) diff --git a/sw/ground_segment/cockpit/compass.ml b/sw/ground_segment/cockpit/compass.ml index 92761bb026..21c531f14c 100644 --- a/sw/ground_segment/cockpit/compass.ml +++ b/sw/ground_segment/cockpit/compass.ml @@ -25,7 +25,7 @@ open Printf open Latlong -module Tm_Pprz = Pprz.Messages (struct let name = "telemetry" end) +module Tm_Pprz = PprzLink.Messages (struct let name = "telemetry" end) let width = 200 let height = width @@ -134,19 +134,19 @@ let _ = let course = ref None in (* deg *) let desired_course = ref 0. in (* deg *) let get_navigation = fun _ values -> - let distance = (try sqrt (Pprz.float_assoc "dist2_wp" values) with _ -> Pprz.float_assoc "dist_wp" values) in + let distance = (try sqrt (PprzLink.float_assoc "dist2_wp" values) with _ -> PprzLink.float_assoc "dist_wp" values) in draw da !desired_course !course distance in ignore (Tm_Pprz.message_bind "NAVIGATION" get_navigation); let get_gps = fun _ values -> (* if speed < 1m/s, the course information is not relevant *) course := - if Pprz.int_assoc "speed" values > 100 then - Some (float (Pprz.int_assoc "course" values) /. 10.) + if PprzLink.int_assoc "speed" values > 100 then + Some (float (PprzLink.int_assoc "course" values) /. 10.) else None in ignore (Tm_Pprz.message_bind "GPS" get_gps); let get_desired = fun _ values -> - desired_course := (Rad>>Deg) (Pprz.float_assoc "course" values) in + desired_course := (Rad>>Deg) (PprzLink.float_assoc "course" values) in ignore (Tm_Pprz.message_bind "DESIRED" get_desired); (** Start the main loop *) diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml index 71505869f8..26f1067f75 100644 --- a/sw/ground_segment/cockpit/live.ml +++ b/sw/ground_segment/cockpit/live.ml @@ -27,9 +27,9 @@ open Latlong module LL = Latlong open Printf -module Tele_Pprz = Pprz.Messages(struct let name = "telemetry" end) -module Ground_Pprz = Pprz.Messages(struct let name = "ground" end) -module Alert_Pprz = Pprz.Messages(struct let name = "alert" end) +module Tele_Pprz = PprzLink.Messages(struct let name = "telemetry" end) +module Ground_Pprz = PprzLink.Messages(struct let name = "ground" end) +module Alert_Pprz = PprzLink.Messages(struct let name = "alert" end) let (//) = Filename.concat @@ -75,7 +75,7 @@ type gps_acc_level = GPS_ACC_HIGH | GPS_ACC_LOW | GPS_ACC_VERY_LOW | GPS_NO_ACC type aircraft = { ac_name : string; ac_speech_name : string; - config : Pprz.values; + config : PprzLink.values; track : MapTrack.track; color: color; fp_group : MapFP.flight_plan; @@ -128,7 +128,7 @@ let find_ac = fun ac_id -> let active_ac = ref "" let get_ac = fun vs -> - let ac_id = Pprz.string_assoc "ac_id" vs in + let ac_id = PprzLink.string_assoc "ac_id" vs in find_ac ac_id let show_fp = fun ac -> @@ -206,11 +206,11 @@ let resize_track = fun ac track -> let send_move_waypoint_msg = fun ac i w -> let wgs84 = w#pos in - let vs = ["ac_id", Pprz.String ac; - "wp_id", Pprz.Int i; - "lat", Pprz.Float ((Rad>>Deg)wgs84.posn_lat); - "long", Pprz.Float ((Rad>>Deg)wgs84.posn_long); - "alt", Pprz.Float w#alt + let vs = ["ac_id", PprzLink.String ac; + "wp_id", PprzLink.Int i; + "lat", PprzLink.Float ((Rad>>Deg)wgs84.posn_lat); + "long", PprzLink.Float ((Rad>>Deg)wgs84.posn_long); + "alt", PprzLink.Float w#alt ] in Ground_Pprz.message_send "gcs" "MOVE_WAYPOINT" vs @@ -243,14 +243,14 @@ let blocks_of_stages = fun stages -> let jump_to_block = fun ac_id id -> Ground_Pprz.message_send "gcs" "JUMP_TO_BLOCK" - ["ac_id", Pprz.String ac_id; "block_id", Pprz.Int id] + ["ac_id", PprzLink.String ac_id; "block_id", PprzLink.Int id] let dl_setting = fun ac_id idx value -> - let vs = ["ac_id", Pprz.String ac_id; "index", Pprz.Int idx;"value", Pprz.Float value] in + let vs = ["ac_id", PprzLink.String ac_id; "index", PprzLink.Int idx;"value", PprzLink.Float value] in Ground_Pprz.message_send "dl" "DL_SETTING" vs let get_dl_setting = fun ac_id idx -> - let vs = ["ac_id", Pprz.String ac_id; "index", Pprz.Int idx] in + let vs = ["ac_id", PprzLink.String ac_id; "index", PprzLink.Int idx] in Ground_Pprz.message_send "dl" "GET_DL_SETTING" vs let menu_entry_of_block = fun ac_id (id, name) -> @@ -304,9 +304,9 @@ let mark = fun (geomap:G.widget) ac_id track plugin_frame -> let lat = (Rad>>Deg)geo.posn_lat and long = (Rad>>Deg)geo.posn_long in Tele_Pprz.message_send ac_id "MARK" - ["ac_id", Pprz.String ac_id; - "lat", Pprz.Float lat; - "long", Pprz.Float long]; + ["ac_id", PprzLink.String ac_id; + "lat", PprzLink.Float lat; + "long", PprzLink.Float long]; let frame = match plugin_frame with None -> geomap#canvas#coerce @@ -436,18 +436,18 @@ let key_press_event = fun keys do_action ev -> (*****************************************************************************) let create_ac = fun ?(confirm_kill=true) alert (geomap:G.widget) (acs_notebook:GPack.notebook) (strips:GPack.box) (ac_id:string) config -> - let color = Pprz.string_assoc "default_gui_color" config - and name = Pprz.string_assoc "ac_name" config in + let color = PprzLink.string_assoc "default_gui_color" config + and name = PprzLink.string_assoc "ac_name" config in (** Get the flight plan **) - let fp_url = Pprz.string_assoc "flight_plan" config in + let fp_url = PprzLink.string_assoc "flight_plan" config in let fp_file = Http.file_of_url fp_url in let fp_xml_dump = ExtXml.parse_file ~noprovedtd:true fp_file in let stages = ExtXml.child fp_xml_dump "stages" in let blocks = blocks_of_stages stages in (** Get the airframe file *) - let af_url = Pprz.string_assoc "airframe" config in + let af_url = PprzLink.string_assoc "airframe" config in let af_file = Http.file_of_url af_url in (* do not check dtd if it is a http url *) let via_http = Str.string_match (Str.regexp "http") af_url 0 in @@ -552,7 +552,7 @@ let create_ac = fun ?(confirm_kill=true) alert (geomap:G.widget) (acs_notebook:G let id = ExtXml.int_attrib block "no" in begin (* Is it a key short cut ? *) try - let key, modifiers = GtkData.AccelGroup.parse (Pprz.key_modifiers_of_string (Xml.attrib block "key")) in + let key, modifiers = GtkData.AccelGroup.parse (Env.key_modifiers_of_string (Xml.attrib block "key")) in keys := (key, (modifiers, id)) :: !keys with _ -> () @@ -627,7 +627,7 @@ let create_ac = fun ?(confirm_kill=true) alert (geomap:G.widget) (acs_notebook:G ignore (ac_notebook#append_page ~tab_label:misc_label#coerce misc_frame#coerce); let misc_page = new Pages.misc ~packing:misc_frame#add misc_frame in - let settings_url = Pprz.string_assoc "settings" config in + let settings_url = PprzLink.string_assoc "settings" config in let settings_file = Http.file_of_url settings_url in let settings_xml = try @@ -732,7 +732,7 @@ let create_ac = fun ?(confirm_kill=true) alert (geomap:G.widget) (acs_notebook:G let msg_items = ["WIND_INFO"; ac_id; "42"; wind_east; wind_north;airspeed] in let value = String.concat ";" msg_items in - let vs = ["ac_id", Pprz.String ac_id; "message", Pprz.String value] in + let vs = ["ac_id", PprzLink.String ac_id; "message", PprzLink.String value] in Ground_Pprz.message_send "dl" "RAW_DATALINK" vs; with exc -> log alert ac_id (sprintf "send_wind (%s): %s" ac_id (Printexc.to_string exc)) @@ -810,7 +810,7 @@ let safe_bind = fun msg cb -> let safe_cb = fun sender vs -> try cb sender vs with AC_not_found -> () (* A/C not yet registed; silently ignore *) - | x -> fprintf stderr "%s: safe_bind (%s:%a): %s\n%!" Sys.argv.(0) msg (fun c vs -> List.iter (fun (_,v) -> fprintf c "%s " (Pprz.string_of_value v)) vs) vs (Printexc.to_string x) in + | x -> fprintf stderr "%s: safe_bind (%s:%a): %s\n%!" Sys.argv.(0) msg (fun c vs -> List.iter (fun (_,v) -> fprintf c "%s " (PprzLink.string_of_value v)) vs) vs (Printexc.to_string x) in ignore (Ground_Pprz.message_bind msg safe_cb) let alert_bind = fun msg cb -> @@ -830,7 +830,7 @@ let ask_config = fun confirm_kill alert geomap fp_notebook strips ac -> if not (Hashtbl.mem aircrafts ac) then create_ac ~confirm_kill alert geomap fp_notebook strips ac values in - Ground_Pprz.message_req "gcs" "CONFIG" ["ac_id", Pprz.String ac] get_config + Ground_Pprz.message_req "gcs" "CONFIG" ["ac_id", PprzLink.String ac] get_config @@ -841,7 +841,7 @@ let one_new_ac = fun confirm_kill alert (geomap:G.widget) fp_notebook strips ac let get_wind_msg = fun (geomap:G.widget) _sender vs -> let ac = get_ac vs in - let value = fun field_name -> Pprz.float_assoc field_name vs in + let value = fun field_name -> PprzLink.float_assoc field_name vs in let airspeed = value "mean_aspeed" in ac.airspeed <- airspeed; ac.strip#set_airspeed airspeed; @@ -852,7 +852,7 @@ let get_wind_msg = fun (geomap:G.widget) _sender vs -> ac.misc_page#set_value "Wind speed" (sprintf "%.1f" ac.wind_speed); ac.misc_page#set_value "Wind direction" (sprintf "%.1f" deg_dir); - let ac_id = Pprz.string_assoc "ac_id" vs in + let ac_id = PprzLink.string_assoc "ac_id" vs in if !active_ac = ac_id && ac.wind_speed > 1. then begin geomap#wind_sock#set_color ac.color; geomap#wind_sock#item#show (); @@ -862,32 +862,32 @@ let get_wind_msg = fun (geomap:G.widget) _sender vs -> let get_fbw_msg = fun alarm _sender vs -> let ac = get_ac vs in - let status = Pprz.string_assoc "rc_status" vs - and rate = (Pprz.int_assoc "rc_rate" vs) / 5 in + let status = PprzLink.string_assoc "rc_status" vs + and rate = (PprzLink.int_assoc "rc_rate" vs) / 5 in (* divide by 5 to have normal values between 0 and 10 *) (* RC rate max approx. 50 Hz *) ac.strip#set_rc rate status; - let mode = Pprz.string_assoc "rc_mode" vs in + let mode = PprzLink.string_assoc "rc_mode" vs in if mode = "FAILSAFE" then begin log_and_say alarm ac.ac_name (sprintf "%s, mayday, AP Failure. Switch to manual." ac.ac_speech_name) end let get_telemetry_status = fun alarm _sender vs -> let ac = get_ac vs in - let link_id = Pprz.string_assoc "link_id" vs in + let link_id = PprzLink.string_assoc "link_id" vs in let link_id = try if int_of_string link_id = -1 then "single" else link_id with _ -> link_id in (* Update color and lost time in the strip *) - let time_lost = Pprz.float_assoc "time_since_last_msg" vs in + let time_lost = PprzLink.float_assoc "time_since_last_msg" vs in let (links_up, total_links) = ac.link_page#links_ratio () in let link_ratio_string = if ac.link_page#multiple_links () then sprintf "%i/%i" links_up total_links else "" in ac.strip#set_label "telemetry_status" (if time_lost > 2. then sprintf "%.0f" time_lost else link_ratio_string); ac.strip#set_color "telemetry_status" (if time_lost > 5. then alert_color else if links_up < total_links then warning_color else ok_color); (* Update link page *) - let rx_msgs_rate = Pprz.float_assoc "rx_bytes_rate" vs - and downlink_bytes_rate = Pprz.int_assoc "downlink_rate" vs - and uplink_lost_time = Pprz.int_assoc "uplink_lost_time" vs in - let ping_time = Pprz.float_assoc "ping_time" vs in + let rx_msgs_rate = PprzLink.float_assoc "rx_bytes_rate" vs + and downlink_bytes_rate = PprzLink.int_assoc "downlink_rate" vs + and uplink_lost_time = PprzLink.int_assoc "uplink_lost_time" vs in + let ping_time = PprzLink.float_assoc "ping_time" vs in if (not (ac.link_page#link_exists link_id)) then begin ac.link_page#add_link link_id; log_and_say alarm ac.ac_name (sprintf "%s, link %s detected" ac.ac_speech_name link_id) @@ -902,16 +902,16 @@ let get_telemetry_status = fun alarm _sender vs -> let get_engine_status_msg = fun _sender vs -> let ac = get_ac vs in - ac.strip#set_throttle ~kill:ac.in_kill_mode (Pprz.float_assoc "throttle" vs); - ac.strip#set_bat (Pprz.float_assoc "bat" vs) + ac.strip#set_throttle ~kill:ac.in_kill_mode (PprzLink.float_assoc "throttle" vs); + ac.strip#set_bat (PprzLink.float_assoc "bat" vs) let get_if_calib_msg = fun _sender vs -> let ac = get_ac vs in match ac.rc_settings_page with None -> () | Some p -> - p#set_rc_setting_mode (Pprz.string_assoc "if_mode" vs); - p#set (Pprz.float_assoc "if_value1" vs) (Pprz.float_assoc "if_value2" vs) + p#set_rc_setting_mode (PprzLink.string_assoc "if_mode" vs); + p#set (PprzLink.float_assoc "if_value1" vs) (PprzLink.float_assoc "if_value2" vs) let listen_wind_msg = fun (geomap:G.widget) -> safe_bind "WIND" (get_wind_msg geomap) @@ -929,7 +929,7 @@ let listen_telemetry_status = fun a -> safe_bind "TELEMETRY_STATUS" (get_telemetry_status a) let aircrafts_msg = fun confirm_kill alert (geomap:G.widget) fp_notebook strips acs -> - let acs = Pprz.string_assoc "ac_list" acs in + let acs = PprzLink.string_assoc "ac_list" acs in let acs = Str.split list_separator acs in List.iter (one_new_ac confirm_kill alert geomap fp_notebook strips) acs @@ -939,7 +939,7 @@ let listen_dl_value = fun () -> let ac = get_ac vs in match ac.dl_settings_page with Some settings -> - let csv = Pprz.string_assoc "values" vs in + let csv = PprzLink.string_assoc "values" vs in let string_value = fun v -> match v with "?" -> None | _ -> Some v in let values = Array.map string_value (Array.of_list (Str.split list_separator csv)) in ac.dl_values <- values; @@ -1091,8 +1091,8 @@ module GCS_icon = struct let timeout = 10000 (* ms : time before changing to outdated color *) let display = fun (geomap:G.widget) vs -> - let lat = Pprz.float_assoc "lat" vs - and lon = Pprz.float_assoc "long" vs in + let lat = PprzLink.float_assoc "lat" vs + and lon = PprzLink.float_assoc "long" vs in let wgs84 = LL.make_geo_deg lat lon in let item = @@ -1126,13 +1126,13 @@ end (* module GCS_icon *) (******************************** FLIGHT_PARAMS ******************************) let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph -> let get_fp = fun _sender vs -> - let ac_id = Pprz.string_assoc "ac_id" vs in + let ac_id = PprzLink.string_assoc "ac_id" vs in if ac_id = gcs_id then GCS_icon.display geomap vs else let ac = get_ac vs in let pfd_page = ac.pfd_page in - let a = fun s -> Pprz.float_assoc s vs in + let a = fun s -> PprzLink.float_assoc s vs in let alt = a "alt" and climb = a "climb" and speed = a "speed" in @@ -1181,16 +1181,16 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph -> let get_ns = fun _sender vs -> let ac = get_ac vs in - let a = fun s -> Pprz.float_assoc s vs in + let a = fun s -> PprzLink.float_assoc s vs in let wgs84 = { posn_lat = (Deg>>Rad)(a "target_lat"); posn_long = (Deg>>Rad)(a "target_long") } in ac.track#move_carrot wgs84; - let cur_block = Pprz.int_assoc "cur_block" vs - and cur_stage = Pprz.int_assoc "cur_stage" vs in + let cur_block = PprzLink.int_assoc "cur_block" vs + and cur_stage = PprzLink.int_assoc "cur_stage" vs in highlight_fp ac cur_block cur_stage; let set_label = fun l f -> - ac.strip#set_label l (sprintf "%.0fm" (Pprz.float_assoc f vs)) in + ac.strip#set_label l (sprintf "%.0fm" (PprzLink.float_assoc f vs)) in set_label "target_altitude" "target_alt"; - let target_alt = Pprz.float_assoc "target_alt" vs in + let target_alt = PprzLink.float_assoc "target_alt" vs in ac.strip#set_label "diff_target_alt" (sprintf "%+.0fm" (ac.alt -. target_alt)); ac.target_alt <- target_alt; let b = try List.assoc cur_block ac.blocks with Not_found -> failwith (sprintf "Error: unknown block %d for A/C %s" cur_block ac.ac_name) in @@ -1199,15 +1199,15 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph -> ac.last_block_name <- b; ac.strip#set_label "block_name" b end; - let block_time = Int64.to_int (Pprz.uint32_assoc "block_time" vs) - and stage_time = Int64.to_int (Pprz.uint32_assoc "stage_time" vs) in + let block_time = Int64.to_int (PprzLink.uint32_assoc "block_time" vs) + and stage_time = Int64.to_int (PprzLink.uint32_assoc "stage_time" vs) in let bt = sprintf "%02d:%02d" (block_time / 60) (block_time mod 60) in ac.strip#set_label "block_time" bt; let st = sprintf "%02d:%02d" (stage_time / 60) (stage_time mod 60) in ac.strip#set_label "stage_time" st; (* Estimated Time Arrival to next waypoint *) - let d = Pprz.float_assoc "dist_to_wp" vs in + let d = PprzLink.float_assoc "dist_to_wp" vs in let label = if d < 0.5 || ac.speed < 0.5 then "N/A" @@ -1238,7 +1238,7 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph -> let get_cam_status = fun _sender vs -> let ac = get_ac vs in - let a = fun s -> Pprz.float_assoc s vs in + let a = fun s -> PprzLink.float_assoc s vs in let cam_wgs84 = { posn_lat = (Deg>>Rad)(a "cam_lat"); posn_long = (Deg>>Rad)(a "cam_long") } and target_wgs84 = { posn_lat = (Deg>>Rad)(a "cam_target_lat"); posn_long = (Deg>>Rad)(a "cam_target_long") } in @@ -1249,16 +1249,16 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph -> let get_circle_status = fun _sender vs -> let ac = get_ac vs in ac.got_track_status_timer <- 0; - let a = fun s -> Pprz.float_assoc s vs in + let a = fun s -> PprzLink.float_assoc s vs in let wgs84 = { posn_lat = (Deg>>Rad)(a "circle_lat"); posn_long = (Deg>>Rad)(a "circle_long") } in - ac.track#draw_circle wgs84 (float_of_string (Pprz.string_assoc "radius" vs)) + ac.track#draw_circle wgs84 (float_of_string (PprzLink.string_assoc "radius" vs)) in safe_bind "CIRCLE_STATUS" get_circle_status; let get_segment_status = fun _sender vs -> let ac = get_ac vs in ac.got_track_status_timer <- 0; - let a = fun s -> Pprz.float_assoc s vs in + let a = fun s -> PprzLink.float_assoc s vs in let geo1 = { posn_lat = (Deg>>Rad)(a "segment1_lat"); posn_long = (Deg>>Rad)(a "segment1_long") } and geo2 = { posn_lat = (Deg>>Rad)(a "segment2_lat"); posn_long = (Deg>>Rad)(a "segment2_long") } in ac.track#draw_segment geo1 geo2; @@ -1271,7 +1271,7 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph -> let get_survey_status = fun _sender vs -> let ac = get_ac vs in - let a = fun s -> Pprz.float_assoc s vs in + let a = fun s -> PprzLink.float_assoc s vs in let geo1 = { posn_lat = (Deg>>Rad)(a "south_lat"); posn_long = (Deg>>Rad)(a "west_long") } and geo2 = { posn_lat = (Deg>>Rad)(a "north_lat"); posn_long = (Deg>>Rad)(a "east_long") } in ac.track#draw_zone geo1 geo2 @@ -1281,14 +1281,14 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph -> let get_ap_status = fun _sender vs -> let ac = get_ac vs in - let flight_time = Int64.to_int (Pprz.uint32_assoc "flight_time" vs) in + let flight_time = Int64.to_int (PprzLink.uint32_assoc "flight_time" vs) in ac.track#update_ap_status (float_of_int flight_time); ac.flight_time <- flight_time; - let ap_mode = Pprz.string_assoc "ap_mode" vs in + let ap_mode = PprzLink.string_assoc "ap_mode" vs in if ap_mode <> ac.last_ap_mode then begin log_and_say alert ac.ac_name (sprintf "%s, %s" ac.ac_speech_name ap_mode); ac.last_ap_mode <- ap_mode; - let label = Pprz.string_assoc "ap_mode" vs in + let label = PprzLink.string_assoc "ap_mode" vs in ac.strip#set_label "AP" (if label="MANUAL" then "MANU" else label); let color = match ap_mode with @@ -1298,17 +1298,17 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph -> | _ -> alert_color in ac.strip#set_color "AP" color; end; - let status_filter_mode = Pprz.string_assoc "state_filter_mode" vs in + let status_filter_mode = PprzLink.string_assoc "state_filter_mode" vs in let gps_mode = if (status_filter_mode <> "UNKNOWN") && (status_filter_mode <> "OK") && (status_filter_mode <> "GPS_LOST") then status_filter_mode - else Pprz.string_assoc "gps_mode" vs in + else PprzLink.string_assoc "gps_mode" vs in ac.strip#set_label "GPS" gps_mode; ac.strip#set_color "GPS" (if gps_mode<>"3D" && gps_mode<>"DGPS" && gps_mode<>"RTK" then alert_color else ok_color); let ft = sprintf "%02d:%02d:%02d" (flight_time / 3600) ((flight_time / 60) mod 60) (flight_time mod 60) in ac.strip#set_label "flight_time" ft; - let kill_mode = Pprz.string_assoc "kill_mode" vs in + let kill_mode = PprzLink.string_assoc "kill_mode" vs in if kill_mode <> "OFF" then begin if not ac.in_kill_mode then log_and_say alert ac.ac_name (sprintf "%s, mayday, kill mode" ac.ac_speech_name); @@ -1327,8 +1327,8 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph -> let listen_waypoint_moved = fun () -> let get_values = fun _sender vs -> let ac = get_ac vs in - let wp_id = Pprz.int_assoc "wp_id" vs in - let a = fun s -> Pprz.float_assoc s vs in + let wp_id = PprzLink.int_assoc "wp_id" vs in + let a = fun s -> PprzLink.float_assoc s vs in let geo = { posn_lat = (Deg>>Rad)(a "lat"); posn_long = (Deg>>Rad)(a "long") } and altitude = a "alt" and ground_alt = a "ground_alt" in @@ -1345,7 +1345,7 @@ let listen_waypoint_moved = fun () -> let get_alert_bat_low = fun a _sender vs -> let ac = get_ac vs in - let level = Pprz.string_assoc "level" vs in + let level = PprzLink.string_assoc "level" vs in let unix_time = Unix.gettimeofday() in if unix_time > (ac.last_bat_warn_time +. 10.) then begin log_and_say a ac.ac_name (sprintf "%s, %s %s" ac.ac_speech_name "BAT LOW" level); @@ -1358,10 +1358,10 @@ let listen_alert = fun a -> let get_svsinfo = fun alarm _sender vs -> let ac = get_ac vs in let gps_page = ac.gps_page in - let svids = Str.split list_separator (Pprz.string_assoc "svid" vs) - and cn0s = Str.split list_separator (Pprz.string_assoc "cno" vs) - and flagss = Str.split list_separator (Pprz.string_assoc "flags" vs) - and ages = Str.split list_separator (Pprz.string_assoc "msg_age" vs) in + let svids = Str.split list_separator (PprzLink.string_assoc "svid" vs) + and cn0s = Str.split list_separator (PprzLink.string_assoc "cno" vs) + and flagss = Str.split list_separator (PprzLink.string_assoc "flags" vs) + and ages = Str.split list_separator (PprzLink.string_assoc "msg_age" vs) in let a = Array.make (List.length svids) (0,0,0,0) in let rec loop = fun i s c f ages -> @@ -1373,7 +1373,7 @@ let get_svsinfo = fun alarm _sender vs -> | _ -> assert false in loop 0 svids cn0s flagss ages; - let pacc = Pprz.int_assoc "pacc" vs in + let pacc = PprzLink.int_assoc "pacc" vs in gps_page#svsinfo pacc a; @@ -1397,10 +1397,10 @@ let message_request = Ground_Pprz.message_req let mark_dcshot = fun (geomap:G.widget) _sender vs -> let ac = find_ac !active_ac in - let photonumber = Pprz.string_assoc "photo_nr" vs in + let photonumber = PprzLink.string_assoc "photo_nr" vs in try - let lat = Pprz.int_assoc "lat" vs - and lon = Pprz.int_assoc "lon" vs in + let lat = PprzLink.int_assoc "lat" vs + and lon = PprzLink.int_assoc "lon" vs in let wgs84 = LL.make_geo_deg (float lat /. 1e7) (float lon /. 1e7) in let group = geomap#background in let point = geomap#photoprojection ~group ~fill_color:"yellow" ~number:photonumber wgs84 3. in @@ -1423,22 +1423,22 @@ let listen_dcshot = fun _geom timestamp -> let listen_error = fun a -> let get_error = fun _sender vs -> - let msg = Pprz.string_assoc "message" vs in + let msg = PprzLink.string_assoc "message" vs in log_and_say a "gcs" msg in safe_bind "TELEMETRY_ERROR" get_error let listen_info_msg = fun a timestamp -> let get_msg = fun a _sender vs -> let ac = find_ac _sender in - let msg_array = Pprz.assoc "msg" vs in - log_and_say a ac.ac_name (Pprz.string_of_value msg_array) in + let msg_array = PprzLink.assoc "msg" vs in + log_and_say a ac.ac_name (PprzLink.string_of_value msg_array) in tele_bind "INFO_MSG" (get_msg a) timestamp let listen_autopilot_version_msg = fun a timestamp -> let get_msg = fun a _sender vs -> let ac = find_ac _sender in - let desc_array = Pprz.assoc "desc" vs in - let version = Pprz.string_of_value desc_array in + let desc_array = PprzLink.assoc "desc" vs in + let version = PprzLink.string_of_value desc_array in if ac.version <> version then log a ac.ac_name (sprintf "%s version:\n%s" ac.ac_name version); ac.version <- version in @@ -1449,7 +1449,7 @@ let listen_tcas = fun a timestamp -> let ac = find_ac _sender in let other_ac = get_ac vs in let resolve = try - match Pprz.int_assoc "resolve" vs with + match PprzLink.int_assoc "resolve" vs with 1 -> "=> LEVEL" | 2 -> "=> CLIMB" | 3 -> "=> DESCEND" @@ -1461,10 +1461,10 @@ let listen_tcas = fun a timestamp -> tele_bind "TCAS_RA" (get_alarm_tcas a "TCAS RA") timestamp let get_intruders = fun (geomap:G.widget) _sender vs -> - let f = fun s -> Pprz.float_assoc s vs in - let i = fun s -> float (Pprz.int_assoc s vs) in - let name = Pprz.string_assoc "name" vs - and id = Pprz.string_assoc "id" vs + let f = fun s -> PprzLink.float_assoc s vs in + let i = fun s -> float (PprzLink.int_assoc s vs) in + let name = PprzLink.string_assoc "name" vs + and id = PprzLink.string_assoc "id" vs and time = Unix.gettimeofday () and lat = (i "lat") /. 1e7 and lon = (i "lon") /. 1e7 in @@ -1484,7 +1484,7 @@ let listen_acs_and_msgs = fun geomap ac_notebook strips confirm_kill my_alert au let _ = GMain.Timeout.add 1000 (fun () -> probe (); !_req_aircrafts) in (** New aircraft message *) - safe_bind "NEW_AIRCRAFT" (fun _sender vs -> one_new_ac confirm_kill my_alert geomap ac_notebook strips (Pprz.string_assoc "ac_id" vs)); + safe_bind "NEW_AIRCRAFT" (fun _sender vs -> one_new_ac confirm_kill my_alert geomap ac_notebook strips (PprzLink.string_assoc "ac_id" vs)); (** Listen for all messages on ivy *) listen_flight_params geomap auto_center_new_ac my_alert alt_graph; diff --git a/sw/ground_segment/cockpit/live.mli b/sw/ground_segment/cockpit/live.mli index 808aeca6d4..b6d6e9390d 100644 --- a/sw/ground_segment/cockpit/live.mli +++ b/sw/ground_segment/cockpit/live.mli @@ -29,7 +29,7 @@ type gps_acc_level = GPS_ACC_HIGH | GPS_ACC_LOW | GPS_ACC_VERY_LOW | GPS_NO_ACC type aircraft = private { ac_name : string; ac_speech_name : string; - config : Pprz.values; + config : PprzLink.values; track : MapTrack.track; color: color; fp_group : MapFP.flight_plan; @@ -71,7 +71,7 @@ type aircraft = private { val aircrafts : (string, aircraft) Hashtbl.t -val safe_bind : string -> (string -> Pprz.values -> unit) -> unit +val safe_bind : string -> (string -> PprzLink.values -> unit) -> unit val track_size : int ref (** Default length for A/C tracks on the 2D view *) diff --git a/sw/ground_segment/cockpit/page_settings.ml b/sw/ground_segment/cockpit/page_settings.ml index 0385cb6b21..4d0c29f498 100644 --- a/sw/ground_segment/cockpit/page_settings.ml +++ b/sw/ground_segment/cockpit/page_settings.ml @@ -36,11 +36,11 @@ object match last_known_value with | None -> raise Not_found | Some v -> - let auc = Pprz.alt_unit_coef_of_xml xml in + let auc = PprzLink.alt_unit_coef_of_xml xml in let (alt_a, alt_b) = Ocaml_tools.affine_transform auc in (v -. alt_b) /. alt_a method current_value = - let auc = Pprz.alt_unit_coef_of_xml xml in + let auc = PprzLink.alt_unit_coef_of_xml xml in let (alt_a, alt_b) = Ocaml_tools.affine_transform auc in (float_of_string current_value#text -. alt_b) /. alt_a method update = fun s -> @@ -74,7 +74,7 @@ let search_index = fun value array -> let add_key = fun xml do_change keys -> - let key, modifiers = GtkData.AccelGroup.parse (Pprz.key_modifiers_of_string (Xml.attrib xml "key")) + let key, modifiers = GtkData.AccelGroup.parse (Env.key_modifiers_of_string (Xml.attrib xml "key")) and value = ExtXml.float_attrib xml "value" in keys := (key, (modifiers, fun () -> do_change value)) :: !keys @@ -94,7 +94,7 @@ let one_setting = fun (i:int) (do_change:int -> float -> unit) ac_id packing dl_ let page_incr = step_incr and page_size = step_incr and show_auto = try ExtXml.attrib dl_setting "auto" = "true" with _ -> false in - let auc = Pprz.alt_unit_coef_of_xml dl_setting in + let auc = PprzLink.alt_unit_coef_of_xml dl_setting in let (alt_a, alt_b) = Ocaml_tools.affine_transform auc in let hbox = GPack.hbox ~packing () in @@ -345,7 +345,7 @@ object (self) | None -> "?", -1 | Some x -> let v = try float_of_string x with _ -> failwith (sprintf "Pages.settings#set:wrong values.(%d) = %s" i x) in - let auc = Pprz.alt_unit_coef_of_xml setting#xml in + let auc = PprzLink.alt_unit_coef_of_xml setting#xml in let (alt_a, alt_b) = Ocaml_tools.affine_transform auc in let v = alt_a *. v +. alt_b in string_of_float v, truncate v diff --git a/sw/ground_segment/cockpit/particules.ml b/sw/ground_segment/cockpit/particules.ml index c780ef4db4..da79bffb94 100644 --- a/sw/ground_segment/cockpit/particules.ml +++ b/sw/ground_segment/cockpit/particules.ml @@ -33,7 +33,7 @@ let list_separator = Str.regexp "," let listen = fun (geomap:MapCanvas.widget) -> let plumes_msg = fun _sender vs -> - let split_val = fun tag -> Str.split list_separator (Pprz.string_assoc tag vs) in + let split_val = fun tag -> Str.split list_separator (PprzLink.string_assoc tag vs) in let ids = split_val "ids" and xs = split_val "lats" and ys = split_val "longs" diff --git a/sw/ground_segment/cockpit/saveSettings.ml b/sw/ground_segment/cockpit/saveSettings.ml index 07ee52a24b..6a7fbdb3c4 100644 --- a/sw/ground_segment/cockpit/saveSettings.ml +++ b/sw/ground_segment/cockpit/saveSettings.ml @@ -145,7 +145,7 @@ let fill_data = fun (model:GTree.tree_store) settings airframe_xml -> | None -> "" in (* Printf.fprintf stderr "param %s: unit_code=%s unit_airframe=%s\n" param unit_code unit_airframe; flush stderr; *) - Pprz.scale_of_units unit_airframe unit_code + PprzLink.scale_of_units unit_airframe unit_code with | Invalid_argument s -> prerr_endline s; flush stderr; raise Exit | _ -> 1. @@ -154,7 +154,7 @@ let fill_data = fun (model:GTree.tree_store) settings airframe_xml -> * settings are displayed in alt_unit specified in settings file * first try the alt_coef, otherwise try to convert the units *) - let display_scale = float_of_string (Pprz.alt_unit_coef_of_xml dl_setting) in + let display_scale = float_of_string (PprzLink.alt_unit_coef_of_xml dl_setting) in let val_list = Str.split (Str.regexp "[ ()]+") airframe_value in let (scale_macros, str_val) = List.partition (fun x -> Str.string_match (Str.regexp "RadOfDeg\\|DegOfRad") x 0) val_list in let extra_scale = diff --git a/sw/ground_segment/cockpit/strip.ml b/sw/ground_segment/cockpit/strip.ml index b7287c4061..f69ceaf120 100644 --- a/sw/ground_segment/cockpit/strip.ml +++ b/sw/ground_segment/cockpit/strip.ml @@ -204,7 +204,7 @@ let add = fun config strip_param (strips:GPack.box) -> let add_label = fun name value -> strip_labels := (name, value) :: !strip_labels in - let ac_name = Pprz.string_assoc "ac_name" config in + let ac_name = PprzLink.string_assoc "ac_name" config in let file = Env.paparazzi_src // "sw" // "ground_segment" // "cockpit" // "gcs.glade" in let strip = new Gtk_strip.eventbox_strip ~file () in diff --git a/sw/ground_segment/cockpit/strip.mli b/sw/ground_segment/cockpit/strip.mli index f639085dda..36ada818d4 100644 --- a/sw/ground_segment/cockpit/strip.mli +++ b/sw/ground_segment/cockpit/strip.mli @@ -61,5 +61,5 @@ type strip_param = { icons_theme : string; } -val add : Pprz.values -> strip_param -> GPack.box -> t +val add : PprzLink.values -> strip_param -> GPack.box -> t (** [add config params] *) diff --git a/sw/ground_segment/joystick/Makefile b/sw/ground_segment/joystick/Makefile index e60205ba18..30062c61f1 100644 --- a/sw/ground_segment/joystick/Makefile +++ b/sw/ground_segment/joystick/Makefile @@ -29,7 +29,7 @@ include ../../Makefile.ocaml CC = gcc PKG = -package pprz,glibivy -LINKPKG = $(PKG) -linkpkg -dllpath-pkg pprz +LINKPKG = $(PKG) -linkpkg -dllpath-pkg pprz,pprzlink GLIB_CFLAGS = -Wall -fPIC $(shell pkg-config glib-2.0 --cflags) GLIB_LDFLAGS = $(shell pkg-config glib-2.0 --libs) diff --git a/sw/ground_segment/joystick/input2ivy.ml b/sw/ground_segment/joystick/input2ivy.ml index 2070ad738e..4fdfc92da8 100644 --- a/sw/ground_segment/joystick/input2ivy.ml +++ b/sw/ground_segment/joystick/input2ivy.ml @@ -49,8 +49,8 @@ let trim_file_name = ref "" let joystick_id = ref (Random.int 255) (** Messages libraries *) -module DL = Pprz.Messages(struct let name = "datalink" end) -module G = Pprz.Messages(struct let name = "ground" end) +module DL = PprzLink.Messages(struct let name = "datalink" end) +module G = PprzLink.Messages(struct let name = "ground" end) (** Syntax for expressions *) module Syntax = Expr_syntax @@ -85,7 +85,7 @@ type input = type msg = { msg_name : string; msg_class : string; - fields : (string * Pprz._type * Syntax.expression) list; + fields : (string * PprzLink._type * Syntax.expression) list; on_event : Syntax.expression option; send_always : bool; has_ac_id : bool @@ -178,7 +178,7 @@ let rank = fun x l -> let eval_settings_and_blocks = fun field_descr expr -> let rec loop = function Syntax.Call ("IndexOfEnum", [Syntax.Ident enum]) -> begin - try Syntax.Int (rank enum field_descr.Pprz.enum) with + try Syntax.Int (rank enum field_descr.PprzLink.enum) with Not_found -> failwith (sprintf "IndexOfEnum: unknown value '%s'" enum) end | Syntax.Call ("IndexOfSetting", [Syntax.Ident var]) -> begin @@ -220,12 +220,12 @@ let parse_value = fun s -> (** Parse a message field and eval *) let parse_msg_field = fun msg_descr field -> let name = Xml.attrib field "name" in - let field_descr = try List.assoc name msg_descr.Pprz.fields with _ -> + let field_descr = try List.assoc name msg_descr.PprzLink.fields with _ -> Printf.printf "parse_msg_field: field %s not found\n" name; raise (Failure "field not found") in let value = eval_settings_and_blocks field_descr (parse_value (Xml.attrib field "value")) in - (name, field_descr.Pprz._type, value) + (name, field_descr.PprzLink._type, value) (** Parse a complete message and build its representation *) let parse_msg = fun msg -> @@ -240,7 +240,7 @@ let parse_msg = fun msg -> let msg_descr = get_message msg_class msg_name in try (List.map (parse_msg_field msg_descr) (Xml.children msg), - List.mem_assoc "ac_id" msg_descr.Pprz.fields) + List.mem_assoc "ac_id" msg_descr.PprzLink.fields) with _ -> failwith (sprintf "Couldn't parse message %s" msg_name) end @@ -492,7 +492,7 @@ let execute_action = fun ac_id inputs buttons hat axis variables message -> List.map (fun (name, _type, expr) -> let v = eval_expr buttons hat axis inputs variables expr in - (name, Pprz.value _type (sprintf "%d" v)) + (name, PprzLink.value _type (sprintf "%d" v)) ) message.fields @@ -506,10 +506,10 @@ let execute_action = fun ac_id inputs buttons hat axis variables message -> if ( ( (on_event, values) <> previous_values ) || message.send_always ) && on_event then begin match message.msg_class with "datalink" -> - let vs = if message.has_ac_id then ("ac_id", Pprz.Int ac_id) :: values else values in + let vs = if message.has_ac_id then ("ac_id", PprzLink.Int ac_id) :: values else values in DL.message_send "input2ivy" message.msg_name vs | "ground" -> - let vs = if message.has_ac_id then ("ac_id", Pprz.String (sprintf "%d" ac_id)) :: values else values in + let vs = if message.has_ac_id then ("ac_id", PprzLink.String (sprintf "%d" ac_id)) :: values else values in G.message_send "input2ivy" message.msg_name vs | "trim_plus" -> trim_adjust message.msg_name trim_step inputs | "trim_minus" -> trim_adjust message.msg_name (-.trim_step) inputs diff --git a/sw/ground_segment/misc/davis2ivy.c b/sw/ground_segment/misc/davis2ivy.c index b4a876bf14..e97897236e 100644 --- a/sw/ground_segment/misc/davis2ivy.c +++ b/sw/ground_segment/misc/davis2ivy.c @@ -186,7 +186,7 @@ void decode_and_send_to_ivy() { if (want_alive_msg) IvySendMsg("%d ALIVE 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0\n", ac_id); - // format has to match declaration in conf/messages.xml + // format has to match declaration in var/messages.xml IvySendMsg("%d WEATHER %f %f %f %f %f\n", ac_id, pstatic_Pa, temp_degC, windspeed_mps, winddir_deg, rel_humidity); } diff --git a/sw/ground_segment/misc/kestrel2ivy.c b/sw/ground_segment/misc/kestrel2ivy.c index 50ad8ef359..ef4951bf99 100644 --- a/sw/ground_segment/misc/kestrel2ivy.c +++ b/sw/ground_segment/misc/kestrel2ivy.c @@ -362,7 +362,7 @@ void decode_and_send_to_ivy() { rel_humidity = values[RH]; } - // format has to match declaration in conf/messages.xml + // format has to match declaration in var/messages.xml IvySendMsg("%d WEATHER %f %f %f %f %f\n", ac_id, pstatic_Pa, temp_degC, windspeed_mps, winddir_deg, rel_humidity); } diff --git a/sw/ground_segment/misc/natnet2ivy.c b/sw/ground_segment/misc/natnet2ivy.c index 9ad0a21318..f2b26e498f 100644 --- a/sw/ground_segment/misc/natnet2ivy.c +++ b/sw/ground_segment/misc/natnet2ivy.c @@ -19,14 +19,14 @@ * 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. - */ +/** \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 @@ -126,8 +126,9 @@ double tracking_offset_angle; ///< The offset from the tracking system to float natnet_latency; /** Parse the packet from NatNet */ -void natnet_parse(unsigned char *in) { - int i,j,k; +void natnet_parse(unsigned char *in) +{ + int i, j, k; // Create a pointer to go trough the packet char *ptr = (char *)in; @@ -143,8 +144,7 @@ void natnet_parse(unsigned char *in) { memcpy(&nBytes, ptr, 2); ptr += 2; printf_natnet("Byte count : %d\n", nBytes); - if(MessageID == NAT_FRAMEOFDATA) // FRAME OF MOCAP DATA packet - { + 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); @@ -154,8 +154,7 @@ void natnet_parse(unsigned char *in) { int nMarkerSets = 0; memcpy(&nMarkerSets, ptr, 4); ptr += 4; printf_natnet("Marker Set Count : %d\n", nMarkerSets); - for (i=0; i < nMarkerSets; i++) - { + for (i = 0; i < nMarkerSets; i++) { // Markerset name char szName[256]; strcpy(szName, ptr); @@ -167,24 +166,22 @@ void natnet_parse(unsigned char *in) { int nMarkers = 0; memcpy(&nMarkers, ptr, 4); ptr += 4; printf_natnet("Marker Count : %d\n", nMarkers); - for(j=0; j < nMarkers; j++) - { + 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); + 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++) - { + 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); + printf_natnet("\tMarker %d : pos = [%3.2f,%3.2f,%3.2f]\n", j, x, y, z); } // ========== RIGID BODIES ========== @@ -194,13 +191,14 @@ void natnet_parse(unsigned char *in) { 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); + 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++) - { + for (j = 0; j < nRigidBodies; j++) { // rigid body pos/ori struct RigidBody old_rigid; memcpy(&old_rigid, &rigidBodies[j], sizeof(struct RigidBody)); @@ -214,15 +212,17 @@ void natnet_parse(unsigned char *in) { 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); + 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.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) { + 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); @@ -231,12 +231,12 @@ void natnet_parse(unsigned char *in) { rigidBodies[j].nSamples++; rigidBodies[j].posSampled = TRUE; - } - else + } else { rigidBodies[j].posSampled = FALSE; + } // When marker id changed, reset the velocity - if(old_rigid.id != rigidBodies[j].id) { + if (old_rigid.id != rigidBodies[j].id) { rigidBodies[j].vel_x = 0; rigidBodies[j].vel_y = 0; rigidBodies[j].vel_z = 0; @@ -249,71 +249,67 @@ void natnet_parse(unsigned char *in) { // 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); + int nBytes = rigidBodies[j].nMarkers * 3 * sizeof(float); + float *markerData = (float *)malloc(nBytes); memcpy(markerData, ptr, nBytes); ptr += nBytes; - if(natnet_major >= 2) - { + if (natnet_major >= 2) { // Associated marker IDs - nBytes = rigidBodies[j].nMarkers*sizeof(int); - int* markerIDs = (int*)malloc(nBytes); + 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); + 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]); + 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) + if (markerIDs) { free(markerIDs); - if(markerSizes) + } + 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]); + } 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) + if (markerData) { free(markerData); + } - if(natnet_major >= 2) - { + 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); } // 2.6 and later - if( ((natnet_major == 2)&&(natnet_minor >= 6)) || (natnet_major > 2) || (natnet_major == 0) ) - { - // params - short params = 0; memcpy(¶ms, ptr, 2); ptr += 2; + if (((natnet_major == 2) && (natnet_minor >= 6)) || (natnet_major > 2) || (natnet_major == 0)) { + // params + short params = 0; memcpy(¶ms, ptr, 2); ptr += 2; // bool bTrackingValid = params & 0x01; // 0x01 : rigid body was successfully tracked in this frame } } // next rigid body // ========== SKELETONS ========== // Skeletons (version 2.1 and later) - if(((natnet_major == 2) && (natnet_minor>0)) || (natnet_major>2)) - { + 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++) - { + for (j = 0; j < nSkeletons; j++) { // Skeleton id int skeletonID = 0; memcpy(&skeletonID, ptr, 4); ptr += 4; @@ -321,8 +317,7 @@ void natnet_parse(unsigned char *in) { int nRigidBodies = 0; memcpy(&nRigidBodies, ptr, 4); ptr += 4; printf_natnet("Rigid Body Count : %d\n", nRigidBodies); - for (j=0; j < nRigidBodies; j++) - { + 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; @@ -333,32 +328,32 @@ void natnet_parse(unsigned char *in) { 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); + 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); + 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); + 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); + 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]); + 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 @@ -366,25 +361,26 @@ void natnet_parse(unsigned char *in) { printf_natnet("Mean marker error: %3.2f\n", fError); // Release resources - if(markerIDs) + if (markerIDs) { free(markerIDs); - if(markerSizes) + } + if (markerSizes) { free(markerSizes); - if(markerData) + } + 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)) - { + 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++) - { + 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; @@ -392,7 +388,7 @@ void natnet_parse(unsigned char *in) { 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("pos : [%3.2f,%3.2f,%3.2f]\n", x, y, z); printf_natnet("size: [%3.2f]\n", size); } } @@ -409,45 +405,47 @@ void natnet_parse(unsigned char *in) { // End of data tag int eod = 0; memcpy(&eod, ptr, 4); ptr += 4; printf_natnet("End Packet\n-------------\n"); - } - else - { + } else { printf("Error: Unrecognized packet type from Optitrack NatNet.\n"); } } /** The transmitter periodic function */ -gboolean timeout_transmit_callback(gpointer data) { +gboolean timeout_transmit_callback(gpointer data) +{ int i; // Loop trough all the available rigidbodies (TODO: optimize) - for(i = 0; i < MAX_RIGIDBODIES; i++) { + 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); + 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 (follow) this rigid - if(aircrafts[rigidBodies[i].id].ac_id == 0) + if (aircrafts[rigidBodies[i].id].ac_id == 0) { continue; + } // When we don track anymore and timeout or start tracking - if(rigidBodies[i].nSamples < 1 - && aircrafts[rigidBodies[i].id].connected - && (natnet_latency - aircrafts[rigidBodies[i].id].lastSample) > CONNECTION_TIMEOUT) { + if (rigidBodies[i].nSamples < 1 + && aircrafts[rigidBodies[i].id].connected + && (natnet_latency - aircrafts[rigidBodies[i].id].lastSample) > CONNECTION_TIMEOUT) { aircrafts[rigidBodies[i].id].connected = FALSE; fprintf(stderr, "#error Lost tracking rigid id %d, aircraft id %d.\n", - rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id); - } - else if(rigidBodies[i].nSamples > 0 && !aircrafts[rigidBodies[i].id].connected) { + rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id); + } else if (rigidBodies[i].nSamples > 0 && !aircrafts[rigidBodies[i].id].connected) { fprintf(stderr, "#pragma message: Now tracking rigid id %d, aircraft id %d.\n", - rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id); + rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id); } // Check if we still track the rigid - if(rigidBodies[i].nSamples < 1) + if (rigidBodies[i].nSamples < 1) { continue; + } // Update the last tracked aircrafts[rigidBodies[i].id].connected = TRUE; @@ -466,15 +464,15 @@ gboolean timeout_transmit_callback(gpointer data) { 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); + 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) { + 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); + ((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; @@ -485,7 +483,7 @@ gboolean timeout_transmit_callback(gpointer data) { 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); + ecef_of_enu_vect_d(&rigidBodies[i].ecef_vel , &tracking_ltp , &speed); } // Copy the quaternions and convert to euler angles for the heading @@ -496,62 +494,103 @@ gboolean timeout_transmit_callback(gpointer data) { double_eulers_of_quat(&orient_eulers, &orient); // Calculate the heading by adding the Natnet offset angle and normalizing it - double heading = -orient_eulers.psi+90.0/57.6 - tracking_offset_angle; //the optitrack axes are 90 degrees rotated wrt ENU + double heading = -orient_eulers.psi + 90.0 / 57.6 - + tracking_offset_angle; //the optitrack axes are 90 degrees rotated wrt ENU NormRadAngle(heading); - printf_debug("[%d -> %d]Samples: %d\t%d\t\tTiming: %3.3f latency\n", rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id - , rigidBodies[i].nSamples, rigidBodies[i].nVelocitySamples, natnet_latency); + printf_debug("[%d -> %d]Samples: %d\t%d\t\tTiming: %3.3f latency\n", rigidBodies[i].id, + aircrafts[rigidBodies[i].id].ac_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); + 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 (either small or big) - if(small_packets) { - /* The GPS messages are most likely too large to be send over either the datalink - * The local position is an int32 and the 10 LSBs of the x and y axis are compressed into + if (small_packets) { + /* The local position is an int32 and the 11 LSBs of the (signed) x and y axis are compressed into * a single integer. The z axis is considered unsigned and only the latter 10 LSBs are * used. */ - uint32_t pos_xyz = (((uint32_t)(pos.x*100.0)) & 0x3FF) << 22; // bits 31-22 x position in cm - pos_xyz |= (((uint32_t)(pos.y*100.0)) & 0x3FF) << 12; // bits 21-12 y position in cm - pos_xyz |= (((uint32_t)(pos.z*100.0)) & 0x3FF) << 2; // bits 11-2 z position in cm - // bits 1 and 0 are free + uint32_t pos_xyz; + // check if position within limits + if (fabs(pos.x * 100) < pow(2, 10)) { + pos_xyz = (((uint32_t)(pos.x * 100.0)) & 0x7FF) << 21; // bits 31-21 x position in cm + } else { + fprintf(stderr, "Warning!! X position out of maximum range of small message (±%.2fm): %.2f", pow(2, 10) / 100, pos.x); + pos_xyz = (((uint32_t)(pow(2, 10) * pos.x / fabs(pos.x))) & 0x7FF) << 21; // bits 31-21 x position in cm + } + + if (fabs(pos.y * 100) < pow(2, 10)) { + pos_xyz |= (((uint32_t)(pos.y * 100.0)) & 0x7FF) << 10; // bits 20-10 y position in cm + } else { + fprintf(stderr, "Warning!! Y position out of maximum range of small message (±%.2fm): %.2f", pow(2, 10) / 100, pos.y); + pos_xyz |= (((uint32_t)(pow(2, 10) * pos.y / fabs(pos.y))) & 0x7FF) << 10; // bits 20-10 y position in cm + } + + if (pos.z * 100 < pow(2, 10)) { + pos_xyz |= (((uint32_t)(pos.z * 100.0)) & 0x3FF); // bits 9-0 z position in cm + } else { + fprintf(stderr, "Warning!! Z position out of maximum range of small message (%.2fm): %.2f", pow(2, 10) / 100, pos.z); + pos_xyz |= (((uint32_t)(pow(2, 10))) & 0x3FF); // bits 9-0 z position in cm + } // printf("ENU Pos: %u (%.2f, %.2f, %.2f)\n", pos_xyz, pos.x, pos.y, pos.z); - uint32_t speed_xy = (((uint32_t)(speed.x*100.0)) & 0x3FF) << 22; // bits 31-22 speed x in cm/s - speed_xy |= (((uint32_t)(speed.y*100.0)) & 0x3FF) << 12; // bits 21-12 speed y in cm/s - speed_xy |= (((uint32_t)(heading*100.0)) & 0x3FF) << 2; // bits 11-2 heading in rad*1e2 (The heading is already subsampled) - // bits 1 and 0 are free + /* The speed is an int32 and the 11 LSBs of the x and y axis and 10 LSBs of z (all signed) are compressed into + * a single integer. + */ + uint32_t speed_xyz; + // check if speed within limits + if (fabs(speed.x * 100) < pow(2, 10)) { + speed_xyz = (((uint32_t)(speed.x * 100.0)) & 0x7FF) << 21; // bits 31-21 speed x in cm/s + } else { + fprintf(stderr, "Warning!! X Speed out of maximum range of small message (±%.2fm/s): %.2f", pow(2, 10) / 100, speed.x); + speed_xyz = (((uint32_t)(pow(2, 10) * speed.x / fabs(speed.x))) & 0x7FF) << 21; // bits 31-21 speed x in cm/s + } + + if (fabs(speed.y * 100) < pow(2, 10)) { + speed_xyz |= (((uint32_t)(speed.y * 100.0)) & 0x7FF) << 10; // bits 20-10 speed y in cm/s + } else { + fprintf(stderr, "Warning!! Y Speed out of maximum range of small message (±%.2fm/s): %.2f", pow(2, 10) / 100, speed.y); + speed_xyz |= (((uint32_t)(pow(2, 10) * speed.y / fabs(speed.y))) & 0x7FF) << 10; // bits 20-10 speed y in cm/s + } + + if (fabs(pos.z * 100) < pow(2, 9)) { + speed_xyz |= (((uint32_t)(speed.z * 100.0)) & 0x3FF); // bits 9-0 speed z in cm/s + } else { + fprintf(stderr, "Warning!! Z Speed out of maximum range of small message (±%.2fm/s): %.2f", pow(2, 9) / 100, speed.z); + speed_xyz |= (((uint32_t)(pow(2, 9) * speed.z / fabs(speed.z))) & 0x3FF); // bits 9-0 speed z in cm/s + } // printf("ENU Vel: %u (%.2f, %.2f, 0.0)\n", speed_xy, speed.x, speed.y); // printf("Heading: %.2f\n", heading); - IvySendMsg("0 REMOTE_GPS_SMALL %d %d %d %d", aircrafts[rigidBodies[i].id].ac_id, // uint8 rigid body ID (1 byte) - (uint8_t)rigidBodies[i].nMarkers, // status (1 byte) - pos_xyz, //uint32 ENU X, Y and Z in CM (4 bytes) - speed_xy); //uint32 ENU velocity X, Y in cm/s and heading in rad*1e2 (4 bytes) - } - else { + IvySendMsg("0 REMOTE_GPS_SMALL %d %d %d %d %d", aircrafts[rigidBodies[i].id].ac_id, // uint8 rigid body ID (1 byte) + (uint8_t)rigidBodies[i].nMarkers, // uint8 Number of markers (sv_num) (1 byte) + pos_xyz, // uint32 ENU X, Y and Z in CM (4 bytes) + speed_xyz, // uint32 ENU velocity X, Y, Z in cm/s (4 bytes) + (int16_t)(heading * 10000)); // int6_t heading in rad*1e4 (2 bytes) + + } else { IvySendMsg("0 REMOTE_GPS %d %d %d %d %d %d %d %d %d %d %d %d %d %d", aircrafts[rigidBodies[i].id].ac_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 + 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) { + if (rigidBodies[i].nVelocitySamples >= min_velocity_samples) { rigidBodies[i].vel_x = 0; rigidBodies[i].vel_y = 0; rigidBodies[i].vel_z = 0; @@ -567,7 +606,8 @@ gboolean timeout_transmit_callback(gpointer data) { } /** The NatNet sampler periodic function */ -static gboolean sample_data(GIOChannel *chan, GIOCondition cond, gpointer data) { +static gboolean sample_data(GIOChannel *chan, GIOCondition cond, gpointer data) +{ static unsigned char buffer_data[MAX_PACKETSIZE]; static int bytes_data = 0; @@ -575,7 +615,7 @@ static gboolean sample_data(GIOChannel *chan, GIOCondition cond, gpointer data) bytes_data += udp_socket_recv(&natnet_data, buffer_data, MAX_PACKETSIZE); // Parse NatNet data - if(bytes_data >= 2 && bytes_data >= buffer_data[1]) { + if (bytes_data >= 2 && bytes_data >= buffer_data[1]) { natnet_parse(buffer_data); bytes_data = 0; } @@ -585,7 +625,8 @@ static gboolean sample_data(GIOChannel *chan, GIOCondition cond, gpointer data) /** Print the program help */ -void print_help(char* filename) { +void print_help(char *filename) +{ static const char *usage = "Usage: %s [options]\n" " Options :\n" @@ -613,8 +654,9 @@ void print_help(char* filename) { } /** Check the amount of arguments */ -void check_argcount(int argc, char** argv, int i, int expected) { - if(i+expected >= argc) { +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); @@ -622,30 +664,31 @@ void check_argcount(int argc, char** argv, int i, int expected) { } /** Parse the options from the commandline */ -static void parse_options(int argc, char** argv) { +static void parse_options(int argc, char **argv) +{ int i, count_ac = 0; - for(i = 1; i < argc; ++i) { + for (i = 1; i < argc; ++i) { // Print help - if(strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-h") == 0) { + 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) { + 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) { + 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) { + 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); @@ -655,19 +698,19 @@ static void parse_options(int argc, char** argv) { } // Set the NatNet multicast address - else if(strcmp(argv[i], "-multicast_addr") == 0) { + 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) { + 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) { + else if (strcmp(argv[i], "-version") == 0) { check_argcount(argc, argv, i, 1); float version = atof(argv[++i]); @@ -675,13 +718,13 @@ static void parse_options(int argc, char** argv) { natnet_minor = (uint8_t)(version * 10.0) % 10; } // Set the NatNet server data port - else if(strcmp(argv[i], "-data_port") == 0) { + 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) { + else if (strcmp(argv[i], "-cmd_port") == 0) { check_argcount(argc, argv, i, 1); natnet_cmd_port = atoi(argv[++i]); @@ -710,31 +753,31 @@ static void parse_options(int argc, char** argv) { 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) { + 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) { + 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) { + else if (strcmp(argv[i], "-vel_samples") == 0) { check_argcount(argc, argv, i, 1); min_velocity_samples = atoi(argv[++i]); } // Set to use small packets - else if(strcmp(argv[i], "-small") == 0) { + else if (strcmp(argv[i], "-small") == 0) { small_packets = TRUE; } // Set the ivy bus - else if(strcmp(argv[i], "-ivy_bus") == 0) { + else if (strcmp(argv[i], "-ivy_bus") == 0) { check_argcount(argc, argv, i, 1); ivy_bus = argv[++i]; @@ -749,14 +792,14 @@ static void parse_options(int argc, char** argv) { } // Check if at least one aircraft is set - if(count_ac < 1) { + 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) +int main(int argc, char **argv) { // Set the default tracking system position and angle struct EcefCoor_d tracking_ecef; @@ -768,10 +811,12 @@ int main(int argc, char** argv) // 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)); + 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); + 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); udp_socket_create(&natnet_data, "", -1, natnet_data_port, 0); // Only receiving udp_socket_subscribe_multicast(&natnet_data, natnet_multicast_addr); udp_socket_set_recvbuf(&natnet_data, 0x100000); // 1MB @@ -787,8 +832,8 @@ int main(int argc, char** argv) // 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); + freq_transmit, min_velocity_samples); + g_timeout_add(1000 / freq_transmit, timeout_transmit_callback, NULL); GIOChannel *sk = g_io_channel_unix_new(natnet_data.sockfd); g_io_add_watch(sk, G_IO_IN | G_IO_NVAL | G_IO_HUP, diff --git a/sw/ground_segment/multimon/Makefile b/sw/ground_segment/multimon/Makefile deleted file mode 100644 index 03f60be811..0000000000 --- a/sw/ground_segment/multimon/Makefile +++ /dev/null @@ -1,151 +0,0 @@ -# Hey Emacs, this is a -*- makefile -*- -# -# Copyright (C) 2003-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. - -# Quiet compilation -# Launch with "make Q=''" to get full command display -Q=@ - -DEBUG=n -MACHINE:=$(shell uname -m) -AS86=as86 -0 -a -LD86=ld86 -0 -AS=as -LD=ld -LDFLAGS=-lm -HOSTCC=gcc -CC=gcc -MAKE=make -CPP=$(CC) -E -AR=ar -STRIP=strip -MKDIR=mkdir - -include ../../Makefile.ocaml - -CFLAGS=-Wall -Wstrict-prototypes -I/usr/X11R6/include -I`$(OCAMLC) -where` -ifeq ($(DEBUG),y) - CFLAGS += -g -O -else - CFLAGS += -O3 -endif - -ifeq ($(MACHINE),'i686') - CFLAGS += -march=i486 -falign-loops=2 -falign-jumps=2 -falign-functions=2 -DARCH_I386 -else -# PositionIndependentCode for all other architectures - CFLAGS += -fPIC -endif - -LDFLAGSX = -lX11 -L/usr/X11R6/lib - -#BINDIR = bin-$(shell uname -m) -BINDIR =. - -UNAME = $(shell uname -s) -ifneq (,$(findstring $(UNAME),linux Linux)) - OBJFILES=pprzlib.o hdlc.o demod_afsk12.o demodml.o costabi.o gen_hdlc.o ml_hdlc.o demod.cmo hdlc.cmo - ALLTARGETS=$(BINDIR)/multimon multimon.cma -endif -ifeq ($(UNAME),Darwin) - OBJFILES=demodml.o ml_hdlc.o demod.cmo hdlc.cmo - ALLTARGETS=multimon.cma -endif - -all: $(ALLTARGETS) - -multimon.cma: $(OBJFILES) - @echo OLD $@ - $(Q)ocamlmklib -o multimon $^ - - -$(BINDIR)/%.s: %.c - $(CC) $(CFLAGS) -S -o $@ $< - -$(BINDIR)/%.o: $(BINDIR)/%.s - $(AS) -c -o $@ $< - -$(BINDIR)/%.o: %.c - @echo CC $< - $(Q)$(CC) $(CFLAGS) -c -o $@ $< - -SRC_L2 = hdlc.c pprz.c -SRC_L1 = demod_afsk48p.c demod_display.c -SRC_MISC = unixinput.c xdisplay.c - -SRC_GEN = gen.c gen_dtmf.c gen_sin.c gen_zvei.c gen_hdlc.c costabi.c - -OBJ_L2 = $(SRC_L2:%.c=$(BINDIR)/%.o) -OBJ_L1 = $(SRC_L1:%.c=$(BINDIR)/%.o) -OBJ_MISC = $(SRC_MISC:%.c=$(BINDIR)/%.o) - -OBJ_GEN = $(SRC_GEN:%.c=$(BINDIR)/%.o) - -$(BINDIR): - $(MKDIR) $(BINDIR) - -$(BINDIR)/multimon: $(OBJ_L2) $(OBJ_L1) $(OBJ_MISC) - @echo LD $@ - $(Q)$(CC) $^ $(LDFLAGS) $(LDFLAGSX) -o $@ - -$(BINDIR)/gen: $(OBJ_GEN) - @echo LD $@ - $(Q)$(CC) $^ $(LDFLAGS) -o $@ - -$(BINDIR)/mkcostab: $(BINDIR)/mkcostab.o - @echo LD $@ - $(Q)$(CC) $^ $(LDFLAGS) -o $@ - -costabi.c costabf.c: $(BINDIR)/mkcostab - @echo EXEC $< - $(Q)$(BINDIR)/mkcostab - - -libtest: pprzlib.o demodml.c demod.ml test.ml - $(OCAMLC) -custom -o $@ pprzlib.o demodml.c -I +lablgtk2 unix.cma lablgtk.cma demod.ml test.ml - -hdlc_test : multimon.cma test_gen_hdlc.ml - $(OCAMLC) -o $@ -custom -I +lablgtk2 -thread unix.cma threads.cma lablgtk.cma gtkThread.cmo -I . $^ -cclib -ljack - -hdlc.cmo : hdlc.cmi - -%.cmo : %.ml - @echo OC $< - $(Q)$(OCAMLC) -c $< - -%.cmi : %.mli - @echo OC $< - $(Q)$(OCAMLC) $< - -clean: - $(Q)rm -fr *.cm* mkcostab .depend - $(Q)$(RM) -f core `find . -name '*.[oas]' -print` *.so - $(Q)$(RM) -f core `find . -name 'core' -print` - $(Q)$(RM) -f core costabi.c costabf.c *~ - $(Q)$(RM) $(BINDIR)/multimon - -.PHONY: all clean depend dep - -depend dep: - $(CPP) -M $(CFLAGS) $(SRC_MISC) $(SRC_L1) $(SRC_L2) $(SRC_GEN) mkcostab.c > $(BINDIR)/.depend - -ifeq ($(BINDIR)/.depend,$(wildcard $(BINDIR)/.depend)) -include $(BINDIR)/.depend -endif diff --git a/sw/ground_segment/multimon/README b/sw/ground_segment/multimon/README deleted file mode 100644 index c98c062bc8..0000000000 --- a/sw/ground_segment/multimon/README +++ /dev/null @@ -1,34 +0,0 @@ -Linux Radio Transmission Decoder -by Thomas Sailer, HB9JNX/AE4WA -Paparazzi adaptions added by Martin Mueller, DL3FCC - -Description - -The multimon software can decode a variety of digital transmission modes commonly found on UHF radio. A standard PC soundcard is used to acquire the signal from a transceiver. The decoding is done completely in software. Currently, the following modes are supported: - - * AX.25 - o 1200 Baud AFSK - o 2400 Baud AFSK (2 variants) - o 4800 Baud HAPN - o 9600 Baud FSK (G3RUH) - * POCSAG - o 512 Baud - o 1200 Baud - o 2400 Baud - * Miscellaneous - o DTMF - o ZVEI - o 4800 Baud AFSK PPRZ (baudot and hdlc) - -An arbitrary set of the above modes may run concurrently on the same input signal (provided the CPU power is sufficient), so you do not have to know in advance which mode is used. Note however that some modes might require modifications to the radio (especially the 9600 baud FSK and the POCSAG modes) to work properly. - -POCSAG (Post Office Code Standards Advisory Group) is a common paging transmission format. -Download - -PPRZ is a proprietary 4800 Baud baudot-like protocol. This is used by the Paparazzi project which is located at http://www.nongnu.org/paparazzi - -Please note that monitoring commercial services may be prohibited in some countries, this software should therefore only be used to monitor the amateur radio service. - -The software is published under the GNU GPL V2 - -The original software can be found at http://www.baycom.org/~tom/ham/linux/multimon.html diff --git a/sw/ground_segment/multimon/demod.ml b/sw/ground_segment/multimon/demod.ml deleted file mode 100644 index 6ca6d13f4f..0000000000 --- a/sw/ground_segment/multimon/demod.ml +++ /dev/null @@ -1,2 +0,0 @@ -external init : string -> Unix.file_descr = "ml_demod_init" -external get_data : unit -> string * string = "ml_demod_get_data" diff --git a/sw/ground_segment/multimon/demod_afsk12.c b/sw/ground_segment/multimon/demod_afsk12.c deleted file mode 100644 index 3c74a459ab..0000000000 --- a/sw/ground_segment/multimon/demod_afsk12.c +++ /dev/null @@ -1,128 +0,0 @@ -/* - * demod_afsk12.c -- 1200 baud AFSK demodulator - * - * Copyright (C) 1996 - * Thomas Sailer (sailer@ife.ee.ethz.ch, hb9jnx@hb9w.che.eu) - * - * This program 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 of the License, or - * (at your option) any later version. - * - * This program 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 this program; if not, write to the Free Software - * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. - */ - -/* ---------------------------------------------------------------------- */ - -#include "multimon.h" -#include "filter.h" -#include -#include - -/* ---------------------------------------------------------------------- */ - -/* - * Standard TCM3105 clock frequency: 4.4336MHz - * Mark frequency: 2200 Hz - * Space frequency: 1200 Hz - */ - -#define FREQ_MARK 1200 -#define FREQ_SPACE 2200 -#define FREQ_SAMP 22050 -#define BAUD 1200 -#define SUBSAMP 2 - -/* ---------------------------------------------------------------------- */ - -#define CORRLEN ((int)(FREQ_SAMP/BAUD)) -#define SPHASEINC (0x10000u*BAUD*SUBSAMP/FREQ_SAMP) - -static float corr_mark_i[CORRLEN]; -static float corr_mark_q[CORRLEN]; -static float corr_space_i[CORRLEN]; -static float corr_space_q[CORRLEN]; - -/* ---------------------------------------------------------------------- */ - -void afsk12_init(struct demod_state *s) -{ - float f; - int i; - - hdlc_init(s); - memset(&s->l1.afsk12, 0, sizeof(s->l1.afsk12)); - for (f = 0, i = 0; i < CORRLEN; i++) { - corr_mark_i[i] = cos(f); - corr_mark_q[i] = sin(f); - f += 2.0*M_PI*FREQ_MARK/FREQ_SAMP; - } - for (f = 0, i = 0; i < CORRLEN; i++) { - corr_space_i[i] = cos(f); - corr_space_q[i] = sin(f); - f += 2.0*M_PI*FREQ_SPACE/FREQ_SAMP; - } -} - -/* ---------------------------------------------------------------------- */ - -static void afsk12_demod(struct demod_state *s, float *buffer, int length) -{ - float f; - unsigned char curbit; - - if (s->l1.afsk12.subsamp) { - int numfill = SUBSAMP - s->l1.afsk12.subsamp; - if (length < numfill) { - s->l1.afsk12.subsamp += length; - return; - } - buffer += numfill; - length -= numfill; - s->l1.afsk12.subsamp = 0; - } - for (; length >= SUBSAMP; length -= SUBSAMP, buffer += SUBSAMP) { - f = fsqr(mac(buffer, corr_mark_i, CORRLEN)) + - fsqr(mac(buffer, corr_mark_q, CORRLEN)) - - fsqr(mac(buffer, corr_space_i, CORRLEN)) - - fsqr(mac(buffer, corr_space_q, CORRLEN)); - s->l1.afsk12.dcd_shreg <<= 1; - s->l1.afsk12.dcd_shreg |= (f > 0); - verbprintf(10, "%c", '0'+(s->l1.afsk12.dcd_shreg & 1)); - /* - * check if transition - */ - if ((s->l1.afsk12.dcd_shreg ^ (s->l1.afsk12.dcd_shreg >> 1)) & 1) { - if (s->l1.afsk12.sphase < (0x8000u-(SPHASEINC/2))) - s->l1.afsk12.sphase += SPHASEINC/8; - else - s->l1.afsk12.sphase -= SPHASEINC/8; - } - s->l1.afsk12.sphase += SPHASEINC; - if (s->l1.afsk12.sphase >= 0x10000u) { - s->l1.afsk12.sphase &= 0xffffu; - s->l1.afsk12.lasts <<= 1; - s->l1.afsk12.lasts |= s->l1.afsk12.dcd_shreg & 1; - curbit = (s->l1.afsk12.lasts ^ - (s->l1.afsk12.lasts >> 1) ^ 1) & 1; - verbprintf(9, " %c ", '0'+curbit); - hdlc_rxbit(s, curbit); - } - } - s->l1.afsk12.subsamp = length; -} - -/* ---------------------------------------------------------------------- */ - -const struct demod_param demod_afsk1200 = { - "AFSK1200", FREQ_SAMP, CORRLEN, afsk12_init, afsk12_demod -}; - -/* ---------------------------------------------------------------------- */ diff --git a/sw/ground_segment/multimon/demod_afsk48p.c b/sw/ground_segment/multimon/demod_afsk48p.c deleted file mode 100644 index f7734b5175..0000000000 --- a/sw/ground_segment/multimon/demod_afsk48p.c +++ /dev/null @@ -1,134 +0,0 @@ -/* - * demod_afsk48p.c -- 4800 baud AFSK demodulator for paparazzi - * - * Copyright (C) 1996 - * Thomas Sailer (sailer@ife.ee.ethz.ch, hb9jnx@hb9w.che.eu) - * - * This program 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 of the License, or - * (at your option) any later version. - * - * This program 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 this program; if not, write to the Free Software - * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. - */ - -/* ---------------------------------------------------------------------- */ - -#include "multimon.h" -#include "filter.h" -#include "pprz.h" -#include -#include -/* ---------------------------------------------------------------------- */ - -/* - * Standard CMX469A clock frequency: 4.032 Mhz - * Xtal used: 4 MHz - * Ratio: 0.992063 - * Mark frequency: 4761.905 Hz - * Space frequency: 2380.952 Hz - */ - -#define FREQ_MARK 4762 -#define FREQ_SPACE 2381 -#define FREQ_SAMP 22050 -//#define FREQ_SAMP 44100 -#define BAUD 4762 - -/* ---------------------------------------------------------------------- */ - -#define CORRLEN (2*(int)(FREQ_SAMP/BAUD)) -#define SPHASEINC (0x10000u*BAUD/FREQ_SAMP) - -static float corr_mark_i[CORRLEN]; -static float corr_mark_q[CORRLEN]; -static float corr_space_i[CORRLEN]; -static float corr_space_q[CORRLEN]; - -/* ---------------------------------------------------------------------- */ - -static void afsk48p_init(struct demod_state *s) -{ - float f; - int i; - - pprz_init(s); - memset(&s->l1.afsk48p, 0, sizeof(s->l1.afsk48p)); - for (f = 0, i = 0; i < CORRLEN; i++) { - corr_mark_i[i] = cos(f); - corr_mark_q[i] = sin(f); - f += 2.0*M_PI*FREQ_MARK/FREQ_SAMP; - } - for (f = 0, i = 0; i < CORRLEN; i++) { - corr_space_i[i] = cos(f); - corr_space_q[i] = sin(f); - f += 2.0*M_PI*FREQ_SPACE/FREQ_SAMP; - } - for (i = 0; i < CORRLEN; i++) { - f = 0.54 - 0.46*cos(2*M_PI*i/(float)(CORRLEN-1)); - corr_mark_i[i] *= f; - corr_mark_q[i] *= f; - corr_space_i[i] *= f; - corr_space_q[i] *= f; - } - - s->l1.afsk48p.sample_count = 0; -} - -/* ---------------------------------------------------------------------- */ - -static void afsk48p_demod(struct demod_state *s, float *buffer, int length) -{ - float f; - unsigned char curbit; - - s->l1.afsk48p.sample_count += length; - if (s->l1.afsk48p.sample_count > FREQ_SAMP) { - s->l1.afsk48p.sample_count -= FREQ_SAMP; - pprz_status(s); - } - - for (; length > 0; length--, buffer++) { - f = fsqr(mac(buffer, corr_mark_i, CORRLEN)) + - fsqr(mac(buffer, corr_mark_q, CORRLEN)) - - fsqr(mac(buffer, corr_space_i, CORRLEN)) - - fsqr(mac(buffer, corr_space_q, CORRLEN)); - s->l1.afsk48p.dcd_shreg <<= 1; - s->l1.afsk48p.dcd_shreg |= (f > 0); - verbprintf(10, "%c", '0'+(s->l1.afsk48p.dcd_shreg & 1)); - /* - * check if transition - */ - if ((s->l1.afsk48p.dcd_shreg ^ (s->l1.afsk48p.dcd_shreg >> 1)) & 1) { - if (s->l1.afsk48p.sphase < (0x8000u-(SPHASEINC/2))) - s->l1.afsk48p.sphase += SPHASEINC/8; - else - s->l1.afsk48p.sphase -= SPHASEINC/8; - } - s->l1.afsk48p.sphase += SPHASEINC; - if (s->l1.afsk48p.sphase >= 0x10000u) { - s->l1.afsk48p.sphase &= 0xffffu; - s->l1.afsk48p.lasts <<= 1; - s->l1.afsk48p.lasts |= s->l1.afsk48p.dcd_shreg & 1; - /* we use direct coded bits, not NRZI */ - curbit = (s->l1.afsk48p.lasts ^ 1) & 1; - verbprintf(9, " %c ", '0'+curbit); - pprz_baudot_rxbit(s, curbit); - } - } -} - -/* ---------------------------------------------------------------------- */ - -const struct demod_param demod_afsk4800p = { - "AFSK4800_P", FREQ_SAMP, CORRLEN, afsk48p_init, afsk48p_demod -}; - -/* ---------------------------------------------------------------------- */ diff --git a/sw/ground_segment/multimon/demod_display.c b/sw/ground_segment/multimon/demod_display.c deleted file mode 100644 index ee9703b933..0000000000 --- a/sw/ground_segment/multimon/demod_display.c +++ /dev/null @@ -1,86 +0,0 @@ -/* - * demod_display.c -- signal display - * - * Copyright (C) 1996 - * Thomas Sailer (sailer@ife.ee.ethz.ch, hb9jnx@hb9w.che.eu) - * - * This program 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 of the License, or - * (at your option) any later version. - * - * This program 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 this program; if not, write to the Free Software - * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. - */ - -/* ---------------------------------------------------------------------- */ - -#include "multimon.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/* ---------------------------------------------------------------------- */ - -#define SAMPLING_RATE 22050 - -/* ---------------------------------------------------------------------- */ - -static void scope_init(struct demod_state *s) -{ - memset(&s->l1.scope, 0, sizeof(s->l1.scope)); - s->l1.scope.dispnum = xdisp_start(); - if (s->l1.scope.dispnum == -1) - return; -} - -/* ---------------------------------------------------------------------- */ - -#define MEMSIZE sizeof(s->l1.scope.data)/sizeof(s->l1.scope.data[0]) - -static void scope_demod(struct demod_state *s, float *buffer, int length) -{ - float *src, *dst; - int i; - - if (s->l1.scope.dispnum == -1) - return; - if (length >= MEMSIZE) { - src = buffer+length-MEMSIZE; - dst = s->l1.scope.data; - i = MEMSIZE; - } else { - i = MEMSIZE-length; - memmove(s->l1.scope.data, s->l1.scope.data+i, - i*sizeof(s->l1.scope.data[0])); - src = buffer; - dst = s->l1.scope.data+i; - i = length; - } - s->l1.scope.datalen += i; - memcpy(dst, src, i*sizeof(s->l1.scope.data[0])); - if (s->l1.scope.datalen < MEMSIZE) - return; - if (xdisp_update(s->l1.scope.dispnum, s->l1.scope.data)) - s->l1.scope.datalen = 0; -} - -/* ---------------------------------------------------------------------- */ - -const struct demod_param demod_scope = { - "SCOPE", SAMPLING_RATE, 0, scope_init, scope_demod -}; - -/* ---------------------------------------------------------------------- */ diff --git a/sw/ground_segment/multimon/demodml.c b/sw/ground_segment/multimon/demodml.c deleted file mode 100644 index c37399458d..0000000000 --- a/sw/ground_segment/multimon/demodml.c +++ /dev/null @@ -1,48 +0,0 @@ -#include -#include -#include -#include - -#include "pprzlib.h" - -value -ml_demod_init(value dev) { -#ifndef __APPLE__ - int fd = pprz_demod_init(String_val(dev)); - return Val_int(fd); -#else - failwith("Not supported under OSX"); - return Val_int(0); -#endif - -} - -value -ml_demod_get_data(value unit) { -#ifndef __APPLE__ - struct data *data = pprz_demod_read_data(); - if (data) { - int i; - - CAMLparam0(); - CAMLlocal3 (result,l, r); - result = alloc(2, 0); - l = alloc_string(data->len_left); - for(i = 0; i < data->len_left; i++) Byte(l, i) = data->data_left[i]; - r = alloc_string(data->len_right); - for(i = 0; i < data->len_right; i++) Byte(r, i) = data->data_right[i]; - - Store_field(result, 0, l); - Store_field(result, 1, r); - CAMLreturn (result); - } else { - failwith("End of file"); - } -#else - CAMLparam0(); - CAMLlocal3 (result,l, r); - result = alloc(2, 0); - failwith("Not supported under OSX"); - CAMLreturn (result); -#endif -} diff --git a/sw/ground_segment/multimon/filter-i386.h b/sw/ground_segment/multimon/filter-i386.h deleted file mode 100644 index 62180d28bd..0000000000 --- a/sw/ground_segment/multimon/filter-i386.h +++ /dev/null @@ -1,458 +0,0 @@ -/* - * filter-i386.h -- optimized filter routines - * - * Copyright (C) 1996 - * Thomas Sailer (sailer@ife.ee.ethz.ch, hb9jnx@hb9w.che.eu) - * - * This program 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 of the License, or - * (at your option) any later version. - * - * This program 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 this program; if not, write to the Free Software - * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. - */ - -/* ---------------------------------------------------------------------- */ - -#ifndef _FILTER_I386_H -#define _FILTER_I386_H - -/* ---------------------------------------------------------------------- */ - -#define __HAVE_ARCH_MAC -#define mac(a,b,size) \ -(__builtin_constant_p(size) ? __mac_c((a),(b),(size)) : __mac_g((a),(b),(size))) - -#include - -static inline float __mac_g(const float *a, const float *b, unsigned int size) -{ - float sum = 0; - unsigned int i; - - for (i = 0; i < size; i++) - sum += (*a++) * (*b++); - return sum; -} - -static inline float __mac_c(const float *a, const float *b, unsigned int size) -{ - float f; - - /* - * inspired from Phil Karn, KA9Q's home page - */ - switch (size) { - case 8: - asm volatile ("flds (%1);\n\t" - "fmuls (%2);\n\t" - "flds 4(%1);\n\t" - "fmuls 4(%2);\n\t" - "flds 8(%1);\n\t" - "fmuls 8(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 12(%1);\n\t" - "fmuls 12(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 16(%1);\n\t" - "fmuls 16(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 20(%1);\n\t" - "fmuls 20(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 24(%1);\n\t" - "fmuls 24(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 28(%1);\n\t" - "fmuls 28(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "faddp;\n\t" : - "=t" (f) : - "r" (a), - "r" (b) : "memory"); - return f; - - case 9: - asm volatile ("flds (%1);\n\t" - "fmuls (%2);\n\t" - "flds 4(%1);\n\t" - "fmuls 4(%2);\n\t" - "flds 8(%1);\n\t" - "fmuls 8(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 12(%1);\n\t" - "fmuls 12(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 16(%1);\n\t" - "fmuls 16(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 20(%1);\n\t" - "fmuls 20(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 24(%1);\n\t" - "fmuls 24(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 28(%1);\n\t" - "fmuls 28(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 32(%1);\n\t" - "fmuls 32(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "faddp;\n\t" : - "=t" (f) : - "r" (a), - "r" (b) : "memory"); - return f; - - case 18: - asm volatile ("flds (%1);\n\t" - "fmuls (%2);\n\t" - "flds 4(%1);\n\t" - "fmuls 4(%2);\n\t" - "flds 8(%1);\n\t" - "fmuls 8(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 12(%1);\n\t" - "fmuls 12(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 16(%1);\n\t" - "fmuls 16(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 20(%1);\n\t" - "fmuls 20(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 24(%1);\n\t" - "fmuls 24(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 28(%1);\n\t" - "fmuls 28(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 32(%1);\n\t" - "fmuls 32(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 36(%1);\n\t" - "fmuls 36(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 40(%1);\n\t" - "fmuls 40(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 44(%1);\n\t" - "fmuls 44(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 48(%1);\n\t" - "fmuls 48(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 52(%1);\n\t" - "fmuls 52(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 56(%1);\n\t" - "fmuls 56(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 60(%1);\n\t" - "fmuls 60(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 64(%1);\n\t" - "fmuls 64(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 68(%1);\n\t" - "fmuls 68(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "faddp;\n\t" : - "=t" (f) : - "r" (a), - "r" (b) : "memory"); - return f; - - case 24: - asm volatile ("flds (%1);\n\t" - "fmuls (%2);\n\t" - "flds 4(%1);\n\t" - "fmuls 4(%2);\n\t" - "flds 8(%1);\n\t" - "fmuls 8(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 12(%1);\n\t" - "fmuls 12(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 16(%1);\n\t" - "fmuls 16(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 20(%1);\n\t" - "fmuls 20(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 24(%1);\n\t" - "fmuls 24(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 28(%1);\n\t" - "fmuls 28(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 32(%1);\n\t" - "fmuls 32(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 36(%1);\n\t" - "fmuls 36(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 40(%1);\n\t" - "fmuls 40(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 44(%1);\n\t" - "fmuls 44(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 48(%1);\n\t" - "fmuls 48(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 52(%1);\n\t" - "fmuls 52(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 56(%1);\n\t" - "fmuls 56(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 60(%1);\n\t" - "fmuls 60(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 64(%1);\n\t" - "fmuls 64(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 68(%1);\n\t" - "fmuls 68(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 72(%1);\n\t" - "fmuls 72(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 76(%1);\n\t" - "fmuls 76(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 80(%1);\n\t" - "fmuls 80(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 84(%1);\n\t" - "fmuls 84(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 88(%1);\n\t" - "fmuls 88(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 92(%1);\n\t" - "fmuls 92(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "faddp;\n\t" : - "=t" (f) : - "r" (a), - "r" (b) : "memory"); - return f; - case 36: - asm volatile ("flds (%1);\n\t" - "fmuls (%2);\n\t" - "flds 4(%1);\n\t" - "fmuls 4(%2);\n\t" - "flds 8(%1);\n\t" - "fmuls 8(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 12(%1);\n\t" - "fmuls 12(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 16(%1);\n\t" - "fmuls 16(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 20(%1);\n\t" - "fmuls 20(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 24(%1);\n\t" - "fmuls 24(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 28(%1);\n\t" - "fmuls 28(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 32(%1);\n\t" - "fmuls 32(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 36(%1);\n\t" - "fmuls 36(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 40(%1);\n\t" - "fmuls 40(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 44(%1);\n\t" - "fmuls 44(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 48(%1);\n\t" - "fmuls 48(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 52(%1);\n\t" - "fmuls 52(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 56(%1);\n\t" - "fmuls 56(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 60(%1);\n\t" - "fmuls 60(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 64(%1);\n\t" - "fmuls 64(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 68(%1);\n\t" - "fmuls 68(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 72(%1);\n\t" - "fmuls 72(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 76(%1);\n\t" - "fmuls 76(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 80(%1);\n\t" - "fmuls 80(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 84(%1);\n\t" - "fmuls 84(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 88(%1);\n\t" - "fmuls 88(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 92(%1);\n\t" - "fmuls 92(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 96(%1);\n\t" - "fmuls 96(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 100(%1);\n\t" - "fmuls 100(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 104(%1);\n\t" - "fmuls 104(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 108(%1);\n\t" - "fmuls 108(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 112(%1);\n\t" - "fmuls 112(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 116(%1);\n\t" - "fmuls 116(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 120(%1);\n\t" - "fmuls 120(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 124(%1);\n\t" - "fmuls 124(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 128(%1);\n\t" - "fmuls 128(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 132(%1);\n\t" - "fmuls 132(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 136(%1);\n\t" - "fmuls 136(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "flds 140(%1);\n\t" - "fmuls 140(%2);\n\t" - "fxch %%st(2);\n\t" - "faddp;\n\t" - "faddp;\n\t" : - "=t" (f) : - "r" (a), - "r" (b) : "memory"); - return f; - - default: - printf("Warning: optimize __mac_c(..., ..., %d)\n", size); - return __mac_g(a, b, size); - } -} - -/* ---------------------------------------------------------------------- */ -#endif /* _FILTER_I386_H */ - - - - diff --git a/sw/ground_segment/multimon/filter.h b/sw/ground_segment/multimon/filter.h deleted file mode 100644 index fbbbd415d1..0000000000 --- a/sw/ground_segment/multimon/filter.h +++ /dev/null @@ -1,110 +0,0 @@ -/* - * filter.h -- optimized filter routines - * - * Copyright (C) 1996 - * Thomas Sailer (sailer@ife.ee.ethz.ch, hb9jnx@hb9w.che.eu) - * - * This program 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 of the License, or - * (at your option) any later version. - * - * This program 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 this program; if not, write to the Free Software - * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. - */ - -/* ---------------------------------------------------------------------- */ - -#ifndef _FILTER_H -#define _FILTER_H - -/* ---------------------------------------------------------------------- */ - -#ifdef ARCH_I386 -#include "filter-i386.h" -#endif /* ARCH_I386 */ - -/* ---------------------------------------------------------------------- */ - -static inline unsigned int hweight32(unsigned int w) - __attribute__ ((unused)); -static inline unsigned int hweight16(unsigned short w) - __attribute__ ((unused)); -static inline unsigned int hweight8(unsigned char w) - __attribute__ ((unused)); - -static inline unsigned int hweight32(unsigned int w) -{ - unsigned int res = (w & 0x55555555) + ((w >> 1) & 0x55555555); - res = (res & 0x33333333) + ((res >> 2) & 0x33333333); - res = (res & 0x0F0F0F0F) + ((res >> 4) & 0x0F0F0F0F); - res = (res & 0x00FF00FF) + ((res >> 8) & 0x00FF00FF); - return (res & 0x0000FFFF) + ((res >> 16) & 0x0000FFFF); -} - -static inline unsigned int hweight16(unsigned short w) -{ - unsigned short res = (w & 0x5555) + ((w >> 1) & 0x5555); - res = (res & 0x3333) + ((res >> 2) & 0x3333); - res = (res & 0x0F0F) + ((res >> 4) & 0x0F0F); - return (res & 0x00FF) + ((res >> 8) & 0x00FF); -} - -static inline unsigned int hweight8(unsigned char w) -{ - unsigned short res = (w & 0x55) + ((w >> 1) & 0x55); - res = (res & 0x33) + ((res >> 2) & 0x33); - return (res & 0x0F) + ((res >> 4) & 0x0F); -} - -static inline unsigned int gcd(unsigned int x, unsigned int y) - __attribute__ ((unused)); -static inline unsigned int lcm(unsigned int x, unsigned int y) - __attribute__ ((unused)); - -static inline unsigned int gcd(unsigned int x, unsigned int y) -{ - for (;;) { - if (!x) - return y; - if (!y) - return x; - if (x > y) - x %= y; - else - y %= x; - } -} - -static inline unsigned int lcm(unsigned int x, unsigned int y) -{ - return x * y / gcd(x, y); -} - -/* ---------------------------------------------------------------------- */ - -#ifndef __HAVE_ARCH_MAC -static inline float mac(const float *a, const float *b, unsigned int size) -{ - float sum = 0; - unsigned int i; - - for (i = 0; i < size; i++) - sum += (*a++) * (*b++); - return sum; -} -#endif /* __HAVE_ARCH_MAC */ - -static inline float fsqr(float f) -{ - return f*f; -} - -/* ---------------------------------------------------------------------- */ -#endif /* _FILTER_H */ diff --git a/sw/ground_segment/multimon/gen.h b/sw/ground_segment/multimon/gen.h deleted file mode 100644 index 35eb20f3f0..0000000000 --- a/sw/ground_segment/multimon/gen.h +++ /dev/null @@ -1,97 +0,0 @@ -/* - * gen.h -- generate different test signals - * - * Copyright (C) 1997 - * Thomas Sailer (sailer@ife.ee.ethz.ch, hb9jnx@hb9w.che.eu) - * - * This program 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 of the License, or - * (at your option) any later version. - * - * This program 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 this program; if not, write to the Free Software - * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. - */ - -/* ---------------------------------------------------------------------- */ - -#define DEFAULT_SAMPLE_RATE 22050 -extern int hdlc_sample_rate; - -#define MS(x) ((float)(x)*SAMPLE_RATE/1000) - -extern const int costabi[0x400]; - -enum gen_type { gentype_dtmf, gentype_sine, gentype_zvei, gentype_hdlc }; - -struct gen_params { - enum gen_type type; - int ampl; - union { - struct { - int duration; - int pause; - char str[256]; - } dtmf; - struct { - int duration; - int freq; - } sine; - struct { - int duration; - int pause; - char str[256]; - } zvei; - struct { - int modulation; - int txdelay; - int pktlen; - unsigned char pkt[256]; - } hdlc; - } p; -}; - -struct gen_state { - union { - struct { - int ch_idx; - int ph_row, ph_col, phinc_row, phinc_col; - int time, time2; - } dtmf; - struct { - int ph, phinc; - int time; - } sine; - struct { - int ch_idx; - int ph, phinc; - int time, time2; - } zvei; - struct { - int lastb; - int ch_idx, bitmask; - unsigned int ph, phinc, bitph; - unsigned int datalen; - unsigned char data[512]; - } hdlc; - } s; -}; - -extern void gen_init_dtmf(struct gen_params *p, struct gen_state *s); -extern int gen_dtmf(signed short *buf, int buflen, struct gen_params *p, struct gen_state *s); - -extern void gen_init_sine(struct gen_params *p, struct gen_state *s); -extern int gen_sine(signed short *buf, int buflen, struct gen_params *p, struct gen_state *s); - -extern void gen_init_zvei(struct gen_params *p, struct gen_state *s); -extern int gen_zvei(signed short *buf, int buflen, struct gen_params *p, struct gen_state *s); - -extern void gen_init_hdlc(struct gen_params *p, struct gen_state *s); -extern int gen_hdlc(signed short *buf, int buflen, struct gen_params *p, struct gen_state *s); - diff --git a/sw/ground_segment/multimon/gen_hdlc.c b/sw/ground_segment/multimon/gen_hdlc.c deleted file mode 100644 index 3fef5f9c08..0000000000 --- a/sw/ground_segment/multimon/gen_hdlc.c +++ /dev/null @@ -1,216 +0,0 @@ -/* - * gen_hdlc.c -- generate DTMF sequences - * - * Copyright (C) 1997 - * Thomas Sailer (sailer@ife.ee.ethz.ch, hb9jnx@hb9w.che.eu) - * - * This program 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 of the License, or - * (at your option) any later version. - * - * This program 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 this program; if not, write to the Free Software - * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. - */ - -/* ---------------------------------------------------------------------- */ - -#include "gen.h" -#include - -#define COS(x) costabi[(((x)>>6)&0x3ffu)] - -/* ---------------------------------------------------------------------- */ -/* - * the CRC routines are stolen from WAMPES - * by Dieter Deyke - */ - -static const unsigned short crc_ccitt_table[] = { - 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, - 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, - 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, - 0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, - 0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd, - 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5, - 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, - 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974, - 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, - 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, - 0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, - 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, - 0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, - 0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, - 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738, - 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, - 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7, - 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, - 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, - 0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, - 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, - 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, - 0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134, - 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c, - 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, - 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb, - 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, - 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, - 0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, - 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, - 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, - 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78 -}; - -/*---------------------------------------------------------------------------*/ - -#if 0 -static inline void append_crc_ccitt(unsigned char *buffer, int len) -{ - unsigned int crc = 0xffff; - - for (;len>0;len--) - crc = (crc >> 8) ^ crc_ccitt_table[(crc ^ *buffer++) & 0xff]; - crc ^= 0xffff; - *buffer++ = crc; - *buffer++ = crc >> 8; -} - -/*---------------------------------------------------------------------------*/ - -static inline int check_crc_ccitt(const unsigned char *buf, int cnt) -{ - unsigned int crc = 0xffff; - - for (; cnt > 0; cnt--) - crc = (crc >> 8) ^ crc_ccitt_table[(crc ^ *buf++) & 0xff]; - return (crc & 0xffff) == 0xf0b8; -} -#endif - -/*---------------------------------------------------------------------------*/ - -static __inline__ int calc_crc_ccitt(const unsigned char *buf, int cnt) -{ - unsigned int crc = 0xffff; - - for (; cnt > 0; cnt--) - crc = (crc >> 8) ^ crc_ccitt_table[(crc ^ *buf++) & 0xff]; - crc ^= 0xffff; - return (crc & 0xffff); -} - -/* ---------------------------------------------------------------------- */ - -struct hdlctx { - unsigned int bitstream; - unsigned int bitbuf; - int numbits; -}; - -static void txb_addbyte(struct gen_state *s, struct hdlctx *hdlctx, - unsigned char bits, unsigned char stuff) -{ - unsigned int mask1, mask2; - unsigned int mask3; - int i; - - if (hdlctx->numbits >= 8) { - if (s->s.hdlc.datalen >= sizeof(s->s.hdlc.data)) - return; - s->s.hdlc.data[s->s.hdlc.datalen++] = hdlctx->bitbuf; - hdlctx->bitbuf >>= 8; - hdlctx->numbits -= 8; - } - hdlctx->bitbuf |= bits << hdlctx->numbits; - hdlctx->bitstream >>= 8; - hdlctx->bitstream |= bits << 8; - mask1 = 0x1f0; - mask2 = 0x100; - mask3 = 0xffffffff >> (31 - hdlctx->numbits); - hdlctx->numbits += 8; - if (!stuff) - goto nostuff; - for(i = 0; i < 8; i++, mask1 <<= 1, mask2 <<= 1, mask3 = (mask3 << 1) | 1) { - if ((hdlctx->bitstream & mask1) != mask1) - continue; - hdlctx->bitstream &= ~mask2; - hdlctx->bitbuf = (hdlctx->bitbuf & mask3) | ((hdlctx->bitbuf & (~mask3)) << 1); - hdlctx->numbits++; - mask3 = (mask3 << 1) | 1; - } - nostuff: - if (hdlctx->numbits >= 8) { - if (s->s.hdlc.datalen >= sizeof(s->s.hdlc.data)) - return; - s->s.hdlc.data[s->s.hdlc.datalen++] = hdlctx->bitbuf; - hdlctx->bitbuf >>= 8; - hdlctx->numbits -= 8; - } -} - -/* ---------------------------------------------------------------------- */ - -void gen_init_hdlc(struct gen_params *p, struct gen_state *s) -{ - struct hdlctx hdlctx = { 0, 0, 0 }; - int i; - - memset(s, 0, sizeof(struct gen_state)); - - s->s.hdlc.bitmask = 1; - for (i = 0; i < (p->p.hdlc.txdelay * (1200/100) / 8); i++) - txb_addbyte(s, &hdlctx, 0x7e, 0); - - txb_addbyte(s, &hdlctx, 0x7e, 0); - - for (i = 0; i < p->p.hdlc.pktlen; i++) - txb_addbyte(s, &hdlctx, p->p.hdlc.pkt[i], 1); - - i = calc_crc_ccitt(p->p.hdlc.pkt, p->p.hdlc.pktlen); - - txb_addbyte(s, &hdlctx, i, 1); - txb_addbyte(s, &hdlctx, i >> 8, 1); - txb_addbyte(s, &hdlctx, 0x7e, 0); - txb_addbyte(s, &hdlctx, 0x7e, 0); - txb_addbyte(s, &hdlctx, 0x7e, 0); - txb_addbyte(s, &hdlctx, 0x7e, 0); - txb_addbyte(s, &hdlctx, 0x7e, 0); - txb_addbyte(s, &hdlctx, 0x7e, 0); -} - - -int hdlc_sample_rate = DEFAULT_SAMPLE_RATE; - -int gen_hdlc(signed short *buf, int buflen, struct gen_params *p, struct gen_state *s) -{ - int num = 0; - - if (!s || s->s.hdlc.ch_idx < 0 || s->s.hdlc.ch_idx >= s->s.hdlc.datalen) - return 0; - for (; buflen > 0; buflen--, buf++, num++) { - s->s.hdlc.bitph += 0x10000*1200 / hdlc_sample_rate; - if (s->s.hdlc.bitph >= 0x10000u) { - s->s.hdlc.bitph &= 0xffffu; - s->s.hdlc.bitmask <<= 1; - if (s->s.hdlc.bitmask >= 0x100) { - s->s.hdlc.bitmask = 1; - s->s.hdlc.ch_idx++; - if (s->s.hdlc.ch_idx >= s->s.hdlc.datalen) - return num; - } - if (!(s->s.hdlc.data[s->s.hdlc.ch_idx] & s->s.hdlc.bitmask)) - s->s.hdlc.lastb = !s->s.hdlc.lastb; - s->s.hdlc.phinc = (s->s.hdlc.lastb) ? - 0x10000*2200/hdlc_sample_rate : 0x10000*1200/hdlc_sample_rate; - } - *buf += (p->ampl * COS(s->s.hdlc.ph)) >> 15; - s->s.hdlc.ph += s->s.hdlc.phinc; - } - return num; -} diff --git a/sw/ground_segment/multimon/hdlc.c b/sw/ground_segment/multimon/hdlc.c deleted file mode 100644 index 485c400ad3..0000000000 --- a/sw/ground_segment/multimon/hdlc.c +++ /dev/null @@ -1,295 +0,0 @@ -/* - * hdlc.c -- hdlc decoder and AX.25 packet dump - * - * Copyright (C) 1996 - * Thomas Sailer (sailer@ife.ee.ethz.ch, hb9jnx@hb9w.che.eu) - * - * This program 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 of the License, or - * (at your option) any later version. - * - * This program 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 this program; if not, write to the Free Software - * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. - */ - -/* ---------------------------------------------------------------------- */ - -#include "multimon.h" -#include -#include - -char hdlc_data_received[HDLC_DATA_LEN]; -int hdlc_data_received_idx; - -/* ---------------------------------------------------------------------- */ - -/* - * the CRC routines are stolen from WAMPES - * by Dieter Deyke - */ - -static const unsigned short crc_ccitt_table[] = { - 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, - 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, - 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, - 0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, - 0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd, - 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5, - 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, - 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974, - 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, - 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, - 0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, - 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, - 0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, - 0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, - 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738, - 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, - 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7, - 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, - 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, - 0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, - 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, - 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, - 0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134, - 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c, - 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, - 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb, - 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, - 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, - 0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, - 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, - 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, - 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78 -}; - -/* ---------------------------------------------------------------------- */ - -static inline int check_crc_ccitt(const unsigned char *buf, int cnt) -{ - unsigned int crc = 0xffff; - - for (; cnt > 0; cnt--) - crc = (crc >> 8) ^ crc_ccitt_table[(crc ^ *buf++) & 0xff]; - return (crc & 0xffff) == 0xf0b8; -} - -/* ---------------------------------------------------------------------- */ - -static void ax25_disp_packet(struct demod_state *s, unsigned char *bp, unsigned int len) -{ - unsigned char v1=1,cmd=0; - unsigned char i,j; - - if (!bp || len < 10) - return; -#if 1 - if (!check_crc_ccitt(bp, len)) - return; -#endif - len -= 2; - if (bp[1] & 1) { - /* - * FlexNet Header Compression - */ - v1 = 0; - cmd = (bp[1] & 2) != 0; - verbprintf(1, "%s: fm ? to ", s->dem_par->name); - i = (bp[2] >> 2) & 0x3f; - if (i) - verbprintf(1, "%c",i+0x20); - i = ((bp[2] << 4) | ((bp[3] >> 4) & 0xf)) & 0x3f; - if (i) - verbprintf(1, "%c",i+0x20); - i = ((bp[3] << 2) | ((bp[4] >> 6) & 3)) & 0x3f; - if (i) - verbprintf(1, "%c",i+0x20); - i = bp[4] & 0x3f; - if (i) - verbprintf(1, "%c",i+0x20); - i = (bp[5] >> 2) & 0x3f; - if (i) - verbprintf(1, "%c",i+0x20); - i = ((bp[5] << 4) | ((bp[6] >> 4) & 0xf)) & 0x3f; - if (i) - verbprintf(1, "%c",i+0x20); - verbprintf(1, "-%u QSO Nr %u", bp[6] & 0xf, (bp[0] << 6) | (bp[1] >> 2)); - bp += 7; - len -= 7; - } else { - /* - * normal header - */ - if (len < 15) - return; - - - if ((bp[6] & 0x80) != (bp[13] & 0x80)) { - v1 = 0; - cmd = (bp[6] & 0x80); - } - - verbprintf(1, "%s: fm ", s->dem_par->name); - for(i = 7; i < 13; i++) - if ((bp[i] &0xfe) != 0x40) - verbprintf(1, "%c",bp[i] >> 1); - verbprintf(1, "-%u to ",(bp[13] >> 1) & 0xf); - for(i = 0; i < 6; i++) - if ((bp[i] &0xfe) != 0x40) - verbprintf(1, "%c",bp[i] >> 1); - verbprintf(1, "-%u",(bp[6] >> 1) & 0xf); - bp += 14; - len -= 14; - if ((!(bp[-1] & 1)) && (len >= 7)) - verbprintf(1, " via "); - while ((!(bp[-1] & 1)) && (len >= 7)) { - for(i = 0; i < 6; i++) - if ((bp[i] &0xfe) != 0x40) - verbprintf(1, "%c",bp[i] >> 1); - verbprintf(1, "-%u",(bp[6] >> 1) & 0xf); - bp += 7; - len -= 7; - if ((!(bp[-1] & 1)) && (len >= 7)) - verbprintf(1, ","); - } - } - - if(!len) - return; - i = *bp++; - len--; - j = v1 ? ((i & 0x10) ? '!' : ' ') : - ((i & 0x10) ? (cmd ? '+' : '-') : (cmd ? '^' : 'v')); - if (!(i & 1)) { - /* - * Info frame - */ - verbprintf(1, " I%u%u%c",(i >> 5) & 7,(i >> 1) & 7,j); - } else if (i & 2) { - /* - * U frame - */ - switch (i & (~0x10)) { - case 0x03: - verbprintf(1, " UI%c",j); - break; - case 0x2f: - verbprintf(1, " SABM%c",j); - break; - case 0x43: - verbprintf(1, " DISC%c",j); - break; - case 0x0f: - verbprintf(1, " DM%c",j); - break; - case 0x63: - verbprintf(1, " UA%c",j); - break; - case 0x87: - verbprintf(1, " FRMR%c",j); - break; - default: - verbprintf(1, " unknown U (0x%x)%c",i & (~0x10),j); - break; - } - } else { - /* - * supervisory - */ - switch (i & 0xf) { - case 0x1: - verbprintf(1, " RR%u%c",(i >> 5) & 7,j); - break; - case 0x5: - verbprintf(1, " RNR%u%c",(i >> 5) & 7,j); - break; - case 0x9: - verbprintf(1, " REJ%u%c",(i >> 5) & 7,j); - break; - default: - verbprintf(1, " unknown S (0x%x)%u%c", i & 0xf, - (i >> 5) & 7, j); - break; - } - } - if (!len) { - verbprintf(1, "\n"); - return; - } - verbprintf(1, " pid=%02X\n", *bp++); - len--; - j = 0; - while (len) { - i = *bp++; - if (hdlc_data_received_idx < HDLC_DATA_LEN) - hdlc_data_received[hdlc_data_received_idx++] = i; - else - fprintf(stderr, "hdlc buffer full: discarding 1 char\n"); - if ((i >= 32) && (i < 128)) { - verbprintf(1, "%c",i); - } else if (i == 13) { - if (j) - verbprintf(1, "\n"); - j = 0; - } else - verbprintf(1, "."); - if (i >= 32) - j = 1; - len--; - } - if (j) - verbprintf(1, "\n"); -} - -/* ---------------------------------------------------------------------- */ - -void hdlc_init(struct demod_state *s) -{ - memset(&s->l2.hdlc, 0, sizeof(s->l2.hdlc)); -} - -/* ---------------------------------------------------------------------- */ - -void hdlc_rxbit(struct demod_state *s, int bit) -{ - s->l2.hdlc.rxbitstream <<= 1; - s->l2.hdlc.rxbitstream |= !!bit; - if ((s->l2.hdlc.rxbitstream & 0xff) == 0x7e) { - if (s->l2.hdlc.rxstate && (s->l2.hdlc.rxptr - s->l2.hdlc.rxbuf) > 2) - ax25_disp_packet(s, s->l2.hdlc.rxbuf, s->l2.hdlc.rxptr - s->l2.hdlc.rxbuf); - s->l2.hdlc.rxstate = 1; - s->l2.hdlc.rxptr = s->l2.hdlc.rxbuf; - s->l2.hdlc.rxbitbuf = 0x80; - return; - } - if ((s->l2.hdlc.rxbitstream & 0x7f) == 0x7f) { - s->l2.hdlc.rxstate = 0; - return; - } - if (!s->l2.hdlc.rxstate) - return; - if ((s->l2.hdlc.rxbitstream & 0x3f) == 0x3e) /* stuffed bit */ - return; - if (s->l2.hdlc.rxbitstream & 1) - s->l2.hdlc.rxbitbuf |= 0x100; - if (s->l2.hdlc.rxbitbuf & 1) { - if (s->l2.hdlc.rxptr >= s->l2.hdlc.rxbuf+sizeof(s->l2.hdlc.rxbuf)) { - s->l2.hdlc.rxstate = 0; - verbprintf(1, "Error: packet size too large\n"); - return; - } - *s->l2.hdlc.rxptr++ = s->l2.hdlc.rxbitbuf >> 1; - s->l2.hdlc.rxbitbuf = 0x80; - return; - } - s->l2.hdlc.rxbitbuf >>= 1; -} - -/* ---------------------------------------------------------------------- */ diff --git a/sw/ground_segment/multimon/hdlc.ml b/sw/ground_segment/multimon/hdlc.ml deleted file mode 100644 index 3cb5a55c57..0000000000 --- a/sw/ground_segment/multimon/hdlc.ml +++ /dev/null @@ -1,8 +0,0 @@ - -external init_gen : string -> Unix.file_descr = "ml_init_gen_hdlc" -external init_dec : string -> Unix.file_descr = "ml_init_dec_hdlc" -external write_data : string -> unit = "ml_gen_hdlc" -external get_data : unit -> string = "ml_get_hdlc" -external write_to_dsp : unit -> unit = "ml_write_to_dsp" - -let write_period = 90 diff --git a/sw/ground_segment/multimon/hdlc.mli b/sw/ground_segment/multimon/hdlc.mli deleted file mode 100644 index 8951a2551e..0000000000 --- a/sw/ground_segment/multimon/hdlc.mli +++ /dev/null @@ -1,9 +0,0 @@ -val init_gen : string -> Unix.file_descr -val write_data : string -> unit -val write_to_dsp : unit -> unit - -val write_period : int -(** (ms) Recommended period to call write_to_dsp *) - -val init_dec : string -> Unix.file_descr -val get_data : unit -> string diff --git a/sw/ground_segment/multimon/mkcostab.c b/sw/ground_segment/multimon/mkcostab.c deleted file mode 100644 index d14199aa23..0000000000 --- a/sw/ground_segment/multimon/mkcostab.c +++ /dev/null @@ -1,63 +0,0 @@ -/* - * mkcostab.c -- cosine table generator - * - * Copyright (C) 1996 - * Thomas Sailer (sailer@ife.ee.ethz.ch, hb9jnx@hb9w.che.eu) - * - * This program 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 of the License, or - * (at your option) any later version. - * - * This program 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 this program; if not, write to the Free Software - * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. - */ - -/* ---------------------------------------------------------------------- */ - -#include -#include -#include -/* ---------------------------------------------------------------------- */ - -#define COSTABSIZE 0x400 - -/* ---------------------------------------------------------------------- */ - -int main(int argc, char *argv[]) -{ - int i; - FILE *fi, *ff; - float f; - - if (!(fi = fopen("costabi.c", "w"))) - exit(1); - if (!(ff = fopen("costabf.c", "w"))) - exit(1); - fprintf(fi, "/*\n * This file is machine generated, DO NOT EDIT!\n */\n\n" - "int costabi[%i] = {", COSTABSIZE); - fprintf(ff, "/*\n * This file is machine generated, DO NOT EDIT!\n */\n\n" - "float costabf[%i] = {", COSTABSIZE); - for (i = 0; i < COSTABSIZE; i++) { - if ((i & 3) == 0) - fprintf(ff, "\n\t"); - if ((i & 7) == 0) - fprintf(fi, "\n\t"); - f = cos(M_PI*2.0*i/COSTABSIZE); - fprintf(ff, "%12.9f", f); - fprintf(fi, "%6i", (int)(32767.0*f)); - if (i < COSTABSIZE-1) { - fprintf(ff, ", "); - fprintf(fi, ", "); - } - } - fprintf(ff, "\n};\n"); - fprintf(fi, "\n};\n"); - exit(0); -} diff --git a/sw/ground_segment/multimon/ml_hdlc.c b/sw/ground_segment/multimon/ml_hdlc.c deleted file mode 100644 index c9f11be799..0000000000 --- a/sw/ground_segment/multimon/ml_hdlc.c +++ /dev/null @@ -1,334 +0,0 @@ -/* - * ml_hdlc.c -- OCaml binding to multimon HDLC library - * - * Copyright (C) 1997 - * Thomas Sailer (sailer@ife.ee.ethz.ch, hb9jnx@hb9w.che.eu) - * Copyright (C) 2007, Pascal Brisset for Paparazzi - * - * This program 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 of the License, or - * (at your option) any later version. - * - * This program 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 this program; if not, write to the Free Software - * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. - */ - -#include -#include -#include -#include -#include -#include -#include - -#ifndef __APPLE__ -#include -#include -#endif - -#include -#include -#include -#include -#include - -#include "gen.h" -#include "multimon.h" - -#define Min(a, b) ((a) < (b) ? (a) : (b)) - -static struct gen_params params; -static struct gen_state state; -int fd; -int fmt = 0; - -#define MY_BUF_LEN 32768 -short my_buffer[MY_BUF_LEN]; -int idx_start, idx_end; - -/** intermediate buffer */ -#define BUF_LEN 16384 -short silence_buffer[BUF_LEN]; - - -#define OUTPUT_BUF_LEN 1024 - - -static struct demod_state dem_st; - - -static int process_buffer(short *buf, int len) -{ - memset(buf, 0, len*sizeof(buf[0])); - return gen_hdlc(buf, len, ¶ms, &state); -} - -value ml_init_gen_hdlc(value device) { - /** From multimon, gen.c:output_sound() */ - -#ifndef __APPLE__ - char *ifname=String_val(device); - int sample_rate = DEFAULT_SAMPLE_RATE; - - int sndparam; - - if ((fd = open(ifname ? ifname : "/dev/dsp", O_WRONLY)) < 0) { - caml_failwith("open"); - exit (10); - } - - /** Doesn't work, stay with 8192 - int frag = (0x7fff << 16) | (14); // 14=16384, http://manuals.opensound.com/developer/SNDCTL_DSP_SETFRAGMENT.html - ioctl(fd, SNDCTL_DSP_SETFRAGMENT, &frag); - */ - - sndparam = AFMT_S16_LE; /* we want 16 bits/sample signed */ - /* little endian; works only on little endian systems! */ - if (ioctl(fd, SNDCTL_DSP_SETFMT, &sndparam) == -1) { - perror("ioctl: SNDCTL_DSP_SETFMT"); - exit (10); - } - if (sndparam != AFMT_S16_LE) { - fmt = 1; - sndparam = AFMT_U8; - if (ioctl(fd, SNDCTL_DSP_SETFMT, &sndparam) == -1) { - perror("ioctl: SNDCTL_DSP_SETFMT"); - exit (10); - } - if (sndparam != AFMT_U8) { - perror("ioctl: SNDCTL_DSP_SETFMT"); - exit (10); - } - } - sndparam = 0; /* we want only 1 channel */ - if (ioctl(fd, SNDCTL_DSP_STEREO, &sndparam) == -1) { - perror("ioctl: SNDCTL_DSP_STEREO"); - exit (10); - } - if (sndparam != 0) { - fprintf(stderr, "gen: Error, cannot set the channel " - "number to 1\n"); - exit (10); - } - sndparam = sample_rate; - if (ioctl(fd, SNDCTL_DSP_SPEED, &sndparam) == -1) { - perror("ioctl: SNDCTL_DSP_SPEED"); - exit (10); - } - if ((10*abs(sndparam-sample_rate)) > sample_rate) { - perror("ioctl: SNDCTL_DSP_SPEED"); - exit (10); - } - if (sndparam != sample_rate) { - fprintf(stderr, "Warning: Sampling rate is %u, " - "requested %u\n", sndparam, sample_rate); - } - - - params.type = gentype_hdlc; - params.ampl = 16384; - params.p.hdlc.modulation = 0; - params.p.hdlc.pkt[0] = ('H') << 1; - params.p.hdlc.pkt[1] = ('B') << 1; - params.p.hdlc.pkt[2] = ('9') << 1; - params.p.hdlc.pkt[3] = ('J') << 1; - params.p.hdlc.pkt[4] = ('N') << 1; - params.p.hdlc.pkt[5] = ('X') << 1; - params.p.hdlc.pkt[6] = (0x00) << 1; - params.p.hdlc.pkt[7] = ('A') << 1; - params.p.hdlc.pkt[8] = ('E') << 1; - params.p.hdlc.pkt[9] = ('4') << 1; - params.p.hdlc.pkt[10] = ('W') << 1; - params.p.hdlc.pkt[11] = ('A') << 1; - params.p.hdlc.pkt[12] = (' ') << 1; - params.p.hdlc.pkt[13] = ((0x00) << 1) | 1; - params.p.hdlc.pkt[14] = 0x03; - params.p.hdlc.pkt[15] = 0xf0; - - /** Generate a buffer full of silence **/ - params.p.hdlc.txdelay = 100; /* 1 s */ - params.p.hdlc.pktlen = 16; - memset(&state, 0, sizeof(state)); - gen_init_hdlc(¶ms, &state); - process_buffer(silence_buffer, sizeof(silence_buffer)/sizeof(silence_buffer[0])); - - idx_start = idx_end = 0; - - return Val_int(fd); -#else - failwith("Not supported under OSX"); -#endif - -} - - -value ml_init_dec_hdlc(value dev) { - char *ifname = String_val(dev); -#ifndef __APPLE__ - if ((fd = open(ifname, O_RDONLY)) < 0) { - caml_failwith("Hdlc.init_dec: open failed"); - } - - int sndparam = AFMT_S16_LE; /* we want 16 bits/sample signed */ - /* little endian; works only on little endian systems! */ - if (ioctl(fd, SNDCTL_DSP_SETFMT, &sndparam) == -1) { - perror("ioctl: SNDCTL_DSP_SETFMT"); - exit (10); - } - if (sndparam != AFMT_S16_LE) { - perror("ioctl: AFMT_S16_LE"); - exit (10); - } - - sndparam = DEFAULT_SAMPLE_RATE; - if (ioctl(fd, SNDCTL_DSP_SPEED, &sndparam) == -1) { - perror("ioctl: SNDCTL_DSP_SPEED"); - exit (10); - } - if ((10*abs(sndparam-DEFAULT_SAMPLE_RATE)) > DEFAULT_SAMPLE_RATE) { - perror("ioctl: SNDCTL_DSP_SPEED"); - exit (10); - } - if (sndparam != DEFAULT_SAMPLE_RATE) { - fprintf(stderr, "Warning: Sampling rate is %u, " - "requested %u\n", sndparam, DEFAULT_SAMPLE_RATE); - } - - memset(&dem_st, 0, sizeof(struct demod_state)); - afsk12_init(&dem_st); - dem_st.dem_par = &demod_afsk1200; - return Val_int(fd); -#else - failwith("Not supported under OSX"); -#endif - -} - - -value ml_gen_hdlc(value val_data) { -#ifndef __APPLE__ - /** Handling the data input */ - int data_len = string_length(val_data); - if (data_len + 16 > sizeof(params.p.hdlc.pkt)) - caml_invalid_argument("Hdlc.write_data: argument too large"); - - char *data = String_val(val_data); - int i; - for(i = 0; i < data_len; i++) { - params.p.hdlc.pkt[16+i] = data[i]; - } - params.p.hdlc.pktlen = 16 + data_len; - params.p.hdlc.txdelay = 10; - - memset(&state, 0, sizeof(state)); - gen_init_hdlc(¶ms, &state); - - /** Intermediate buffer */ - short s[BUF_LEN]; - - int n = process_buffer(s, sizeof(s)/sizeof(s[0])); - assert(state.s.hdlc.ch_idx == state.s.hdlc.datalen); - - int occupied = (idx_end + MY_BUF_LEN - idx_start) % MY_BUF_LEN; - if (occupied + n > MY_BUF_LEN) { - caml_failwith("buffer full"); - } else { - int i; - for(i = 0; i < n; i++) - my_buffer[(idx_end+i) % MY_BUF_LEN] = s[i]; - idx_end = (idx_end + n) % MY_BUF_LEN; - } - - return Val_unit; -#else - failwith("Not supported under OSX"); -#endif - -} - - -/* max ospace = 8192 = 4096 samples = 4096/22050 seconds = 0.18 */ -value ml_write_to_dsp(value _) { -#ifndef __APPLE__ - audio_buf_info bi; - ioctl(fd, SNDCTL_DSP_GETOSPACE, &bi); - int ospace = bi.bytes; - - int n = ospace/sizeof(short); - - int available = (idx_end + MY_BUF_LEN - idx_start) % MY_BUF_LEN; - /* complete with silence */ - int i; - for(i = available; i < n; i++) { - my_buffer[(idx_start+i)%MY_BUF_LEN] = silence_buffer[i]; - } - - int k = Min(n, MY_BUF_LEN - idx_start); - int num = write(fd, my_buffer+idx_start, k*sizeof(short)); - num += write(fd, my_buffer+0, (n - k)*sizeof(short)); - if (available <= n) { - /* buffer is empty */ - idx_start = idx_end = 0; - } else { - idx_start = (idx_start + n) % MY_BUF_LEN; - } - - assert(num == n*sizeof(short)); - - return Val_unit -#else - failwith("Not supported under OSX"); -#endif -; -} - -static union { - short s[8192]; - unsigned char b[8192]; -} b; -static float fbuf[16384]; -static unsigned int fbuf_cnt = 0; - - -value ml_get_hdlc(value unit) { -#ifndef __APPLE__ - unsigned int overlap = demod_afsk1200.overlap; - short *sp; - int i = read(fd, sp = b.s, sizeof(b.s)); - if(i < 0) - caml_failwith("Hdlc.get_data: read failed"); - - CAMLparam0(); - CAMLlocal1 (result); - - hdlc_data_received_idx = 0; - if(i > 0) { - for (; i >= sizeof(b.s[0]); i -= sizeof(b.s[0]), sp++) - fbuf[fbuf_cnt++] = (*sp) * (1.0/32768.0); - if (i) - fprintf(stderr, "warning: noninteger number of samples read\n"); - if (fbuf_cnt > overlap) { - demod_afsk1200.demod(&dem_st, fbuf, fbuf_cnt-overlap); - - memmove(fbuf, fbuf+fbuf_cnt-overlap, overlap*sizeof(fbuf[0])); - fbuf_cnt = overlap; - } - } - - /* Alloc the caml string and fill it */ - result = alloc_string(hdlc_data_received_idx); - for(i = 0; i < hdlc_data_received_idx; i++) - Byte(result, i) = hdlc_data_received[i]; - CAMLreturn(result); -#else - failwith("Not supported under OSX"); -#endif - -} diff --git a/sw/ground_segment/multimon/multimon.h b/sw/ground_segment/multimon/multimon.h deleted file mode 100644 index 1aa463ecd0..0000000000 --- a/sw/ground_segment/multimon/multimon.h +++ /dev/null @@ -1,120 +0,0 @@ -/* - * multimon.h -- Monitor for many different modulation formats - * - * Copyright (C) 1996 - * Thomas Sailer (sailer@ife.ee.ethz.ch, hb9jnx@hb9w.che.eu) - * - * This program 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 of the License, or - * (at your option) any later version. - * - * This program 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 this program; if not, write to the Free Software - * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. - */ - -/* ---------------------------------------------------------------------- */ - -#ifndef _MULTIMON_H -#define _MULTIMON_H - -/* ---------------------------------------------------------------------- */ - -extern const float costabf[0x400]; -#define COS(x) costabf[(((x)>>6)&0x3ffu)] -#define SIN(x) COS((x)+0xc000) - -/* ---------------------------------------------------------------------- */ - -struct demod_state { - const struct demod_param *dem_par; - union { - struct l2_state_hdlc { - unsigned char rxbuf[512]; - unsigned char *rxptr; - unsigned int rxstate; - unsigned int rxbitstream; - unsigned int rxbitbuf; - } hdlc; - - struct l2_state_pprz { - unsigned char rxbuf[512]; - unsigned char *rxptr; - unsigned int rxstate; - unsigned int rxbitstream; - unsigned int rxbitbuf; - char* pipe_path; - int pipe_fd; - } pprz; - } l2; - union { - struct l1_state_afsk48p { - unsigned int dcd_shreg; - unsigned int sphase; - unsigned int lasts; - unsigned int dcd_count; - unsigned int sample_count; - } afsk48p; - - struct l1_state_scope { - int datalen; - int dispnum; - float data[512]; - } scope; - - struct l1_state_afsk12 { - unsigned int dcd_shreg; - unsigned int sphase; - unsigned int lasts; - unsigned int subsamp; - } afsk12; - } l1; -}; - -struct demod_param { - const char *name; - unsigned int samplerate; - unsigned int overlap; - void (*init)(struct demod_state *s); - void (*demod)(struct demod_state *s, float *buffer, int length); -}; - - -#define HDLC_DATA_LEN 256 -extern char hdlc_data_received[HDLC_DATA_LEN]; -extern int hdlc_data_received_idx; - - -/* ---------------------------------------------------------------------- */ - -extern const struct demod_param demod_afsk4800p; -extern const struct demod_param demod_afsk1200; -extern const struct demod_param demod_scope; - -#define ALL_DEMOD &demod_afsk4800p,&demod_scope - -/* ---------------------------------------------------------------------- */ - -void verbprintf(int verb_level, const char *fmt, ...); - -void hdlc_init(struct demod_state *s); -void hdlc_rxbit(struct demod_state *s, int bit); - -void afsk12_init(struct demod_state *s); - -void pprz_init(struct demod_state *s); -void pprz_rxbit(struct demod_state *s, int bit); -void pprz_status(struct demod_state *s); - -void xdisp_terminate(int cnum); -int xdisp_start(void); -int xdisp_update(int cnum, float *f); - -/* ---------------------------------------------------------------------- */ -#endif /* _MULTIMON_H */ diff --git a/sw/ground_segment/multimon/pprz.c b/sw/ground_segment/multimon/pprz.c deleted file mode 100644 index b3e4a3bc32..0000000000 --- a/sw/ground_segment/multimon/pprz.c +++ /dev/null @@ -1,193 +0,0 @@ -/* - * pprz.c -- paparazzi decoder - * - * Copyright (C) 1996 - * Thomas Sailer (sailer@ife.ee.ethz.ch, hb9jnx@hb9w.che.eu) - * Copyright (C) 2005 - * Martin Mueller (ma.mueller@tu-bs.de) - * - * This program 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 of the License, or - * (at your option) any later version. - * - * This program 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 this program; if not, write to the Free Software - * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. - */ - -/* ---------------------------------------------------------------------- */ - -#include "multimon.h" -#include -#include -#include -#include -#include -#include -#include - -#define STX 0x02 -#define ETX 0x03 - -#define MSG_DATA 0 -#define MSG_ERROR 1 -#define MSG_CD 2 -#define MSG_DEBUG 3 -#define MSG_VALIM 4 - -#define INPUT_BUF_LEN 10 - -#define MULTIMON_PIPE_NAME "/tmp/multimon" - - -/* ---------------------------------------------------------------------- */ - -static void pprz_tmtc_send(struct demod_state *s, unsigned char *data, unsigned int len, unsigned char type) -{ - unsigned char msg[INPUT_BUF_LEN+6]; - unsigned int count = 0; - int ret; - - unsigned char checksum = 0; - const unsigned char real_len = len + 2; - unsigned char i; - - msg[count++] = STX; - msg[count++] = real_len; - checksum ^= real_len; - msg[count++] = type; - checksum ^= type; - for (i=0; i < len; i++) { - msg[count++] = data[i]; - checksum ^= data[i]; - } - msg[count++] = checksum; - msg[count++] = ETX; - - ret = write(s->l2.pprz.pipe_fd, msg, count); - - if (count != ret) - perror("write pipe"); -} - -/* ---------------------------------------------------------------------- */ - -char multimon_pipe_name[1024] = MULTIMON_PIPE_NAME; - -void pprz_init(struct demod_state *s) -{ - memset(&s->l2.hdlc, 0, sizeof(s->l2.hdlc)); - - /* open named pipe */ - struct stat st; - if (stat(multimon_pipe_name, &st)) { - if (mkfifo(multimon_pipe_name, 0644) == -1) { - perror("make pipe"); - exit (10); - } - } - if ((s->l2.pprz.pipe_fd = open(multimon_pipe_name, O_WRONLY /*| O_NONBLOCK*/)) < 0) { - perror("open pipe"); - exit (10); - } - - /* reset buffer pointer */ - s->l2.pprz.rxptr = s->l2.hdlc.rxbuf; -} - -/* ---------------------------------------------------------------------- */ - -void pprz_baudot_rxbit(struct demod_state *s, int bit) -{ - /* (stop)bit found? */ - if (bit & 1) { - /* check for sync and start bit */ - if ((s->l2.hdlc.rxbitbuf & 1) && - !(s->l2.hdlc.rxbitbuf & 2)) { - /* found new byte */ - *s->l2.hdlc.rxptr++ = (s->l2.hdlc.rxbitbuf >> 2) & 0xFF; - verbprintf(8, "0x%02X ", (s->l2.hdlc.rxbitbuf >> 2) & 0xFF); - /* reset bit buffer */ - s->l2.hdlc.rxbitbuf = 0x400; - } else { - /* add data bit */ - s->l2.hdlc.rxbitbuf |= 0x400; - } - } - - if ((s->l2.hdlc.rxptr - s->l2.hdlc.rxbuf) >= INPUT_BUF_LEN) { - pprz_tmtc_send(s, s->l2.hdlc.rxbuf, s->l2.hdlc.rxptr - s->l2.hdlc.rxbuf, MSG_DATA); - /* reset data buffer */ - s->l2.hdlc.rxptr = s->l2.hdlc.rxbuf; - } - - /* frame end / out of sync */ - if (s->l2.hdlc.rxbitbuf & 1) - s->l2.hdlc.rxbitbuf |= 2; - - /* shift in new data */ - s->l2.hdlc.rxbitbuf >>= 1; - - return; -} - -/* ---------------------------------------------------------------------- */ - -void pprz_hdlc_rxbit(struct demod_state *s, int bit) -{ - s->l2.hdlc.rxbitstream <<= 1; - s->l2.hdlc.rxbitstream |= !!bit; - if ((s->l2.hdlc.rxbitstream & 0xff) == 0x7e) { - if (s->l2.hdlc.rxstate && (s->l2.hdlc.rxptr - s->l2.hdlc.rxbuf) > 2) - pprz_tmtc_send(s, s->l2.hdlc.rxbuf, s->l2.hdlc.rxptr - s->l2.hdlc.rxbuf, MSG_DATA); - s->l2.hdlc.rxstate = 1; - s->l2.hdlc.rxptr = s->l2.hdlc.rxbuf; - s->l2.hdlc.rxbitbuf = 0x80; - return; - } - if ((s->l2.hdlc.rxbitstream & 0x7f) == 0x7f) { - s->l2.hdlc.rxstate = 0; - return; - } - if (!s->l2.hdlc.rxstate) - return; - if ((s->l2.hdlc.rxbitstream & 0x3f) == 0x3e) /* stuffed bit */ - return; - if (s->l2.hdlc.rxbitstream & 1) - s->l2.hdlc.rxbitbuf |= 0x100; - if (s->l2.hdlc.rxbitbuf & 1) { - if (s->l2.hdlc.rxptr >= s->l2.hdlc.rxbuf+sizeof(s->l2.hdlc.rxbuf)) { - s->l2.hdlc.rxstate = 0; - verbprintf(1, "Error: packet size too large\n"); - return; - } - *s->l2.hdlc.rxptr++ = s->l2.hdlc.rxbitbuf >> 1; - s->l2.hdlc.rxbitbuf = 0x80; - return; - } - s->l2.hdlc.rxbitbuf >>= 1; -} - -/* ---------------------------------------------------------------------- */ - -void pprz_status(struct demod_state *s) -{ - unsigned char data[2]; - - /* TODO find carrier */ - data[0] = 0x01; pprz_tmtc_send(s, data, 1, MSG_CD); - /* just write some useful value (get it from acpi?) */ - data[0] = 0x00; data[1] = 0x04; pprz_tmtc_send(s, data, 2, MSG_VALIM); - /* no debug value */ - data[0] = 0x00; pprz_tmtc_send(s, data, 1, MSG_DEBUG); - - return; -} - -/* ---------------------------------------------------------------------- */ diff --git a/sw/ground_segment/multimon/pprz.h b/sw/ground_segment/multimon/pprz.h deleted file mode 100644 index a4d7779983..0000000000 --- a/sw/ground_segment/multimon/pprz.h +++ /dev/null @@ -1,12 +0,0 @@ -#ifndef PPRZ_H -#define PPRZ_H - -extern char multimon_pipe_name[]; - - -void pprz_init(struct demod_state *s); -void pprz_baudot_rxbit(struct demod_state *s, int bit); -void pprz_hdlc_rxbit(struct demod_state *s, int bit); -void pprz_status(struct demod_state *s); - -#endif /* PPRZ_H */ diff --git a/sw/ground_segment/multimon/pprzlib.c b/sw/ground_segment/multimon/pprzlib.c deleted file mode 100644 index 6b3da717a4..0000000000 --- a/sw/ground_segment/multimon/pprzlib.c +++ /dev/null @@ -1,345 +0,0 @@ -/* - * Copyright (C) 1996 - * Thomas Sailer (sailer@ife.ee.ethz.ch, hb9jnx@hb9w.che.eu) - * Copyright (C) 2005 Pascal Brisset, Antoine Drouin - * - * This program 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 of the License, or - * (at your option) any later version. - * - * This program 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 this program; if not, write to the Free Software - * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. - */ - -/* ---------------------------------------------------------------------- */ - - -/* ---------------------------------------------------------------------- */ - - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -#include "multimon.h" -#include "filter.h" -#include "pprz.h" -#include "pprzlib.h" - -#ifndef TRUE -#define TRUE 1 -#define FALSE 0 -#endif - -#define LEFT 0 -#define RIGHT 1 - -/* ---------------------------------------------------------------------- */ - -/* - * Standard CMX469A clock frequency: 4.032 Mhz - * Xtal used: 4 MHz - * Ratio: 0.992063 - * Mark frequency: 4761.905 Hz - * Space frequency: 2380.952 Hz - */ - -#define FREQ_MARK 4762 -#define FREQ_SPACE 2381 -#define FREQ_SAMP 22050 -//#define FREQ_SAMP 44100 -#define BAUD 4762 - -/* ---------------------------------------------------------------------- */ - -#define CORRLEN (2*(int)(FREQ_SAMP/BAUD)) -#define SPHASEINC (0x10000u*BAUD/FREQ_SAMP) - -static float corr_mark_i[CORRLEN]; -static float corr_mark_q[CORRLEN]; -static float corr_space_i[CORRLEN]; -static float corr_space_q[CORRLEN]; - -/* ---------------------------------------------------------------------- */ - -#define TEMP_BUF_LEN 10 -#define OUTPUT_BUF_LEN 512 - - -static int verbose_level = 0; - -void -verbprintf(int verb_level, const char *fmt, ...) -{ - va_list args; - - va_start(args, fmt); - if (verb_level <= verbose_level) - vfprintf(stdout, fmt, args); - va_end(args); -} - - -static int glob_data_received_len = 0; /** Easier with a global var */ -static void -pprz_tmtc_send(unsigned char *data, unsigned int len, char* data_received) -{ - unsigned char i; - for (i=0; i < len && glob_data_received_len < OUTPUT_BUF_LEN; i++) { - data_received[glob_data_received_len++] = data[i]; - } -} - - -void -my_pprz_baudot_rxbit(struct demod_state *s, int bit, char* data) -{ - /* (stop)bit found? */ - if (bit & 1) { - /* check for sync and start bit */ - if ((s->l2.hdlc.rxbitbuf & 1) && - !(s->l2.hdlc.rxbitbuf & 2)) { - /* found new byte */ - *s->l2.hdlc.rxptr++ = (s->l2.hdlc.rxbitbuf >> 2) & 0xFF; - verbprintf(8, "0x%02X ", (s->l2.hdlc.rxbitbuf >> 2) & 0xFF); - /* reset bit buffer */ - s->l2.hdlc.rxbitbuf = 0x400; - } else { - /* add data bit */ - s->l2.hdlc.rxbitbuf |= 0x400; - } - } - - if ((s->l2.hdlc.rxptr - s->l2.hdlc.rxbuf) >= TEMP_BUF_LEN) { - pprz_tmtc_send(s->l2.hdlc.rxbuf, s->l2.hdlc.rxptr - s->l2.hdlc.rxbuf, data); - /* reset data buffer */ - s->l2.hdlc.rxptr = s->l2.hdlc.rxbuf; - } - - /* frame end / out of sync */ - if (s->l2.hdlc.rxbitbuf & 1) - s->l2.hdlc.rxbitbuf |= 2; - - /* shift in new data */ - s->l2.hdlc.rxbitbuf >>= 1; - - return; -} - -void -pprz_init(struct demod_state *s) -{ - memset(&s->l2.hdlc, 0, sizeof(s->l2.hdlc)); - - /* reset buffer pointer */ - s->l2.pprz.rxptr = s->l2.hdlc.rxbuf; -} - -static void -afsk48p_init(struct demod_state *s) -{ - float f; - int i; - - pprz_init(s); - memset(&s->l1.afsk48p, 0, sizeof(s->l1.afsk48p)); - for (f = 0, i = 0; i < CORRLEN; i++) { - corr_mark_i[i] = cos(f); - corr_mark_q[i] = sin(f); - f += 2.0*M_PI*FREQ_MARK/FREQ_SAMP; - } - for (f = 0, i = 0; i < CORRLEN; i++) { - corr_space_i[i] = cos(f); - corr_space_q[i] = sin(f); - f += 2.0*M_PI*FREQ_SPACE/FREQ_SAMP; - } - for (i = 0; i < CORRLEN; i++) { - f = 0.54 - 0.46*cos(2*M_PI*i/(float)(CORRLEN-1)); - corr_mark_i[i] *= f; - corr_mark_q[i] *= f; - corr_space_i[i] *= f; - corr_space_q[i] *= f; - } - - s->l1.afsk48p.sample_count = 0; -} - -static void -afsk48p_demod(struct demod_state *s, float *buffer, int length, char* data) -{ - float f; - unsigned char curbit; - - s->l1.afsk48p.sample_count += length; - if (s->l1.afsk48p.sample_count > FREQ_SAMP) { - s->l1.afsk48p.sample_count -= FREQ_SAMP; - // pprz_status(s); - } - - for (; length > 0; length--, buffer++) { - f = fsqr(mac(buffer, corr_mark_i, CORRLEN)) + - fsqr(mac(buffer, corr_mark_q, CORRLEN)) - - fsqr(mac(buffer, corr_space_i, CORRLEN)) - - fsqr(mac(buffer, corr_space_q, CORRLEN)); - s->l1.afsk48p.dcd_shreg <<= 1; - s->l1.afsk48p.dcd_shreg |= (f > 0); - verbprintf(10, "%c", '0'+(s->l1.afsk48p.dcd_shreg & 1)); - /* - * check if transition - */ - if ((s->l1.afsk48p.dcd_shreg ^ (s->l1.afsk48p.dcd_shreg >> 1)) & 1) { - if (s->l1.afsk48p.sphase < (0x8000u-(SPHASEINC/2))) - s->l1.afsk48p.sphase += SPHASEINC/8; - else - s->l1.afsk48p.sphase -= SPHASEINC/8; - } - s->l1.afsk48p.sphase += SPHASEINC; - if (s->l1.afsk48p.sphase >= 0x10000u) { - s->l1.afsk48p.sphase &= 0xffffu; - s->l1.afsk48p.lasts <<= 1; - s->l1.afsk48p.lasts |= s->l1.afsk48p.dcd_shreg & 1; - /* we use direct coded bits, not NRZI */ - curbit = (s->l1.afsk48p.lasts ^ 1) & 1; - verbprintf(9, " %c ", '0'+curbit); - my_pprz_baudot_rxbit(s, curbit, data); - } - } -} - -int fd; -union { - short s[8192]; - unsigned char b[8192]; -} b; -int stereo = 1; -unsigned int sample_rate = FREQ_SAMP; - -static int -input_init(const char *ifname) { - int sndparam; - - if ((fd = open(ifname, O_RDONLY)) < 0) { - perror("open"); - exit (10); - } - if (!strncmp("/dev", ifname, 4)) { - sndparam = AFMT_S16_LE; /* we want 16 bits/sample signed */ - /* little endian; works only on little endian systems! */ - if (ioctl(fd, SNDCTL_DSP_SETFMT, &sndparam) == -1) { - perror("ioctl: SNDCTL_DSP_SETFMT"); - exit (10); - } - if (sndparam != AFMT_S16_LE) { - perror("ioctl: AFMT_S16_LE"); - exit (10); - } - stereo = TRUE; - sndparam = 1; /* we want 2 channels */ - if (ioctl(fd, SNDCTL_DSP_STEREO, &sndparam) == -1) { - perror("ioctl: SNDCTL_DSP_STEREO"); - exit (10); - } - if (sndparam == 0) { - fprintf(stderr, "soundif: Error, cannot set the channel " - "number to 2: using mono\n"); - stereo=FALSE; - } else if (sndparam != 1) { - fprintf(stderr, "soundif: Error, cannot set the channel " - "number to 2\n"); - exit (10); - } - sndparam = sample_rate; - if (ioctl(fd, SNDCTL_DSP_SPEED, &sndparam) == -1) { - perror("ioctl: SNDCTL_DSP_SPEED"); - exit (10); - } - if ((10*abs(sndparam-sample_rate)) > sample_rate) { - perror("ioctl: SNDCTL_DSP_SPEED"); - exit (10); - } - if (sndparam != sample_rate) { - fprintf(stderr, "Warning: Sampling rate is %u, " - "requested %u\n", sndparam, sample_rate); - } - } - return fd; -} - - -static struct demod_state dem_st[2]; - -char data_left[OUTPUT_BUF_LEN+1]; -char data_right[OUTPUT_BUF_LEN+1]; -struct data data_received = {data_left, 0, data_right, 0}; -unsigned int fbuf_cnt = 0; -float fbuf[2][16384]; - -struct data* -pprz_demod_read_data(void) { - unsigned int overlap = CORRLEN; - int i; - short *sp; - - i = read(fd, sp = b.s, sizeof(b.s)); - if (i < 0 && errno != EAGAIN) { - perror("read"); - exit(4); - } - if (i > 0) { - data_received.len_left = 0; - data_received.len_right = 0; - if (stereo) { - for (; i >= sizeof(b.s[0]); i -= (sizeof(b.s[0])*2), sp+=2) { - fbuf[LEFT][fbuf_cnt] = (*sp) * (1.0/32768.0); - fbuf[RIGHT][fbuf_cnt++] = (*(sp+1)) * (1.0/32768.0); - } - } else { - for (; i >= sizeof(b.s[0]); i -= sizeof(b.s[0]), sp++) - fbuf[LEFT][fbuf_cnt++] = (*sp) * (1.0/32768.0); - } - if (i) - fprintf(stderr, "warning: noninteger number of samples read\n"); - if (fbuf_cnt > overlap) { - - glob_data_received_len = 0; - afsk48p_demod(&dem_st[LEFT], fbuf[LEFT], fbuf_cnt-overlap, data_received.data_left); - - data_received.len_left = glob_data_received_len; - memmove(fbuf[LEFT], fbuf[LEFT]+fbuf_cnt-overlap, overlap*sizeof(fbuf[LEFT][0])); - if (stereo) { - glob_data_received_len = 0; - afsk48p_demod(&dem_st[RIGHT], fbuf[RIGHT], fbuf_cnt-overlap, data_received.data_right); - data_received.len_right = glob_data_received_len; - memmove(fbuf[RIGHT], fbuf[RIGHT]+fbuf_cnt-overlap, overlap*sizeof(fbuf[RIGHT][0])); - } - fbuf_cnt = overlap; - } - return &data_received; - } else - return NULL; -} - - -int pprz_demod_init(char *dev) { - memset(dem_st, 0, 2*sizeof(struct demod_state)); - afsk48p_init(&dem_st[LEFT]); - afsk48p_init(&dem_st[RIGHT]); - return input_init(dev); -} diff --git a/sw/ground_segment/multimon/pprzlib.h b/sw/ground_segment/multimon/pprzlib.h deleted file mode 100644 index f90c6b8951..0000000000 --- a/sw/ground_segment/multimon/pprzlib.h +++ /dev/null @@ -1,5 +0,0 @@ -int pprz_demod_init(char *dev); - -struct data { char* data_left; int len_left; char* data_right; int len_right;}; -struct data* pprz_demod_read_data(void); -/** Returns NULL if no characters are available on the device */ diff --git a/sw/ground_segment/multimon/test.ml b/sw/ground_segment/multimon/test.ml deleted file mode 100644 index 83e141c30b..0000000000 --- a/sw/ground_segment/multimon/test.ml +++ /dev/null @@ -1,18 +0,0 @@ -let print_hex = fun s -> - for i = 0 to String.length s - 1 do - Printf.printf "%02x" (Char.code s.[i]) - done; - print_newline () - - -let _ = - let dsp = Demod.init "/dev/dsp" in - - let cb = fun _ -> - let l, r = Demod.get_data () in - print_hex l; print_hex r; true in - - ignore (Glib.Io.add_watch [`IN] cb (Glib.Io.channel_of_descr dsp)); - - let loop = Glib.Main.create true in - while Glib.Main.is_running loop do ignore (Glib.Main.iteration true) done diff --git a/sw/ground_segment/multimon/test_gen_hdlc.ml b/sw/ground_segment/multimon/test_gen_hdlc.ml deleted file mode 100644 index e155315441..0000000000 --- a/sw/ground_segment/multimon/test_gen_hdlc.ml +++ /dev/null @@ -1,27 +0,0 @@ -let my_write = fun fd buf -> - let rec loop = fun i -> - Printf.printf "i=%d\n%!" i; - let r = String.length buf - i in - if r > 0 then - loop (i + Unix.write fd buf i (min 8192 r)) in - loop 0 - - -let _ = - let _fd = Hdlc.init_gen "/dev/dsp" in - - let i = ref 0 in - let cb = fun _ -> - incr i; - (***) - Hdlc.write_data - (let s = Printf.sprintf "coucou [%d]" !i in prerr_endline s; s); - true in - -(***) ignore (Glib.Timeout.add 1000 cb); (***) - ignore (Glib.Timeout.add 90 (fun _ -> Hdlc.write_to_dsp (); true)); - (** ignore (Glib.Timeout.add 100 (fun _ -> prerr_endline "x"; true)); **) - - (** Threaded main loop (blocking write) *) - GtkThread.main () - diff --git a/sw/ground_segment/multimon/unixinput.c b/sw/ground_segment/multimon/unixinput.c deleted file mode 100644 index e4c3392b48..0000000000 --- a/sw/ground_segment/multimon/unixinput.c +++ /dev/null @@ -1,433 +0,0 @@ -/* - * unixinput.c -- input sound samples - * - * Copyright (C) 1996 - * Thomas Sailer (sailer@ife.ee.ethz.ch, hb9jnx@hb9w.che.eu) - * - * This program 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 of the License, or - * (at your option) any later version. - * - * This program 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 this program; if not, write to the Free Software - * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. - */ - -/* ---------------------------------------------------------------------- */ - -#include "multimon.h" -#include "pprz.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -/* ---------------------------------------------------------------------- */ - -static const char *allowed_types[] = { - "raw", "aiff", "au", "hcom", "sf", "voc", "cdr", "dat", - "smp", "wav", "maud", "vwe", NULL -}; - -/* ---------------------------------------------------------------------- */ - -static const struct demod_param *dem[] = { ALL_DEMOD }; - -#define NUMDEMOD (sizeof(dem)/sizeof(dem[0])) - -static struct demod_state dem_st[NUMDEMOD]; -static unsigned int dem_mask[(NUMDEMOD+31)/32]; - -#define MASK_SET(n) dem_mask[(n)>>5] |= 1<<((n)&0x1f) -#define MASK_RESET(n) dem_mask[(n)>>5] &= ~(1<<((n)&0x1f)) -#define MASK_ISSET(n) (dem_mask[(n)>>5] & 1<<((n)&0x1f)) - -/* ---------------------------------------------------------------------- */ - -static int verbose_level = 0; - -/* ---------------------------------------------------------------------- */ - -void verbprintf(int verb_level, const char *fmt, ...) -{ - va_list args; - - va_start(args, fmt); - if (verb_level <= verbose_level) - vfprintf(stdout, fmt, args); - va_end(args); -} - -/* ---------------------------------------------------------------------- */ - -static void process_buffer(float *buf, unsigned int len) -{ - int i; - - for (i = 0; i < NUMDEMOD; i++) - if (MASK_ISSET(i) && dem[i]->demod) - dem[i]->demod(dem_st+i, buf, len); -} - -/* ---------------------------------------------------------------------- */ - -static void input_sound(unsigned int sample_rate, unsigned int overlap, - const char *ifname) -{ - int sndparam; - int fd; - union { - short s[8192]; - unsigned char b[8192]; - } b; - float fbuf[16384]; - unsigned int fbuf_cnt = 0; - int i; - short *sp; - unsigned char *bp; - int fmt = 0; - int stereo = 0; - - if ((fd = open(ifname ? ifname : "/dev/dsp", O_RDONLY)) < 0) { - perror("open"); - exit (10); - } - sndparam = AFMT_S16_LE; /* we want 16 bits/sample signed */ - /* little endian; works only on little endian systems! */ - if (ioctl(fd, SNDCTL_DSP_SETFMT, &sndparam) == -1) { - perror("ioctl: SNDCTL_DSP_SETFMT"); - exit (10); - } - if (sndparam != AFMT_S16_LE) { - fmt = 1; - sndparam = AFMT_U8; - if (ioctl(fd, SNDCTL_DSP_SETFMT, &sndparam) == -1) { - perror("ioctl: SNDCTL_DSP_SETFMT"); - exit (10); - } - if (sndparam != AFMT_U8) { - perror("ioctl: SNDCTL_DSP_SETFMT"); - exit (10); - } - } - sndparam = 0; /* we want only 1 channel */ - if (ioctl(fd, SNDCTL_DSP_STEREO, &sndparam) == -1) { - perror("ioctl: SNDCTL_DSP_STEREO"); - exit (10); - } - if (sndparam == 1) { - fprintf(stderr, "soundif: Warning, cannot set the channel " - "number to 1, will use stereo\n"); - stereo=1; - } else - if (sndparam != 0) { - fprintf(stderr, "soundif: Error, cannot set the channel " - "number to 1\n"); - exit (10); - } - sndparam = sample_rate; - if (ioctl(fd, SNDCTL_DSP_SPEED, &sndparam) == -1) { - perror("ioctl: SNDCTL_DSP_SPEED"); - exit (10); - } - if ((10*abs(sndparam-sample_rate)) > sample_rate) { - perror("ioctl: SNDCTL_DSP_SPEED"); - exit (10); - } - if (sndparam != sample_rate) { - fprintf(stderr, "Warning: Sampling rate is %u, " - "requested %u\n", sndparam, sample_rate); - } -#if 0 - sndparam = 4; - if (ioctl(fd, SOUND_PCM_SUBDIVIDE, &sndparam) == -1) { - perror("ioctl: SOUND_PCM_SUBDIVIDE"); - } - if (sndparam != 4) { - perror("ioctl: SOUND_PCM_SUBDIVIDE"); - } -#endif - for (;;) { - if (fmt) { - i = read(fd, bp = b.b, sizeof(b.b)); - if (i < 0 && errno != EAGAIN) { - perror("read"); - exit(4); - } - if (!i) - break; - if (i > 0) { - for (; i >= sizeof(b.b[0]); i -= sizeof(b.b[0]), sp++) - fbuf[fbuf_cnt++] = ((int)(*bp)-0x80) * (1.0/128.0); - if (i) - fprintf(stderr, "warning: noninteger number of samples read\n"); - if (fbuf_cnt > overlap) { - process_buffer(fbuf, fbuf_cnt-overlap); - memmove(fbuf, fbuf+fbuf_cnt-overlap, overlap*sizeof(fbuf[0])); - fbuf_cnt = overlap; - } - } - } else { - i = read(fd, sp = b.s, sizeof(b.s)); - if (i < 0 && errno != EAGAIN) { - perror("read"); - exit(4); - } - if (!i) - break; - if (i > 0) { - if (stereo) { - for (; i >= sizeof(b.s[0]); i -= (sizeof(b.s[0])*2), sp+=2) - fbuf[fbuf_cnt++] = (*sp) * (1.0/32768.0); - } else { - for (; i >= sizeof(b.s[0]); i -= sizeof(b.s[0]), sp++) - fbuf[fbuf_cnt++] = (*sp) * (1.0/32768.0); - } - if (i) - fprintf(stderr, "warning: noninteger number of samples read\n"); - if (fbuf_cnt > overlap) { - process_buffer(fbuf, fbuf_cnt-overlap); - memmove(fbuf, fbuf+fbuf_cnt-overlap, overlap*sizeof(fbuf[0])); - fbuf_cnt = overlap; - } - } - } - } - close(fd); -} - -/* ---------------------------------------------------------------------- */ - -static void input_file(unsigned int sample_rate, unsigned int overlap, - const char *fname, const char *type) -{ - struct stat statbuf; - int pipedes[2]; - int pid = 0, soxstat; - int fd; - int i; - short buffer[8192]; - float fbuf[16384]; - unsigned int fbuf_cnt = 0; - short *sp; - - /* - * if the input type is not raw, sox is started to convert the - * samples to the requested format - */ - if (!type || !strcmp(type, "raw")) { - if ((fd = open(fname, O_RDONLY)) < 0) { - perror("open"); - exit(10); - } - } else { - if (stat(fname, &statbuf)) { - perror("stat"); - exit(10); - } - if (pipe(pipedes)) { - perror("pipe"); - exit(10); - } - if (!(pid = fork())) { - char srate[8]; - /* - * child starts here... first set up filedescriptors, - * then start sox... - */ - sprintf(srate, "%d", sample_rate); - close(pipedes[0]); /* close reading pipe end */ - close(1); /* close standard output */ - if (dup2(pipedes[1], 1) < 0) - perror("dup2"); - close(pipedes[1]); /* close writing pipe end */ - execlp("sox", "sox", - "-t", type, fname, - "-t", "raw", "-s", "-w", "-r", srate, "-", - NULL); - perror("execlp"); - exit(10); - } - if (pid < 0) { - perror("fork"); - exit(10); - } - close(pipedes[1]); /* close writing pipe end */ - fd = pipedes[0]; - } - /* - * demodulate - */ - for (;;) { - i = read(fd, sp = buffer, sizeof(buffer)); - if (i < 0 && errno != EAGAIN) { - perror("read"); - exit(4); - } - if (!i) - break; - if (i > 0) { - for (; i >= sizeof(buffer[0]); i -= sizeof(buffer[0]), sp++) - fbuf[fbuf_cnt++] = (*sp) * (1.0/32768.0); - if (i) - fprintf(stderr, "warning: noninteger number of samples read\n"); - if (fbuf_cnt > overlap) { - process_buffer(fbuf, fbuf_cnt-overlap); - memmove(fbuf, fbuf+fbuf_cnt-overlap, overlap*sizeof(fbuf[0])); - fbuf_cnt = overlap; - } - } - } - close(fd); - waitpid(pid, &soxstat, 0); -} - -/* ---------------------------------------------------------------------- */ - -static const char usage_str[] = "multimod\n" - "Demodulates many different radio transmission formats\n" - "(C) 1996 by Thomas Sailer HB9JNX/AE4WA\n" - " -t : input file type (any other type than raw requires sox)\n" - " -a : add demodulator\n" - " -p : output\n" - " -s : subtract demodulator\n"; - -int main(int argc, char *argv[]) -{ - int c; - int errflg = 0; - int i; - char **itype; - int mask_first = 1; - int sample_rate = -1; - unsigned int overlap = 0; - char *input_type = "hw"; - - fprintf(stdout, "multimod (C) 1996/1997 by Tom Sailer HB9JNX/AE4WA\n" - "available demodulators:"); - for (i = 0; i < NUMDEMOD; i++) - fprintf(stdout, " %s", dem[i]->name); - fprintf(stdout, "\n"); - while ((c = getopt(argc, argv, "t:a:p:s:v:")) != EOF) { - switch (c) { - case '?': - errflg++; - break; - - case 'v': - verbose_level = strtoul(optarg, 0, 0); - break; - - case 't': - for (itype = (char **)allowed_types; *itype; itype++) - if (!strcmp(*itype, optarg)) { - input_type = *itype; - goto intypefound; - } - fprintf(stderr, "invalid input type \"%s\"\n" - "allowed types: ", optarg); - for (itype = (char **)allowed_types; *itype; itype++) - fprintf(stderr, "%s ", *itype); - fprintf(stderr, "\n"); - errflg++; - intypefound: - break; - - case 'a': - if (mask_first) - memset(dem_mask, 0, sizeof(dem_mask)); - mask_first = 0; - for (i = 0; i < NUMDEMOD; i++) - if (!strcasecmp(optarg, dem[i]->name)) { - MASK_SET(i); - break; - } - if (i >= NUMDEMOD) { - fprintf(stderr, "invalid mode \"%s\"\n", optarg); - errflg++; - } - break; - - case 'p': - printf("pipe=%s\n", optarg); - strcpy(multimon_pipe_name, optarg); - break; - - case 's': - if (mask_first) - memset(dem_mask, 0xff, sizeof(dem_mask)); - mask_first = 0; - for (i = 0; i < NUMDEMOD; i++) - if (!strcasecmp(optarg, dem[i]->name)) { - MASK_RESET(i); - break; - } - if (i >= NUMDEMOD) { - fprintf(stderr, "invalid mode \"%s\"\n", optarg); - errflg++; - } - break; - - } - } - if (errflg) { - (void)fprintf(stderr, usage_str); - exit(2); - } - if (mask_first) - memset(dem_mask, 0xff, sizeof(dem_mask)); - - fprintf(stdout, "Enabled demodulators:"); - for (i = 0; i < NUMDEMOD; i++) - if (MASK_ISSET(i)) { - fprintf(stdout, " %s", dem[i]->name); - memset(dem_st+i, 0, sizeof(dem_st[i])); - dem_st[i].dem_par = dem[i]; - if (dem[i]->init) - dem[i]->init(dem_st+i); - if (sample_rate == -1) - sample_rate = dem[i]->samplerate; - else if (sample_rate != dem[i]->samplerate) { - fprintf(stdout, "\n"); - fprintf(stderr, "Error: Current sampling rate %d, " - " demodulator \"%s\" requires %d\n", - sample_rate, dem[i]->name, dem[i]->samplerate); - exit(3); - } - if (dem[i]->overlap > overlap) - overlap = dem[i]->overlap; - } - fprintf(stdout, "\n"); - - if (!strcmp(input_type, "hw")) { - if ((argc - optind) >= 1) - input_sound(sample_rate, overlap, argv[optind]); - else - input_sound(sample_rate, overlap, NULL); - exit(0); - } - if ((argc - optind) < 1) { - (void)fprintf(stderr, "no source files specified\n"); - exit(4); - } - for (i = optind; i < argc; i++) - input_file(sample_rate, overlap, argv[i], input_type); - exit(0); -} - -/* ---------------------------------------------------------------------- */ diff --git a/sw/ground_segment/multimon/xdisplay.c b/sw/ground_segment/multimon/xdisplay.c deleted file mode 100644 index 6319e3a91c..0000000000 --- a/sw/ground_segment/multimon/xdisplay.c +++ /dev/null @@ -1,441 +0,0 @@ -/* - * xdisplay.c -- actually displaying things - * - * Copyright (C) 1996 - * Thomas Sailer (sailer@ife.ee.ethz.ch, hb9jnx@hb9w.che.eu) - * - * This program 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 of the License, or - * (at your option) any later version. - * - * This program 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 this program; if not, write to the Free Software - * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. - */ - -/* ---------------------------------------------------------------------- */ - -#include "multimon.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/* ---------------------------------------------------------------------- */ - -#define WIDTH 512 -#define HEIGHT 256 - -union comdata { - short s[WIDTH]; - unsigned char b[0]; -}; - -#define SAMPLING_RATE 22050 - -/* ---------------------------------------------------------------------- */ - -/* - * child data structures - */ - -static Display *display = NULL; -static Window window; -static GC gc; -static Pixmap pixmap; -static unsigned long col_zeroline; -static unsigned long col_background; -static unsigned long col_trace; - -static int cmdpipe[2]; -static int datapipe[2]; - -/* - * parent data - */ - -struct xdisp { - int used; - pid_t pid; - int cmdfd; - int datafd; -}; - -#define NUMCLI 16 - -static struct xdisp cli[NUMCLI] = { { 0, }, }; - -/* ---------------------------------------------------------------------- */ - -static int x_error_handler(Display *disp, XErrorEvent *evt) -{ - char err_buf[256], mesg[256], number[256]; - char *mtype = "XlibMessage"; - - XGetErrorText(disp, evt->error_code, err_buf, sizeof(err_buf)); - (void)fprintf(stderr, "X Error: %s\n", err_buf); - XGetErrorDatabaseText(disp, mtype, "MajorCode", - "Request Major code %d", mesg, sizeof(mesg)); - (void)fprintf(stderr, mesg, evt->request_code); - (void)sprintf(number, "%d", evt->request_code); - XGetErrorDatabaseText(disp, "XRequest", number, "", err_buf, - sizeof(err_buf)); - (void)fprintf(stderr, " (%s)\n", err_buf); - - abort(); -} - -/* ---------------------------------------------------------------------- */ - -static Bool predicate(Display *display, XEvent *event, char *arg) -{ - return True; -} - -/* ---------------------------------------------------------------------- */ - -static char *x_getkey(void) -{ - XWindowAttributes winattrs; - XEvent evt; - static char kbuf[32]; - int i; - - if (!display) - return NULL; - while (XCheckIfEvent(display, &evt, predicate, NULL)) { - switch (evt.type) { - case KeyPress: - i = XLookupString((XKeyEvent *)&evt, kbuf, - sizeof(kbuf)-1, NULL, NULL); - if (i) { - kbuf[i] = 0; - return kbuf; - } - continue; - case DestroyNotify: - XCloseDisplay(display); - exit(0); - case Expose: - XGetWindowAttributes(display, window, &winattrs); - XCopyArea(display, window, pixmap, gc, 0, 0, - winattrs.width, winattrs.height, 0, 0); - continue; - default: - continue; - } - } - return NULL; -} - -/* ---------------------------------------------------------------------- */ - -static void process_keystrokes(void) -{ - char *cp; - - while ((cp = x_getkey())) { - printf("X: Keys pressed: %s\n", cp); - } -} - -/* ---------------------------------------------------------------------- */ - -static int do_x_select(int fd, int wr) -{ - int *xconn, xconnnum; - int max = fd, i; - fd_set rmask, wmask; - - if (!XInternalConnectionNumbers(display, &xconn, &xconnnum)) { - perror("XInternalConnectionNumbers"); - exit(1); - } - FD_ZERO(&rmask); - FD_ZERO(&wmask); - if (wr) - FD_SET(fd, &wmask); - else - FD_SET(fd, &rmask); - for (i = 0; i < xconnnum; i++) { - FD_SET(xconn[i], &rmask); - if (xconn[i] > max) - max = xconn[i]; - } - i = select(max+1, &rmask, &wmask, NULL, NULL); - if (i < 0) { - perror("select"); - exit(1); - } - for (i = 0; i < xconnnum; i++) - if (FD_ISSET(xconn[i], &rmask)) - XProcessInternalConnection(display, xconn[i]); - XFree(xconn); - process_keystrokes(); - if (wr) - return FD_ISSET(fd, &wmask); - else - return FD_ISSET(fd, &rmask); -} - -/* ---------------------------------------------------------------------- */ - -static void child_win_init(void) -{ - XSetWindowAttributes attr; - XGCValues gcv; - XColor color, dummy; - XSizeHints sizehints; - - /* - * start graphical output - */ - if (!(display = XOpenDisplay(NULL))) { - fprintf(stderr, "X: Unable to open X display\n"); - exit(1); - } - XSetErrorHandler(x_error_handler); - XAllocNamedColor(display, DefaultColormap(display, 0), "red", - &color, &dummy); - col_zeroline = color.pixel; - col_background = WhitePixel(display, 0); - col_trace = BlackPixel(display, 0); - attr.background_pixel = col_background; - window = XCreateWindow(display, XRootWindow(display, 0), - 200, 200, WIDTH, HEIGHT, 5, - DefaultDepth(display, 0), - InputOutput, DefaultVisual(display, 0), - CWBackPixel, &attr); - if (!(pixmap = XCreatePixmap(display, window, WIDTH, HEIGHT, - DefaultDepth(display, 0)))) { - fprintf(stderr, "X: unable to open offscreen pixmap\n"); - exit(1); - } - XSelectInput(display, window, KeyPressMask | StructureNotifyMask - | ExposureMask) ; - gcv.line_width = 1; - gcv.line_style = LineSolid; - gc = XCreateGC(display, pixmap, GCForeground | GCLineWidth, &gcv); - /* - * Do not allow the window to be resized - */ - memset(&sizehints, 0, sizeof(sizehints)); - sizehints.min_width = sizehints.max_width = WIDTH; - sizehints.min_height = sizehints.max_height = HEIGHT; - sizehints.flags = PMinSize | PMaxSize; - XSetWMNormalHints(display, window, &sizehints); - XMapWindow(display, window); - XSynchronize(display, 1); -} - -/* ---------------------------------------------------------------------- */ - -#define YCOORD(x) (((x)>>8)+(HEIGHT/2)) - -static void child_process(void) -{ - union comdata d; - unsigned char *bp; - int i, j; - - /* - * main loop - */ - for (;;) { - /* - * send synchronisation mark - */ - while (!do_x_select(cmdpipe[1], 1)); - i = write(cmdpipe[1], "r", 1); - if (i < 1) { - perror("write"); - exit(1); - } - /* - * read data - */ - for (j = sizeof(d), bp = d.b; j > 0; ) { - while (!do_x_select(datapipe[0], 0)); - i = read(datapipe[0], bp, j); - if (i < 1) { - perror("read"); - exit(1); - } - j -= i; - bp += i; - } - /* - * clear pixmap - */ - XSetState(display, gc, col_background, col_background, - GXcopy, AllPlanes); - XFillRectangle(display, pixmap, gc, 0, 0, - WIDTH, HEIGHT); - /* - * draw zero line - */ - XSetForeground(display, gc, col_zeroline); - XDrawLine(display, pixmap, gc, 0, YCOORD(0), WIDTH, - YCOORD(0)); - /* - * draw input - */ - XSetForeground(display, gc, col_trace); - for (i = 1; i < WIDTH; i++) - XDrawLine(display, pixmap, gc, i-1, YCOORD(d.s[i-1]), - i, YCOORD(d.s[i])); - XCopyArea(display, pixmap, window, gc, 0, 0, - WIDTH, HEIGHT, 0, 0); - /* XSync(display, 0); */ - } - XDestroyWindow(display, window); - XCloseDisplay(display); -} - -/* ---------------------------------------------------------------------- */ - -static void sigchld_handler(int sig) -{ - pid_t pid; - int st; - unsigned int cnum; - - while ((pid = wait4(0, &st, WNOHANG, NULL)) != (pid_t)-1) { - for (cnum = 0; cnum < NUMCLI; cnum++) - if (cli[cnum].used && cli[cnum].pid == pid) { - cli[cnum].used = 0; - close(cli[cnum].cmdfd); - close(cli[cnum].datafd); - cli[cnum].pid = (pid_t)-1; - fprintf(stderr, "child process %i died, " - "status %i\n", (int)pid, st); - } - } -} - -/* ---------------------------------------------------------------------- */ - -void xdisp_terminate(int cnum) -{ - if (cnum < 0 || cnum >= NUMCLI) - return; - kill(cli[cnum].pid, SIGTERM); -} - -/* ---------------------------------------------------------------------- */ - -int xdisp_start(void) -{ - unsigned int cnum; - - /* - * find free client struct - */ - for (cnum = 0; (cnum < NUMCLI) && cli[cnum].used; cnum++); - if (cnum >= NUMCLI) - return -1; - signal(SIGCHLD, sigchld_handler); - /* - * start "IPC" mechanism (using the pipes) - */ - if (pipe(cmdpipe)) - return -1; - if (pipe(datapipe)) { - close(cmdpipe[0]); - close(cmdpipe[1]); - } - if ((cli[cnum].pid = fork())) { - if (cli[cnum].pid == (pid_t)-1) { - /* error */ - close(cmdpipe[0]); - close(cmdpipe[1]); - close(datapipe[0]); - close(datapipe[1]); - return -1; - } - /* parent */ - cli[cnum].cmdfd = cmdpipe[0]; - close(cmdpipe[1]); - close(datapipe[0]); - cli[cnum].datafd = datapipe[1]; - cli[cnum].used = 1; - fcntl(cmdpipe[0], F_SETFL, - (fcntl(cmdpipe[0], F_GETFL, 0) | O_NDELAY)); - return cnum; - } - /* - * child; the X process - */ - close(cmdpipe[0]); - close(datapipe[1]); - close(0); /* close stdin */ - child_win_init(); - child_process(); - exit(0); -} - -/* ---------------------------------------------------------------------- */ - -int xdisp_update(int cnum, float *f) -{ - unsigned char *bp; - short *sp; - int i, j; - char c; - union comdata d; - - if (cnum < 0 || cnum >= NUMCLI) - return 0; - i = read(cli[cnum].cmdfd, &c, 1); - if (i < 0 && errno != EAGAIN) { - perror("read"); - xdisp_terminate(cnum); - return 0; - } - if (i < 1) - return 0; - if (c != 'r') - return 0; - for (sp = d.s, i = 0; i < WIDTH; i++, sp++, f++) { - if (*f >= 1) - *sp = 32767; - else if (*f <= -1) - *sp = -32767; - else - *sp = 32767.0 * (*f); - } - bp = d.b; - j = sizeof(d); - while (j > 0) { - i = write(cli[cnum].datafd, bp, j); - if (i < 0 && errno != EAGAIN) { - perror("write"); - xdisp_terminate(cnum); - return 0; - } - if (i > 0) { - bp += i; - j -= i; - } - } - return 1; -} - -/* ---------------------------------------------------------------------- */ diff --git a/sw/ground_segment/python/dashboard/radiowatchframe.py b/sw/ground_segment/python/dashboard/radiowatchframe.py index 8b3578c2b2..274f88ee52 100644 --- a/sw/ground_segment/python/dashboard/radiowatchframe.py +++ b/sw/ground_segment/python/dashboard/radiowatchframe.py @@ -8,7 +8,7 @@ import math import pynotify import pygame.mixer -sys.path.append(os.getenv("PAPARAZZI_HOME") + "/sw/lib/python") +sys.path.append(os.getenv("PAPARAZZI_HOME") + "/sw/ext/pprzlink/lib/v1.0/python") from ivy_msg_interface import IvyMessagesInterface diff --git a/sw/ground_segment/python/guided_mode_example.py b/sw/ground_segment/python/guided_mode_example.py index 748cbd66ed..67ac19f7a0 100755 --- a/sw/ground_segment/python/guided_mode_example.py +++ b/sw/ground_segment/python/guided_mode_example.py @@ -9,9 +9,10 @@ from os import path, getenv # file is a reasonable substitute PPRZ_SRC = getenv("PAPARAZZI_SRC", path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../../'))) sys.path.append(PPRZ_SRC + "/sw/lib/python") +sys.path.append(PPRZ_SRC + "/sw/ext/pprzlink/lib/v1.0/python") from ivy_msg_interface import IvyMessagesInterface -from pprz_msg.message import PprzMessage +from pprzlink.message import PprzMessage from settings_xml_parse import PaparazziACSettings from math import radians diff --git a/sw/ground_segment/python/messages_app/messagesframe.py b/sw/ground_segment/python/messages_app/messagesframe.py index f3190a13c6..2c475371b8 100644 --- a/sw/ground_segment/python/messages_app/messagesframe.py +++ b/sw/ground_segment/python/messages_app/messagesframe.py @@ -10,11 +10,12 @@ from os import path, getenv # file is a reasonable substitute PPRZ_SRC = getenv("PAPARAZZI_SRC", path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../../../'))) sys.path.append(PPRZ_SRC + "/sw/lib/python") +sys.path.append(PPRZ_SRC + "/sw/ext/pprzlink/lib/v1.0/python") PPRZ_HOME = getenv("PAPARAZZI_HOME", PPRZ_SRC) from ivy_msg_interface import IvyMessagesInterface -from pprz_msg.message import PprzMessage +from pprzlink.message import PprzMessage WIDTH = 450 LABEL_WIDTH = 166 diff --git a/sw/ground_segment/python/move_waypoint_example.py b/sw/ground_segment/python/move_waypoint_example.py index 128b143060..093d01eee2 100755 --- a/sw/ground_segment/python/move_waypoint_example.py +++ b/sw/ground_segment/python/move_waypoint_example.py @@ -9,10 +9,10 @@ from time import sleep # if PAPARAZZI_SRC not set, then assume the tree containing this # file is a reasonable substitute PPRZ_SRC = getenv("PAPARAZZI_SRC", path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../../'))) -sys.path.append(PPRZ_SRC + "/sw/lib/python") +sys.path.append(PPRZ_SRC + "/sw/ext/pprzlink/lib/v1.0/python") from ivy_msg_interface import IvyMessagesInterface -from pprz_msg.message import PprzMessage +from pprzlink.message import PprzMessage class WaypointMover(object): diff --git a/sw/ground_segment/python/real_time_plot/messagepicker.py b/sw/ground_segment/python/real_time_plot/messagepicker.py index ff33b7365c..81c3412eb7 100755 --- a/sw/ground_segment/python/real_time_plot/messagepicker.py +++ b/sw/ground_segment/python/real_time_plot/messagepicker.py @@ -10,10 +10,10 @@ from os import path, getenv # if PAPARAZZI_SRC not set, then assume the tree containing this # file is a reasonable substitute PPRZ_SRC = getenv("PAPARAZZI_SRC", path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../../../'))) -sys.path.append(PPRZ_SRC + "/sw/lib/python") +sys.path.append(PPRZ_SRC + "/sw/ext/pprzlink/lib/v1.0/python") from ivy_msg_interface import IvyMessagesInterface -from pprz_msg.message import PprzMessage +from pprzlink.message import PprzMessage class Message(PprzMessage): diff --git a/sw/ground_segment/python/real_time_plot/plotpanel.py b/sw/ground_segment/python/real_time_plot/plotpanel.py index c4172af87a..9674f8c89e 100644 --- a/sw/ground_segment/python/real_time_plot/plotpanel.py +++ b/sw/ground_segment/python/real_time_plot/plotpanel.py @@ -15,9 +15,10 @@ import messagepicker # file is a reasonable substitute PPRZ_SRC = getenv("PAPARAZZI_SRC", path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../../../'))) sys.path.append(PPRZ_SRC + "/sw/lib/python") +sys.path.append(PPRZ_SRC + "/sw/ext/pprzlink/lib/v1.0/python") import pprz_env -from pprz_msg import messages_xml_map +from pprzlink import messages_xml_map class PlotData: diff --git a/sw/ground_segment/python/redundant_link/link_combiner.py b/sw/ground_segment/python/redundant_link/link_combiner.py index 07aaacaf42..6f850b61b1 100755 --- a/sw/ground_segment/python/redundant_link/link_combiner.py +++ b/sw/ground_segment/python/redundant_link/link_combiner.py @@ -31,9 +31,10 @@ from ivy.std_api import * PPRZ_HOME = os.getenv("PAPARAZZI_HOME") sys.path.append(PPRZ_HOME + "/sw/lib/python") +sys.path.append(PPRZ_HOME + "/sw/ext/pprzlink/lib/v1.0/python") import pprz_env -from pprz_msg import messages_xml_map +from pprzlink import messages_xml_map class Circular_Buffer: def __init__(self, size): diff --git a/sw/ground_segment/python/udp_link/udp_link.py b/sw/ground_segment/python/udp_link/udp_link.py index 4fc09d3367..56c6d9b872 100755 --- a/sw/ground_segment/python/udp_link/udp_link.py +++ b/sw/ground_segment/python/udp_link/udp_link.py @@ -14,9 +14,10 @@ import time PPRZ_SRC = os.getenv("PAPARAZZI_SRC", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), '../../../../'))) sys.path.append(PPRZ_SRC + "/sw/lib/python") +sys.path.append(PPRZ_SRC + "/sw/ext/pprzlink/lib/v1.0/python") import pprz_env -from pprz_msg import messages_xml_map +from pprzlink import messages_xml_map PING_PERIOD = 5.0 STATUS_PERIOD = 1.0 diff --git a/sw/ground_segment/tmtc/150m.ml b/sw/ground_segment/tmtc/150m.ml index 694206f1c6..df3f02eb99 100644 --- a/sw/ground_segment/tmtc/150m.ml +++ b/sw/ground_segment/tmtc/150m.ml @@ -17,18 +17,18 @@ let index_pprz_mode = 5 (* From var/include/basic.h *) let autopilot_HOME_mode_value = 3 (* From sw/airborne/autopilot.h *) (* Build the module to listen telemetry *) -module Tele_Pprz = Pprz.Messages(struct let name = "telemetry" end) +module Tele_Pprz = PprzLink.Messages(struct let name = "telemetry" end) (* Build the module to send uplink messages *) -module Datalink_Pprz = Pprz.Messages(struct let name = "datalink" end) +module Datalink_Pprz = PprzLink.Messages(struct let name = "datalink" end) (******************************* Send the message to the A/C to set it in HOME mode *) let set_to_HOME = fun () -> - let vs = ["ac_id", Pprz.String ac_id; - "index", Pprz.Int index_pprz_mode; - "value", Pprz.Float (float autopilot_HOME_mode_value)] in + let vs = ["ac_id", PprzLink.String ac_id; + "index", PprzLink.Int index_pprz_mode; + "value", PprzLink.Float (float autopilot_HOME_mode_value)] in Datalink_Pprz.message_send "dl" "SETTING" vs @@ -36,10 +36,10 @@ let set_to_HOME = fun () -> model, and set to HOME if higher than 150m *) let get_gps_message = fun label _sender vs -> (* Extract data from the message *) - let alt_m = Pprz.int_assoc "alt" vs / 100 - and utm_east = Pprz.int_assoc "utm_east" vs / 100 - and utm_north = Pprz.int_assoc "utm_north" vs / 100 - and utm_zone = Pprz.int_assoc "utm_zone" vs in + let alt_m = PprzLink.int_assoc "alt" vs / 100 + and utm_east = PprzLink.int_assoc "utm_east" vs / 100 + and utm_north = PprzLink.int_assoc "utm_north" vs / 100 + and utm_zone = PprzLink.int_assoc "utm_zone" vs in (* Build the geographic position *) let utm = { Latlong.utm_x = float utm_east; diff --git a/sw/ground_segment/tmtc/Makefile b/sw/ground_segment/tmtc/Makefile index 660b86fafc..e866263a53 100644 --- a/sw/ground_segment/tmtc/Makefile +++ b/sw/ground_segment/tmtc/Makefile @@ -28,25 +28,22 @@ include ../../Makefile.ocaml CONF = ../../../conf VAR = ../../../var -INCLUDES= -I ../multimon +INCLUDES= PKG = -package glibivy,pprz -LINKPKG = $(PKG) -linkpkg -dllpath-pkg pprz +LINKPKG = $(PKG) -linkpkg -dllpath-pkg pprz,pprzlink XPKG = -package pprz.xlib -XLINKPKG = $(XPKG) -linkpkg -dllpath-pkg pprz.xlib - -LIBMULTIMONCMA=../multimon/multimon.cma -LIBMULTIMONDLL= multimon.cma -dllpath $(PAPARAZZI_SRC)/sw/ground_segment/multimon +XLINKPKG = $(XPKG) -linkpkg -dllpath-pkg pprz.xlib,pprzlink SERVERCMO = server_globals.cmo aircraft.cmo wind.cmo airprox.cmo kml.cmo fw_server.ml rotorcraft_server.ml intruder.cmo server.cmo SERVERCMX = $(SERVERCMO:.cmo=.cmx) -all: link server messages settings dia diadec ivy_tcp_aircraft ivy_tcp_controller broadcaster ivy2udp ivy_serial_bridge app_server ivy2nmea +all: link server messages settings ivy_tcp_aircraft ivy_tcp_controller broadcaster ivy2udp ivy_serial_bridge app_server ivy2nmea 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 app_server gpsd2ivy c_ivy_client_example_1 c_ivy_client_example_2 c_ivy_client_example_3 ivy2nmea + $(Q)rm -f link server messages settings *.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 ivy2nmea messages : messages.cmo $(LIBPPRZCMA) @echo OL $@ @@ -64,24 +61,24 @@ server.opt : $(SERVERCMX) $(LIBPPRZCMXA) @echo OOL $@ $(Q)$(OCAMLOPT) $(INCLUDES) -o $@ -package glibivy,pprz -linkpkg $(SERVERCMX) -link : link.cmo $(LIBMULTIMONCMA) $(LIBPPRZCMA) +link : link.cmo $(LIBPPRZCMA) @echo OL $@ - $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $(LIBMULTIMONDLL) $< + $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $< -ivy_tcp_aircraft : ivy_tcp_aircraft.cmo $(LIBMULTIMONCMA) $(LIBPPRZCMA) +ivy_tcp_aircraft : ivy_tcp_aircraft.cmo $(LIBPPRZCMA) @echo OL $@ - $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $(LIBMULTIMONDLL) $< + $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $< -ivy_tcp_controller : ivy_tcp_controller.cmo $(LIBMULTIMONCMA) $(LIBPPRZCMA) +ivy_tcp_controller : ivy_tcp_controller.cmo $(LIBPPRZCMA) @echo OL $@ - $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $(LIBMULTIMONDLL) $< + $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $< -broadcaster : broadcaster.cmo $(LIBMULTIMONCMA) $(LIBPPRZCMA) +broadcaster : broadcaster.cmo $(LIBPPRZCMA) @echo OL $@ - $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $(LIBMULTIMONDLL) $< + $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $< ivy2udp : ivy2udp.cmo $(LIBPPRZCMA) @@ -89,16 +86,6 @@ ivy2udp : ivy2udp.cmo $(LIBPPRZCMA) $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $< -dia : dia.cmo $(LIBMULTIMONCMA) $(LIBPPRZCMA) - @echo OL $@ - $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $(LIBMULTIMONDLL) $< - - -diadec : diadec.cmo $(LIBMULTIMONCMA) $(LIBPPRZCMA) - @echo OL $@ - $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $(LIBMULTIMONDLL) $< - - 150m : 150m.cmo $(LIBPPRZCMA) @echo OL $@ $(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) gtkInit.cmo $< @@ -171,7 +158,7 @@ ivy_serial_bridge: ivy_serial_bridge.c .depend: Makefile @echo DEPEND $@ - $(Q)$(OCAMLDEP) -I $(LIBPPRZDIR) -I ../multimon *.ml* > .depend + $(Q)$(OCAMLDEP) -I $(LIBPPRZDIR) *.ml* > .depend ifneq ($(MAKECMDGOALS),clean) -include .depend diff --git a/sw/ground_segment/tmtc/airprox.ml b/sw/ground_segment/tmtc/airprox.ml index 539ee3e088..9782eb5fc1 100644 --- a/sw/ground_segment/tmtc/airprox.ml +++ b/sw/ground_segment/tmtc/airprox.ml @@ -25,7 +25,7 @@ open Aircraft open Latlong -module Alerts_Pprz = Pprz.Messages(struct let name = "alert" end) +module Alerts_Pprz = PprzLink.Messages(struct let name = "alert" end) (** computes distance between 2d points *) let distance = fun (x1, y1) (x2, y2) -> diff --git a/sw/ground_segment/tmtc/dia.ml b/sw/ground_segment/tmtc/dia.ml deleted file mode 100644 index 2474c5d7ba..0000000000 --- a/sw/ground_segment/tmtc/dia.ml +++ /dev/null @@ -1,122 +0,0 @@ -(* - * Copyright (C) 2007- ENAC, Pascal Brisset, 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. - * - *) - -(** Encode telemetry messages in an audio stream (to be mixed with a - video stream). Listen messages from the "ground" class (a server - must be running) and write message(s) of the "DIA" class. -*) - -open Printf - -let msg_period = 500 (* ms *) -let ac_id = ref 1 - -module Ground_Pprz = Pprz.Messages(struct let name = "ground" end) -module Sub_Pprz = Pprz.Messages(struct let name = "DIA" end) - -type state = { - mutable lat : float; - mutable long : float; - mutable alt : int; - - mutable course : int; - mutable speed : int; - - mutable cam_roll : int; - mutable cam_pitch : int; -} - -let state = { - lat = 0.; long = 0.; alt = 0; - course = 0; speed = 0; - cam_roll = 0; cam_pitch = 0; -} - -let msg_id, _ = Sub_Pprz.message_of_name "NAV_INFO" -let send_msg = fun () -> - let t = (Unix.gettimeofday ()) -. 1e9 in - let vs = [ - "unix_time", Pprz.Float t; - - "lat", Pprz.Float state.lat; - "long", Pprz.Float state.long; - "alt", Pprz.Int state.alt; - - "course", Pprz.Int state.course; - "speed", Pprz.Int state.speed; - - "cam_roll", Pprz.Int state.cam_roll; - "cam_pitch", Pprz.Int state.cam_pitch - ] in - let s = Sub_Pprz.payload_of_values msg_id !ac_id vs in - Debug.call 'l' (fun f -> fprintf f "sending: %s\n" (Debug.xprint (Serial.string_of_payload s))); - Hdlc.write_data (Serial.string_of_payload s) - -let fp_msg = fun _sender vs -> - if int_of_string (Pprz.string_assoc "ac_id" vs) = !ac_id then begin - state.lat <- Pprz.float_assoc "lat" vs; - state.long <- Pprz.float_assoc "long" vs; - state.alt <- truncate (Pprz.float_assoc "alt" vs); - state.course <- truncate (Pprz.float_assoc "course" vs); - state.speed <- truncate (Pprz.float_assoc "speed" vs *. 100.); - state.cam_roll <- truncate (Pprz.float_assoc "roll" vs); (* FIXME *) - state.cam_pitch <- truncate (Pprz.float_assoc "pitch" vs); (* FIXME *) - end - - - - - -let _ = - let ivy_bus = ref Defivybus.default_ivy_bus - and port = ref "/dev/dsp" in - let options = [ - "-b", Arg.Set_string ivy_bus, (sprintf " Default is %s" !ivy_bus); - "-d", Arg.Set_string port, (sprintf " Default is %s" !port); - "-ac", Arg.Set_int ac_id, (sprintf " Default is %d" !ac_id); - ] in - - Arg.parse - options - (fun _x -> ()) - "Usage: "; - - (* Connect to Ivy bus *) - Ivy.init "Link" "READY" (fun _ _ -> ()); - Ivy.start !ivy_bus; - - (* Listen for telemetry messages *) - ignore (Ground_Pprz.message_bind "FLIGHT_PARAM" fp_msg); - - (* Open the audio output and launch the periodic writings *) - let _fd = Hdlc.init_gen "/dev/dsp" in - ignore (Glib.Timeout.add Hdlc.write_period (fun _ -> Hdlc.write_to_dsp (); true)); - - (* Periodically send messages *) - ignore (Glib.Timeout.add msg_period (fun () -> send_msg (); true)); - - - (* Main Loop *) - let loop = Glib.Main.create true in - while Glib.Main.is_running loop do - ignore (Glib.Main.iteration true) - done diff --git a/sw/ground_segment/tmtc/diadec.ml b/sw/ground_segment/tmtc/diadec.ml deleted file mode 100644 index e6d2132fe8..0000000000 --- a/sw/ground_segment/tmtc/diadec.ml +++ /dev/null @@ -1,63 +0,0 @@ -(* - * Copyright (C) 2007- ENAC, Pascal Brisset, 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. - * - *) - -(** Decode Data In Audio stream and print the messages on stdout *) - -open Printf - -module Sub_Pprz = Pprz.Messages(struct let name = "DIA" end) -module PprzTransport = Serial.Transport(Pprz.Transport) - - -let use_tele_message = fun buf -> - let payload = Serial.payload_of_string buf in - Debug.call 'l' (fun f -> fprintf f "pprz receiving: %s\n" (Debug.xprint buf)); - try - let (msg_id, ac_id, values) = Sub_Pprz.values_of_payload payload in - let msg = Sub_Pprz.message_of_id msg_id in - printf "%d %s\n%!" ac_id (Sub_Pprz.string_of_message msg values) - with - _ -> - Debug.call 'W' (fun f -> fprintf f "Warning, cannot use: %s\n" (Debug.xprint buf)) - - -let _ = - let port = ref "/dev/dsp" in - let options = [ - "-d", Arg.Set_string port, (sprintf " Default is %s" !port); - ] in - - Arg.parse - options - (fun _x -> ()) - "Usage: "; - - (* Open the audio output and launch the periodic writings *) - let fd = Hdlc.init_dec "/dev/dsp" in - - ignore (Glib.Io.add_watch [`IN] (fun _ -> use_tele_message (Hdlc.get_data ()); true) (GMain.Io.channel_of_descr fd)); - - (* Main Loop *) - let loop = Glib.Main.create true in - while Glib.Main.is_running loop do - ignore (Glib.Main.iteration true) - done diff --git a/sw/ground_segment/tmtc/fw_server.ml b/sw/ground_segment/tmtc/fw_server.ml index 6367f5c19c..25c8eaf4bd 100644 --- a/sw/ground_segment/tmtc/fw_server.ml +++ b/sw/ground_segment/tmtc/fw_server.ml @@ -28,7 +28,7 @@ open Aircraft open Latlong module LL = Latlong module U = Unix -module Dl_Pprz = Pprz.Messages (struct let name = "datalink" end) +module Dl_Pprz = PprzLink.Messages (struct let name = "datalink" end) (* FIXME: bound the loop *) @@ -42,18 +42,18 @@ let rec norm_course = let fvalue = fun x -> match x with - Pprz.Float x -> x - | Pprz.Int32 x -> Int32.to_float x - | Pprz.Int64 x -> Int64.to_float x - | Pprz.Int x -> float_of_int x - | _ -> failwith (sprintf "Receive.log_and_parse: float expected, got '%s'" (Pprz.string_of_value x)) + PprzLink.Float x -> x + | PprzLink.Int32 x -> Int32.to_float x + | PprzLink.Int64 x -> Int64.to_float x + | PprzLink.Int x -> float_of_int x + | _ -> failwith (sprintf "Receive.log_and_parse: float expected, got '%s'" (PprzLink.string_of_value x)) let ivalue = fun x -> match x with - Pprz.Int x -> x - | Pprz.Int32 x -> Int32.to_int x - | Pprz.Int64 x -> Int64.to_int x + PprzLink.Int x -> x + | PprzLink.Int32 x -> Int32.to_int x + | PprzLink.Int64 x -> Int64.to_int x | _ -> failwith "Receive.log_and_parse: int expected" let format_string_field = fun s -> @@ -88,20 +88,20 @@ let update_waypoint = fun ac wp_id p alt -> let heading_from_course = ref false let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> - let value = fun x -> try Pprz.assoc x values with Not_found -> failwith (sprintf "Error: field '%s' not found\n" x) in + let value = fun x -> try PprzLink.assoc x values with Not_found -> failwith (sprintf "Error: field '%s' not found\n" x) in let fvalue = fun x -> let f = fvalue (value x) in match classify_float f with FP_infinite | FP_nan -> - let msg = sprintf "Non normal number: %f in '%s %s %s'" f ac_name msg.Pprz.name (string_of_values values) in + let msg = sprintf "Non normal number: %f in '%s %s %s'" f ac_name msg.PprzLink.name (string_of_values values) in raise (Telemetry_error (ac_name, format_string_field msg)) | _ -> f and ivalue = fun x -> ivalue (value x) in - if not (msg.Pprz.name = "DOWNLINK_STATUS") then + if not (msg.PprzLink.name = "DOWNLINK_STATUS") then a.last_msg_date <- U.gettimeofday (); - match msg.Pprz.name with + match msg.PprzLink.name with "GPS" -> a.gps_mode <- check_index (ivalue "mode") gps_modes "GPS_MODE"; if a.gps_mode = _3D then begin @@ -177,7 +177,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> | "ATTITUDE" -> let roll = fvalue "phi" and pitch = fvalue "theta" in - if (List.assoc "phi" msg.Pprz.fields).Pprz._type = Pprz.Scalar "int16" then begin (* Compatibility with old message in degrees *) + if (List.assoc "phi" msg.PprzLink.fields).PprzLink._type = PprzLink.Scalar "int16" then begin (* Compatibility with old message in degrees *) a.roll <- roll /. 180. *. pi; a.pitch <- pitch /. 180. *. pi; heading_from_course := true; (* Awfull hack to get heading from GPS *) @@ -345,9 +345,9 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> Dl_Pprz.message_send "ground_dl" "FORMATION_STATUS" values | "TCAS_RA" -> let vs = [ - "ac_id", Pprz.Int (ivalue "ac_id"); - "ac_id_conflict", Pprz.Int (int_of_string a.id); - "resolve", Pprz.Int (ivalue "resolve") + "ac_id", PprzLink.Int (ivalue "ac_id"); + "ac_id_conflict", PprzLink.Int (int_of_string a.id); + "resolve", PprzLink.Int (ivalue "resolve") ] in Dl_Pprz.message_send "ground_dl" "TCAS_RESOLVE" vs | "DATALINK_REPORT" -> diff --git a/sw/ground_segment/tmtc/fw_server.mli b/sw/ground_segment/tmtc/fw_server.mli index 3185e1d816..98e3e3dd73 100644 --- a/sw/ground_segment/tmtc/fw_server.mli +++ b/sw/ground_segment/tmtc/fw_server.mli @@ -23,6 +23,6 @@ *) val log_and_parse : - string -> Aircraft.aircraft -> Pprz.message -> Pprz.values -> unit + string -> Aircraft.aircraft -> PprzLink.message -> PprzLink.values -> unit (** [log_and_parse ac_id ac msg vs] *) diff --git a/sw/ground_segment/tmtc/ivy2udp.ml b/sw/ground_segment/tmtc/ivy2udp.ml index 4d71c3891f..3f0eafe1f0 100644 --- a/sw/ground_segment/tmtc/ivy2udp.ml +++ b/sw/ground_segment/tmtc/ivy2udp.ml @@ -24,9 +24,9 @@ let my_id = 0 -module Tm_Pprz = Pprz.Messages(struct let name = "telemetry" end) -module Dl_Pprz = Pprz.Messages(struct let name = "datalink" end) -module PprzTransport = Serial.Transport(Pprz.Transport) +module Tm_Pprz = PprzLink.Messages(struct let name = "telemetry" end) +module Dl_Pprz = PprzLink.Messages(struct let name = "datalink" end) +module PprzTransport = Protocol.Transport(Pprz_transport.Transport) open Printf @@ -60,7 +60,7 @@ let () = try let (msg_id, vs) = Tm_Pprz.values_of_string args.(0) in let payload = Tm_Pprz.payload_of_values msg_id (int_of_string !id) vs in - let buf = Pprz.Transport.packet payload in + let buf = Pprz_transport.Transport.packet payload in let n = String.length buf in let n' = Unix.sendto socket buf 0 n [] sockaddr in assert (n = n') @@ -83,10 +83,10 @@ let () = Debug.trace 'x' (Debug.xprint b); let use_dl_message = fun payload -> - Debug.trace 'x' (Debug.xprint (Serial.string_of_payload payload)); + Debug.trace 'x' (Debug.xprint (Protocol.string_of_payload payload)); let (msg_id, ac_id, values) = Dl_Pprz.values_of_payload payload in let msg = Dl_Pprz.message_of_id msg_id in - Dl_Pprz.message_send "ground_dl" msg.Pprz.name values in + Dl_Pprz.message_send "ground_dl" msg.PprzLink.name values in assert (PprzTransport.parse use_dl_message b = n) with diff --git a/sw/ground_segment/tmtc/ivy_tcp_aircraft.ml b/sw/ground_segment/tmtc/ivy_tcp_aircraft.ml index a42861a87a..e02a961b02 100644 --- a/sw/ground_segment/tmtc/ivy_tcp_aircraft.ml +++ b/sw/ground_segment/tmtc/ivy_tcp_aircraft.ml @@ -1,7 +1,7 @@ let my_id = 0 -module Tm_Pprz = Pprz.Messages(struct let name = "telemetry" end) -module Dl_Pprz = Pprz.Messages(struct let name = "datalink" end) -module PprzTransport = Serial.Transport(Pprz.Transport) +module Tm_Pprz = PprzLink.Messages(struct let name = "telemetry" end) +module Dl_Pprz = PprzLink.Messages(struct let name = "datalink" end) +module PprzTransport = Protocol.Transport(Pprz_transport.Transport) open Printf let () = @@ -33,7 +33,7 @@ let () = try let (msg_id, vs) = Tm_Pprz.values_of_string args.(0) in let payload = Tm_Pprz.payload_of_values msg_id (int_of_string !id) vs in - let buf = Pprz.Transport.packet payload in + let buf = Pprz_transport.Transport.packet payload in fprintf o "%s%!" buf with _ -> () in let _b = Ivy.bind get_ivy_message (sprintf "^%s (.*)" !id) in @@ -49,10 +49,10 @@ let () = Debug.trace 'x' (Debug.xprint b); let use_dl_message = fun payload -> - Debug.trace 'x' (Debug.xprint (Serial.string_of_payload payload)); + Debug.trace 'x' (Debug.xprint (Protocol.string_of_payload payload)); let (msg_id, ac_id, values) = Dl_Pprz.values_of_payload payload in let msg = Dl_Pprz.message_of_id msg_id in - Dl_Pprz.message_send "ground_dl" msg.Pprz.name values in + Dl_Pprz.message_send "ground_dl" msg.PprzLink.name values in assert (PprzTransport.parse use_dl_message b = n) with diff --git a/sw/ground_segment/tmtc/ivy_tcp_controller.ml b/sw/ground_segment/tmtc/ivy_tcp_controller.ml index 8674bd6667..92d5498cb8 100644 --- a/sw/ground_segment/tmtc/ivy_tcp_controller.ml +++ b/sw/ground_segment/tmtc/ivy_tcp_controller.ml @@ -1,8 +1,8 @@ open Printf -module Tm_Pprz = Pprz.Messages(struct let name = "telemetry" end) -module Dl_Pprz = Pprz.Messages(struct let name = "datalink" end) -module PprzTransport = Serial.Transport(Pprz.Transport) +module Tm_Pprz = PprzLink.Messages(struct let name = "telemetry" end) +module Dl_Pprz = PprzLink.Messages(struct let name = "datalink" end) +module PprzTransport = Protocol.Transport(Pprz_transport.Transport) let () = let host = ref "10.31.1.98" @@ -37,10 +37,10 @@ let () = Debug.trace 'x' (Debug.xprint b); let use_tele_message = fun payload -> - Debug.trace 'x' (Debug.xprint (Serial.string_of_payload payload)); + Debug.trace 'x' (Debug.xprint (Protocol.string_of_payload payload)); let (msg_id, ac_id, values) = Tm_Pprz.values_of_payload payload in let msg = Tm_Pprz.message_of_id msg_id in - Tm_Pprz.message_send (string_of_int ac_id) msg.Pprz.name values in + Tm_Pprz.message_send (string_of_int ac_id) msg.PprzLink.name values in ignore (PprzTransport.parse use_tele_message b) with @@ -56,9 +56,9 @@ let () = let get_ivy_message = fun _ args -> try let (msg_id, vs) = Dl_Pprz.values_of_string args.(0) in - let ac_id = Pprz.int_assoc "ac_id" vs in + let ac_id = PprzLink.int_assoc "ac_id" vs in let payload = Dl_Pprz.payload_of_values msg_id ac_id vs in - let buf = Pprz.Transport.packet payload in + let buf = Pprz_transport.Transport.packet payload in fprintf o "%s%!" buf with exc -> prerr_endline (Printexc.to_string exc) in let _b = Ivy.bind get_ivy_message "^ground_dl (.*)" in diff --git a/sw/ground_segment/tmtc/link.ml b/sw/ground_segment/tmtc/link.ml index 36646dbcfc..e486890492 100644 --- a/sw/ground_segment/tmtc/link.ml +++ b/sw/ground_segment/tmtc/link.ml @@ -28,20 +28,17 @@ open Latlong open Printf (* Handlers for the modem and Ivy messages *) -module Tm_Pprz = Pprz.Messages (struct let name = "telemetry" end) -module Ground_Pprz = Pprz.Messages (struct let name = "ground" end) -module Dl_Pprz = Pprz.Messages (struct let name = "datalink" end) -module PprzTransport = Serial.Transport (Pprz.Transport) -module PprzTransportExtended = Serial.Transport (Pprz.TransportExtended) +module Tm_Pprz = PprzLink.Messages (struct let name = "telemetry" end) +module Ground_Pprz = PprzLink.Messages (struct let name = "ground" end) +module Dl_Pprz = PprzLink.Messages (struct let name = "datalink" end) +module PprzTransport = Protocol.Transport (Pprz_transport.Transport) (* Modem transport layer *) type transport = - Pprz (* Paparazzi protocol, with A/C id, message id and CRC *) - | Pprz2 (* Paparazzi protocol, with timestamp, A/C id, message id and CRC *) + | Pprz (* Paparazzi protocol, with A/C id, message id and CRC *) | XBee (* Maxstream protocol, API mode *) let transport_of_string = function - "pprz" -> Pprz - | "pprz2" -> Pprz2 + | "pprz" -> Pprz | "xbee" -> XBee | x -> invalid_arg (sprintf "transport_of_string: %s" x) @@ -83,7 +80,7 @@ let dead_aircraft_time_ms = ref 5000 let send_message_over_ivy = fun sender name vs -> let timestamp = match !add_timestamp with - None -> None + | None -> None | Some start_time -> Some (Unix.gettimeofday () -. start_time) in if !red_link then Tm_Pprz.message_send ?timestamp ~link_id:!link_id sender name vs @@ -93,7 +90,7 @@ let send_message_over_ivy = fun sender name vs -> let send_ground_over_ivy = fun sender name vs -> let timestamp = match !add_timestamp with - None -> None + | None -> None | Some start_time -> Some (Unix.gettimeofday () -. start_time) in Ground_Pprz.message_send ?timestamp sender name vs @@ -131,7 +128,7 @@ let update_status = fun ?udp_peername ac_id buf_size is_pong -> let s = { initial_status with udp_peername = udp_peername } in begin match s.udp_peername with - Some (Unix.ADDR_INET (peername, port)) -> + | Some (Unix.ADDR_INET (peername, port)) -> Debug.call 'b' (fun f -> fprintf f "Adding AC %i with udp_peername %s port %i\n" ac_id (Unix.string_of_inet_addr peername) port) | _ -> Debug.call 'b' (fun f -> fprintf f "Adding AC %i\n" ac_id) end; @@ -175,17 +172,17 @@ let send_status_msg = and msg_rate = float (status.rx_msg - status.last_rx_msg) /. dt in status.last_rx_msg <- status.rx_msg; status.last_rx_byte <- status.rx_byte; - let vs = ["ac_id", Pprz.String (string_of_int ac_id); - "link_id", Pprz.String (string_of_int !link_id); - "run_time", Pprz.Int64 (Int64.of_int t); - "rx_lost_time", Pprz.Int64 (Int64.of_int (status.ms_since_last_msg / 1000)); - "rx_bytes", Pprz.Int64 (Int64.of_int status.rx_byte); - "rx_msgs", Pprz.Int64 (Int64.of_int status.rx_msg); - "rx_err", Pprz.Int64 (Int64.of_int status.rx_err); - "rx_bytes_rate", Pprz.Float byte_rate; - "rx_msgs_rate", Pprz.Float msg_rate; - "tx_msgs", Pprz.Int64 (Int64.of_int status.tx_msg); - "ping_time", Pprz.Float (1000. *. (status.last_pong -. status.last_ping)) + let vs = ["ac_id", PprzLink.String (string_of_int ac_id); + "link_id", PprzLink.String (string_of_int !link_id); + "run_time", PprzLink.Int64 (Int64.of_int t); + "rx_lost_time", PprzLink.Int64 (Int64.of_int (status.ms_since_last_msg / 1000)); + "rx_bytes", PprzLink.Int64 (Int64.of_int status.rx_byte); + "rx_msgs", PprzLink.Int64 (Int64.of_int status.rx_msg); + "rx_err", PprzLink.Int64 (Int64.of_int status.rx_err); + "rx_bytes_rate", PprzLink.Float byte_rate; + "rx_msgs_rate", PprzLink.Float msg_rate; + "tx_msgs", PprzLink.Int64 (Int64.of_int status.tx_msg); + "ping_time", PprzLink.Float (1000. *. (status.last_pong -. status.last_ping)) ] in send_ground_over_ivy "link" "LINK_REPORT" vs) statuss @@ -197,14 +194,14 @@ let update_ms_since_last_msg = statuss let use_tele_message = fun ?udp_peername ?raw_data_size payload -> - let raw_data_size = match raw_data_size with None -> String.length (Serial.string_of_payload payload) | Some d -> d in - let buf = Serial.string_of_payload payload in + let raw_data_size = match raw_data_size with None -> String.length (Protocol.string_of_payload payload) | Some d -> d in + let buf = Protocol.string_of_payload payload in Debug.call 'l' (fun f -> fprintf f "pprz receiving: %s\n" (Debug.xprint buf)); try let (msg_id, ac_id, values) = Tm_Pprz.values_of_payload payload in let msg = Tm_Pprz.message_of_id msg_id in - send_message_over_ivy (string_of_int ac_id) msg.Pprz.name values; - update_status ?udp_peername ac_id raw_data_size (msg.Pprz.name = "PONG") + send_message_over_ivy (string_of_int ac_id) msg.PprzLink.name values; + update_status ?udp_peername ac_id raw_data_size (msg.PprzLink.name = "PONG") with exc -> prerr_endline (Printexc.to_string exc); @@ -213,26 +210,6 @@ let use_tele_message = fun ?udp_peername ?raw_data_size payload -> type priority = Null | Low | Normal | High -module Aerocomm = struct - let set_command_mode = fun fd -> - Serial.set_dtr fd true - - let set_data_mode = fun fd -> - Serial.set_dtr fd false - - let write_destination_address = fun fd address -> - assert (0 <= address && address <= 0xffffff); - let o = Unix.out_channel_of_descr fd in - let s = String.create 5 in - s.[0] <- Char.chr 0xcc; - s.[1] <- Char.chr 0x10; - s.[2] <- Char.chr (address lsr 16); - s.[3] <- Char.chr ((address lsr 8) land 0xff); - s.[4] <- Char.chr (address land 0xff); - fprintf o "%s%!" s -end - - module XB = struct (** XBee module *) let nb_retries = ref 10 let retry_delay = 200 (* ms *) @@ -244,16 +221,16 @@ module XB = struct (** XBee module *) let switch_to_api = fun device -> let o = Unix.out_channel_of_descr device.fd in Debug.trace 'x' "config xbee"; - fprintf o "%s%!" (Xbee.at_set_my !my_addr); - fprintf o "%s%!" Xbee.at_api_enable; - fprintf o "%s%!" Xbee.at_exit; + fprintf o "%s%!" (Xbee_transport.at_set_my !my_addr); + fprintf o "%s%!" Xbee_transport.at_api_enable; + fprintf o "%s%!" Xbee_transport.at_exit; Debug.trace 'x' "end init xbee" let init = fun device -> Debug.trace 'x' "init xbee"; let o = Unix.out_channel_of_descr device.fd in ignore (Glib.Timeout.add at_init_period (fun () -> - fprintf o "%s%!" Xbee.at_command_sequence; + fprintf o "%s%!" Xbee_transport.at_command_sequence; ignore (Glib.Timeout.add at_init_period (fun () -> switch_to_api device; false)); false)) @@ -272,14 +249,14 @@ module XB = struct (** XBee module *) let oversize_packet = 4 (* Start + msb_len + lsb_len + cksum *) let use_message = fun device frame_data -> - let frame_data = Serial.string_of_payload frame_data in + let frame_data = Protocol.string_of_payload frame_data in Debug.trace 'x' (Debug.xprint frame_data); - match Xbee.api_parse_frame frame_data with - Xbee.Modem_Status x -> + match Xbee_transport.api_parse_frame frame_data with + | Xbee_transport.Modem_Status x -> Debug.trace 'x' (sprintf "getting XBee status %d" x) - | Xbee.AT_Command_Response (frame_id, comm, status, value) -> + | Xbee_transport.AT_Command_Response (frame_id, comm, status, value) -> Debug.trace 'x' (sprintf "getting XBee AT command response: %d %s %d %s" frame_id comm status (Debug.xprint value)) - | Xbee.TX_Status (frame_id,status) | Xbee.TX868_Status (frame_id,status,_) -> + | Xbee_transport.TX_Status (frame_id,status) | Xbee_transport.TX868_Status (frame_id,status,_) -> Debug.trace 'x' (sprintf "getting XBee TX status: %d %d" frame_id status); if status = 1 then (* no ack, retry *) let (packet, nb_prev_retries) = packets.(frame_id) in @@ -293,27 +270,27 @@ module XB = struct (** XBee module *) false)); end - | Xbee.RX_Packet_64 (addr64, rssi, options, data) -> + | Xbee_transport.RX_Packet_64 (addr64, rssi, options, data) -> Debug.trace 'x' (sprintf "getting XBee RX64: %Lx %d %d %s" addr64 rssi options (Debug.xprint data)); - use_tele_message ~raw_data_size:(String.length frame_data + oversize_packet) (Serial.payload_of_string data) - | Xbee.RX868_Packet (addr64, options, data) -> + use_tele_message ~raw_data_size:(String.length frame_data + oversize_packet) (Protocol.payload_of_string data) + | Xbee_transport.RX868_Packet (addr64, options, data) -> Debug.trace 'x' (sprintf "getting XBee868 RX: %Lx %d %s" addr64 options (Debug.xprint data)); - use_tele_message ~raw_data_size:(String.length frame_data + oversize_packet) (Serial.payload_of_string data) - | Xbee.RX_Packet_16 (addr16, rssi, options, data) -> + use_tele_message ~raw_data_size:(String.length frame_data + oversize_packet) (Protocol.payload_of_string data) + | Xbee_transport.RX_Packet_16 (addr16, rssi, options, data) -> Debug.trace 'x' (sprintf "getting XBee RX16: from=%x %d %d %s" addr16 rssi options (Debug.xprint data)); - use_tele_message ~raw_data_size:(String.length frame_data + oversize_packet) (Serial.payload_of_string data) + use_tele_message ~raw_data_size:(String.length frame_data + oversize_packet) (Protocol.payload_of_string data) let send = fun ?ac_id device rf_data -> let ac_id = match ac_id with None -> 0xffff | Some a -> a in - let rf_data = Serial.string_of_payload rf_data in + let rf_data = Protocol.string_of_payload rf_data in let frame_id = gen_frame_id () in let frame_data = - if !Xbee.mode868 then - Xbee.api_tx64 ~frame_id (Int64.of_int ac_id) rf_data + if !Xbee_transport.mode868 then + Xbee_transport.api_tx64 ~frame_id (Int64.of_int ac_id) rf_data else - Xbee.api_tx16 ~frame_id ac_id rf_data in - let packet = Xbee.Protocol.packet (Serial.payload_of_string frame_data) in + Xbee_transport.api_tx16 ~frame_id ac_id rf_data in + let packet = Xbee_transport.Transport.packet (Protocol.payload_of_string frame_data) in (* Store the packet for further retry *) packets.(frame_id) <- (packet, 1); @@ -331,7 +308,7 @@ let local_broadcast_address = (Unix.inet_addr_of_string "127.255.255.255") let udp_send = fun fd payload peername -> - let buf = Pprz.Transport.packet payload in + let buf = Pprz_transport.Transport.packet payload in let len = String.length buf in let addr = if !udp_broadcast then (Unix.inet_addr_of_string !udp_broadcast_addr) else peername in Debug.call 'u' (fun f -> fprintf f "udp_send to %s port %i\n" (Unix.string_of_inet_addr addr) !udp_uplink_port); @@ -348,13 +325,13 @@ let send = fun ac_id device payload _priority -> () with Not_found -> () in match udp_peername ac_id with - Some (Unix.ADDR_INET (peername, _port)) -> + | Some (Unix.ADDR_INET (peername, _port)) -> udp_send device.fd payload peername | _ -> match device.transport with - Pprz | Pprz2 -> + | Pprz -> let o = Unix.out_channel_of_descr device.fd in - let buf = Pprz.Transport.packet payload in + let buf = Pprz_transport.Transport.packet payload in Printf.fprintf o "%s" buf; flush o; Debug.call 's' (fun f -> fprintf f "mm sending: %s\n" (Debug.xprint buf)); | XBee -> @@ -374,37 +351,20 @@ let broadcast = fun device payload _priority -> statuss else match device.transport with - Pprz -> + | Pprz -> let o = Unix.out_channel_of_descr device.fd in - let buf = Pprz.Transport.packet payload in + let buf = Pprz_transport.Transport.packet payload in Printf.fprintf o "%s" buf; flush o; Debug.call 'l' (fun f -> fprintf f "mm sending: %s\n" (Debug.xprint buf)); - | Pprz2 -> - let o = Unix.out_channel_of_descr device.fd in - let buf = Pprz.TransportExtended.packet payload in - Printf.fprintf o "%s" buf; flush o; - Debug.call 'l' (fun f -> fprintf f "mm sending: %s\n" (Debug.xprint buf)); | XBee -> XB.send device payload -(*************** Audio *******************************************************) -module Audio = struct - let use_data = - let buffer = ref "" in - fun data -> - let b = !buffer ^ data in - let n = PprzTransport.parse use_tele_message b in - buffer := String.sub b n (String.length b - n) -end - - - let parser_of_device = fun device -> match device.transport with - Pprz -> + | Pprz -> let use = fun s -> - let raw_data_size = String.length (Serial.string_of_payload s) + 4 (*stx,len,ck_a, ck_b*) in + let raw_data_size = String.length (Protocol.string_of_payload s) + 4 (*stx,len,ck_a, ck_b*) in let udp_peername = if !udp then Some !last_udp_peername @@ -412,18 +372,8 @@ let parser_of_device = fun device -> None in use_tele_message ?udp_peername ~raw_data_size s in PprzTransport.parse use - | Pprz2 -> - let use = fun s -> - let raw_data_size = String.length (Serial.string_of_payload s) + 8 (*stx,len, timestamp, ck_a, ck_b*) in - let udp_peername = - if !udp then - Some !last_udp_peername - else - None in - use_tele_message ?udp_peername ~raw_data_size s in - PprzTransportExtended.parse use | XBee -> - let module XbeeTransport = Serial.Transport (Xbee.Protocol) in + let module XbeeTransport = Protocol.Transport (Xbee_transport.Transport) in XbeeTransport.parse (XB.use_message device) @@ -436,7 +386,7 @@ let hangup = fun _ -> let message_uplink = fun device -> let forwarder = fun name _sender vs -> Debug.call 'f' (fun f -> fprintf f "forward %s\n" name); - let ac_id = Pprz.int_assoc "ac_id" vs in + let ac_id = PprzLink.int_assoc "ac_id" vs in let msg_id, _ = Dl_Pprz.message_of_name name in let s = Dl_Pprz.payload_of_values msg_id my_id vs in send ac_id device s High in @@ -454,9 +404,9 @@ let message_uplink = fun device -> (* Set a forwarder or a broadcaster for all messages tagged in messages.xml *) Hashtbl.iter (fun _m_id msg -> - match msg.Pprz.link with - Some Pprz.Forwarded -> set_forwarder msg.Pprz.name - | Some Pprz.Broadcasted -> if !ac_info then set_broadcaster msg.Pprz.name + match msg.PprzLink.link with + | Some PprzLink.Forwarded -> set_forwarder msg.PprzLink.name + | Some PprzLink.Broadcasted -> if !ac_info then set_broadcaster msg.PprzLink.name | _ -> ()) Dl_Pprz.messages @@ -479,17 +429,12 @@ let () = and hw_flow_control = ref false and transport = ref "pprz" and uplink = ref true - and audio = ref false - and aerocomm = ref false and udp_port = ref 4242 in (* Parse command line options *) let options = - [ "-aerocomm", Arg.Set aerocomm, "Set serial Aerocomm data mode"; - "-audio", Arg.Unit (fun () -> audio := true; port := "/dev/dsp"), (sprintf "Listen a modulated audio signal on . Sets to /dev/dsp (the -d option must used after this one if needed)"); - "-b", Arg.Set_string ivy_bus, (sprintf " Default is %s" !ivy_bus); + [ "-b", Arg.Set_string ivy_bus, (sprintf " Default is %s" !ivy_bus); "-d", Arg.Set_string port, (sprintf " Default is %s" !port); - "-dtr", Arg.Set aerocomm, "Set serial DTR to false (deprecated)"; "-fg", Arg.Set gen_stat_trafic, "Enable trafic statistics on standard output"; "-noac_info", Arg.Clear ac_info, (sprintf "Disables AC traffic info (uplink)."); "-nouplink", Arg.Clear uplink, (sprintf "Disables the uplink (from the ground to the aircraft)."); @@ -505,7 +450,7 @@ let () = "-uplink", Arg.Set uplink, (sprintf "Deprecated (now default)"); "-xbee_addr", Arg.Set_int XB.my_addr, (sprintf " (%d)" !XB.my_addr); "-xbee_retries", Arg.Set_int XB.my_addr, (sprintf " (%d)" !XB.nb_retries); - "-xbee_868", Arg.Set Xbee.mode868, (sprintf "Enables the 868 protocol"); + "-xbee_868", Arg.Set Xbee_transport.mode868, (sprintf "Enables the 868 protocol"); "-redlink", Arg.Set red_link, (sprintf "Sets whether the link is a redundant link. Set this flag and the id flag to use multiple links"); "-id", Arg.Set_int link_id, (sprintf " Sets the link id. If multiple links are used, each must have a unique id. Default is %i" !link_id); "-status_period", Arg.Set_int status_msg_period, (sprintf " Sets the period (in ms) of the LINK_REPORT status message. Default is %i" !status_msg_period); @@ -524,22 +469,22 @@ let () = try let transport = transport_of_string !transport in - (** Listen on audio input or on a serial device or on multimon pipe *) + (** Listen on a udp port or a serial device or on pipe *) let on_serial_device = String.length !port >= 4 && String.sub !port 0 4 = "/dev" in (* FIXME *) let fd = - if !udp then begin - let sockaddr = Unix.ADDR_INET (Unix.inet_addr_any, !udp_port) - and socket = Unix.socket Unix.PF_INET Unix.SOCK_DGRAM 0 in - if !udp_broadcast then - Unix.setsockopt socket Unix.SO_BROADCAST true; - Unix.bind socket sockaddr; - socket - end else if !audio then - Demod.init !port - else if on_serial_device then + if !udp then + begin + let sockaddr = Unix.ADDR_INET (Unix.inet_addr_any, !udp_port) + and socket = Unix.socket Unix.PF_INET Unix.SOCK_DGRAM 0 in + if !udp_broadcast then + Unix.setsockopt socket Unix.SO_BROADCAST true; + Unix.bind socket sockaddr; + socket + end + else if on_serial_device then Serial.opendev !port (Serial.speed_of_baudrate !baudrate) !hw_flow_control - else + else Unix.openfile !port [Unix.O_RDWR] 0o640 in @@ -549,24 +494,18 @@ let () = (* The function to be called when data is available *) let read_fd = - if !audio then - fun _io_event -> (* Demodulation *) - let (data_left, _data_right) = Demod.get_data () in - Audio.use_data data_left; - true (* Returns true to be called again *) - else (* Buffering and parsing *) - let buffered_parser = - (* Get the specific parser for the given transport protocol *) - let parser = parser_of_device device in - let read = if !udp then udp_read else Unix.read in - (* Wrap the parser into the buffered bytes reader *) - match Serial.input ~read parser with Serial.Closure f -> f in - fun _io_event -> - begin - try buffered_parser fd with - exc -> prerr_endline (Printexc.to_string exc) - end; - true (* Returns true to be called again *) + let buffered_parser = + (* Get the specific parser for the given transport protocol *) + let parser = parser_of_device device in + let read = if !udp then udp_read else Unix.read in + (* Wrap the parser into the buffered bytes reader *) + match Serial.input ~read parser with Serial.Closure f -> f in + fun _io_event -> + begin + try buffered_parser fd with + exc -> prerr_endline (Printexc.to_string exc) + end; + true (* Returns true to be called again *) in ignore (Glib.Io.add_watch [`HUP] hangup (GMain.Io.channel_of_descr fd)); ignore (Glib.Io.add_watch [`IN] read_fd (GMain.Io.channel_of_descr fd)); @@ -583,10 +522,8 @@ let () = ignore (Glib.Timeout.add !ping_msg_period (fun () -> send_ping_msg device; true)); false in ignore (Glib.Timeout.add status_ping_diff start_ping); - if !aerocomm then - Aerocomm.set_data_mode fd; match transport with - XBee -> + | XBee -> XB.init device | _ -> () end; @@ -598,5 +535,5 @@ let () = ignore (Glib.Main.iteration true) done with - Xml.Error e -> prerr_endline (Xml.error e); exit 1 + | Xml.Error e -> prerr_endline (Xml.error e); exit 1 | exn -> fprintf stderr "%s\n" (Printexc.to_string exn); exit 1 diff --git a/sw/ground_segment/tmtc/messages.ml b/sw/ground_segment/tmtc/messages.ml index 6ba64182a6..6a0cbdec8c 100644 --- a/sw/ground_segment/tmtc/messages.ml +++ b/sw/ground_segment/tmtc/messages.ml @@ -71,7 +71,7 @@ let one_page = fun sender class_name (notebook:GPack.notebook) (topnote:GPack.no let literal_values = values_of_field f in let alt_value = try - let coeff = float_of_string (Pprz.alt_unit_coef_of_xml ~auto:"display" f) + let coeff = float_of_string (PprzLink.alt_unit_coef_of_xml ~auto:"display" f) and unit = Xml.attrib f "alt_unit" in fun value -> sprintf "%s (%f%s)" value (coeff*.float_of_string value) unit with @@ -79,12 +79,12 @@ let one_page = fun sender class_name (notebook:GPack.notebook) (topnote:GPack.no let update = fun (_a, x) -> value := try - let i = Pprz.int_of_value x in + let i = PprzLink.int_of_value x in sprintf "%s (%d)" literal_values.(i) i with _ -> match format_ with - | Some f -> alt_value (Pprz.formatted_string_of_value f x) - | _ -> alt_value (Pprz.string_of_value x) + | Some f -> alt_value (PprzLink.formatted_string_of_value f x) + | _ -> alt_value (PprzLink.string_of_value x) and display_value = fun () -> if notebook#page_num v#coerce = notebook#current_page then if l#label <> !value then l#set_text !value in @@ -92,11 +92,11 @@ let one_page = fun sender class_name (notebook:GPack.notebook) (topnote:GPack.no (* box dragger *) field_label#drag#source_set dnd_targets ~modi:[`BUTTON1] ~actions:[`COPY]; let data_get = fun _ (sel:GObj.selection_context) ~info ~time -> - let scale = Pprz.alt_unit_coef_of_xml ~auto:"display" f in + let scale = PprzLink.alt_unit_coef_of_xml ~auto:"display" f in let v = List.hd (Str.split (Str.regexp " ") l#text) in (* get value *) let nb = List.length (Str.split (Str.regexp ",") v) in (* get number of values if array *) let range = if nb > 1 then sprintf "0-%d" (nb-1) else "0" in - if Pprz.is_array_type type_ then + if PprzLink.is_array_type type_ then match GToolbox.input_string ~title:"Index of value to drag" ~text:range "Index or range in the array ?" with None -> () | Some i -> sel#return (sprintf "%s:%s:%s:%s[%s]:%s" sender class_name id field_name i scale) @@ -173,7 +173,7 @@ let one_page = fun sender class_name (notebook:GPack.notebook) (topnote:GPack.no let rec one_class = fun (notebook:GPack.notebook) (help_label:GObj.widget) (window:GWindow.window) timestamp force (ident, xml_class, sender) -> let class_name = (Xml.attrib xml_class "name") in let messages = Xml.children xml_class in - let module P = Pprz.Messages (struct let name = class_name end) in + let module P = PprzLink.Messages (struct let name = class_name end) in let senders = Hashtbl.create 5 in match sender with | Some "*" -> @@ -237,7 +237,7 @@ let _ = (** Get the XML description of the required classes *) let xml_classes = - let xml = Pprz.messages_xml () in + let xml = PprzLink.messages_xml () in let class_of = fun n -> try List.find (fun x -> ExtXml.attrib x "name" = n) (Xml.children xml) diff --git a/sw/ground_segment/tmtc/rotorcraft_server.ml b/sw/ground_segment/tmtc/rotorcraft_server.ml index 99e00ba9c1..72a6b14df8 100644 --- a/sw/ground_segment/tmtc/rotorcraft_server.ml +++ b/sw/ground_segment/tmtc/rotorcraft_server.ml @@ -28,7 +28,7 @@ open Aircraft open Latlong module LL = Latlong module U = Unix -module Dl_Pprz = Pprz.Messages (struct let name = "datalink" end) +module Dl_Pprz = PprzLink.Messages (struct let name = "datalink" end) (* FIXME: bound the loop *) @@ -42,30 +42,30 @@ let rec norm_course = let fvalue = fun x -> match x with - Pprz.Float x -> x - | Pprz.Int32 x -> Int32.to_float x - | Pprz.Int64 x -> Int64.to_float x - | Pprz.Int x -> float_of_int x - | _ -> failwith (sprintf "Receive.log_and_parse: float expected, got '%s'" (Pprz.string_of_value x)) + PprzLink.Float x -> x + | PprzLink.Int32 x -> Int32.to_float x + | PprzLink.Int64 x -> Int64.to_float x + | PprzLink.Int x -> float_of_int x + | _ -> failwith (sprintf "Receive.log_and_parse: float expected, got '%s'" (PprzLink.string_of_value x)) let ivalue = fun x -> match x with - Pprz.Int x -> x - | Pprz.Int32 x -> Int32.to_int x - | Pprz.Int64 x -> Int64.to_int x + PprzLink.Int x -> x + | PprzLink.Int32 x -> Int32.to_int x + | PprzLink.Int64 x -> Int64.to_int x | _ -> failwith "Receive.log_and_parse: int expected" (* let i32value = fun x -> match x with - Pprz.Int32 x -> x + PprzLink.Int32 x -> x | _ -> failwith "Receive.log_and_parse: int32 expected" *) let foi32value = fun x -> match x with - Pprz.Int32 x -> Int32.to_float x + PprzLink.Int32 x -> Int32.to_float x | _ -> failwith "Receive.log_and_parse: int32 expected" let format_string_field = fun s -> @@ -130,22 +130,22 @@ let hmsl_of_ref = fun nav_ref d_hmsl -> | _ -> 0. let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> - let value = fun x -> try Pprz.assoc x values with Not_found -> failwith (sprintf "Error: field '%s' not found\n" x) in + let value = fun x -> try PprzLink.assoc x values with Not_found -> failwith (sprintf "Error: field '%s' not found\n" x) in let fvalue = fun x -> let f = fvalue (value x) in match classify_float f with FP_infinite | FP_nan -> - let msg = sprintf "Non normal number: %f in '%s %s %s'" f ac_name msg.Pprz.name (string_of_values values) in + let msg = sprintf "Non normal number: %f in '%s %s %s'" f ac_name msg.PprzLink.name (string_of_values values) in raise (Telemetry_error (ac_name, format_string_field msg)) | _ -> f and ivalue = fun x -> ivalue (value x) (*and i32value = fun x -> i32value (value x)*) and foi32value = fun x -> foi32value (value x) in - if not (msg.Pprz.name = "DOWNLINK_STATUS") then + if not (msg.PprzLink.name = "DOWNLINK_STATUS") then a.last_msg_date <- U.gettimeofday (); - match msg.Pprz.name with + match msg.PprzLink.name with "ROTORCRAFT_FP" -> begin match a.nav_ref with None -> (); (* No nav_ref yet *) diff --git a/sw/ground_segment/tmtc/rotorcraft_server.mli b/sw/ground_segment/tmtc/rotorcraft_server.mli index 06978f4b24..66626b7532 100644 --- a/sw/ground_segment/tmtc/rotorcraft_server.mli +++ b/sw/ground_segment/tmtc/rotorcraft_server.mli @@ -23,6 +23,6 @@ *) val log_and_parse : - string -> Aircraft.aircraft -> Pprz.message -> Pprz.values -> unit + string -> Aircraft.aircraft -> PprzLink.message -> PprzLink.values -> unit (** [log_and_parse ac_id ac msg vs] *) diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml index ae7649fa40..584d9d1a7c 100644 --- a/sw/ground_segment/tmtc/server.ml +++ b/sw/ground_segment/tmtc/server.ml @@ -36,10 +36,10 @@ module U = Unix module LL = Latlong module Ground = struct let name = "ground" end -module Ground_Pprz = Pprz.Messages(Ground) -module Tm_Pprz = Pprz.Messages (struct let name = "telemetry" end) -module Alerts_Pprz = Pprz.Messages(struct let name = "alert" end) -module Dl_Pprz = Pprz.Messages (struct let name = "datalink" end) +module Ground_Pprz = PprzLink.Messages(Ground) +module Tm_Pprz = PprzLink.Messages (struct let name = "telemetry" end) +module Alerts_Pprz = PprzLink.Messages(struct let name = "alert" end) +module Dl_Pprz = PprzLink.Messages (struct let name = "datalink" end) let dl_id = "ground_dl" (* Hack, should be [my_id] *) @@ -67,7 +67,7 @@ let aircraft_alerts_period = 1000 (* ms *) let send_aircrafts_msg = fun _asker _values -> assert(_values = []); let names = String.concat "," (Hashtbl.fold (fun k _v r -> k::r) aircrafts []) ^ "," in - ["ac_list", Pprz.String names] + ["ac_list", PprzLink.String names] let expand_aicraft x = @@ -96,7 +96,7 @@ let log_xml = fun timeofday data_file -> make_element "configuration" ["time_of_day", string_of_float timeofday; "data_file", data_file] - [expanded_conf; Pprz.messages_xml ()] + [expanded_conf; PprzLink.messages_xml ()] let start_time = U.gettimeofday () @@ -145,20 +145,20 @@ let log = fun ?timestamp logging ac_name msg_name values -> (** Callback for a message from a registered A/C *) let ac_msg = fun messages_xml logging ac_name ac -> - let module Tele_Pprz = Pprz.MessagesOfXml(struct let xml = messages_xml let name="telemetry" end) in + let module Tele_Pprz = PprzLink.MessagesOfXml(struct let xml = messages_xml let name="telemetry" end) in fun ts m -> try let timestamp = try Some (float_of_string ts) with _ -> None in let (msg_id, values) = Tele_Pprz.values_of_string m in let msg = Tele_Pprz.message_of_id msg_id in - log ?timestamp logging ac_name msg.Pprz.name values; + log ?timestamp logging ac_name msg.PprzLink.name values; Fw_server.log_and_parse ac_name ac msg values; Rotorcraft_server.log_and_parse ac_name ac msg values with Telemetry_error (ac_name, msg) -> - Ground_Pprz.message_send my_id "TELEMETRY_ERROR" ["ac_id", Pprz.String ac_name;"message", Pprz.String msg]; + Ground_Pprz.message_send my_id "TELEMETRY_ERROR" ["ac_id", PprzLink.String ac_name;"message", PprzLink.String msg]; prerr_endline msg - | Pprz.Unknown_msg_name (x, c) -> + | PprzLink.Unknown_msg_name (x, c) -> fprintf stderr "Unknown message %s in class %s from %s: %s\n%!" x c ac_name m | x -> prerr_endline (Printexc.to_string x) @@ -184,26 +184,26 @@ let send_cam_status = fun a -> and north = dx *. sin alpha +. dy *. cos alpha in let wgs84 = Aircraft.add_pos_to_nav_ref (Geo a.pos) (east, north) in let twgs84 = Aircraft.add_pos_to_nav_ref nav_ref a.cam.target in - let values = ["ac_id", Pprz.String a.id; - "cam_lat", Pprz.Float ((Rad>>Deg)wgs84.posn_lat); - "cam_long", Pprz.Float ((Rad>>Deg)wgs84.posn_long); - "cam_target_lat", Pprz.Float ((Rad>>Deg)twgs84.posn_lat); - "cam_target_long", Pprz.Float ((Rad>>Deg)twgs84.posn_long)] in + let values = ["ac_id", PprzLink.String a.id; + "cam_lat", PprzLink.Float ((Rad>>Deg)wgs84.posn_lat); + "cam_long", PprzLink.Float ((Rad>>Deg)wgs84.posn_long); + "cam_target_lat", PprzLink.Float ((Rad>>Deg)twgs84.posn_lat); + "cam_target_long", PprzLink.Float ((Rad>>Deg)twgs84.posn_long)] in Ground_Pprz.message_send my_id "CAM_STATUS" values let send_if_calib = fun a -> let if_mode = get_indexed_value if_modes a.inflight_calib.if_mode in - let values = ["ac_id", Pprz.String a.id; - "if_mode", Pprz.String if_mode; - "if_value1", Pprz.Float a.inflight_calib.if_val1; - "if_value2", Pprz.Float a.inflight_calib.if_val2] in + let values = ["ac_id", PprzLink.String a.id; + "if_mode", PprzLink.String if_mode; + "if_value1", PprzLink.Float a.inflight_calib.if_val1; + "if_value2", PprzLink.Float a.inflight_calib.if_val2] in Ground_Pprz.message_send my_id "INFLIGH_CALIB" values let send_fbw = fun a -> - let values = [ "ac_id", Pprz.String a.id; - "rc_mode", Pprz.String a.fbw.rc_mode; - "rc_status", Pprz.String a.fbw.rc_status; - "rc_rate", Pprz.Int a.fbw.rc_rate ] in + let values = [ "ac_id", PprzLink.String a.id; + "rc_mode", PprzLink.String a.fbw.rc_mode; + "rc_status", PprzLink.String a.fbw.rc_status; + "rc_rate", PprzLink.Int a.fbw.rc_rate ] in Ground_Pprz.message_send my_id "FLY_BY_WIRE" values let send_dl_values = fun a -> @@ -214,7 +214,7 @@ let send_dl_values = fun a -> | None -> csv := sprintf "%s?," !csv | Some s -> csv := sprintf "%s%f," !csv s done; - let vs = ["ac_id", Pprz.String a.id; "values", Pprz.String !csv] in + let vs = ["ac_id", PprzLink.String a.id; "values", PprzLink.String !csv] in Ground_Pprz.message_send my_id "DL_VALUES" vs let send_svsinfo = fun a -> @@ -236,9 +236,9 @@ 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 vs = ["ac_id", Pprz.String a.id; - "pacc", Pprz.Int a.gps_Pacc; + let f = fun s r -> (s, PprzLink.String !r) in + let vs = ["ac_id", PprzLink.String a.id; + "pacc", PprzLink.Int a.gps_Pacc; f "svid" svid; f "flags" flags; f "qi" qi; f "msg_age" age; f "cno" cno; f "elev" elev; f "azim" azim] in Ground_Pprz.message_send my_id "SVSINFO" vs @@ -246,30 +246,30 @@ let send_svsinfo = fun a -> let send_horiz_status = fun a -> match a.horiz_mode with Circle (geo, r) -> - let vs = [ "ac_id", Pprz.String a.id; - "circle_lat", Pprz.Float ((Rad>>Deg)geo.posn_lat); - "circle_long", Pprz.Float ((Rad>>Deg)geo.posn_long); - "radius", Pprz.Int r ] in + let vs = [ "ac_id", PprzLink.String a.id; + "circle_lat", PprzLink.Float ((Rad>>Deg)geo.posn_lat); + "circle_long", PprzLink.Float ((Rad>>Deg)geo.posn_long); + "radius", PprzLink.Int r ] in Ground_Pprz.message_send my_id "CIRCLE_STATUS" vs | Segment (geo1, geo2) -> - let vs = [ "ac_id", Pprz.String a.id; - "segment1_lat", Pprz.Float ((Rad>>Deg)geo1.posn_lat); - "segment1_long", Pprz.Float ((Rad>>Deg)geo1.posn_long); - "segment2_lat", Pprz.Float ((Rad>>Deg)geo2.posn_lat); - "segment2_long", Pprz.Float ((Rad>>Deg)geo2.posn_long) ] in + let vs = [ "ac_id", PprzLink.String a.id; + "segment1_lat", PprzLink.Float ((Rad>>Deg)geo1.posn_lat); + "segment1_long", PprzLink.Float ((Rad>>Deg)geo1.posn_long); + "segment2_lat", PprzLink.Float ((Rad>>Deg)geo2.posn_lat); + "segment2_long", PprzLink.Float ((Rad>>Deg)geo2.posn_long) ] in Ground_Pprz.message_send my_id "SEGMENT_STATUS" vs | UnknownHorizMode -> () let send_survey_status = fun a -> match a.survey with None -> - Ground_Pprz.message_send my_id "SURVEY_STATUS" ["ac_id", Pprz.String a.id] + Ground_Pprz.message_send my_id "SURVEY_STATUS" ["ac_id", PprzLink.String a.id] | Some (geo1, geo2) -> - let vs = [ "ac_id", Pprz.String a.id; - "south_lat", Pprz.Float ((Rad>>Deg)geo1.posn_lat); - "west_long", Pprz.Float ((Rad>>Deg)geo1.posn_long); - "north_lat", Pprz.Float ((Rad>>Deg)geo2.posn_lat); - "east_long", Pprz.Float ((Rad>>Deg)geo2.posn_long) ] in + let vs = [ "ac_id", PprzLink.String a.id; + "south_lat", PprzLink.Float ((Rad>>Deg)geo1.posn_lat); + "west_long", PprzLink.Float ((Rad>>Deg)geo1.posn_long); + "north_lat", PprzLink.Float ((Rad>>Deg)geo2.posn_lat); + "east_long", PprzLink.Float ((Rad>>Deg)geo2.posn_long) ] in Ground_Pprz.message_send my_id "SURVEY_STATUS" vs @@ -279,11 +279,11 @@ let send_wind = fun a -> try let (wind_cap_rad, wind_polar, mean, stddev, _nb_sample) = Wind.get id in let vs = - ["ac_id", Pprz.String id; - "dir", Pprz.Float ((Rad>>Deg)wind_cap_rad); - "wspeed", Pprz.Float wind_polar; - "mean_aspeed", Pprz.Float mean; - "stddev", Pprz.Float stddev] in + ["ac_id", PprzLink.String id; + "dir", PprzLink.Float ((Rad>>Deg)wind_cap_rad); + "wspeed", PprzLink.Float wind_polar; + "mean_aspeed", PprzLink.Float mean; + "stddev", PprzLink.Float stddev] in Ground_Pprz.message_send my_id "WIND" vs with _exc -> () @@ -291,18 +291,18 @@ let send_wind = fun a -> let send_telemetry_status = fun a -> let id = a.id in let tl_payload = fun link_id datalink_status link_status -> - [ "ac_id", Pprz.String id; - "link_id", Pprz.String link_id; - "time_since_last_msg", Pprz.Float (U.gettimeofday () -. a.last_msg_date); (* don't use rx_lost_time from LINK_REPORT so it also works in simulation *) - "rx_bytes", Pprz.Int64 (Int64.of_int link_status.rx_bytes); - "rx_msgs", Pprz.Int64 (Int64.of_int link_status.rx_msgs); - "rx_bytes_rate", Pprz.Float link_status.rx_bytes_rate; - "tx_msgs", Pprz.Int64 (Int64.of_int link_status.tx_msgs); - "uplink_lost_time", Pprz.Int64 (Int64.of_int datalink_status.uplink_lost_time); - "uplink_msgs", Pprz.Int datalink_status.uplink_msgs; - "downlink_msgs", Pprz.Int datalink_status.downlink_msgs; - "downlink_rate", Pprz.Int datalink_status.downlink_rate; - "ping_time", Pprz.Float link_status.ping_time] + [ "ac_id", PprzLink.String id; + "link_id", PprzLink.String link_id; + "time_since_last_msg", PprzLink.Float (U.gettimeofday () -. a.last_msg_date); (* don't use rx_lost_time from LINK_REPORT so it also works in simulation *) + "rx_bytes", PprzLink.Int64 (Int64.of_int link_status.rx_bytes); + "rx_msgs", PprzLink.Int64 (Int64.of_int link_status.rx_msgs); + "rx_bytes_rate", PprzLink.Float link_status.rx_bytes_rate; + "tx_msgs", PprzLink.Int64 (Int64.of_int link_status.tx_msgs); + "uplink_lost_time", PprzLink.Int64 (Int64.of_int datalink_status.uplink_lost_time); + "uplink_msgs", PprzLink.Int datalink_status.uplink_msgs; + "downlink_msgs", PprzLink.Int datalink_status.downlink_msgs; + "downlink_rate", PprzLink.Int datalink_status.downlink_rate; + "ping_time", PprzLink.Float link_status.ping_time] in (* if no link send anyway for rx_lost_time with special link id *) if Hashtbl.length a.link_status = 0 then @@ -325,12 +325,12 @@ let send_moved_waypoints = fun a -> (fun wp_id wp -> let geo = wp.wp_geo in let vs = - ["ac_id", Pprz.String a.id; - "wp_id", Pprz.Int wp_id; - "long", Pprz.Float ((Rad>>Deg)geo.posn_long); - "lat", Pprz.Float ((Rad>>Deg)geo.posn_lat); - "alt", Pprz.Float wp.altitude; - "ground_alt", Pprz.Float (try float (Srtm.of_wgs84 geo) with _ -> a.ground_alt)] in + ["ac_id", PprzLink.String a.id; + "wp_id", PprzLink.Int wp_id; + "long", PprzLink.Float ((Rad>>Deg)geo.posn_long); + "lat", PprzLink.Float ((Rad>>Deg)geo.posn_lat); + "alt", PprzLink.Float wp.altitude; + "ground_alt", PprzLink.Float (try float (Srtm.of_wgs84 geo) with _ -> a.ground_alt)] in Ground_Pprz.message_send my_id "WAYPOINT_MOVED" vs) a.waypoints @@ -341,16 +341,16 @@ let send_moved_waypoints = fun a -> let send_aircraft_msg = fun ac -> try let a = Hashtbl.find aircrafts ac in - let f = fun x -> Pprz.Float x in + let f = fun x -> PprzLink.Float x in let wgs84 = try a.pos with _ -> LL.make_geo 0. 0. in - let values = ["ac_id", Pprz.String ac; + let values = ["ac_id", PprzLink.String ac; "roll", f (Geometry_2d.rad2deg a.roll); "pitch", f (Geometry_2d.rad2deg a.pitch); "heading", f (Geometry_2d.rad2deg a.heading); "lat", f ((Rad>>Deg)wgs84.posn_lat); "long", f ((Rad>>Deg) wgs84.posn_long); "unix_time", f a.unix_time; - "itow", Pprz.Int64 a.itow; + "itow", PprzLink.Int64 a.itow; "speed", f a.gspeed; "airspeed", f a.airspeed; (* negative value is sent if no airspeed available *) "course", f (Geometry_2d.rad2deg a.course); @@ -362,17 +362,17 @@ let send_aircraft_msg = fun ac -> (** send ACINFO messages if more than one A/C registered *) if Hashtbl.length aircrafts > 1 then begin - let cm_of_m_32 = fun f -> Pprz.Int32 (Int32.of_int (truncate (100. *. f))) in - let cm_of_m = fun f -> Pprz.Int (truncate (100. *. f)) in + let cm_of_m_32 = fun f -> PprzLink.Int32 (Int32.of_int (truncate (100. *. f))) in + let cm_of_m = fun f -> PprzLink.Int (truncate (100. *. f)) in let pos = LL.utm_of WGS84 a.pos in - let ac_info = ["ac_id", Pprz.String ac; + let ac_info = ["ac_id", PprzLink.String ac; "utm_east", cm_of_m_32 pos.utm_x; "utm_north", cm_of_m_32 pos.utm_y; - "course", Pprz.Int (truncate (10. *. (Geometry_2d.rad2deg a.course))); + "course", PprzLink.Int (truncate (10. *. (Geometry_2d.rad2deg a.course))); "alt", cm_of_m_32 a.alt; "speed", cm_of_m a.gspeed; "climb", cm_of_m a.climb; - "itow", Pprz.Int64 a.itow] in + "itow", PprzLink.Int64 a.itow] in Dl_Pprz.message_send dl_id "ACINFO" ac_info; end; @@ -382,30 +382,30 @@ let send_aircraft_msg = fun ac -> begin match a.nav_ref with Some nav_ref -> - let values = ["ac_id", Pprz.String ac; - "cur_block", Pprz.Int a.cur_block; - "cur_stage", Pprz.Int a.cur_stage; - "stage_time", Pprz.Int64 (Int64.of_int a.stage_time); - "block_time", Pprz.Int64 (Int64.of_int a.block_time); + let values = ["ac_id", PprzLink.String ac; + "cur_block", PprzLink.Int a.cur_block; + "cur_stage", PprzLink.Int a.cur_stage; + "stage_time", PprzLink.Int64 (Int64.of_int a.stage_time); + "block_time", PprzLink.Int64 (Int64.of_int a.block_time); "target_lat", f ((Rad>>Deg)a.desired_pos.posn_lat); "target_long", f ((Rad>>Deg)a.desired_pos.posn_long); - "target_alt", Pprz.Float a.desired_altitude; - "target_climb", Pprz.Float a.desired_climb; - "target_course", Pprz.Float ((Rad>>Deg)a.desired_course); - "dist_to_wp", Pprz.Float a.dist_to_wp + "target_alt", PprzLink.Float a.desired_altitude; + "target_climb", PprzLink.Float a.desired_climb; + "target_course", PprzLink.Float ((Rad>>Deg)a.desired_course); + "dist_to_wp", PprzLink.Float a.dist_to_wp ] in Ground_Pprz.message_send my_id "NAV_STATUS" values | None -> () (* No nav_ref yet *) end; - let values = ["ac_id", Pprz.String ac; + let values = ["ac_id", PprzLink.String ac; "throttle", f a.throttle; "throttle_accu", f a.throttle_accu; "rpm", f a.rpm; "temp", f a.temp; "bat", f a.bat; "amp", f a.amp; - "energy", Pprz.Int a.energy] in + "energy", PprzLink.Int a.energy] in Ground_Pprz.message_send my_id "ENGINE_STATUS" values; let ap_mode = get_indexed_value (modes_of_type a.vehicle_type) a.ap_mode in @@ -415,15 +415,15 @@ let send_aircraft_msg = fun ac -> let gps_mode = get_indexed_value gps_modes a.gps_mode in let state_filter_mode = get_indexed_value state_filter_modes a.state_filter_mode and kill_mode = if a.kill_mode then "ON" else "OFF" in - let values = ["ac_id", Pprz.String ac; - "flight_time", Pprz.Int64 (Int64.of_int a.flight_time); - "ap_mode", Pprz.String ap_mode; - "gaz_mode", Pprz.String gaz_mode; - "lat_mode", Pprz.String lat_mode; - "horiz_mode", Pprz.String horiz_mode; - "gps_mode", Pprz.String gps_mode; - "state_filter_mode", Pprz.String state_filter_mode; - "kill_mode", Pprz.String kill_mode + let values = ["ac_id", PprzLink.String ac; + "flight_time", PprzLink.Int64 (Int64.of_int a.flight_time); + "ap_mode", PprzLink.String ap_mode; + "gaz_mode", PprzLink.String gaz_mode; + "lat_mode", PprzLink.String lat_mode; + "horiz_mode", PprzLink.String horiz_mode; + "gps_mode", PprzLink.String gps_mode; + "state_filter_mode", PprzLink.String state_filter_mode; + "kill_mode", PprzLink.String kill_mode ] in Ground_Pprz.message_send my_id "AP_STATUS" values; @@ -477,23 +477,23 @@ let check_md5sum = fun ac_name alive_md5sum aircraft_conf_dir -> try match alive_md5sum with - Pprz.Array array -> + PprzLink.Array array -> let n = Array.length array in assert(n = String.length md5sum / 2); for i = 0 to n - 1 do let x = int_of_string (sprintf "0x%c%c" md5sum.[2*i] md5sum.[2*i+1]) in - assert (x = Pprz.int_of_value array.(i)) + assert (x = PprzLink.int_of_value array.(i)) done | _ -> failwith "Array expected here" with _ -> try match alive_md5sum with - Pprz.Array array -> + PprzLink.Array array -> let n = Array.length array in assert(n = String.length md5sum / 2); for i = 0 to n - 1 do let x = 0 in - assert (x = Pprz.int_of_value array.(i)) + assert (x = PprzLink.int_of_value array.(i)) done; fprintf stderr "MD5 is ZERO, be carefull with configurations\n%!" | _ -> failwith "Array expected here" @@ -533,7 +533,7 @@ let new_aircraft = fun get_alive_md5sum real_id -> ignore (Glib.Timeout.add 1000 (fun _ -> update (); true)); - let messages_xml = Xml.parse_file (Env.paparazzi_home // root_dir // "conf" // "messages.xml") in + let messages_xml = Xml.parse_file (Env.paparazzi_home // root_dir // "var" // "messages.xml") in ac, messages_xml let check_alerts = fun a -> @@ -546,9 +546,9 @@ let check_alerts = fun a -> and warning_level = fvalue "LOW_BAT_LEVEL" 10.5 in let send = fun level -> - let vs = [ "ac_id", Pprz.String a.id; - "level", Pprz.String level; - "value", Pprz.Float a.bat] in + let vs = [ "ac_id", PprzLink.String a.id; + "level", PprzLink.String level; + "value", PprzLink.Float a.bat] in Alerts_Pprz.message_send my_id "BAT_LOW" vs in if a.bat < 1. then send "INVALID" else if a.bat < catastrophic_level then send "CATASTROPHIC" @@ -556,7 +556,7 @@ let check_alerts = fun a -> else if a.bat < warning_level then send "WARNING" let wind_clear = fun _sender vs -> - Wind.clear (Pprz.string_assoc "ac_id" vs) + Wind.clear (PprzLink.string_assoc "ac_id" vs) let periodic = fun period cb -> Glib.Timeout.add period (fun () -> cb (); true) @@ -582,7 +582,7 @@ let periodic_airprox_check = fun name -> None -> () | Some level -> let vs = - ["ac_id", Pprz.String (thisac.id ^ "," ^ ac.id) ; "level", Pprz.String level] in + ["ac_id", PprzLink.String (thisac.id ^ "," ^ ac.id) ; "level", PprzLink.String level] in Alerts_Pprz.message_send my_id "AIR_PROX" vs with x -> fprintf stderr "check_airprox: %s\n%!" (Printexc.to_string x) @@ -613,7 +613,7 @@ let ident_msg = fun log timestamp name vs -> try if not (Hashtbl.mem aircrafts name) && not (Hashtbl.mem unknown_aircrafts name) then - let get_md5sum = fun () -> Pprz.assoc "md5sum" vs in + let get_md5sum = fun () -> PprzLink.assoc "md5sum" vs in let ac, messages_xml = new_aircraft get_md5sum name in let ac_msg_closure = ac_msg messages_xml log name ac in let tsregexp = if timestamp then "(([0-9]+\\.[0-9]+) )?" else "" in @@ -621,7 +621,7 @@ let ident_msg = fun log timestamp name vs -> Ivy.bind (fun _ args -> if timestamp then ac_msg_closure args.(1) args.(2) else ac_msg_closure "" args.(0)) (sprintf "^%s%s +(.*)" tsregexp name) in register_aircraft name ac; - Ground_Pprz.message_send my_id "NEW_AIRCRAFT" ["ac_id", Pprz.String name] + Ground_Pprz.message_send my_id "NEW_AIRCRAFT" ["ac_id", PprzLink.String name] with exc -> prerr_endline (Printexc.to_string exc) @@ -636,19 +636,19 @@ let listen_acs = fun log timestamp -> ignore (Tm_Pprz.message_bind "PPRZ_MODE" (ident_msg log timestamp)) let send_intruder_acinfo = fun id intruder -> - let cm_of_m_32 = fun f -> Pprz.Int32 (Int32.of_int (truncate (100. *. f))) in - let cm_of_m = fun f -> Pprz.Int (truncate (100. *. f)) in + let cm_of_m_32 = fun f -> PprzLink.Int32 (Int32.of_int (truncate (100. *. f))) in + let cm_of_m = fun f -> PprzLink.Int (truncate (100. *. f)) in let pos = LL.utm_of WGS84 intruder.Intruder.pos in (* TODO: find a better way to map intruders to AC_IDs *) let ac_id = 200 + ((int_of_string id) mod 50) in - let ac_info = ["ac_id", Pprz.Int ac_id; + let ac_info = ["ac_id", PprzLink.Int ac_id; "utm_east", cm_of_m_32 pos.utm_x; "utm_north", cm_of_m_32 pos.utm_y; - "course", Pprz.Int (truncate (10. *. (Geometry_2d.rad2deg intruder.Intruder.course))); + "course", PprzLink.Int (truncate (10. *. (Geometry_2d.rad2deg intruder.Intruder.course))); "alt", cm_of_m_32 intruder.Intruder.alt; "speed", cm_of_m intruder.Intruder.gspeed; "climb", cm_of_m intruder.Intruder.climb; - "itow", Pprz.Int64 intruder.Intruder.itow] in + "itow", PprzLink.Int64 intruder.Intruder.itow] in Dl_Pprz.message_send dl_id "ACINFO" ac_info let periodic_handle_intruders = fun () -> @@ -664,26 +664,26 @@ let periodic_handle_intruders = fun () -> Hashtbl.iter (send_intruder_acinfo) intruders let add_intruder = fun vs -> - let id = Pprz.string_assoc "id" vs in - let name = Pprz.string_assoc "name" vs in + let id = PprzLink.string_assoc "id" vs in + let name = PprzLink.string_assoc "name" vs in let intruder = Intruder.new_intruder id name in Hashtbl.add intruders id intruder let update_intruder = fun logging _sender vs -> try - let id = Pprz.string_assoc "id" vs in + let id = PprzLink.string_assoc "id" vs in (*prerr_endline (sprintf "update_intruder %s" id);*) if not (Hashtbl.mem intruders id) then add_intruder vs; let i = Hashtbl.find intruders id in - let lat = Pprz.int_assoc "lat" vs - and lon = Pprz.int_assoc "lon" vs in + let lat = PprzLink.int_assoc "lat" vs + and lon = PprzLink.int_assoc "lon" vs in let geo = make_geo_deg (float lat /. 1e7) (float lon /. 1e7) in i.Intruder.pos <- geo; - i.Intruder.alt <- float (Pprz.int_assoc "alt" vs) /. 1000.; - i.Intruder.course <- Pprz.float_assoc "course" vs; - i.Intruder.gspeed <- Pprz.float_assoc "speed" vs; - i.Intruder.climb <- Pprz.float_assoc "climb" vs; + i.Intruder.alt <- float (PprzLink.int_assoc "alt" vs) /. 1000.; + i.Intruder.course <- PprzLink.float_assoc "course" vs; + i.Intruder.gspeed <- PprzLink.float_assoc "speed" vs; + i.Intruder.climb <- PprzLink.float_assoc "climb" vs; i.Intruder.unix_time <- U.gettimeofday (); log logging "ground" "INTRUDER" vs with @@ -695,7 +695,7 @@ let listen_intruders = fun log -> ignore(Ground_Pprz.message_bind "INTRUDER" (update_intruder log)) let send_config = fun http _asker args -> - let ac_id' = Pprz.string_assoc "ac_id" args in + let ac_id' = PprzLink.string_assoc "ac_id" args in try let _is_replayed, ac_id, root_dir, conf_xml = replayed ac_id' in @@ -716,13 +716,13 @@ let send_config = fun http _asker args -> "settings.xml") else "file://replay" in let col = try Xml.attrib conf "gui_color" with _ -> new_color () in let ac_name = try Xml.attrib conf "name" with _ -> "" in - [ "ac_id", Pprz.String ac_id; - "flight_plan", Pprz.String fp; - "airframe", Pprz.String af; - "radio", Pprz.String rc; - "settings", Pprz.String settings; - "default_gui_color", Pprz.String col; - "ac_name", Pprz.String ac_name ] + [ "ac_id", PprzLink.String ac_id; + "flight_plan", PprzLink.String fp; + "airframe", PprzLink.String af; + "radio", PprzLink.String rc; + "settings", PprzLink.String settings; + "default_gui_color", PprzLink.String col; + "ac_name", PprzLink.String ac_name ] with Not_found -> failwith (sprintf "ground UNKNOWN %s" ac_id') @@ -732,83 +732,83 @@ let ivy_server = fun http -> ignore (Ground_Pprz.message_answerer my_id "CONFIG" (send_config http)) (** Convert to cm, with rounding *) -let cm_of_m = fun f -> Pprz.Int (truncate ((100. *. f) +. 0.5)) +let cm_of_m = fun f -> PprzLink.Int (truncate ((100. *. f) +. 0.5)) (** Convert to mm, with rounding *) -let mm_of_m_32 = fun f -> Pprz.Int32 (Int32.of_int (truncate ((1000. *. f) +. 0.5))) +let mm_of_m_32 = fun f -> PprzLink.Int32 (Int32.of_int (truncate ((1000. *. f) +. 0.5))) (** Got a ground.MOVE_WAYPOINT and send a datalink.MOVE_WP *) let move_wp = fun logging _sender vs -> let f = fun a -> List.assoc a vs - and ac_id = Pprz.string_assoc "ac_id" vs - and deg7 = fun f -> Pprz.Int32 (Int32.of_float (Pprz.float_assoc f vs *. 1e7)) in + and ac_id = PprzLink.string_assoc "ac_id" vs + and deg7 = fun f -> PprzLink.Int32 (Int32.of_float (PprzLink.float_assoc f vs *. 1e7)) in let vs = [ "wp_id", f "wp_id"; - "ac_id", Pprz.String ac_id; + "ac_id", PprzLink.String ac_id; "lat", deg7 "lat"; "lon", deg7 "long"; - "alt", mm_of_m_32 (Pprz.float_assoc "alt" vs) ] in + "alt", mm_of_m_32 (PprzLink.float_assoc "alt" vs) ] in Dl_Pprz.message_send dl_id "MOVE_WP" vs; log logging ac_id "MOVE_WP" vs (** Got a DL_SETTING, and send an SETTING *) let setting = fun logging _sender vs -> - let ac_id = Pprz.string_assoc "ac_id" vs in + let ac_id = PprzLink.string_assoc "ac_id" vs in let vs = [ "index", List.assoc "index" vs; - "ac_id", Pprz.String ac_id; + "ac_id", PprzLink.String ac_id; "value", List.assoc "value" vs] in Dl_Pprz.message_send dl_id "SETTING" vs; log logging ac_id "SETTING" vs; (* mark the setting as not yet confirmed *) let ac = Hashtbl.find aircrafts ac_id in - let idx = Pprz.int_of_value (List.assoc "index" vs) in + let idx = PprzLink.int_of_value (List.assoc "index" vs) in ac.dl_setting_values.(idx) <- None (** Got a GET_DL_SETTING, and send an GET_SETTING *) let get_setting = fun logging _sender vs -> - let ac_id = Pprz.string_assoc "ac_id" vs in + let ac_id = PprzLink.string_assoc "ac_id" vs in let vs = [ "index", List.assoc "index" vs; - "ac_id", Pprz.String ac_id ] in + "ac_id", PprzLink.String ac_id ] in Dl_Pprz.message_send dl_id "GET_SETTING" vs; log logging ac_id "GET_SETTING" vs; (* mark the setting as not yet confirmed *) let ac = Hashtbl.find aircrafts ac_id in - let idx = Pprz.int_of_value (List.assoc "index" vs) in + let idx = PprzLink.int_of_value (List.assoc "index" vs) in ac.dl_setting_values.(idx) <- None (** Got a JUMP_TO_BLOCK, and send an BLOCK *) let jump_block = fun logging _sender vs -> - let ac_id = Pprz.string_assoc "ac_id" vs in - let vs = ["block_id", List.assoc "block_id" vs; "ac_id", Pprz.String ac_id] in + let ac_id = PprzLink.string_assoc "ac_id" vs in + let vs = ["block_id", List.assoc "block_id" vs; "ac_id", PprzLink.String ac_id] in Dl_Pprz.message_send dl_id "BLOCK" vs; log logging ac_id "BLOCK" vs (** Got a RAW_DATALINK, send its contents *) let raw_datalink = fun logging _sender vs -> - let ac_id = Pprz.string_assoc "ac_id" vs - and m = Pprz.string_assoc "message" vs in + let ac_id = PprzLink.string_assoc "ac_id" vs + and m = PprzLink.string_assoc "message" vs in for i = 0 to String.length m - 1 do if m.[i] = ';' then m.[i] <- ' ' done; let msg_id, vs = Dl_Pprz.values_of_string m in let msg = Dl_Pprz.message_of_id msg_id in - Dl_Pprz.message_send dl_id msg.Pprz.name vs; - log logging ac_id msg.Pprz.name vs + Dl_Pprz.message_send dl_id msg.PprzLink.name vs; + log logging ac_id msg.PprzLink.name vs (** Got a LINK_REPORT, update state but don't send (done asynchronously) *) let link_report = fun logging _sender vs -> - let ac_id = Pprz.string_assoc "ac_id" vs - and link_id = int_of_string (Pprz.string_assoc "link_id" vs) in + let ac_id = PprzLink.string_assoc "ac_id" vs + and link_id = int_of_string (PprzLink.string_assoc "link_id" vs) in try let ac = Hashtbl.find aircrafts ac_id in let link_status = { - Aircraft.rx_lost_time = Pprz.int_assoc "rx_lost_time" vs; - rx_bytes = Pprz.int_assoc "rx_bytes" vs; - rx_msgs = Pprz.int_assoc "rx_msgs" vs; - rx_bytes_rate = Pprz.float_assoc "rx_bytes_rate" vs; - tx_msgs = Pprz.int_assoc "tx_msgs" vs; - ping_time = Pprz.float_assoc "ping_time" vs; + Aircraft.rx_lost_time = PprzLink.int_assoc "rx_lost_time" vs; + rx_bytes = PprzLink.int_assoc "rx_bytes" vs; + rx_msgs = PprzLink.int_assoc "rx_msgs" vs; + rx_bytes_rate = PprzLink.float_assoc "rx_bytes_rate" vs; + tx_msgs = PprzLink.int_assoc "tx_msgs" vs; + ping_time = PprzLink.float_assoc "ping_time" vs; } in Hashtbl.replace ac.link_status link_id link_status; log logging ac_id "LINK_REPORT" vs diff --git a/sw/ground_segment/tmtc/server_globals.ml b/sw/ground_segment/tmtc/server_globals.ml index b58823546f..959859ced6 100644 --- a/sw/ground_segment/tmtc/server_globals.ml +++ b/sw/ground_segment/tmtc/server_globals.ml @@ -18,4 +18,4 @@ let horiz_modes = [|"WAYPOINT";"ROUTE";"CIRCLE";"ATTITUDE"|] let if_modes = [|"OFF";"DOWN";"UP"|] let string_of_values = fun values -> - String.concat " " (List.map (fun (_, v) -> Pprz.string_of_value v) values) + String.concat " " (List.map (fun (_, v) -> PprzLink.string_of_value v) values) diff --git a/sw/ground_segment/tmtc/settings.ml b/sw/ground_segment/tmtc/settings.ml index 4f98ca5e87..bdcbca8d6e 100644 --- a/sw/ground_segment/tmtc/settings.ml +++ b/sw/ground_segment/tmtc/settings.ml @@ -23,8 +23,8 @@ *) open Printf -module Ground_Pprz = Pprz.Messages(struct let name = "ground" end) -module Tele_Pprz = Pprz.Messages(struct let name = "telemetry" end) +module Ground_Pprz = PprzLink.Messages(struct let name = "ground" end) +module Tele_Pprz = PprzLink.Messages(struct let name = "telemetry" end) let (//) = Filename.concat let conf_dir = Env.paparazzi_home // "conf" @@ -45,9 +45,9 @@ let one_ac = fun (notebook:GPack.notebook) ac_name -> (* Call to ivy to set a value *) let callback = fun idx value -> - let vs = ["ac_id", Pprz.String ac_id; "index", Pprz.Int idx] in + let vs = ["ac_id", PprzLink.String ac_id; "index", PprzLink.Int idx] in if classify_float value = FP_normal || classify_float value =FP_zero then - let vs' = ("value", Pprz.Float value) :: vs in + let vs' = ("value", PprzLink.Float value) :: vs in Ground_Pprz.message_send "dl" "DL_SETTING" vs' else Ground_Pprz.message_send "dl" "GET_DL_SETTING" vs in @@ -59,7 +59,7 @@ let one_ac = fun (notebook:GPack.notebook) ac_name -> (* Bind to values updates *) let get_dl_value = fun _sender vs -> - settings#set (Pprz.int_assoc "index" vs) (Some (string_of_float (Pprz.float_assoc "value" vs))) + settings#set (PprzLink.int_assoc "index" vs) (Some (string_of_float (PprzLink.float_assoc "value" vs))) in ignore (Tele_Pprz.message_bind "DL_VALUE" get_dl_value); diff --git a/sw/ground_segment/tmtc/stereo_demod.ml b/sw/ground_segment/tmtc/stereo_demod.ml deleted file mode 100644 index 4da42da774..0000000000 --- a/sw/ground_segment/tmtc/stereo_demod.ml +++ /dev/null @@ -1,137 +0,0 @@ -(* - * Hardware modem receiver - * - * Copyright (C) 2004 CENA/ENAC, Pascal Brisset, 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. - * - *) - -open Printf - - -let modem_msg_period = 1000 (** ms *) - -module Tele_Class = struct let name = "telemetry_ap" end -module Tele_Pprz = Pprz.Protocol(Tele_Class) -module PprzTransport = Serial.Transport(Tele_Pprz) - -(** Monitoring of the message reception *) -type status = { - mutable ac_id : string; - mutable rx_byte : int; - mutable rx_msg : int; - mutable rx_err : int -} -(** Ivy messages are initially tagged "modem" and with the A/C - id as soon as it is identified (IDENT message) *) -let make_status = fun id -> - { ac_id = id; rx_byte = 0; rx_msg = 0; rx_err = 0 } - -let status_left = make_status "modem_left" -let status_right = make_status "modem_right" - - -(** Callback for each decoded message *) -let use_pprz_message = fun status (msg_id, values) -> - status.rx_msg <- status.rx_msg + 1; (** Monitoring update *) - let msg = Tele_Pprz.message_of_id msg_id in - if msg.Pprz.name = "IDENT" then - status.ac_id <- Pprz.string_assoc "id" values; - prerr_endline (status.ac_id^":"^msg.Pprz.name); - Tele_Pprz.message_send status.ac_id msg.Pprz.name values - -(** Listen on a dsp device *) -let listen_pprz_modem = fun pprz_message_cb devdsp -> - let fd = Demod.init devdsp in - - (** Callback for a checksumed pprz message *) - let use_pprz_buf = fun status buf -> - status.rx_byte <- status.rx_byte + String.length buf; - Debug.call 'P' (fun f -> fprintf f "use_pprz: %s\n" (Debug.xprint buf)); - pprz_message_cb status (Tele_Pprz.values_of_bin buf) in - - (** Callback for available chars *) - let cb = fun status buffer data -> - (** Accumulate in a buffer *) - let b = !buffer ^ data in - Debug.call 'M' (fun f -> fprintf f "Pprz buffer: %s\n" (Debug.xprint b)); - (** Parse as pprz message and ... *) - let x = PprzTransport.parse (use_pprz_buf status) b in - status.rx_err <- !PprzTransport.nb_err; - (** ... remove from the buffer the chars which have been used *) - buffer := String.sub b x (String.length b - x) - in - let buffer_left = ref "" and buffer_right = ref "" in - let cb_stereo = fun _ -> - let (data_left, data_right) = Demod.get_data () in - cb status_left buffer_left data_left; - cb status_right buffer_right data_right; - true in - - (** Attach the callback to the channel *) - ignore (Glib.Io.add_watch [`IN] cb_stereo (Glib.Io.channel_of_descr fd)) - -(** Modem monitoring messages *) -let send_modem_msg = fun status -> - let rx_msg = ref 0 - and rx_byte = ref 0 - and start = Unix.gettimeofday () in - fun () -> - let dt = float modem_msg_period /. 1000. in - let t = int_of_float (Unix.gettimeofday () -. start) in - let byte_rate = float (status.rx_byte - !rx_byte) /. dt - and msg_rate = float (status.rx_msg - !rx_msg) /. dt in - rx_msg := status.rx_msg; - rx_byte := status.rx_byte; - let vs = ["run_time", Pprz.Int t; - "rx_bytes_rate", Pprz.Float byte_rate; - "rx_msgs_rate", Pprz.Float msg_rate; - "rx_err", Pprz.Int status.rx_err; - "rx_bytes", Pprz.Int status.rx_byte; - "rx_msgs", Pprz.Int status.rx_msg - ] in - Tele_Pprz.message_send status.ac_id "DOWNLINK_STATUS" vs - -(* main loop *) -let _ = - let ivy_bus = Defivybus.default_ivy_bus in - let port = ref "/dev/dsp" in - let options = - [ "-b", Arg.Set_string ivy_bus, (sprintf "Ivy bus (%s)" !ivy_bus); - "-d", Arg.Set_string port, (sprintf "Port (%s)" !port)] in - Arg.parse - options - (fun x -> fprintf stderr "Warning:ignoring %s\n" x) - "Usage: "; - - Ivy.init "Paparazzi stereo demod" "READY" (fun _ _ -> ()); - Ivy.start !ivy_bus; - - (** Listening on the given port (serial device or multimon fifo)*) - listen_pprz_modem use_pprz_message !port; - - (** Sending periodically modem and downlink status messages *) - let send_left = send_modem_msg status_left - and send_right = send_modem_msg status_right in - ignore (Glib.Timeout.add modem_msg_period (fun () -> send_left (); send_right (); true)); - - let loop = Glib.Main.create true in - while Glib.Main.is_running loop do - ignore (Glib.Main.iteration true) - done diff --git a/sw/lib/ocaml/META.pprz.template b/sw/lib/ocaml/META.pprz.template index 909bc5a994..b2907e3ec5 100644 --- a/sw/lib/ocaml/META.pprz.template +++ b/sw/lib/ocaml/META.pprz.template @@ -1,5 +1,5 @@ description = "Paparazzi UAS package" -requires = "unix,str,xml-light,lablgtk2,glibivy,netclient" +requires = "unix,str,pprzlink,xml-light,lablgtk2,glibivy,netclient" version = "1.0" directory = "" diff --git a/sw/lib/ocaml/Makefile b/sw/lib/ocaml/Makefile index 4cd48400db..b022fa8d56 100644 --- a/sw/lib/ocaml/Makefile +++ b/sw/lib/ocaml/Makefile @@ -24,13 +24,14 @@ Q=@ -OCAMLC=ocamlfind ocamlc -OCAMLOPT=ocamlfind ocamlopt -OCAMLDEP=ocamlfind ocamldep -OCAMLLEX=ocamllex -OCAMLYACC=ocamlyacc -OCAMLMKLIB=ocamlmklib -OCAMLLIBDIR=$(shell $(OCAMLC) -where) +#OCAMLC=ocamlfind ocamlc +#OCAMLOPT=ocamlfind ocamlopt +#OCAMLDEP=ocamlfind ocamldep +#OCAMLLEX=ocamllex +#OCAMLYACC=ocamlyacc +#OCAMLMKLIB=ocamlmklib +#OCAMLLIBDIR=$(shell $(OCAMLC) -where) +include ../../Makefile.ocaml # verbose ocamlmklib: Print commands before executing them #VERBOSITY = -verbose @@ -62,11 +63,11 @@ PP_CMO = $(PP_SRC:.ml=.cmo) PP_CMX = $(PP_SRC:.ml=.cmx) INCLUDES= -PKGCOMMON=xml-light,netclient,glibivy,lablgtk2 +PKGCOMMON=pprzlink,xml-light,netclient,glibivy,lablgtk2 XINCLUDES= -XPKGCOMMON=xml-light,glibivy,$(LABLGTK2GNOMECANVAS),lablgtk2.glade +XPKGCOMMON=pprzlink,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 fp_proc.ml gen_common.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 ubx.ml xmlCom.ml os_calls.ml editAirframe.ml defivybus.ml fp_proc.ml gen_common.ml CMO = $(SRC:.ml=.cmo) CMX = $(SRC:.ml=.cmx) diff --git a/sw/lib/ocaml/convert.c b/sw/lib/ocaml/convert.c deleted file mode 100644 index 3a16ec5c4c..0000000000 --- a/sw/lib/ocaml/convert.c +++ /dev/null @@ -1,193 +0,0 @@ -/* - $Id$ - - Copyright (C) 2004 Pascal Brisset, Antoine Drouin - - Ocaml low level conversions - - 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 -#include -#include -#include -#include -#include -#include -#include - -#ifdef ARCH_ALIGN_DOUBLE -value -c_float_of_indexed_bytes(value s, value index) -{ - CAMLparam2 (s, index); - char *x = (char *)(String_val(s) + Int_val(index)); - - //Assert(sizeof(float) == 4); - union { char b[4]; float f; } buffer; - buffer.b[0] = x[0]; - buffer.b[1] = x[1]; - buffer.b[2] = x[2]; - buffer.b[3] = x[3]; - - CAMLreturn (copy_double((double)buffer.f)); -} - -value -c_double_of_indexed_bytes(value s, value index) -{ - CAMLparam2 (s, index); - char *x = (char *)(String_val(s) + Int_val(index)); - - //Assert(sizeof(double) == 8); - union { char b[sizeof(double)]; double d; } buffer; - int i; - for (i=0; i < sizeof(double); i++) { - buffer.b[i] = x[i]; - } - CAMLreturn (copy_double(buffer.d)); -} - -#else /* no ARCH_ALIGN_DOUBLE */ -value -c_float_of_indexed_bytes(value s, value index) -{ - CAMLparam2 (s, index); - float *x = (float*)(String_val(s) + Int_val(index)); - CAMLreturn (copy_double((double)(*x))); -} - -value -c_double_of_indexed_bytes(value s, value index) -{ - CAMLparam2 (s, index); - double *x = (double*)(String_val(s) + Int_val(index)); - CAMLreturn (copy_double(*x)); -} -#endif /* ARCH_ALIGN_DOUBLE */ - -value -c_sprint_float(value s, value index, value f) { - CAMLparam3 (s, index, f); - float *p = (float*) (String_val(s) + Int_val(index)); - double ff = Double_val(f); - *p = (float)ff; - CAMLreturn (Val_unit); -} - -value -c_sprint_double(value s, value index, value f) { - CAMLparam3 (s, index, f); - double *p = (double*) (String_val(s) + Int_val(index)); - *p = Double_val(f); - CAMLreturn (Val_unit); -} - -value -c_sprint_int16(value s, value index, value f) { - CAMLparam3 (s, index, f); - int16_t *p = (int16_t*) (String_val(s) + Int_val(index)); - *p = (int16_t)Int_val(f); - CAMLreturn (Val_unit); -} - -value -c_sprint_int8(value s, value index, value f) { - CAMLparam3 (s, index, f); - int8_t *p = (int8_t*) (String_val(s) + Int_val(index)); - *p = (int8_t)Int_val(f); - CAMLreturn (Val_unit); -} - -value -c_sprint_int32(value s, value index, value x) { - CAMLparam3 (s, index, x); - int32_t *p = (int32_t*) (String_val(s) + Int_val(index)); - *p = (int32_t)Int32_val(x); - CAMLreturn (Val_unit); -} - -value -c_sprint_int64(value s, value index, value x) { - CAMLparam3 (s, index, x); - int64_t *p = (int64_t*) (String_val(s) + Int_val(index)); - *p = (int64_t)Int64_val(x); - CAMLreturn (Val_unit); -} - -value -c_int16_of_indexed_bytes(value s, value index) -{ - CAMLparam2 (s, index); - int16_t *x = (int16_t*)(String_val(s) + Int_val(index)); - CAMLreturn (Val_int(*x)); -} - -value -c_int8_of_indexed_bytes(value s, value index) -{ - CAMLparam2 (s, index); - int8_t *x = (int8_t*)(String_val(s) + Int_val(index)); - CAMLreturn (Val_int(*x)); -} - -value -c_int32_of_indexed_bytes(value s, value index) -{ - CAMLparam2 (s, index); - int32_t *x = (int32_t*)(String_val(s) + Int_val(index)); - CAMLreturn (copy_int32(*x)); -} - -value -c_uint32_of_indexed_bytes(value s, value index) -{ - CAMLparam2 (s, index); - uint32_t *x = (uint32_t*)(String_val(s) + Int_val(index)); - - /* since OCaml doesn't have unsigned integers, - * we represent it as 64bit signed int to make sure it doesn't overflow - */ - int64_t y = *x; - CAMLreturn (copy_int64(y)); -} - -#ifdef ARCH_ALIGN_INT64 -value -c_int64_of_indexed_bytes(value s, value index) -{ - CAMLparam2 (s, index); - char *x = (char *)(String_val(s) + Int_val(index)); - - union { char b[sizeof(int64_t)]; int64_t i; } buffer; - int i; - for (i=0; i < sizeof(int64_t); i++) { - buffer.b[i] = x[i]; - } - CAMLreturn (copy_int64(buffer.i)); -} -#else -value -c_int64_of_indexed_bytes(value s, value index) -{ - CAMLparam2 (s, index); - int64_t *x = (int64_t*)(String_val(s) + Int_val(index)); - CAMLreturn (copy_int64(*x)); -} -#endif diff --git a/sw/lib/ocaml/env.ml b/sw/lib/ocaml/env.ml index 35e5a9ab01..aa123f57c5 100644 --- a/sw/lib/ocaml/env.ml +++ b/sw/lib/ocaml/env.ml @@ -184,3 +184,17 @@ let get_paparazzi_version = fun () -> try Str.replace_first (Str.regexp "[ \n]+$") "" (read_process (paparazzi_src ^ "/paparazzi_version")) with _ -> "UNKNOWN" + + +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 + diff --git a/sw/lib/ocaml/env.mli b/sw/lib/ocaml/env.mli index 993eefb59e..58a2518eb8 100644 --- a/sw/lib/ocaml/env.mli +++ b/sw/lib/ocaml/env.mli @@ -81,3 +81,10 @@ val expand_ac_xml : ?raise_exception:bool -> Xml.xml -> Xml.xml val get_paparazzi_version : unit -> string (** read the current paparazzi_version *) + +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 + *) + diff --git a/sw/lib/ocaml/logpprz.ml b/sw/lib/ocaml/logpprz.ml deleted file mode 100644 index 338f4a2dbe..0000000000 --- a/sw/lib/ocaml/logpprz.ml +++ /dev/null @@ -1,100 +0,0 @@ -(* - * Reading logged timestamped messages - * - * Copyright (C) 2003 Pascal Brisset, 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. - * - *) - -open Printf - -type message = { - source : int; - timestamp : int32; - pprz_data : Serial.payload -} - - -module Transport = struct - let stx = Char.chr 0x99 (** sw/airborne/pprz_transport.h *) - let offset_length = 1 - let offset_source = 2 - let offset_timestamp = 3 - let offset_payload = 2 - - let index_start = fun buf -> - String.index buf stx - - let length = fun buf start -> - let len = String.length buf - start in - if len > offset_length then - let l = Char.code buf.[start+offset_length] in - Debug.call 'T' (fun f -> fprintf f "LogPprz len=%d\n" l); - l + 8 (* pprz data + stx, length, source, time, ck *) - else - raise Serial.Not_enough - - let (+=) = fun r x -> r := (!r + x) land 0xff - let compute_checksum = fun msg -> - let l = String.length msg in - let ck_a = ref 0 in - for i = 1 to l - 2 do - ck_a += Char.code msg.[i] - done; - !ck_a - - let checksum = fun msg -> - let l = String.length msg in - let ck_a = compute_checksum msg in - Debug.call 'T' (fun f -> fprintf f "LogPprz cs: %d %d\n" ck_a (Char.code msg.[l-1])); - ck_a = Char.code msg.[l-1] - - let payload = fun msg -> - let l = String.length msg in - assert(Char.code msg.[offset_length] + 8 = l); - Serial.payload_of_string (String.sub msg offset_payload (l-3)) - - let packet = fun payload -> - let payload = Serial.string_of_payload payload in - let n = String.length payload in - let msg_length = n + 3 in (** + stx, len, ck_a *) - if msg_length >= 256 then - invalid_arg "Pprz.Transport.packet"; - let m = String.create msg_length in - String.blit payload 0 m offset_payload n; - m.[0] <- stx; - m.[offset_length] <- Char.chr (msg_length - 8); - let ck_a = compute_checksum m in - m.[msg_length-1] <- Char.chr ck_a; - m -end - -let pprz_payload_of_payload = fun s -> - let n = String.length s in - Serial.payload_of_string (String.sub s 5 (n - 5)) - -let parse = fun p -> - let s = Serial.string_of_payload p in - let source = Char.code s.[0] - and timestamp = Pprz.int32_of_bytes s 1 in - { - source = source; - timestamp = timestamp; - pprz_data = pprz_payload_of_payload s - } diff --git a/sw/lib/ocaml/logpprz.mli b/sw/lib/ocaml/logpprz.mli deleted file mode 100644 index 1b710da019..0000000000 --- a/sw/lib/ocaml/logpprz.mli +++ /dev/null @@ -1,12 +0,0 @@ -type message = { - source : int; - timestamp : int32; - pprz_data : Serial.payload - } - -module Transport : Serial.PROTOCOL -(** Pprz frame stored by the logger *) - -val parse : Serial.payload -> message - - diff --git a/sw/lib/ocaml/papget.ml b/sw/lib/ocaml/papget.ml index 0a038a4bb0..440e9287f8 100644 --- a/sw/lib/ocaml/papget.ml +++ b/sw/lib/ocaml/papget.ml @@ -69,15 +69,15 @@ object method type_ = "message_field" initializer - let module P = Pprz.Messages (struct let name = class_name end) in + let module P = PprzLink.Messages (struct let name = class_name end) in let process_message = fun _sender values -> let (field_name, index) = base_and_index field_descr in let value = - match Pprz.assoc field_name values with - Pprz.Array array -> array.(index) + match PprzLink.assoc field_name values with + PprzLink.Array array -> array.(index) | scalar -> scalar in - last_value <- Pprz.string_of_value value; + last_value <- PprzLink.string_of_value value; List.iter (fun cb -> cb last_value) callbacks in ignore (P.message_bind ?sender msg_name process_message) diff --git a/sw/lib/ocaml/pprz.ml b/sw/lib/ocaml/pprz.ml deleted file mode 100644 index 066b91d631..0000000000 --- a/sw/lib/ocaml/pprz.ml +++ /dev/null @@ -1,808 +0,0 @@ -(* - * Downlink protocol (handling messages.xml) - * - * Copyright (C) 2003 Pascal Brisset, 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. - * - *) - -open Printf -open Latlong - -type message_id = int -type ac_id = int -type class_name = string -type format = string -type _type = - Scalar of string - | ArrayType of string - | FixedArrayType of string * int -type value = - Int of int | Float of float | String of string | Int32 of int32 | Char of char | Int64 of int64 - | Array of value array -type field = { - _type : _type; - fformat : format; - alt_unit_coef : string; - enum : string list -} - -type link_mode = Forwarded | Broadcasted -type message = { - name : string; (** Lowercase *) - fields : (string * field) list; - link : link_mode option -} - -type type_descr = { - format : string ; - glib_type : string; - inttype : string; - size : int; - value : value -} - -type values = (string * value) list - -type payload = string - - -let separator = "," -let regexp_separator = Str.regexp "," -let split_array = fun s -> Str.split regexp_separator s - - -let (//) = Filename.concat -let messages_file = Env.paparazzi_src // "conf" // "messages.xml" -let lazy_messages_xml = lazy (Xml.parse_file messages_file) -let messages_xml = fun () -> Lazy.force lazy_messages_xml -let units_file = Env.paparazzi_src // "conf" // "units.xml" - -external float_of_bytes : string -> int -> float = "c_float_of_indexed_bytes" -external double_of_bytes : string -> int -> float = "c_double_of_indexed_bytes" -external int8_of_bytes : string -> int -> int = "c_int8_of_indexed_bytes" -external int16_of_bytes : string -> int -> int = "c_int16_of_indexed_bytes" -external int32_of_bytes : string -> int -> int32 = "c_int32_of_indexed_bytes" -external uint32_of_bytes : string -> int -> int64 = "c_uint32_of_indexed_bytes" -external int64_of_bytes : string -> int -> int64 = "c_int64_of_indexed_bytes" -external sprint_float : string -> int -> float -> unit = "c_sprint_float" -external sprint_double : string -> int -> float -> unit = "c_sprint_double" -external sprint_int64 : string -> int -> int64 -> unit = "c_sprint_int64" -external sprint_int32 : string -> int -> int32 -> unit = "c_sprint_int32" -external sprint_int16 : string -> int -> int -> unit = "c_sprint_int16" -external sprint_int8 : string -> int -> int -> unit = "c_sprint_int8" - -let types = [ - ("uint8", { format = "%u"; glib_type = "guint8"; inttype = "uint8_t"; size = 1; value=Int 42 }); - ("uint16", { format = "%u"; glib_type = "guint16"; inttype = "uint16_t"; size = 2; value=Int 42 }); - ("uint32", { format = "%Lu" ; glib_type = "guint32"; inttype = "uint32_t"; size = 4; value=Int 42 }); (* uint32 should be lu, but doesn't fit into Int32 so Int64 (Lu) is used *) - ("uint64", { format = "%Lu" ; glib_type = "guint64"; inttype = "uint64_t"; size = 8; value=Int 42 }); - ("int8", { format = "%d"; glib_type = "gint8"; inttype = "int8_t"; size = 1; value= Int 42 }); - ("int16", { format = "%d"; glib_type = "gint16"; inttype = "int16_t"; size = 2; value= Int 42 }); - ("int32", { format = "%ld" ; glib_type = "gint32"; inttype = "int32_t"; size = 4; value=Int 42 }); - ("int64", { format = "%Ld" ; glib_type = "gint64"; inttype = "int64_t"; size = 8; value=Int 42 }); - ("float", { format = "%f" ; glib_type = "gfloat"; inttype = "float"; size = 4; value=Float 4.2 }); - ("double", { format = "%f" ; glib_type = "gdouble"; inttype = "double"; size = 8; value=Float 4.2 }); - ("char", { format = "%c" ; glib_type = "gchar"; inttype = "char"; size = 1; value=Char '*' }); - ("string", { format = "%s" ; glib_type = "gchar*"; inttype = "char*"; size = max_int; value=String "42" }) -] - -let is_array_type = fun s -> - let n = String.length s in - n >= 2 && String.sub s (n-2) 2 = "[]" - -let type_of_array_type = fun s -> - let n = String.length s in - String.sub s 0 (n-2) - -let is_fixed_array_type = fun s -> - let type_parts = Str.full_split (Str.regexp "[][]") s in - match type_parts with - | [Str.Text _; Str.Delim "["; Str.Text _ ; Str.Delim "]"] -> true - | _ -> false - -let type_of_fixed_array_type = fun s -> - try - let type_parts = Str.full_split (Str.regexp "[][]") s in - match type_parts with - | [Str.Text ty; Str.Delim "["; Str.Text len ; Str.Delim "]"] -> begin ignore( int_of_string (len)); ty end - | _ -> failwith("Pprz.type_of_fixed_array_type is not a fixed array type") - with - | Failure str -> failwith(sprintf "Pprz.type_of_fixed_array_type: length is not an integer") - -let length_of_fixed_array_type = fun s -> - try - let type_parts = Str.full_split (Str.regexp "[][]") s in - match type_parts with - | [Str.Text ty; Str.Delim "["; Str.Text len ; Str.Delim "]"] -> begin ignore( int_of_string (len)); len end - | _ -> failwith("Pprz.type_of_fixed_array_type is not a fixed array type") - with - | Failure str -> failwith(sprintf "Pprz.type_of_fixed_array_type: length is not an integer") - -let int_of_string = fun x -> - try int_of_string x with - _ -> try int_of_string ("0x"^x) with (* try hex format in case *) - _ -> failwith (sprintf "Pprz.int_of_string: %s" x) - -let rec value = fun t v -> - match t with - Scalar ("uint8" | "uint16" | "int8" | "int16") -> Int (int_of_string v) - | Scalar "int32" -> Int32 (Int32.of_string v) - | Scalar "uint32" -> Int64 (Int64.of_string v) - | Scalar ("uint64" | "int64") -> Int64 (Int64.of_string v) - | Scalar ("float" | "double") -> Float (float_of_string v) - | ArrayType "char" | FixedArrayType ("char", _) | Scalar "string" -> String v - | Scalar "char" -> Char v.[0] - | ArrayType t' -> - Array (Array.map (value (Scalar t')) (Array.of_list (split_array v))) - | FixedArrayType (t',l') -> - Array (Array.map (value (Scalar t')) (Array.of_list (split_array v))) - | Scalar t -> failwith (sprintf "Pprz.value: Unexpected type: %s" t) - - -let rec string_of_value = function - Int x -> string_of_int x - | Float x -> string_of_float x - | Int32 x -> Int32.to_string x - | Int64 x -> Int64.to_string x - | Char c -> String.make 1 c - | String s -> s - | Array a -> - let l = (Array.to_list (Array.map string_of_value a)) in - match a.(0) with - | Char _ -> "\""^(String.concat "" l)^"\"" - | _ -> String.concat separator l - - -let rec formatted_string_of_value = fun format v -> - let f = fun x -> Scanf.format_from_string format x in - match v with - | Int x -> sprintf (f "%d") x - | Float x -> sprintf (f "%f") x - | Int32 x -> sprintf (f "%ld") x - | Int64 x -> sprintf (f "%Ld") x - | Char x -> sprintf (f "%c") x - | String x ->sprintf "%s" x - | Array a -> - let l = (Array.to_list (Array.map (formatted_string_of_value format) a)) in - match a.(0) with - | Char _ -> "\""^(String.concat "" l)^"\"" - | _ -> String.concat separator l - - -let sizeof = fun f -> - match f with - Scalar t -> (List.assoc t types).size - | ArrayType t -> failwith "sizeof: Array" - | FixedArrayType (t,l) -> failwith "sizeof: Array" -let size_of_field = fun f -> sizeof f._type -let default_format = function Scalar x | ArrayType x | FixedArrayType (x,_) -> - try (List.assoc x types).format with - Not_found -> failwith (sprintf "Unknown format '%s'" x) -let default_value = fun x -> - match x with - Scalar t -> (List.assoc t types).value - | ArrayType t -> failwith "default_value: Array" - | FixedArrayType (t,l) -> failwith "default_value: Array" - -let payload_size_of_message = fun message -> - List.fold_right - (fun (_, f) s -> size_of_field f + s) - message.fields - 2 (** + message id + aircraft id *) - -exception Unit_conversion_error of string -exception Unknown_conversion of string * string -exception No_automatic_conversion of string * string - -let scale_of_units = fun ?auto from_unit to_unit -> - if (from_unit = to_unit) then - 1.0 - else - try - let units_xml = Xml.parse_file units_file in - (* find the first occurence of matching units or raise Not_found *) - let _unit = List.find (fun u -> - (* will raise Xml.No_attribute if not a valid attribute *) - let f = Xml.attrib u "from" - and t = Xml.attrib u "to" - and a = try Some (Xml.attrib u "auto") with _ -> None in - let a = match auto, a with - | Some _, None | None, None -> "" (* No auto conversion *) - | Some t, Some _ | None, Some t -> String.lowercase t (* param auto is used before attribute *) - in - if (f = from_unit || a = "display") && (t = to_unit || a = "code") then true else false - ) (Xml.children units_xml) in - (* return coef, raise Failure if coef is not a numerical value *) - float_of_string (Xml.attrib _unit "coef") - with Xml.File_not_found _ -> raise (Unit_conversion_error ("Parse error of conf/units.xml")) - | Xml.No_attribute _ | Xml.Not_element _ -> raise (Unit_conversion_error ("File conf/units.xml has errors")) - | Failure "float_of_string" -> raise (Unit_conversion_error ("Unit coef is not numerical value")) - | Not_found -> - if from_unit = "" || to_unit = "" then raise (No_automatic_conversion (from_unit, to_unit)) - else raise (Unknown_conversion (from_unit, to_unit)) - | _ -> raise (Unknown_conversion (from_unit, to_unit)) - - -let alt_unit_coef_of_xml = fun ?auto xml -> - try Xml.attrib xml "alt_unit_coef" - with _ -> - let u = try Xml.attrib xml "unit" with _ -> "" in - let au = try Xml.attrib xml "alt_unit" with _ -> "" in - let coef = try string_of_float (match auto with - | None -> scale_of_units u au - | Some a -> scale_of_units u au ~auto:a) - with - | Unit_conversion_error s -> prerr_endline (sprintf "Unit conversion error: %s" s); flush stderr; "1." (* Use coef 1. *) - | Unknown_conversion _ -> "1." (* Use coef 1. *) - | _ -> "1." - 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 -> - let t = ExtXml.attrib xml "type" in - let t = if is_array_type t then ArrayType (type_of_array_type t) - else if is_fixed_array_type t then FixedArrayType (type_of_fixed_array_type t, int_of_string(length_of_fixed_array_type t)) - else Scalar t in - let f = try Xml.attrib xml "format" with _ -> default_format t in - let auc = alt_unit_coef_of_xml xml in - let values = try Str.split pipe_regexp (Xml.attrib xml "values") with _ -> [] in - - ( String.lowercase (ExtXml.attrib xml "name"), - { _type = t; fformat = f; alt_unit_coef = auc; enum=values }) - -let string_of_values = fun vs -> - String.concat " " (List.map (fun (a,v) -> sprintf "%s=%s" a (string_of_value v)) vs) - -let assoc = fun a vs -> - try List.assoc (String.lowercase a) vs with Not_found -> - failwith (sprintf "Attribute '%s' not found in '%s'" a (string_of_values vs)) - -let float_assoc = fun (a:string) vs -> - match assoc a vs with - Float x -> x - | _ -> invalid_arg "Pprz.float_assoc" - -let int_of_value = fun value -> - match value with - Int x -> x - | Int32 x -> - let i = Int32.to_int x in - if Int32.compare x (Int32.of_int i) <> 0 then - failwith "Pprz.int_assoc: Int32 too large to be converted into an int"; - i - | Int64 x -> - let i = Int64.to_int x in - if Int64.compare x (Int64.of_int i) <> 0 then - failwith "Pprz.int_assoc: Int64 too large to be converted into an int"; - i - | _ -> invalid_arg "Pprz.int_assoc" - -let int_assoc = fun (a:string) vs -> - int_of_value (assoc a vs) - -let int32_assoc = fun (a:string) vs -> - match assoc a vs with - Int32 x -> x - | _ -> invalid_arg "Pprz.int32_assoc" - -let uint32_assoc = fun (a:string) vs -> - match assoc a vs with - Int64 x -> x - | _ -> invalid_arg "Pprz.uint32_assoc" - -let int64_assoc = fun (a:string) vs -> - match assoc a vs with - Int64 x -> x - | _ -> invalid_arg "Pprz.int64_assoc" - -let string_assoc = fun (a:string) (vs:values) -> string_of_value (assoc a vs) - -let char_assoc = fun (a:string) (vs:values) -> (string_of_value (assoc a vs)).[0] - -let link_mode_of_string = function -"forwarded" -> Forwarded - | "broadcasted" -> Broadcasted - | x -> invalid_arg (sprintf "link_mode_of_string: %s" x) - -let parse_class = fun xml_class -> - let by_id = Hashtbl.create 13 - and by_name = Hashtbl.create 13 in - List.iter - (fun xml_msg -> - let name = ExtXml.attrib xml_msg "name" - and link = - try - Some (link_mode_of_string (Xml.attrib xml_msg "link")) - with - Xml.No_attribute("link") -> None - in - (* only keep a "field" nodes *) - let xml_children = List.filter (fun f -> Xml.tag f = "field") (Xml.children xml_msg) in - let msg = { - name = name; - fields = List.map field_of_xml xml_children; - link = link - } in - let id = int_of_string (ExtXml.attrib xml_msg "id") in - if Hashtbl.mem by_id id then - failwith (sprintf "Duplicated id in messages.xml: %d" id); - Hashtbl.add by_id id msg; - Hashtbl.add by_name name (id, msg)) - (Xml.children xml_class); - (by_id, by_name) - - -(** Returns a value and its length *) -let rec value_of_bin = fun buffer index _type -> - match _type with - Scalar "uint8" -> Int (Char.code buffer.[index]), sizeof _type - | Scalar "char" -> Char (buffer.[index]), sizeof _type - | Scalar "int8" -> Int (int8_of_bytes buffer index), sizeof _type - | Scalar "uint16" -> Int (Char.code buffer.[index+1] lsl 8 + Char.code buffer.[index]), sizeof _type - | Scalar "int16" -> Int (int16_of_bytes buffer index), sizeof _type - | Scalar "float" -> Float (float_of_bytes buffer index), sizeof _type - | Scalar "double" -> Float (double_of_bytes buffer index), sizeof _type - | Scalar "int32" -> Int32 (int32_of_bytes buffer index), sizeof _type - | Scalar "uint32" -> Int64 (uint32_of_bytes buffer index), sizeof _type - | Scalar ("int64" | "uint64") -> Int64 (int64_of_bytes buffer index), sizeof _type - | ArrayType t -> - (** First get the number of values *) - let n = int8_of_bytes buffer index in - let type_of_elt = Scalar t in - let s = sizeof type_of_elt in - let size = 1 + n * s in - (Array (Array.init n - (fun i -> fst (value_of_bin buffer (index+1+i*s) type_of_elt))), size) - | FixedArrayType (t,l) -> - (** First get the number of values *) - let n = l in - let type_of_elt = Scalar t in - let s = sizeof type_of_elt in - let size = 0 + n * s in - (Array (Array.init n - (fun i -> fst (value_of_bin buffer (index+0+i*s) type_of_elt))), size) - | Scalar "string" -> - let n = Char.code buffer.[index] in - (String (String.sub buffer (index+1) n), (1+n)) - | _ -> failwith "value_of_bin" - -let value_field = fun buf index field -> - value_of_bin buf index field._type - -let byte = fun x -> Char.chr (x land 0xff) - -(** Returns the size of outputed data *) -let rec sprint_value = fun buf i _type v -> - match _type, v with - Scalar "uint8", Int x -> - if x < 0 || x > 0xff then - failwith (sprintf "Value too large to fit in a uint8: %d" x); - buf.[i] <- Char.chr x; sizeof _type - | Scalar "int8", Int x -> - if x < -0x7f || x > 0x7f then - failwith (sprintf "Value too large to fit in a int8: %d" x); - sprint_int8 buf i x; sizeof _type - | Scalar "float", Float f -> sprint_float buf i f; sizeof _type - | Scalar "double", Float f -> sprint_double buf i f; sizeof _type - | Scalar "int32", Int32 x -> sprint_int32 buf i x; sizeof _type - | Scalar ("int64"|"uint64"|"uint32"), Int64 x -> sprint_int64 buf i x; sizeof _type - | Scalar "int16", Int x -> sprint_int16 buf i x; sizeof _type - | Scalar ("int32" | "uint32"), Int value -> - assert (_type <> Scalar "uint32" || value >= 0); - buf.[i+3] <- byte (value asr 24); - buf.[i+2] <- byte (value lsr 16); - buf.[i+1] <- byte (value lsr 8); - buf.[i+0] <- byte value; - sizeof _type - | Scalar ("int64" | "uint64"), Int value -> - assert (_type <> Scalar "uint64" || value >= 0); - buf.[i+7] <- byte (value asr 56); - buf.[i+6] <- byte (value lsr 48); - buf.[i+5] <- byte (value lsr 40); - buf.[i+4] <- byte (value lsr 32); - buf.[i+3] <- byte (value lsr 24); - buf.[i+2] <- byte (value lsr 16); - buf.[i+1] <- byte (value lsr 8); - buf.[i+0] <- byte value; - sizeof _type - | Scalar "uint16", Int value -> - assert (value >= 0); - buf.[i+1] <- byte (value lsr 8); - buf.[i+0] <- byte value; - sizeof _type - | ArrayType t, Array values -> - (** Put the size first, then the values *) - let n = Array.length values in - ignore (sprint_value buf i (Scalar "uint8") (Int n)); - let type_of_elt = Scalar t in - let s = sizeof type_of_elt in - for j = 0 to n - 1 do - ignore (sprint_value buf (i+1+j*s) type_of_elt values.(j)) - done; - 1 + n * s - | FixedArrayType (t,l), Array values -> - (** Put the size first, then the values *) - let n = Array.length values in - let type_of_elt = Scalar t in - let s = sizeof type_of_elt in - for j = 0 to n - 1 do - ignore (sprint_value buf (i+0+j*s) type_of_elt values.(j)) - done; - 0 + n * s - | Scalar "string", String s -> - let n = String.length s in - assert (n < 256); - (** Put the length first, then the bytes *) - buf.[i] <- Char.chr n; - if (i + n >= String.length buf) then - failwith "Error in sprint_value: message too long"; - String.blit s 0 buf (i+1) n; - 1 + n - | Scalar "char", Char c -> - buf.[i] <- c; sizeof _type - | (Scalar x|ArrayType x), _ -> failwith (sprintf "Pprz.sprint_value (%s)" x) - | FixedArrayType (x,l), _ -> failwith (sprintf "Pprz.sprint_value (%s)" x) - - - -let hex_of_int_array = function -Array array -> - let n = Array.length array in - (* One integer -> 2 chars *) - let s = String.create (2*n) in - Array.iteri - (fun i dec -> - let x = int_of_value array.(i) in - assert (0 <= x && x <= 0xff); - let hex = sprintf "%02x" x in - String.blit hex 0 s (2*i) 2) - array; - s - | value -> - failwith (sprintf "Error: expecting array in Pprz.hex_of_int_array, found %s" (string_of_value value)) - - - -exception Unknown_msg_name of string * string - -module type TRANSPORT_TYPE = sig - val stx : char - val offset_length : int - val offset_payload : int - val offset_timestamp : int - val overhead_length : int -end - -module PprzTransportBase(SubType:TRANSPORT_TYPE) = struct - let index_start = fun buf -> - String.index buf SubType.stx - - let length = fun buf start -> - let len = String.length buf - start in - if len > SubType.offset_length then - let l = Char.code buf.[start+SubType.offset_length] in - Debug.call 'T' (fun f -> fprintf f "Pprz len=%d\n" l); - max l SubType.overhead_length (** if l<4 (4=stx+length+ck_a+ck_b), it's not a valid length *) - else - raise Serial.Not_enough - - let (+=) = fun r x -> r := (!r + x) land 0xff - let compute_checksum = fun msg -> - let l = String.length msg in - let ck_a = ref 0 and ck_b = ref 0 in - for i = 1 to l - 3 do - ck_a += Char.code msg.[i]; - ck_b += !ck_a - done; - !ck_a, !ck_b - - let checksum = fun msg -> - let l = String.length msg in - let ck_a, ck_b = compute_checksum msg in - Debug.call 'T' (fun f -> fprintf f "Pprz cs: %d %d | %d %d\n" ck_a (Char.code msg.[l-2]) ck_b (Char.code msg.[l-1])); - ck_a = Char.code msg.[l-2] && ck_b = Char.code msg.[l-1] - - let payload = fun msg -> - let l = String.length msg in - assert(Char.code msg.[SubType.offset_length] = l); - assert(l >= SubType.overhead_length); - Serial.payload_of_string (String.sub msg SubType.offset_payload (l-SubType.overhead_length)) - - let packet = fun payload -> - let payload = Serial.string_of_payload payload in - let n = String.length payload in - let msg_length = n + SubType.overhead_length in (** + stx, len, ck_a and ck_b *) - if msg_length >= 256 then - invalid_arg "Pprz.Transport.packet"; - let m = String.create msg_length in - String.blit payload 0 m SubType.offset_payload n; - m.[0] <- SubType.stx; - m.[SubType.offset_length] <- Char.chr msg_length; - let (ck_a, ck_b) = compute_checksum m in - m.[msg_length-2] <- Char.chr ck_a; - m.[msg_length-1] <- Char.chr ck_b; - m -end - -module PprzTypeStandard = struct - let stx = Char.chr 0x99 - let offset_length = 1 - let offset_timestamp = -1 - let offset_payload = 2 - let overhead_length = 4 -end - -module PprzTypeTimestamp = struct - let stx = Char.chr 0x98 - let offset_length = 1 - let offset_timestamp = 2 - let offset_payload = 6 - let overhead_length = 8 -end - -module Transport = PprzTransportBase (PprzTypeStandard) -module TransportExtended = PprzTransportBase (PprzTypeTimestamp) - -let offset_ac_id = 0 -let offset_msg_id = 1 -let offset_fields = 2 - -module type CLASS_Xml = sig - val xml : Xml.xml - val name : string -end - -module type CLASS = sig - val name : string -end - -module type MESSAGES = sig - val messages : (message_id, message) Hashtbl.t - val message_of_id : message_id -> message - val message_of_name : string -> message_id * message - val values_of_payload : Serial.payload -> message_id * ac_id * values - (** [values_of_bin payload] Parses a raw payload, returns the - message id, the A/C id and the list of (field_name, value) *) - - val payload_of_values : message_id -> ac_id -> values -> Serial.payload - (** [payload_of_values id ac_id vs] Returns a payload *) - - val values_of_string : string -> message_id * values - (** May raise [(Unknown_msg_name msg_name)] *) - - val string_of_message : ?sep:string -> message -> values -> string - (** [string_of_message ?sep msg values] Default [sep] is space *) - - val message_send : ?timestamp:float -> ?link_id:int -> string -> string -> values -> unit - (** [message_send sender link_id msg_name values] *) - - val message_bind : ?sender:string -> ?timestamp:bool -> string -> (string -> values -> unit) -> Ivy.binding - (** [message_bind ?sender msg_name callback] *) - - val message_answerer : string -> string -> (string -> values -> values) -> Ivy.binding - (** [message_answerer sender msg_name callback] *) - - val message_req : string -> string -> values -> (string -> values -> unit) -> unit -(** [message_answerer sender msg_name values receiver] Sends a request on the Ivy bus for the specified message. On reception, [receiver] will be applied on [sender_name] and expected values. *) -end - - - -module MessagesOfXml(Class:CLASS_Xml) = struct - let max_length = 256 - let messages_by_id, messages_by_name = - try - let select = fun x -> Xml.attrib x "name" = Class.name in - let xml_class = try ExtXml.child Class.xml ~select "msg_class" with Not_found -> ExtXml.child Class.xml ~select "class" in - parse_class xml_class - with - Not_found -> failwith (sprintf "Unknown message class: %s" Class.name) - let messages = messages_by_id - let message_of_id = fun id -> try Hashtbl.find messages_by_id id with Not_found -> fprintf stderr "message_of_id :%d\n%!" id; raise Not_found - let message_of_name = fun name -> - try - Hashtbl.find messages_by_name name - with - Not_found -> raise (Unknown_msg_name (name, Class.name)) - - - let values_of_payload = fun buffer -> - let buffer = Serial.string_of_payload buffer in - try - let id = Char.code buffer.[offset_msg_id] in - let ac_id = Char.code buffer.[offset_ac_id] in - let message = message_of_id id in - Debug.call 'T' (fun f -> fprintf f "Pprz.values id=%d\n" id); - let rec loop = fun index fields -> - match fields with - [] -> - if index = String.length buffer then - [] - else - failwith (sprintf "Pprz.values_of_payload, too many bytes in message %s: %s" message.name (Debug.xprint buffer)) - | (field_name, field_descr)::fs -> - let (value, n) = value_field buffer index field_descr in - (field_name, value) :: loop (index+n) fs in - (id, ac_id, loop offset_fields message.fields) - with - Invalid_argument("index out of bounds") -> - failwith (sprintf "Pprz.values_of_payload, wrong argument: %s" (Debug.xprint buffer)) - - - let payload_of_values = fun id ac_id values -> - let message = message_of_id id in - - (** The actual length is computed from the values *) - let p = String.make max_length '#' in - - p.[offset_msg_id] <- Char.chr id; - p.[offset_ac_id] <- Char.chr ac_id; - let i = ref offset_fields in - List.iter - (fun (field_name, field) -> - let v = - try List.assoc field_name values with - Not_found -> default_value field._type in - let size = sprint_value p !i field._type v in - i := !i + size - ) - message.fields; - - (** Cut to the actual length *) - let p = String.sub p 0 !i in - Serial.payload_of_string p - - - let space = Str.regexp "[ \t]+" - let array_sep = Str.regexp "[\"|]" (* also search for old separator '|' for backward compatibility *) - let values_of_string = fun s -> - (* 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 "\""]) | (Str.Delim "|")::((Str.Text l)::[Str.Delim "|"]) -> [l] - | (Str.Delim "\"")::((Str.Text l)::((Str.Delim "\"")::xs)) | (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 - let msg_id, msg = message_of_name msg_name in - let values = List.map2 (fun (field_name, field) v -> (field_name, value field._type v)) msg.fields args in - (msg_id, values) - with - Invalid_argument "List.map2" -> failwith (sprintf "Pprz.values_of_string: incorrect number of fields in '%s'" s) - end - | [] -> invalid_arg (sprintf "Pprz.values_of_string: %s" s) - - let string_of_message = fun ?(sep=" ") msg values -> - (** Check that the values are compatible with this message *) - List.iter - (fun (k, _) -> - if not (List.mem_assoc k msg.fields) - then invalid_arg (sprintf "Pprz.string_of_message: unknown field '%s' in message '%s'" k msg.name)) - values; - - String.concat sep - (msg.name:: - List.map - (fun (field_name, field) -> - let v = - try List.assoc field_name values with - Not_found -> - default_value field._type in - formatted_string_of_value field.fformat v) - msg.fields) - - let message_send = fun ?timestamp ?link_id sender msg_name values -> - let m = snd (message_of_name msg_name) in - let s = string_of_message m values in - let timestamp_string = - match timestamp with - None -> "" - | Some x -> sprintf "%f " x in - let msg = sprintf "%s%s %s" timestamp_string sender s in - let n = String.length msg in - if n > 10000 then (** prevent really long Ivy message, should not happen with normal usage *) - fprintf stderr "Discarding long ivy message %s (%d bytes)\n%!" msg_name n - else - match link_id with - None -> Ivy.send msg - | Some the_link_id -> begin - let index = ref 0 in - let modified_msg = String.copy msg in - let func = fun c -> - match c with - ' ' -> begin - String.set modified_msg !index ';'; - index := !index + 1 - end - | x -> index := !index + 1; in - String.iter func modified_msg; - Ivy.send ( Printf.sprintf "redlink TELEMETRY_MESSAGE %s %i %s" sender the_link_id modified_msg); - end - - let message_bind = fun ?sender ?(timestamp=false) msg_name cb -> - let tsregexp, tsoffset = if timestamp then "([0-9]+\\.[0-9]+ )?", 1 else "", 0 in - match sender with - None -> - Ivy.bind - (fun _ args -> - let values = try snd (values_of_string args.(1+tsoffset)) with exc -> prerr_endline (Printexc.to_string exc); [] in - cb args.(tsoffset) values) - (sprintf "^%s([^ ]*) +(%s( .*|$))" tsregexp msg_name) - | Some s -> - Ivy.bind - (fun _ args -> - let values = try snd (values_of_string args.(tsoffset)) with exc -> prerr_endline (Printexc.to_string exc); [] in - cb s values) - (sprintf "^%s%s +(%s( .*|$))" tsregexp s msg_name) - - let message_answerer = fun sender msg_name cb -> - let ivy_cb = fun _ args -> - let asker = args.(0) - and asker_id = args.(1) in - try (** Against [cb] exceptions *) - let values = cb asker (snd (values_of_string args.(2))) in - let m = string_of_message (snd (message_of_name msg_name)) values in - Ivy.send (sprintf "%s %s %s" asker_id sender m) - with - exc -> fprintf stderr "Pprz.answerer %s:%s: %s\n%!" sender msg_name (Printexc.to_string exc) - in - Ivy.bind ivy_cb (sprintf "^([^ ]*) +([^ ]*) +(%s_REQ.*)" msg_name) - - let gen_id = let r = ref 0 in fun () -> incr r; !r - let message_req = fun sender msg_name values (f:string -> (string * value) list -> unit) -> - let b = ref (Obj.magic ()) in - let cb = fun _ args -> - Ivy.unbind !b; - f args.(0) (snd (values_of_string args.(1))) in - let id = sprintf "%d_%d" (Unix.getpid ()) (gen_id ()) in - let r = sprintf "^%s ([^ ]*) +(%s.*)" id msg_name in - b := Ivy.bind cb r; - let msg_name_req = msg_name ^ "_REQ" in - let m = sprintf "%s %s %s" sender id (string_of_message (snd (message_of_name msg_name_req)) values) in - Ivy.send m -end - -module Messages(Class:CLASS) = struct - include MessagesOfXml(struct - let xml = messages_xml () - let name = Class.name - end) -end diff --git a/sw/lib/ocaml/pprz.mli b/sw/lib/ocaml/pprz.mli deleted file mode 100644 index b669670ec9..0000000000 --- a/sw/lib/ocaml/pprz.mli +++ /dev/null @@ -1,200 +0,0 @@ -(* - * Downlink protocol (handling messages.xml) - * - * Copyright (C) 2003 Pascal Brisset, 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. - * - *) - -val messages_xml : unit -> Xml.xml - -type class_name = string -type message_id = int -type ac_id = int -type format = string -type _type = - Scalar of string - | ArrayType of string - | FixedArrayType of string * int -type value = - Int of int | Float of float | String of string | Int32 of int32 | Char of char | Int64 of int64 - | Array of value array -type field = { - _type : _type; - fformat : format; - alt_unit_coef : string; (* May be empty *) - enum : string list (* 'values' attribute *) - } -type link_mode = Forwarded | Broadcasted -type message = { - name : string; - fields : (string * field) list; - link : link_mode option - } -(** Message specification *) - - -external int32_of_bytes : string -> int -> int32 = "c_int32_of_indexed_bytes" -external uint32_of_bytes : string -> int -> int64 = "c_uint32_of_indexed_bytes" -external int64_of_bytes : string -> int -> int64 = "c_int64_of_indexed_bytes" -(** [int32_of_bytes buffer offset] *) - -val separator : string -(** Separator in array values *) - -val is_array_type : string -> bool -val is_fixed_array_type : string -> bool - -val size_of_field : field -> int -val string_of_value : value -> string -val formatted_string_of_value : format -> value -> string -val int_of_value : value -> int (* May raise Invalid_argument *) -type type_descr = { - format : string ; - glib_type : string; - inttype : string; - size : int; - value : value - } -val types : (string * type_descr) list -type values = (string * value) list - -val value : _type -> string -> value -(** return a value from a string and a type *) - -val assoc : string -> values -> value -(** Safe assoc taking into accound characters case. May raise Failure ... *) - -val string_assoc : string -> values -> string -(** May raise Not_found *) - -val float_assoc : string -> values -> float -val int_assoc : string -> values -> int -val int32_assoc : string -> values -> Int32.t -val uint32_assoc : string -> values -> Int64.t -val int64_assoc : string -> values -> Int64.t -(** May raise Not_found or Invalid_argument *) - -val hex_of_int_array : value -> string -(** Returns the hexadecimal string of an array of integers *) - -exception Unit_conversion_error of string -(** Unit_conversion_error raised when parsing error occurs *) -exception Unknown_conversion of string * string -(** Unknown_conversion raised when conversion fails *) -exception No_automatic_conversion of string * string -(** No_automatic_conversion raised when no conversion found - * and from_unit or to_unit are empty string - *) - -val scale_of_units : ?auto:string -> string -> string -> float -(** scale_of_units from to - * Returns conversion factor between two units - * The possible conversions are described in conf/units.xml - * May raise Invalid_argument if one of the unit is not valid - * or if units.xml is not valid - *) - -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]. *) - -module Transport : Serial.PROTOCOL -(** Pprz frame (sw/airborne/pprz_transport.h): - |STX|length|... payload=(length-4) bytes ...|Checksum A|Checksum B| - Where checksum is computed over length and payload: - ck_A = ck_B = 0; - for all byte b in payload - ck_A += b; ck_b += ck_A - - STX = 0x99 - [packet] raises Invalid_Argument if length >= 256 - *) - -module TransportExtended : Serial.PROTOCOL -(** Pprz frame (sw/airborne/pprz_transport.h): - |STX|length|timestamp|... payload=(length-4) bytes ...|Checksum A|Checksum B| - Where checksum is computed over length and payload: - ck_A = ck_B = 0; - for all byte b in payload - ck_A += b; ck_b += ck_A - - STX = 0x98 - [packet] raises Invalid_Argument if length >= 256 - *) - -val offset_fields : int - -module type CLASS = sig - val name : string -end - -module type CLASS_Xml = sig - val xml : Xml.xml - val name : string -end - -module type MESSAGES = sig - val messages : (message_id, message) Hashtbl.t - val message_of_id : message_id -> message - val message_of_name : string -> message_id * message - - val values_of_payload : Serial.payload -> message_id * ac_id * values - (** [values_of_bin payload] Parses a raw payload, returns the - message id, the A/C id and the list of (field_name, value) *) - - val payload_of_values : message_id -> ac_id -> values -> Serial.payload - (** [payload_of_values id ac_id vs] Returns a payload *) - - val values_of_string : string -> message_id * values - (** May raise [(Unknown_msg_name msg_name)] *) - - val string_of_message : ?sep:string -> message -> values -> string - (** [string_of_message ?sep msg values] Default [sep] is space *) - - val message_send : ?timestamp:float -> ?link_id:int -> string -> string -> values -> unit - (** [message_send sender msg_name values] *) - - val message_bind : ?sender:string -> ?timestamp:bool -> string -> (string -> values -> unit) -> Ivy.binding - (** [message_bind ?sender msg_name callback] *) - - val message_answerer : string -> string -> (string -> values -> values) -> Ivy.binding - (** [message_answerer sender msg_name callback] Set a handler for a - [message_req] (which will send a [msg_name]_REQ message). - [callback asker args] must return the list of attributes of the answer. *) - - val message_req : string -> string -> values -> (string -> values -> unit) -> unit - (** [message_req sender msg_name values receiver] Sends a request on the Ivy - bus for the specified message. A [msg_name]_REQ message is send and a - [msg_name] message is expected for the reply. On reception, [receiver] - will be applied on [sender_name] and attribute values of the values. *) -end - -module Messages : functor (Class : CLASS) -> MESSAGES -module MessagesOfXml : functor (Class : CLASS_Xml) -> MESSAGES diff --git a/sw/lib/ocaml/serial.ml b/sw/lib/ocaml/serial.ml index 1c20a73869..42edcf2cfa 100644 --- a/sw/lib/ocaml/serial.ml +++ b/sw/lib/ocaml/serial.ml @@ -69,12 +69,6 @@ let speed_of_baudrate = fun baudrate -> | _ -> invalid_arg "Serial.speed_of_baudrate" -type payload = string - -let string_of_payload = fun x -> x -let payload_of_string = fun x -> x - - external init_serial : string -> speed -> bool -> Unix.file_descr = "c_init_serial" external set_dtr : Unix.file_descr -> bool -> unit = "c_set_dtr" external set_speed : Unix.file_descr -> speed -> unit = "c_serial_set_baudrate" @@ -121,56 +115,3 @@ let input = fun ?(read = Unix.read) f -> parse 0 n) -exception Not_enough - -module type PROTOCOL = sig - val index_start : string -> int (* raise Not_found *) - val length : string -> int -> int (* raise Not_enough *) - val checksum : string -> bool - val payload : string -> payload - val packet : payload -> string -end - -module Transport(Protocol:PROTOCOL) = struct - let nb_err = ref 0 - let discarded_bytes = ref 0 - let rec parse = fun use buf -> - Debug.call 'T' (fun f -> fprintf f "Transport.parse: %s\n" (Debug.xprint buf)); - (** ref required due to Not_enough exception raised by Protocol.length *) - let start = ref 0 - and n = String.length buf in - try - (* Looks for the beginning of the frame. May raise Not_found exception *) - start := Protocol.index_start buf; - - (* Discards skipped characters *) - discarded_bytes := !discarded_bytes + !start; - - (* Get length of the frame (may raise Not_enough exception) *) - let length = Protocol.length buf !start in - let end_ = !start + length in - - (* Checks if the complete frame is available in the buffer. *) - if n < end_ then - raise Not_enough; - - (* Extracts the complete frame *) - let msg = String.sub buf !start length in - - (* Checks sum *) - if Protocol.checksum msg then begin - (* Calls the handler with the message *) - use (Protocol.payload msg) - end else begin - (* Reports the error *) - incr nb_err; - discarded_bytes := !discarded_bytes + length; - Debug.call 'T' (fun f -> fprintf f "Transport.chk: %s\n" (Debug.xprint msg)) - end; - - (* Continues with the rest of the message *) - end_ + parse use (String.sub buf end_ (String.length buf - end_)) - with - Not_found -> String.length buf - | Not_enough -> !start -end diff --git a/sw/lib/ocaml/serial.mli b/sw/lib/ocaml/serial.mli index ef2b7e26b4..32195ff861 100644 --- a/sw/lib/ocaml/serial.mli +++ b/sw/lib/ocaml/serial.mli @@ -60,36 +60,3 @@ characters are available on the stream. These characters are stored in a a buffer. [f] is then called on the buffer. [f] must return the number of consumed characters. Default [read] is [Unix.read] *) -type payload - -val string_of_payload : payload -> string -val payload_of_string : string -> payload - -exception Not_enough -module type PROTOCOL = - sig - val index_start : string -> int - (** Must return the index of the first char of the the first message. - May raise Not_found or Not_enough *) - - val length : string -> int -> int - (** [length buf start] Must return the length of the message starting at - [start]. May raise Not_enough *) - - val checksum : string -> bool - (** [checksum message] *) - - val payload : string -> payload - val packet : payload -> string - end - -(** Builds a parser module from a [PROTOCOL] description *) -module Transport : - functor (Protocol : PROTOCOL) -> - sig - val nb_err : int ref (* Errors on checksum *) - val discarded_bytes : int ref - val parse : (payload -> unit) -> string -> int - (** [parse f buf] Scans [buf] according to [Protocol] and applies [f] on - payload of every recognised message. Returns the number of consumed bytes. *) - end diff --git a/sw/lib/ocaml/test/test_logpprz.ml b/sw/lib/ocaml/test/test_logpprz.ml index a25374dfc7..030d73b9b4 100644 --- a/sw/lib/ocaml/test/test_logpprz.ml +++ b/sw/lib/ocaml/test/test_logpprz.ml @@ -1,14 +1,14 @@ -module Tm_Pprz = Pprz.Messages (struct let name = "telemetry" end) +module Tm_Pprz = PprzLink.Messages (struct let name = "telemetry" end) -module Parser = Serial.Transport(Logpprz.Transport) +module Parser = Protocol.Transport(Pprzlog_transport.Transport) let convert_file = fun file -> let use_payload = fun payload -> - let log_msg = Logpprz.parse payload in + let log_msg = Pprzlog_transport.parse payload in let (msg_id, ac_id, vs) = - Tm_Pprz.values_of_payload log_msg.Logpprz.pprz_data in + Tm_Pprz.values_of_payload log_msg.Pprzlog_transport.pprz_data in let msg_descr = Tm_Pprz.message_of_id msg_id in - Printf.printf "%.3f %d %s\n" (Int32.to_float log_msg.Logpprz.timestamp /. 1e4) ac_id (Tm_Pprz.string_of_message msg_descr vs) in + Printf.printf "%.3f %d %s\n" (Int32.to_float log_msg.Pprzlog_transport.timestamp /. 1e4) ac_id (Tm_Pprz.string_of_message msg_descr vs) in let parser = Parser.parse use_payload in let Serial.Closure reader = Serial.input parser in diff --git a/sw/lib/ocaml/ubx.ml b/sw/lib/ocaml/ubx.ml index f891d16177..a34bc4a7f9 100644 --- a/sw/lib/ocaml/ubx.ml +++ b/sw/lib/ocaml/ubx.ml @@ -22,7 +22,7 @@ * *) -module Protocol = struct +module UbxProtocol = struct (** SYNC1 SYNC2 CLASS ID LENGTH(2) UBX_PAYLOAD CK_A CK_B LENGTH is the lentgh of UBX_PAYLOAD For us, the 'payload' includes also CLASS, ID and the LENGTH *) @@ -47,10 +47,10 @@ module Protocol = struct if len >= offset_length+2 then payload_length buf start + 4 else - raise Serial.Not_enough + raise Protocol.Not_enough let payload = fun buf -> - Serial.payload_of_string (String.sub buf offset_payload (payload_length buf 0)) + Protocol.payload_of_string (String.sub buf offset_payload (payload_length buf 0)) let uint8_t = fun x -> x land 0xff let (+=) = fun r x -> r := uint8_t (!r + x) @@ -69,7 +69,7 @@ module Protocol = struct ck_a = Char.code buf.[offset_payload+l+1] && ck_b = Char.code buf.[offset_payload+l+2] let packet = fun payload -> - let payload = Serial.string_of_payload payload in + let payload = Protocol.string_of_payload payload in let n = String.length payload in let msg_length = n + 4 in let m = String.create msg_length in @@ -185,4 +185,4 @@ let payload = fun class_name msg_name values -> m.[2] <- Char.chr (n land 0xff); m.[3] <- Char.chr ((n land 0xff00) lsr 8); String.blit u_payload 0 m 4 n; - Serial.payload_of_string m + Protocol.payload_of_string m diff --git a/sw/lib/ocaml/ubx.mli b/sw/lib/ocaml/ubx.mli index b5d9847c59..f8fbc6bb1b 100644 --- a/sw/lib/ocaml/ubx.mli +++ b/sw/lib/ocaml/ubx.mli @@ -22,7 +22,7 @@ * *) -module Protocol : Serial.PROTOCOL +module UbxProtocol : Protocol.PROTOCOL type message_spec @@ -31,4 +31,4 @@ type msg_id = int val message : string -> string -> class_id*msg_id*message_spec val ubx_payload : message_spec -> (string * int) list -> string -val payload : string -> string -> (string * int) list -> Serial.payload +val payload : string -> string -> (string * int) list -> Protocol.payload diff --git a/sw/lib/ocaml/xbee.ml b/sw/lib/ocaml/xbee.ml deleted file mode 100644 index e91763bcfb..0000000000 --- a/sw/lib/ocaml/xbee.ml +++ /dev/null @@ -1,201 +0,0 @@ -(* - * Copyright (C) 2006 ENAC, Pascal Brisset, 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. - * - *) - - -module Protocol = struct - let start_delimiter = Char.chr 0x7e - let offset_length = 1 - let offset_payload = 3 - let size_packet = 4 - let index_start = fun s -> - String.index s start_delimiter - - let length = fun s i -> - if String.length s < i+offset_length+2 then - raise Serial.Not_enough - else - (Char.code s.[i+offset_length] lsl 8) lor (Char.code s.[i+offset_length+1]) + size_packet - - let compute_checksum = fun s -> - let cs = ref 0 in - for i = offset_payload to String.length s - 2 do - cs := (!cs + Char.code s.[i]) land 0xff - done; - 0xff - !cs - - let checksum = fun s -> - let c = compute_checksum s in - Debug.call 'x' (fun f -> Printf.fprintf f "BX.cs=%x\n" c); - c = Char.code s.[String.length s-1] - - let payload = fun s -> - Serial.payload_of_string (String.sub s offset_payload (String.length s - size_packet)) - - let packet = fun payload -> - let payload = Serial.string_of_payload payload in - let n = String.length payload in - let msg_length = n + size_packet in - let m = String.create msg_length in - String.blit payload 0 m offset_payload n; - m.[0] <- start_delimiter; - m.[offset_length] <- Char.chr (n lsr 8); - m.[offset_length+1] <- Char.chr (n land 0xff); - let cs = compute_checksum m in - m.[msg_length-1] <- Char.chr cs; - m -end - -type frame_data = string -type frame_id = int -type addr64 = Int64.t -type addr16 = int -type byte = int -type rssi = int -type frame = - Modem_Status of int - | AT_Command_Response of frame_id * string * int * string - | TX_Status of frame_id * byte - | TX868_Status of frame_id * byte * int - | RX_Packet_64 of addr64 * int * int * string - | RX868_Packet of addr64 * byte * string - | RX_Packet_16 of addr16 * int * int * string - - -let mode868 = ref false - -let check_not_in_868 = fun s -> - if !mode868 then - failwith (Printf.sprintf "Xbee.%s not available in mode868" s) - - -let api_tx64_id = Char.chr 0x00 -let api868_tx64_id = Char.chr 0x10 -let api_tx16_id = Char.chr 0x01 -let api_rx64_id = Char.chr 0x80 -let api_rx16_id = Char.chr 0x81 -let api_at_command_response_id = Char.chr 0x88 -let api_tx_status_id = Char.chr 0x89 -let api868_tx_status_id = Char.chr 0x8b -let api_modem_status_id = Char.chr 0x8a -let api_rx64_id = Char.chr 0x80 -let api868_rx64_id = Char.chr 0x90 -let api_rx16_id = Char.chr 0x81 - -let write_int64 = fun buf offset x -> - for i = 0 to 7 do - buf.[offset+7-i] <- Char.chr (Int64.to_int (Int64.shift_right x (8*i)) land 0xff) - done - -let read_int64 = fun buf offset -> - let x = ref Int64.zero in - for i = 0 to 7 do - x := Int64.logor (Int64.shift_left !x 8) (Int64.of_int (Char.code buf.[offset+i])) - done; - !x - -let write_int16 = fun buf offset x -> - buf.[offset] <- Char.chr (x lsr 8); - buf.[offset+1] <- Char.chr (x land 0xff) - -let read_int16 = fun buf offset -> - (Char.code buf.[offset] lsl 8) lor (Char.code buf.[offset+1]) - -let api_tx64 = fun ?(frame_id = 0) dest data -> - assert (frame_id >=0 && frame_id < 0x100); - let n = String.length data in - assert (n <= 100); - let optional868 = if !mode868 then 3 else 0 in - let l = 1 + 1 + 8 + optional868 + 1 + n in - let s = String.create l in - s.[0] <- api_tx64_id; - s.[1] <- Char.chr frame_id; - if !mode868 then begin - s.[10] <- Char.chr 0xff; - s.[11] <- Char.chr 0xfe; - s.[12] <- Char.chr 0x0; - end; - write_int64 s 2 dest; - s.[10+optional868] <- Char.chr 0; - String.blit data 0 s (11+ optional868) n; - s - -let api_tx16 = fun ?(frame_id = 0) dest data -> - check_not_in_868 "api_tx16"; - assert (frame_id >=0 && frame_id < 0x100); - let n = String.length data in - assert (n <= 100); - let l = 1 + 1 + 2 + 1 + n in - let s = String.create l in - s.[0] <- api_tx16_id; - s.[1] <- Char.chr frame_id; - assert (dest >= 0 && dest < 0x10000); - write_int16 s 2 dest; - s.[4] <- Char.chr 0; - String.blit data 0 s 5 n; - s - - -let at_command_sequence = "+++" -let at_set_my = fun addr -> - assert (addr >= 0 && addr < 0x10000); - Printf.sprintf "ATMY%04x\r" addr -let baud_rates = [1200, 0; 2400, 1; 4800, 2; 9600, 3; 19200, 4; - 38400, 5; 57600, 6; 115200, 7] -let at_set_baud_rate = fun baud -> - try - Printf.sprintf "ATBD%d\r" (List.assoc baud baud_rates) - with - Not_found -> invalid_arg "at_set_baud_rate" - -let at_exit = "ATCN\r" -let at_api_enable = "ATAP1\r" - -let api_parse_frame = fun s -> - let n = String.length s in - assert(n>0); - match s.[0] with - x when x = api_at_command_response_id -> - assert(n >= 5); - AT_Command_Response (Char.code s.[1], String.sub s 2 2, - Char.code s.[4], String.sub s 5 (n-5)) - | x when not !mode868 && x = api_tx_status_id -> - assert(n = 3); - TX_Status (Char.code s.[1], Char.code s.[2]) - | x when !mode868 && x = api868_tx_status_id -> - assert(n = 7); - TX868_Status (Char.code s.[1], Char.code s.[5], Char.code s.[4]) - | x when x = api_modem_status_id -> - Modem_Status (Char.code s.[1]) - | x when not !mode868 && x = api_rx64_id -> - assert(n >= 11); - RX_Packet_64 (read_int64 s 1, Char.code s.[9], - Char.code s.[10], String.sub s 11 (n-11)) - | x when !mode868 && x = api868_rx64_id -> - let idx_data = 12 in - assert(n >= idx_data); - RX868_Packet (read_int64 s 1, - Char.code s.[11], String.sub s idx_data (n-idx_data)) - | x when not !mode868 && (x = api_rx16_id || x = api_tx16_id) -> - (* tx16 here allows to receive simulated xbee messages *) - RX_Packet_16 (read_int16 s 1, Char.code s.[3], Char.code s.[4], String.sub s 5 (n-5)) - | x -> failwith (Printf.sprintf "Xbee.parse_frame: unknown frame id '%d'" (Char.code x)) - diff --git a/sw/lib/ocaml/xbee.mli b/sw/lib/ocaml/xbee.mli deleted file mode 100644 index 21631d9702..0000000000 --- a/sw/lib/ocaml/xbee.mli +++ /dev/null @@ -1,63 +0,0 @@ -(* - * Copyright (C) 2006 ENAC, Pascal Brisset, 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. - * - *) - -(** Utilities for XBee modules (AT and API modes) *) - - -(** AT commands *) -val at_command_sequence : string -val at_set_my : int -> string -val at_exit : string -val at_api_enable : string -val at_set_baud_rate : int -> string - - -(** API protocol (payload is a frame_data) *) -module Protocol : Serial.PROTOCOL - -(** The data inside the frames *) -type frame_data = string - -(** Parsing frames sent by the module *) -type frame_id = int -type addr64 = Int64.t -type addr16 = int -type byte = int -type rssi = int -type frame = - Modem_Status of byte - | AT_Command_Response of frame_id * string * int * string - | TX_Status of frame_id * byte (** [(frame_if, status)] *) - | TX868_Status of frame_id * byte * int (** [(frame_if, status, nb_retries)] *) - | RX_Packet_64 of addr64 * rssi * byte * string - | RX868_Packet of addr64 * byte * string - | RX_Packet_16 of addr16 * rssi * byte * string -val api_parse_frame : frame_data -> frame - -(** Default to false *) -val mode868 : bool ref - -(** Building API frames data *) -val api_tx64 : ?frame_id:int -> int64 -> string -> frame_data -val api_tx16 : ?frame_id:int -> int -> string -> frame_data - - diff --git a/sw/lib/python/ivy_msg_interface.py b/sw/lib/python/ivy_msg_interface.py deleted file mode 100644 index 48a691201b..0000000000 --- a/sw/lib/python/ivy_msg_interface.py +++ /dev/null @@ -1,116 +0,0 @@ -from __future__ import absolute_import, division, print_function - -from ivy.std_api import * -from ivy.ivy import IvyIllegalStateError -import logging -import os -import sys -import re -import pprz_env - -sys.path.append(pprz_env.PAPARAZZI_SRC + "/sw/lib/python") - -from pprz_msg.message import PprzMessage -from pprz_msg import messages_xml_map - - -class IvyMessagesInterface(object): - def __init__(self, callback=None, init=True, verbose=False, bind_regex='(.*)', ivy_bus=pprz_env.IVY_BUS): - self.callback = callback - self.ivy_id = 0 - self.verbose = verbose - self.ivy_bus = ivy_bus - # make sure all messages are parsed before we start creating them in callbacks - messages_xml_map.parse_messages() - self.init_ivy(init, bind_regex) - - def stop(self): - IvyUnBindMsg(self.ivy_id) - - def shutdown(self): - try: - IvyStop() - self.stop() - except IvyIllegalStateError as e: - print(e) - - def __del__(self): - try: - self.shutdown() - except: - pass - - def init_ivy(self, init=True, bind_regex='(.*)'): - if init: - IvyInit("Messages %i" % os.getpid(), "READY", 0, lambda x,y: y, lambda x,y: y) - logging.getLogger('Ivy').setLevel(logging.WARN) - IvyStart(self.ivy_bus) - self.ivy_id = IvyBindMsg(self.on_ivy_msg, bind_regex) - - def on_ivy_msg(self, agent, *larg): - """ Split ivy message up into the separate parts - Basically parts/args in string are separated by space, but char array can also contain a space: - |f,o,o, ,b,a,r| in old format or "foo bar" in new format - """ - # return if no callback is set - if self.callback is None: - return - - # first split on array delimiters - l = re.split('([|\"][^|\"]*[|\"])', larg[0]) - # strip spaces and filter out emtpy strings - l = [str.strip(s) for s in l if str.strip(s) is not ''] - data = [] - for s in l: - # split non-array strings further up - if '|' not in s and '"' not in s: - data += s.split(' ') - else: - data.append(s) - # ignore ivy message with less than 3 elements - if len(data) < 3: - return - - # check which message class it is - msg_name = data[1] - msg_class, msg_name = messages_xml_map.find_msg_by_name(msg_name) - if msg_class is None: - print("Ignoring unknown message " + larg[0]) - return - # pass non-telemetry messages with ac_id 0 - if msg_class == "telemetry": - try: - ac_id = int(data[0]) - except ValueError: - print("ignoring message " + larg[0]) - sys.stdout.flush() - else: - ac_id = 0 - values = list(filter(None, data[2:])) - msg = PprzMessage(msg_class, msg_name) - msg.set_values(values) - self.callback(ac_id, msg) - - def send_raw_datalink(self, msg): - if not isinstance(msg, PprzMessage): - print("Can only send PprzMessage") - return - if "datalink" not in msg.msg_class: - print("Message to embed in RAW_DATALINK needs to be of 'datalink' class") - return - raw = PprzMessage("ground", "RAW_DATALINK") - raw['ac_id'] = msg['ac_id'] - raw['message'] = msg.to_csv() - self.send(raw) - - def send(self, msg, ac_id=None): - if isinstance(msg, PprzMessage): - if "telemetry" in msg.msg_class: - if ac_id is None: - print("ac_id needed to send telemetry message.") - else: - IvySendMsg("%d %s %s" % (ac_id, msg.name, msg.payload_to_ivy_string())) - else: - IvySendMsg("%s %s %s" % (msg.msg_class, msg.name, msg.payload_to_ivy_string())) - else: - IvySendMsg(msg) diff --git a/sw/lib/python/pprz_msg/__init__.py b/sw/lib/python/pprz_msg/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/sw/lib/python/pprz_msg/message.py b/sw/lib/python/pprz_msg/message.py deleted file mode 100644 index 2ca5679a74..0000000000 --- a/sw/lib/python/pprz_msg/message.py +++ /dev/null @@ -1,229 +0,0 @@ -""" -Paparazzi message representation - -""" - -from __future__ import division, print_function -import sys -import json -import struct -import messages_xml_map - - -class PprzMessageError(Exception): - def __init__(self, message, inner_exception=None): - self.message = message - self.inner_exception = inner_exception - self.exception_info = sys.exc_info() - - def __str__(self): - return self.message - - -class PprzMessage(object): - """base Paparazzi message class""" - - def __init__(self, class_name, msg): - self._class_name = class_name - if isinstance(msg, int): - self._id = msg - self._name = messages_xml_map.get_msg_name(class_name, msg) - else: - self._name = msg - self._id = messages_xml_map.get_msg_id(class_name, msg) - self._fieldnames = messages_xml_map.get_msg_fields(class_name, self._name) - self._fieldtypes = messages_xml_map.get_msg_fieldtypes(class_name, self._id) - self._fieldcoefs = messages_xml_map.get_msg_fieldcoefs(class_name, self._id) - self._fieldvalues = [] - # set empty values according to type - for t in self._fieldtypes: - if t == "char[]": - self._fieldvalues.append('') - elif '[' in t: - self._fieldvalues.append([0]) - else: - self._fieldvalues.append(0) - - @property - def name(self): - """Get the message name.""" - return self._name - - @property - def msg_id(self): - """Get the message id.""" - return self._id - - @property - def msg_class(self): - """Get the message class.""" - return self._class_name - - @property - def fieldnames(self): - """Get list of field names.""" - return self._fieldnames - - @property - def fieldvalues(self): - """Get list of field values.""" - return self._fieldvalues - - @property - def fieldtypes(self): - """Get list of field types.""" - return self._fieldtypes - - @property - def fieldcoefs(self): - """Get list of field coefs.""" - return self._fieldcoefs - - def fieldbintypes(self, t): - """Get type and length for binary format""" - data_types = { - 'float': ['f', 4], - 'double': ['d', 8], - 'uint8': ['B', 1], - 'uint16': ['H', 2], - 'uint32': ['L', 4], - 'int8': ['b', 1], - 'int16': ['h', 2], - 'int32': ['l', 4], - 'char': ['c', 1] - } - base_type = t.split('[')[0] - return data_types[base_type] - - def get_field(self, idx): - """Get field value by index.""" - return self._fieldvalues[idx] - - def __getattr__(self, attr): - # Try to dynamically return the field value for the given name - for idx, f in enumerate(self.fieldnames): - if f == attr: - return self.fieldvalues[idx] - raise AttributeError("No such attribute %s" % attr) - - def __getitem__(self, key): - # Try to dynamically return the field value for the given name - for idx, f in enumerate(self.fieldnames): - if f == key: - return self.fieldvalues[idx] - raise AttributeError("Msg %s has no field of name %s" % (self.name, key)) - - def __setitem__(self, key, value): - self.set_value_by_name(key, value) - - def set_values(self, values): - #print("msg %s: %s" % (self.name, ", ".join(self.fieldnames))) - if len(values) == len(self.fieldnames): - self._fieldvalues = values - else: - raise PprzMessageError("Error: Msg %s has %d fields, tried to set %i values" % - (self.name, len(self.fieldnames), len(values))) - - def set_value_by_name(self, name, value): - # Try to set a value from its name - for idx, f in enumerate(self.fieldnames): - if f == name: - self._fieldvalues[idx] = value - return - raise AttributeError("Msg %s has no field of name %s" % (self.name, name)) - - def __str__(self): - ret = '%s.%s {' % (self.msg_class, self.name) - for idx, f in enumerate(self.fieldnames): - ret += '%s : %s, ' % (f, self.fieldvalues[idx]) - ret = ret[0:-2] + '}' - return ret - - def to_dict(self, payload_only=False): - d = {} - if not payload_only: - d['msgname'] = self.name - d['msgclass'] = self.msg_class - for idx, f in enumerate(self.fieldnames): - d[f] = self.fieldvalues[idx] - return d - - def to_json(self, payload_only=False): - return json.dumps(self.to_dict(payload_only)) - - def to_csv(self, payload_only=False): - """ return message as CSV string for use with RAW_DATALINK - msg_name;field1;field2; - """ - return str(self.name) + ';' + self.payload_to_ivy_string(sep=';') - - def payload_to_ivy_string(self, sep=' '): - ivy_str = '' - for idx, t in enumerate(self.fieldtypes): - if "char[" in t: - ivy_str += '"' + self.fieldvalues[idx] + '"' - elif '[' in t: - ivy_str += ','.join([str(x) for x in self.fieldvalues[idx]]) - else: - ivy_str += str(self.fieldvalues[idx]) - ivy_str += sep - return ivy_str - - def payload_to_binary(self): - struct_string = "<" - data = [] - length = 0 - for idx, t in enumerate(self.fieldtypes): - bin_type = self.fieldbintypes(t) - struct_string += bin_type[0] - array_length = 1 - if "char[" in t: - array_length = len(self.fieldvalues[idx]) - for c in self.fieldvalues[idx]: - data.append(int(c)) - elif '[' in t: - array_length = len(self.fieldvalues[idx]) - for x in self.fieldvalues[idx]: - data.append(x) - else: - data.append(self.fieldvalues[idx]) - length += bin_type[1] * array_length - msg = struct.pack(struct_string, *data) - return msg - - def binary_to_payload(self, data): - msg_offset = 0 - values = [] - for idx, t in enumerate(self.fieldtypes): - bin_type = self.fieldbintypes(t) - if '[' in t: - array_length = data[msg_offset] - msg_offset += 1 - array_value = [] - for count in range(0, array_length): - array_value.append(struct.unpack('<' + bin_type[0], data[msg_offset:msg_offset + bin_type[1]])[0]) - msg_offset = msg_offset + bin_type[1] - values.append(array_value) - else: - value = struct.unpack('<' + bin_type[0], data[msg_offset:msg_offset + bin_type[1]])[0] - msg_offset = msg_offset + bin_type[1] - values.append(value) - self.set_values(values) - - -def test(): - import argparse - - parser = argparse.ArgumentParser() - parser.add_argument("-f", "--file", help="path to messages.xml file") - parser.add_argument("-c", "--class", help="message class", dest="msg_class", default="telemetry") - args = parser.parse_args() - messages_xml_map.parse_messages(args.file) - messages = [PprzMessage(args.msg_class, n) for n in messages_xml_map.get_msgs(args.msg_class)] - print("Listing %i messages in '%s' msg_class" % (len(messages), args.msg_class)) - for msg in messages: - print(msg) - - -if __name__ == '__main__': - test() diff --git a/sw/lib/python/pprz_msg/messages_xml_map.py b/sw/lib/python/pprz_msg/messages_xml_map.py deleted file mode 100755 index c0c78b0b0b..0000000000 --- a/sw/lib/python/pprz_msg/messages_xml_map.py +++ /dev/null @@ -1,171 +0,0 @@ -#!/usr/bin/env python - -from __future__ import absolute_import, print_function - -import os - -# if PAPARAZZI_HOME not set, then assume the tree containing this -# file is a reasonable substitute -PPRZ_HOME = os.getenv("PAPARAZZI_HOME", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), - '../../../..'))) - -default_messages_file = '%s/conf/messages.xml' % PPRZ_HOME - -message_dictionary = {} -message_dictionary_types = {} -message_dictionary_coefs = {} -message_dictionary_id_name = {} -message_dictionary_name_id = {} - - -class MessagesNotFound(Exception): - def __init__(self, filename): - self.filename = filename - - def __str__(self): - return "messages file " + repr(self.filename) + " not found" - - -def parse_messages(messages_file=''): - if not messages_file: - messages_file = default_messages_file - if not os.path.isfile(messages_file): - raise MessagesNotFound(messages_file) - #print("Parsing %s" % messages_file) - from lxml import etree - tree = etree.parse(messages_file) - for the_class in tree.xpath("//msg_class[@name]"): - class_name = the_class.attrib['name'] - if class_name not in message_dictionary: - message_dictionary_id_name[class_name] = {} - message_dictionary_name_id[class_name] = {} - message_dictionary[class_name] = {} - message_dictionary_types[class_name] = {} - message_dictionary_coefs[class_name] = {} - for the_message in the_class.xpath("message[@name]"): - message_name = the_message.attrib['name'] - if 'id' in the_message.attrib: - message_id = the_message.attrib['id'] - else: - message_id = the_message.attrib['ID'] - if message_id[0:2] == "0x": - message_id = int(message_id, 16) - else: - message_id = int(message_id) - - message_dictionary_id_name[class_name][message_id] = message_name - message_dictionary_name_id[class_name][message_name] = message_id - - # insert this message into our dictionary as a list with room for the fields - message_dictionary[class_name][message_name] = [] - message_dictionary_types[class_name][message_id] = [] - message_dictionary_coefs[class_name][message_id] = [] - - for the_field in the_message.xpath('field[@name]'): - # for now, just save the field names -- in the future maybe expand this to save a struct? - message_dictionary[class_name][message_name].append(the_field.attrib['name']) - message_dictionary_types[class_name][message_id].append(the_field.attrib['type']) - try: - message_dictionary_coefs[class_name][message_id].append(float(the_field.attrib['alt_unit_coef'])) - except KeyError: - # print("no such key") - message_dictionary_coefs[class_name][message_id].append(1.) - - -def find_msg_by_name(name): - if not message_dictionary: - parse_messages() - for msg_class in message_dictionary: - if name in message_dictionary[msg_class]: - #print("found msg name %s in class %s" % (name, msg_class)) - return msg_class, name - print("Error: msg_name %s not found." % name) - return None, None - - -def get_msgs(msg_class): - if not message_dictionary: - parse_messages() - if msg_class in message_dictionary: - return message_dictionary[msg_class] - else: - print("Error: msg_class %s not found." % msg_class) - return [] - - -def get_msg_name(msg_class, msg_id): - if not message_dictionary: - parse_messages() - if msg_class in message_dictionary: - if msg_id in message_dictionary_id_name[msg_class]: - return message_dictionary_id_name[msg_class][msg_id] - else: - print("Error: msg_id %d not found in msg_class %s." % (msg_id, msg_class)) - else: - print("Error: msg_class %s not found." % msg_class) - return "" - - -def get_msg_fields(msg_class, msg_name): - if not message_dictionary: - parse_messages() - if msg_class in message_dictionary: - if msg_name in message_dictionary[msg_class]: - return message_dictionary[msg_class][msg_name] - else: - print("Error: msg_name %s not found in msg_class %s." % (msg_name, msg_class)) - else: - print("Error: msg_class %s not found." % msg_class) - return [] - - -def get_msg_id(msg_class, msg_name): - if not message_dictionary: - parse_messages() - try: - return message_dictionary_name_id[msg_class][msg_name] - except KeyError: - print("Error: msg_name %s not found in msg_class %s." % (msg_name, msg_class)) - return 0 - - -def get_msg_fieldtypes(msg_class, msg_id): - if not message_dictionary: - parse_messages() - if msg_class in message_dictionary_types: - if msg_id in message_dictionary_types[msg_class]: - return message_dictionary_types[msg_class][msg_id] - else: - print("Error: message with ID %d not found in msg_class %s." % (msg_id, msg_class)) - else: - print("Error: msg_class %s not found." % msg_class) - return [] - -def get_msg_fieldcoefs(msg_class, msg_id): - if not message_dictionary: - parse_messages() - if msg_class in message_dictionary_coefs: - if msg_id in message_dictionary_coefs[msg_class]: - return message_dictionary_coefs[msg_class][msg_id] - else: - print("Error: message with ID %d not found in msg_class %s." % (msg_id, msg_class)) - else: - print("Error: msg_class %s not found." % msg_class) - return [] - - -def test(): - import argparse - parser = argparse.ArgumentParser() - parser.add_argument("-f", "--file", help="path to messages.xml file") - parser.add_argument("-l", "--list", help="list parsed messages", action="store_true", dest="list_messages") - parser.add_argument("-c", "--class", help="message class", dest="msg_class", default="telemetry") - args = parser.parse_args() - parse_messages(args.file) - if args.list_messages: - print("Listing %i messages in '%s' msg_class" % (len(message_dictionary[args.msg_class]), args.msg_class)) - for msg_name, msg_fields in message_dictionary[args.msg_class].iteritems(): - print(msg_name + ": " + ", ".join(msg_fields)) - -if __name__ == '__main__': - test() diff --git a/sw/lib/python/pprz_msg/pprz_transport.py b/sw/lib/python/pprz_msg/pprz_transport.py deleted file mode 100644 index 4de72990fa..0000000000 --- a/sw/lib/python/pprz_msg/pprz_transport.py +++ /dev/null @@ -1,110 +0,0 @@ -""" -Paparazzi transport encoding utilities - -""" - -from __future__ import absolute_import, division, print_function -import struct -from .message import PprzMessage - -# use Enum from python 3.4 if available (https://www.python.org/dev/peps/pep-0435/) -# (backports as enum34 on pypi) -try: - from enum import Enum -except ImportError: - Enum = object - -STX = 0x99 -STX_TS = 0x98 - -class PprzParserState(Enum): - WaitSTX = 1 - GotSTX = 2 - GotLength = 3 - GotPayload = 4 - GotCRC1 = 5 - -class PprzTransport(object): - """parser for binary Paparazzi messages""" - def __init__(self, msg_class='telemetry'): - self.msg_class = msg_class - self.reset_parser() - - def reset_parser(self): - self.state = PprzParserState.WaitSTX - self.length = 0 - self.buf = [] - self.ck_a = 0 - self.ck_b = 0 - self.idx = 0 - - def parse_byte(self, c): - """parse new byte, return True when a new full message is available""" - b = struct.unpack(" try - Pprz.string_of_value (Hashtbl.find last_values (m,String.lowercase field)) + PprzLink.string_of_value (Hashtbl.find last_values (m,String.lowercase field)) with Not_found -> "" in diff --git a/sw/logalizer/export.mli b/sw/logalizer/export.mli index aeabc711b9..abb97fa444 100644 --- a/sw/logalizer/export.mli +++ b/sw/logalizer/export.mli @@ -1,5 +1,5 @@ val popup : ?no_gui:bool -> Xml.xml -> - string -> (float * string * (string * Pprz.value) list) list -> unit + string -> (float * string * (string * PprzLink.value) list) list -> unit (** [popup ?no_gui protocol filename data] *) diff --git a/sw/logalizer/logplotter.ml b/sw/logalizer/logplotter.ml index eefa071526..ef58b26722 100644 --- a/sw/logalizer/logplotter.ml +++ b/sw/logalizer/logplotter.ml @@ -508,18 +508,18 @@ class plot = fun ~width ~height ~packing () -> let pprz_float = function - Pprz.Int i -> float i - | Pprz.Float f -> f - | Pprz.Int32 i -> Int32.to_float i - | Pprz.Int64 i -> Int64.to_float i - | Pprz.String s -> let v = try float_of_string s with _ -> 0. in v - | Pprz.Char c -> let v = try float_of_string (String.make 1 c) with _ -> 0. in v - | Pprz.Array _ -> 0. + PprzLink.Int i -> float i + | PprzLink.Float f -> f + | PprzLink.Int32 i -> Int32.to_float i + | PprzLink.Int64 i -> Int64.to_float i + | PprzLink.String s -> let v = try float_of_string s with _ -> 0. in v + | PprzLink.Char c -> let v = try float_of_string (String.make 1 c) with _ -> 0. in v + | PprzLink.Array _ -> 0. let rec select_gps_values = function [] -> [] - | (m, values)::_ when m.Pprz.name = "GPS" -> + | (m, values)::_ when m.PprzLink.name = "GPS" -> let xs = List.assoc "utm_east" values and ys = List.assoc "utm_north" values and zs = List.assoc "utm_zone" values @@ -536,7 +536,7 @@ let rec select_gps_values = function l := (t, of_utm WGS84 utm, a) :: !l done; List.rev !l - | (m, values)::_ when m.Pprz.name = "GPS_INT" -> + | (m, values)::_ when m.PprzLink.name = "GPS_INT" -> let lats = List.assoc "lat" values and lons = List.assoc "lon" values and alts = List.assoc "hmsl" values in @@ -613,7 +613,7 @@ let add_ac_submenu = fun ?(export=false) protocol ?(factor=object method text="1 (* Build the msg menus *) List.iter (fun (msg, l) -> - let msg_name = msg.Pprz.name in + let msg_name = msg.PprzLink.name in let menu = menu_fact#add_submenu (double__ msg_name) in let menu_fact = new GMenu.factory menu in (* Build the field menus *) @@ -623,7 +623,7 @@ let add_ac_submenu = fun ?(export=false) protocol ?(factor=object method text="1 (* Remove the . for an array field name *) let f' = List.hd (Str.split bracket_regexp f) in - let alt_unit_coef = (List.assoc f' msg.Pprz.fields).Pprz.alt_unit_coef in + let alt_unit_coef = (List.assoc f' msg.PprzLink.fields).PprzLink.alt_unit_coef in let name = sprintf "%s:%s:%s:%s" menu_name msg_name f factor#text and (a, b) = Ocaml_tools.affine_transform factor#text and (a', b') = Ocaml_tools.affine_transform alt_unit_coef in @@ -677,7 +677,7 @@ let load_log = fun ?export ?factor (plot:plot) (menubar:GMenu.menu_shell GMenu.f Debug.call 'p' (fun f -> fprintf f "class_name: %s\n" class_name); let module M = struct let name = class_name let xml = protocol end in - let module P = Pprz.MessagesOfXml(M) in + let module P = PprzLink.MessagesOfXml(M) in let f = try @@ -714,7 +714,7 @@ let load_log = fun ?export ?factor (plot:plot) (menubar:GMenu.menu_shell GMenu.f List.iter (fun (f, value) -> match value with - Pprz.Array array -> + PprzLink.Array array -> Array.iteri (fun i scalar -> let f = sprintf "%s[%d]" f i in @@ -724,7 +724,7 @@ let load_log = fun ?export ?factor (plot:plot) (menubar:GMenu.menu_shell GMenu.f Hashtbl.add fields f (t, scalar)) vs; - let msg_name = (P.message_of_id msg_id).Pprz.name in + let msg_name = (P.message_of_id msg_id).PprzLink.name in raw_msgs := (t, msg_name, vs) :: !raw_msgs ) with diff --git a/sw/logalizer/matlab_log/dialog.m b/sw/logalizer/matlab_log/dialog.m index 06aee49d61..a16cbf76c8 100644 --- a/sw/logalizer/matlab_log/dialog.m +++ b/sw/logalizer/matlab_log/dialog.m @@ -161,9 +161,9 @@ X0=0; try node=xmlread(fullfile(pp_home,'messages.xml')); catch - warning('messages.xml not found. trying conf/messages.xml...') + warning('messages.xml not found. trying var/messages.xml...') try - node=xmlread(fullfile(pp_home,'conf/messages.xml')); + node=xmlread(fullfile(pp_home,'var/messages.xml')); catch warning('messages.xml not found. Exiting...'); close(gcf); diff --git a/sw/logalizer/play_core.ml b/sw/logalizer/play_core.ml index 2dff4c2409..67c06e4f03 100644 --- a/sw/logalizer/play_core.ml +++ b/sw/logalizer/play_core.ml @@ -24,8 +24,8 @@ open Printf -module Ground_Pprz = Pprz.Messages(struct let name = "ground" end) -module Tm_Pprz = Pprz.Messages(struct let name = "telemetry" end) +module Ground_Pprz = PprzLink.Messages(struct let name = "ground" end) +module Tm_Pprz = PprzLink.Messages(struct let name = "telemetry" end) let (//) = Filename.concat let replay_dir = Env.paparazzi_home // "var" // "replay" @@ -83,7 +83,7 @@ let store_conf = fun conf acs -> write_xml (replay_dir // "conf" // "conf.xml") orig_conf let store_messages = fun protocol -> - write_xml (replay_dir // "conf" // "messages.xml") protocol + write_xml (replay_dir // "var" // "messages.xml") protocol let time_of = fun (t, _, _) -> t @@ -163,7 +163,7 @@ let run = fun serial_port log adj i0 speed no_gui -> try let msg_id, vs = Tm_Pprz.values_of_string m in let payload = Tm_Pprz.payload_of_values msg_id (int_of_string ac) vs in - let buf = Pprz.Transport.packet payload in + let buf = Pprz_transport.Transport.packet payload in Debug.call 'o' (fun f -> fprintf f "%s\n" (Debug.xprint buf)); fprintf channel "%s%!" buf with @@ -220,7 +220,7 @@ let init = fun () -> end in let world_update_time = fun _ vs -> - speed#set_value (Pprz.float_assoc "time_scale" vs) + speed#set_value (PprzLink.float_assoc "time_scale" vs) in ignore (Ground_Pprz.message_bind "WORLD_ENV" world_update_time); diff --git a/sw/logalizer/plotter.ml b/sw/logalizer/plotter.ml index f9555a1293..7f530f7dff 100644 --- a/sw/logalizer/plotter.ml +++ b/sw/logalizer/plotter.ml @@ -34,13 +34,13 @@ let set_float_value = fun (a:GData.adjustment) v -> a#set_value v let pprz_float = function - Pprz.Int i -> float i - | Pprz.Float f -> f - | Pprz.Int32 i -> Int32.to_float i - | Pprz.Int64 i -> Int64.to_float i - | Pprz.String s -> float_of_string s - | Pprz.Char c -> float_of_string (String.make 1 c) - | Pprz.Array _ -> 0. + PprzLink.Int i -> float i + | PprzLink.Float f -> f + | PprzLink.Int32 i -> Int32.to_float i + | PprzLink.Int64 i -> Int64.to_float i + | PprzLink.String s -> float_of_string s + | PprzLink.Char c -> float_of_string (String.make 1 c) + | PprzLink.Array _ -> 0. let dnd_targets = [ { Gtk.target = "STRING"; flags = []; info = 0} ] @@ -496,14 +496,14 @@ let rec plot_window = fun window -> let cb = fun _sender values -> let (field_name, index) = base_and_index field_descr in let value = - match Pprz.assoc field_name values with - Pprz.Array array -> array.(index) + match PprzLink.assoc field_name values with + PprzLink.Array array -> array.(index) | scalar -> scalar in let float = pprz_float value in let v = float *. a +. b in plot#add_value name v in - let module P = Pprz.Messages (struct let name = class_name end) in + let module P = PprzLink.Messages (struct let name = class_name end) in let binding = if sender = "*" then P.message_bind msg_name cb diff --git a/sw/logalizer/sd2log.ml b/sw/logalizer/sd2log.ml index 5d99953454..2273fe71bf 100644 --- a/sw/logalizer/sd2log.ml +++ b/sw/logalizer/sd2log.ml @@ -31,10 +31,10 @@ let default_logs_path = var_path // "logs" let conf_xml = Xml.parse_file (Env.paparazzi_home // "conf" // "conf.xml") -module Tm_Pprz = Pprz.Messages (struct let name = "telemetry" end) -module Dl_Pprz = Pprz.Messages (struct let name = "datalink" end) +module Tm_Pprz = PprzLink.Messages (struct let name = "telemetry" end) +module Dl_Pprz = PprzLink.Messages (struct let name = "datalink" end) -module Parser = Serial.Transport(Logpprz.Transport) +module Parser = Protocol.Transport(Pprzlog_transport.Transport) let run_command = fun com -> if Sys.command com <> 0 then begin @@ -60,41 +60,41 @@ let log_xml = fun ac_id -> make_element "configuration" [] - [expanded_conf; Pprz.messages_xml ()] + [expanded_conf; PprzLink.messages_xml ()] (* AWFUL : modules should be replaced by objects in pprz.ml ... or/and "datalink" and "telemetry" classes should be merged *) let values_of_payload = fun log_msg -> - match log_msg.Logpprz.source with + match log_msg.Pprzlog_transport.source with 0 -> Tm_Pprz.values_of_payload | 1 -> Dl_Pprz.values_of_payload | x -> failwith (sprintf "Unexpected source:%d in log msg" x) let message_of_id = fun log_msg -> - match log_msg.Logpprz.source with + match log_msg.Pprzlog_transport.source with 0 -> Tm_Pprz.message_of_id | 1 -> Dl_Pprz.message_of_id | x -> failwith (sprintf "Unexpected source:%d in log msg" x) let string_of_message = fun log_msg -> - match log_msg.Logpprz.source with + match log_msg.Pprzlog_transport.source with 0 -> Tm_Pprz.string_of_message | 1 -> Dl_Pprz.string_of_message | x -> failwith (sprintf "Unexpected source:%d in log msg" x) let hex_of_array = function - | Pprz.Array array -> + | PprzLink.Array array -> let n = Array.length array in (* One integer -> 2 chars *) let s = String.create (2*n) in Array.iteri (fun i dec -> - let hex = sprintf "%02x" (Pprz.int_of_value array.(i)) in + let hex = sprintf "%02x" (PprzLink.int_of_value array.(i)) in String.blit hex 0 s (2*i) 2) array; s | value -> - failwith (sprintf "Error: expecting array, found %s" (Pprz.string_of_value value)) + failwith (sprintf "Error: expecting array, found %s" (PprzLink.string_of_value value)) let xml_parse_compressed_file = fun file -> @@ -137,34 +137,34 @@ let convert_file = fun ?(output_dir=None) file -> let use_payload = fun payload -> try - let log_msg = Logpprz.parse payload in - if log_msg.Logpprz.source > 1 then - fprintf stderr "Invalid source (%d), skipping message\n" log_msg.Logpprz.source + let log_msg = Pprzlog_transport.parse payload in + if log_msg.Pprzlog_transport.source > 1 then + fprintf stderr "Invalid source (%d), skipping message\n" log_msg.Pprzlog_transport.source else - let (msg_id, ac_id, vs) = values_of_payload log_msg log_msg.Logpprz.pprz_data in + let (msg_id, ac_id, vs) = values_of_payload log_msg log_msg.Pprzlog_transport.pprz_data in - if log_msg.Logpprz.source = 0 && !single_ac_id < 0 then + if log_msg.Pprzlog_transport.source = 0 && !single_ac_id < 0 then single_ac_id := ac_id; - if ac_id <> !single_ac_id && log_msg.Logpprz.source = 0 then + if ac_id <> !single_ac_id && log_msg.Pprzlog_transport.source = 0 then fprintf stderr "Discarding message with ac_id %d, previous one was %d\n%!" ac_id !single_ac_id else let msg_descr = message_of_id log_msg msg_id in - let timestamp = Int32.to_float log_msg.Logpprz.timestamp /. 1e4 in + let timestamp = Int32.to_float log_msg.Pprzlog_transport.timestamp /. 1e4 in fprintf f_out "%.4f %d %s\n" timestamp ac_id (string_of_message log_msg msg_descr vs); (** Looking for a date from a GPS message and a md5 from an ALIVE *) - if log_msg.Logpprz.source = 0 then - match msg_descr.Pprz.name with + if log_msg.Pprzlog_transport.source = 0 then + match msg_descr.PprzLink.name with "GPS" when !start_unix_time = None - && ( Pprz.int_assoc "mode" vs = 3 - || Pprz.int_assoc "week" vs > 0) -> - let itow = Pprz.int_assoc "itow" vs / 1000 - and week = Pprz.int_assoc "week" vs in + && ( PprzLink.int_assoc "mode" vs = 3 + || PprzLink.int_assoc "week" vs > 0) -> + let itow = PprzLink.int_assoc "itow" vs / 1000 + and week = PprzLink.int_assoc "week" vs in let unix_time = Latlong.unix_time_of_tow ~week itow in start_unix_time := Some (unix_time -. timestamp) | "ALIVE" when !md5 = "" -> - md5 := hex_of_array (Pprz.assoc "md5sum" vs) + md5 := hex_of_array (PprzLink.assoc "md5sum" vs) | _ -> () with _ -> fprintf stderr "Parsing error, skipping message\n" in diff --git a/sw/simulator/Makefile b/sw/simulator/Makefile index e4f3f5d018..2fe404dfce 100644 --- a/sw/simulator/Makefile +++ b/sw/simulator/Makefile @@ -29,7 +29,7 @@ include ../Makefile.ocaml OCAMLC += -g INCLUDES = PKG = -package glibivy,pprz -LINKPKG = $(PKG) -linkpkg -dllpath-pkg pprz +LINKPKG = $(PKG) -linkpkg -dllpath-pkg pprz,pprzlink SIMML = stdlib.ml data.ml flightModel.ml gps.ml SIMHML = $(SIMML) hitl.ml sim.ml diff --git a/sw/simulator/data.ml b/sw/simulator/data.ml index 05c4fe7918..f28d6d3c51 100644 --- a/sw/simulator/data.ml +++ b/sw/simulator/data.ml @@ -26,14 +26,13 @@ open Printf let (//) = Filename.concat -(* let pprz_conf_path = Env.paparazzi_src // "conf" *) let user_conf_path = Env.paparazzi_home // "conf" +let user_var_path = Env.paparazzi_home // "var" let conf_xml = Xml.parse_file (user_conf_path // "conf.xml") let messages_ap = -(* let xml = Xml.parse_file (pprz_conf_path // "messages.xml") in *) - let xml = Xml.parse_file (user_conf_path // "messages.xml") in + let xml = Xml.parse_file (user_var_path // "messages.xml") in try ExtXml.child xml ~select:(fun x -> Xml.attrib x "name" = "telemetry") "msg_class" with diff --git a/sw/simulator/diffusion.ml b/sw/simulator/diffusion.ml index 1718345d97..5f50e24367 100644 --- a/sw/simulator/diffusion.ml +++ b/sw/simulator/diffusion.ml @@ -1,6 +1,6 @@ open Printf -module Ground_Pprz = Pprz.Messages(struct let name = "ground" end) +module Ground_Pprz = PprzLink.Messages(struct let name = "ground" end) module LL = Latlong open LL @@ -72,10 +72,10 @@ let send_on_ivy = fun () -> and ys = String.concat "," !ys and vs = String.concat "," !vs in Ground_Pprz.message_send my_id "PLUMES" - [ "ids", Pprz.String ids; - "lats", Pprz.String xs; - "longs", Pprz.String ys; - "values", Pprz.String vs ] + [ "ids", PprzLink.String ids; + "lats", PprzLink.String xs; + "longs", PprzLink.String ys; + "values", PprzLink.String vs ] let debug = let i = ref 0 in fun () -> incr i; @@ -89,17 +89,17 @@ let detect_distance = 20. let flight_param_msg = fun _sender vs -> - let lat = Pprz.float_assoc "lat" vs - and long = Pprz.float_assoc "long" vs in + let lat = PprzLink.float_assoc "lat" vs + and long = PprzLink.float_assoc "long" vs in let utm_ac = utm_of WGS84 {LL.posn_lat=(Deg>>Rad)lat; posn_long=(Deg>>Rad)long} in Hashtbl.iter (fun id plume -> let utm_plume = {LL.utm_zone = plume.utm_zone; utm_x = plume.utm_x; utm_y = plume.utm_y } in let d = utm_distance utm_ac utm_plume in if d < detect_distance then begin - let ac_id = Pprz.string_assoc "ac_id" vs in + let ac_id = PprzLink.string_assoc "ac_id" vs in for i = 0 to 2 do Ground_Pprz.message_send my_id "DL_SETTING" - ["ac_id", Pprz.String ac_id; "index", Pprz.Int i(***FIXME***); "value", Pprz.Float (float plume.value)] + ["ac_id", PprzLink.String ac_id; "index", PprzLink.Int i(***FIXME***); "value", PprzLink.Float (float plume.value)] done end) plumes @@ -112,9 +112,9 @@ let safe_bind = fun msg cb -> ignore (Ground_Pprz.message_bind msg safe_cb) let gaia = fun time_scale _sender vs -> - time_scale#set_value (Pprz.float_assoc "time_scale" vs); - wind_x := (Pprz.float_assoc "wind_east" vs); - wind_y := (Pprz.float_assoc "wind_north" vs) + time_scale#set_value (PprzLink.float_assoc "time_scale" vs); + wind_x := (PprzLink.float_assoc "wind_east" vs); + wind_y := (PprzLink.float_assoc "wind_north" vs) let _ = diff --git a/sw/simulator/flightModel.ml b/sw/simulator/flightModel.ml index 3f647c6fd6..9c03da79f5 100644 --- a/sw/simulator/flightModel.ml +++ b/sw/simulator/flightModel.ml @@ -111,13 +111,13 @@ module Make(A:Data.MISSION) = struct let cu = try ExtXml.attrib t "code_unit" with _ -> "" in (* default value for code_unit is rad[/s] when unit is deg[/s] *) try match (u, cu) with - ("deg", "") -> Pprz.scale_of_units u "rad" (* implicit conversion to rad *) - | ("deg/s", "") -> Pprz.scale_of_units u "rad/s" (* implicit conversion to rad/s *) + ("deg", "") -> PprzLink.scale_of_units u "rad" (* implicit conversion to rad *) + | ("deg/s", "") -> PprzLink.scale_of_units u "rad/s" (* implicit conversion to rad/s *) | (_, "") -> failwith "Unit conversion error" (* code unit is not defined and no implicit conversion *) - | (_,_) -> Pprz.scale_of_units u cu (* try to convert *) + | (_,_) -> PprzLink.scale_of_units u cu (* try to convert *) with - Pprz.Unit_conversion_error s -> prerr_endline (sprintf "Unit conversion error: %s" s); flush stderr; exit 1 - | Pprz.Unknown_conversion (su, scu) -> prerr_endline (sprintf "Warning: unknown unit conversion: from %s to %s" su scu); flush stderr; failwith "Unknown unit conversion" + PprzLink.Unit_conversion_error s -> prerr_endline (sprintf "Unit conversion error: %s" s); flush stderr; exit 1 + | PprzLink.Unknown_conversion (su, scu) -> prerr_endline (sprintf "Warning: unknown unit conversion: from %s to %s" su scu); flush stderr; failwith "Unknown unit conversion" | _ -> failwith "Unit conversion error" let code_value = fun section s -> diff --git a/sw/simulator/gaia.ml b/sw/simulator/gaia.ml index 6f662787b4..7df1c2c806 100644 --- a/sw/simulator/gaia.ml +++ b/sw/simulator/gaia.ml @@ -28,7 +28,7 @@ open Latlong let my_id = "gaia" let sending_period = 5000 (* ms *) -module Ground_Pprz = Pprz.Messages(struct let name = "ground" end) +module Ground_Pprz = PprzLink.Messages(struct let name = "ground" end) let ivy_bus = ref Defivybus.default_ivy_bus let time_scale = ref 1. @@ -72,12 +72,12 @@ let _ = and wind_north = -. wind_speed *. sin wind_dir_rad in let wind_up = wind_up_adj#value in - [ "wind_east", Pprz.Float wind_east; - "wind_north", Pprz.Float wind_north; - "wind_up", Pprz.Float wind_up; - "ir_contrast", Pprz.Float infrared_contrast_adj#value; - "time_scale", Pprz.Float time_scale_adj#value; - "gps_availability", Pprz.Int (if gps_sa#active then 0 else 1) + [ "wind_east", PprzLink.Float wind_east; + "wind_north", PprzLink.Float wind_north; + "wind_up", PprzLink.Float wind_up; + "ir_contrast", PprzLink.Float infrared_contrast_adj#value; + "time_scale", PprzLink.Float time_scale_adj#value; + "gps_availability", PprzLink.Int (if gps_sa#active then 0 else 1) ] in let world_send = fun () -> Ground_Pprz.message_send my_id "WORLD_ENV" (world_values []) in diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.c b/sw/simulator/nps/nps_autopilot_rotorcraft.c index 823a7099a0..b3563cf196 100644 --- a/sw/simulator/nps/nps_autopilot_rotorcraft.c +++ b/sw/simulator/nps/nps_autopilot_rotorcraft.c @@ -39,7 +39,7 @@ #include "subsystems/abi.h" -#include "messages.h" +#include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" // for datalink_time hack diff --git a/sw/simulator/nps/nps_ivy.c b/sw/simulator/nps/nps_ivy.c index 69d5bc7589..1a1b9c7354 100644 --- a/sw/simulator/nps/nps_ivy.c +++ b/sw/simulator/nps/nps_ivy.c @@ -85,7 +85,7 @@ static void on_WORLD_ENV(IvyClientPtr app __attribute__((unused)), } #include "generated/settings.h" -#include "dl_protocol.h" +#include "pprzlink/dl_protocol.h" #include "subsystems/datalink/downlink.h" static void on_DL_SETTING(IvyClientPtr app __attribute__((unused)), void *user_data __attribute__((unused)), diff --git a/sw/simulator/sim.ml b/sw/simulator/sim.ml index 623e8f7608..ee0b7d8789 100644 --- a/sw/simulator/sim.ml +++ b/sw/simulator/sim.ml @@ -26,7 +26,7 @@ open Printf open Stdlib open Latlong -module Ground_Pprz = Pprz.Messages(struct let name = "ground" end) +module Ground_Pprz = PprzLink.Messages(struct let name = "ground" end) let float_attrib xml a = float_of_string (ExtXml.attrib xml a) @@ -159,12 +159,12 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct and gps_availability = ref 1 in let world_update = fun _ vs -> - gps_availability := Pprz.int_assoc "gps_availability" vs; - wind_x := Pprz.float_assoc "wind_east" vs; - wind_y := Pprz.float_assoc "wind_north" vs; - wind_z := Pprz.float_assoc "wind_up" vs; - infrared_contrast := Pprz.float_assoc "ir_contrast" vs; - time_scale#set_value (Pprz.float_assoc "time_scale" vs) + gps_availability := PprzLink.int_assoc "gps_availability" vs; + wind_x := PprzLink.float_assoc "wind_east" vs; + wind_y := PprzLink.float_assoc "wind_north" vs; + wind_z := PprzLink.float_assoc "wind_up" vs; + infrared_contrast := PprzLink.float_assoc "ir_contrast" vs; + time_scale#set_value (PprzLink.float_assoc "time_scale" vs) in let ask_for_world_env = fun () -> @@ -173,7 +173,7 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct let gps_sol = compute_gps_state (x,y,z) (FlightModel.get_time !state) in - let float = fun f -> Pprz.Float f in + let float = fun f -> PprzLink.Float f in let values = ["east", float x; "north", float y; "up", float z; "lat", float ((Rad>>Deg)gps_sol.Gps.wgs84.posn_lat); "long", float ((Rad>>Deg)gps_sol.Gps.wgs84.posn_long); diff --git a/sw/simulator/sitl.ml b/sw/simulator/sitl.ml index b7cb05924e..f260a5188e 100644 --- a/sw/simulator/sitl.ml +++ b/sw/simulator/sitl.ml @@ -24,8 +24,8 @@ open Printf -module Ground_Pprz = Pprz.Messages(struct let name = "ground" end) -module Dl_Pprz = Pprz.Messages(struct let name = "datalink" end) +module Ground_Pprz = PprzLink.Messages(struct let name = "ground" end) +module Dl_Pprz = PprzLink.Messages(struct let name = "datalink" end) let ground_id = 0 (* cf tmtc/link.ml *) @@ -178,11 +178,11 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct let set = fun () -> let msg_id, _ = Dl_Pprz.message_of_name name in let s = Dl_Pprz.payload_of_values msg_id ground_id vs in - set_message (Serial.string_of_payload s) in - let ac_id = Pprz.int_assoc "ac_id" vs in + set_message (Protocol.string_of_payload s) in + let ac_id = PprzLink.int_assoc "ac_id" vs in match link_mode with - Pprz.Forwarded when ac_id = !my_id -> if dl_button#active then set () - | Pprz.Broadcasted -> if dl_button#active then set () + PprzLink.Forwarded when ac_id = !my_id -> if dl_button#active then set () + | PprzLink.Broadcasted -> if dl_button#active then set () | _ -> () let message_bind = fun name link_mode -> @@ -198,8 +198,8 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct (* Forward or broacast messages according to "link" mode *) Hashtbl.iter (fun _m_id msg -> - match msg.Pprz.link with - Some x -> message_bind msg.Pprz.name x + match msg.PprzLink.link with + Some x -> message_bind msg.PprzLink.name x | _ -> ()) Dl_Pprz.messages;; diff --git a/sw/supervision/Makefile b/sw/supervision/Makefile index 35153682b1..d0535ebf30 100644 --- a/sw/supervision/Makefile +++ b/sw/supervision/Makefile @@ -39,7 +39,7 @@ endif INCLUDES = XPKG = -package pprz.xlib,$(LABLGTK2GNOMEUI) -XLINKPKG = $(XPKG) -linkpkg -dllpath-pkg pprz.xlib +XLINKPKG = $(XPKG) -linkpkg -dllpath-pkg pprz.xlib,pprzlink PAPARAZZICENTERCMO = gtk_pc.cmo gtk_process.cmo pc_common.cmo pc_control_panel.cmo pc_aircraft.cmo paparazzicenter.cmo diff --git a/sw/tools/Makefile b/sw/tools/Makefile index 1565800749..178389bf3e 100644 --- a/sw/tools/Makefile +++ b/sw/tools/Makefile @@ -28,7 +28,7 @@ include ../Makefile.ocaml INCLUDES = PKG = -package pprz -LINKPKG = $(PKG) -linkpkg -dllpath-pkg pprz +LINKPKG = $(PKG) -linkpkg -dllpath-pkg pprz,pprzlink all: find_free_msg_id.out mergelogs diff --git a/sw/tools/bluegiga_usb_dongle/main.c b/sw/tools/bluegiga_usb_dongle/main.c index e098263059..458ac0f1fa 100644 --- a/sw/tools/bluegiga_usb_dongle/main.c +++ b/sw/tools/bluegiga_usb_dongle/main.c @@ -134,6 +134,7 @@ FUNCTION ANALYSIS: // uncomment the following line to show outgoing/incoming BGAPI packet data //#define DEBUG +#define DEBUG_BROADCAST // timeout for serial port read operations #define UART_TIMEOUT 1000 @@ -166,7 +167,7 @@ int count[8] = {0, 0, 0, 0, 0, 0, 0, 0}, send_count[8] = {0, 0, 0, 0, 0, 0, 0, 0 int rssi_count = 0; int last0 = 0, last1 = 0; -uint8_t connection_interval = 10; // connection interval in ms +float connection_interval = 10.; // connection interval in ms int ac_id[8] = { -1, -1, -1, -1, -1, -1, -1, -1}; @@ -182,6 +183,7 @@ unsigned int send_extract_idx[8] = {0, 0, 0, 0, 0, 0, 0, 0}; int connected_devices = 0; //bd_addr found_devices[MAX_DEVICES]; int connected[] = {0, 0, 0, 0, 0, 0, 0, 0}; +bd_addr connected_addr[MAX_DEVICES]; int connect_all = 0; uint8 MAC_ADDR[] = {0x00, 0x00, 0x2d, 0x80, 0x07, 0x00}; @@ -190,6 +192,8 @@ uint8 MAC_ADDR[] = {0x00, 0x00, 0x2d, 0x80, 0x07, 0x00}; // {0x00,0x00,0x2d,0x80,0x07,0x00}; // beginning of all modules adresses +FILE* rssi_fp = NULL; + // list all possible pending actions enum actions { action_none, @@ -197,7 +201,8 @@ enum actions { action_connect, action_info, action_connect_all, - action_broadcast + action_broadcast, + action_broadcast_connect }; enum actions action = action_none; @@ -312,6 +317,19 @@ void print_bdaddr(bd_addr bdaddr) bdaddr.addr[0]); } +/** + * Copy Bluetooth MAC address in hexadecimal notation + * + * @param dst Bluetooth MAC address, destination + * @param src Bluetooth MAC address, source + */ +void cpy_bdaddr(uint8_t* dst, const uint8_t* src) +{ + uint8_t i = 0; + for(i = 0; i < 6; i++) + dst[i] = src[i]; +} + /** * Display raw BGAPI packet in hexadecimal notation * @@ -344,7 +362,7 @@ void send_api_packet(uint8 len1, uint8 *data1, uint16 len2, uint8 *data2) { #ifdef DEBUG // display outgoing BGAPI packet - print_raw_packet((struct ble_header *)data1, data2, 1); +// print_raw_packet((struct ble_header *)data1, data2); #endif if (uart_tx(len1, data1) || uart_tx(len2, data2)) { // uart_tx returns non-zero on failure @@ -386,7 +404,7 @@ int read_api_packet(int timeout_ms) #ifdef DEBUG // display incoming BGAPI packet - print_raw_packet(&hdr, data, 0); + print_raw_packet(&hdr, data); #endif if (!msg) { @@ -446,26 +464,41 @@ void ble_rsp_system_get_info(const struct ble_msg_system_get_info_rsp_t *msg) */ void ble_evt_gap_scan_response(const struct ble_msg_gap_scan_response_evt_t *msg) { - if (action == action_broadcast) { - fprintf(stderr, "advertisement from: "); - print_bdaddr(msg->sender); - fprintf(stderr, " data: "); - int i; - for (i = 0; i < msg->data.len; i++) { + +#ifdef DEBUG_BROADCAST + if(cmp_addr(msg->sender.addr, MAC_ADDR) >= 3 )// && msg->sender.addr[0] == 0xdf) + { + gettimeofday(&tm, NULL); //Time zone struct is obsolete, hence NULL + mytime = (double)tm.tv_sec + (double)tm.tv_usec / 1000000.0; + fprintf(stderr, "%f %x %d, ", mytime, msg->sender.addr[0], msg->rssi); + uint8_t i = 0; + for(i = 0; i < msg->data.len; i++) fprintf(stderr, "%02x ", msg->data.data[i]); - } fprintf(stderr, "\n"); + } +#endif + + if(rssi_fp) + { + fprintf(rssi_fp, "%f %d %d\n", mytime, msg->sender.addr[0], msg->rssi); + fflush(rssi_fp); + } + + if (action == action_broadcast) { if (sock[0]) - sendto(sock[0], msg->data.data, msg->data.len, MSG_DONTWAIT, + sendto(sock[0], msg->data.data+13, msg->data.len-13, MSG_DONTWAIT, (struct sockaddr *)&send_addr[0], sizeof(struct sockaddr)); - } else { + //printf("first: %02x, last: %02x\n", msg->data.data[13], msg->data.data[msg->data.len]); + } + + if (action != action_broadcast) { uint8_t i, j; char *name = NULL; // Check if this device already found, if not add to the list if (!connect_all) { - fprintf(stderr, "New device found: "); + //fprintf(stderr, "New device found: "); // Parse data for (i = 0; i < msg->data.len;) { @@ -535,7 +568,15 @@ void ble_evt_gap_scan_response(const struct ble_msg_gap_scan_response_evt_t *msg } // automatically connect if responding device has appropriate mac address header - if (connect_all && cmp_addr(msg->sender.addr, MAC_ADDR) >= 4) { + // check if bluegiga drone and connectable + if (connect_all && msg->packet_type == 0 && cmp_addr(msg->sender.addr, MAC_ADDR) >= 3) { + uint8 i = 0; + while(i++ < MAX_DEVICES) + { + if (!cmp_addr(msg->sender.addr, connected_addr[i].addr)) + return; + } + fprintf(stderr, "Trying to connect to "); print_bdaddr(msg->sender); fprintf(stderr, "\n"); //change_state(state_connecting); // connection interval unit 1.25ms @@ -575,6 +616,7 @@ void ble_evt_connection_status(const struct ble_msg_connection_status_evt_t *msg // Connection request completed else if (msg->flags & connection_completed) { if (msg->connection + 1 > connected_devices) { connected_devices++; } + cpy_bdaddr(connected_addr[msg->connection].addr, msg->address.addr); //change_state(state_connected); connection_interval = msg->conn_interval * 1.25; fprintf(stderr, "Connected, nr: %d, connection interval: %d = %fms\n", msg->connection, msg->conn_interval, @@ -612,13 +654,16 @@ void ble_evt_connection_status(const struct ble_msg_connection_status_evt_t *msg change_state(state_listening_measurements); enable_indications(msg->connection, drone_handle_configuration); if (connect_all) { - ble_cmd_gap_discover(gap_discover_generic); + ble_cmd_gap_discover(gap_discover_generic); } } // Find primary services else { change_state(state_finding_services); ble_cmd_attclient_read_by_group_type(msg->connection, FIRST_HANDLE, LAST_HANDLE, 2, primary_service_uuid); + if (connect_all) { + ble_cmd_gap_discover(gap_discover_generic); + } } } } @@ -677,9 +722,6 @@ void ble_evt_attclient_procedure_completed(const struct ble_msg_attclient_proced else { change_state(state_listening_measurements); enable_indications(msg->connection, drone_handle_configuration); - if (connect_all) { - ble_cmd_gap_discover(gap_discover_generic); - } } } @@ -708,6 +750,13 @@ void ble_evt_attclient_find_information_found(const struct ble_msg_attclient_fin drone_handle_measurement = msg->chrhandle; } else if (uuid == DRONE_BROADCAST_UUID) { drone_handle_broadcast = msg->chrhandle; + + // Handle for Drone Data configuration already known + if (action == action_broadcast_connect) { + unsigned char var[] = {1}; + printf("Request sent: %d\n", var[0]); + ble_cmd_attclient_attribute_write(msg->connection, drone_handle_broadcast, 1, &var); + } } } } @@ -749,6 +798,8 @@ void ble_evt_attclient_attribute_value(const struct ble_msg_attclient_attribute_ if (sock[msg->connection]) sendto(sock[msg->connection], msg->value.data, msg->value.len, MSG_DONTWAIT, (struct sockaddr *)&send_addr[msg->connection], sizeof(struct sockaddr)); + //printf("%02x %02x %02x %02x\n", msg->value.data[0], msg->value.data[1], msg->value.data[2], msg->value.data[3]); + //printf("%d\n", msg->value.len); } @@ -824,12 +875,19 @@ void *send_msg() // fprintf(stderr,"Long msg: %d, buff size: %d\n", bt_msg_len, diff); //ble_cmd_attclient_attribute_write(device, drone_handle_measurement, bt_msg_len[device], &data_buf[device][extract_idx[device]]); - ble_cmd_attclient_write_command(device, drone_handle_measurement, bt_msg_len, &data_buf[device][extract_idx[device]]); - extract_idx[device] = (extract_idx[device] + bt_msg_len) % BUF_SIZE; + uint16_t i = 0; + unsigned char buf[19]; + for (i = 0; i < bt_msg_len; i++) + { + buf[i] = data_buf[device][extract_idx[device]]; + extract_idx[device] = (extract_idx[device] + 1) % BUF_SIZE; + } + + ble_cmd_attclient_write_command(device, drone_handle_measurement, bt_msg_len, buf); } device++; } // next device - usleep(connection_interval * 1000); // send messages at max intervals of the connection interval + usleep(connection_interval * 1000*2); // send messages at max intervals of the connection interval, 2 safety factor } // repeat } pthread_exit(NULL); @@ -858,18 +916,21 @@ void *recv_paparazzi_comms() if (connected[device] && sock[device]) { bytes_recv = recvfrom(sock[device], recv_data, BUF_SIZE, MSG_DONTWAIT, (struct sockaddr *)&rec_addr[device], (socklen_t *)&sin_size); - if (bytes_recv > 0) { // TODO: can overtake extract! + // ensure we don't overtake reading + if (bytes_recv > 0 && bytes_recv - 1 <= (extract_idx[device] - insert_idx[device] - 1 + BUF_SIZE) % BUF_SIZE ) { + uint16_t i = 0; + for (i = 0; i < bytes_recv; i++){ + data_buf[device][insert_idx[device]] = recv_data[i]; + insert_idx[device] = (insert_idx[device] + 1) % BUF_SIZE; + } send_count[device] += bytes_recv; - memcpy(&data_buf[device][insert_idx[device]], recv_data, bytes_recv); - - insert_idx[device] = (insert_idx[device] + bytes_recv) % BUF_SIZE; } } device++; } } } - usleep(20000); // assuming connection interval 10ms, give a bit of overhead + usleep(connection_interval * 1000 * 2); // assuming connection interval 10ms, give a bit of overhead } pthread_exit(NULL); } @@ -918,7 +979,7 @@ int main(int argc, char *argv[]) action = action_scan; } else if (strcmp(argv[CLARG_ACTION], "info") == 0) { action = action_info; - } else if (strcmp(argv[CLARG_ACTION], "broadcast") == 0) { + } else if (strcmp(argv[CLARG_ACTION], "broadcast") == 0 || strcmp(argv[CLARG_ACTION], "broadcast_connect") == 0) { action = action_broadcast; if (argc > CLARG_ACTION + 2) { send_port = atoi(argv[CLARG_ACTION + 1]); @@ -927,6 +988,10 @@ int main(int argc, char *argv[]) usage(argv[0]); return 1; } + if (strcmp(argv[CLARG_ACTION], "broadcast_connect") == 0) { + connect_all = 1; + action = action_broadcast_connect; + } } else if (strcmp(argv[CLARG_ACTION], "all") == 0) { connect_all = 1; action = action_scan; @@ -968,6 +1033,23 @@ int main(int argc, char *argv[]) return 1; } + size_t i = 0; + for (i = 0; i < argc; i++) + { + if(strcmp(argv[i],"log") == 0){ + time_t timev; + time(&timev); + char timedate[256]; + strftime(timedate, 256, "var/logs/%Y%m%d_%H%M%S.rssilog", localtime(&timev)); + rssi_fp = fopen(timedate, "w"); + if (!rssi_fp) + { + fprintf(stderr,"Unable to open file for logging: %s\n Make sure to run from paparazzi home\n", timedate); + return -1; + } + } + } + // set BGLib output function pointer to "send_api_packet" function bglib_output = send_api_packet; @@ -1003,8 +1085,12 @@ int main(int argc, char *argv[]) // get the mac address of the dongle ble_cmd_system_address_get(); + // advertise interval scales 625us, min, max, channels (0x07 = 3, 0x03 = 2, 0x04 = 1) + if (action == action_broadcast) + ble_cmd_gap_set_adv_parameters(0x20, 0x28, 0x07); + // Execute action - if (action == action_scan) { + if (action == action_scan || action == action_broadcast || action == action_broadcast_connect) { ble_cmd_gap_discover(gap_discover_generic); } else if (action == action_info) { ble_cmd_system_get_info(); @@ -1012,9 +1098,6 @@ int main(int argc, char *argv[]) fprintf(stderr, "Trying to connect\n"); change_state(state_connecting); ble_cmd_gap_connect_direct(&connect_addr, gap_address_type_public, 6, 16, 100, 0); - } else if (action == action_broadcast) { - ble_cmd_gap_set_adv_parameters(0x200, 0x200, - 0x07); // advertise interval scales 625us, min, max, channels (0x07 = 3, 0x03 = 2, 0x04 = 1) } pthread_create(&threads[0], NULL, send_msg, NULL); @@ -1032,4 +1115,7 @@ int main(int argc, char *argv[]) uart_close(); pthread_exit(NULL); + + if (rssi_fp) + fclose(rssi_fp); } diff --git a/sw/tools/calibration/calibrate.py b/sw/tools/calibration/calibrate.py index 74f15b0c37..fb28092b4f 100755 --- a/sw/tools/calibration/calibrate.py +++ b/sw/tools/calibration/calibrate.py @@ -44,9 +44,9 @@ def main(): parser.add_option("-p", "--plot", help="Show resulting plots", action="store_true", dest="plot") - parser.add_option("-a", "--auto_threshold", - help="Try to automatically determine noise threshold", - action="store_true", dest="auto_threshold") + parser.add_option("--noise_threshold", + help="specify noise threshold instead of automatically determining it", + action="store", dest="noise_threshold", default=0) parser.add_option("-v", "--verbose", action="store_true", dest="verbose") (options, args) = parser.parse_args() @@ -75,7 +75,7 @@ def main(): sensor_ref = 9.81 sensor_res = 10 noise_window = 20 - noise_threshold = 40 + noise_threshold = options.noise_threshold elif options.sensor == "MAG": sensor_ref = 1. sensor_res = 11 @@ -98,10 +98,12 @@ def main(): print("Error: all IMU_"+options.sensor+"_RAW measurements are zero!") sys.exit(1) - # estimate the noise threshold - # find the median of measurement vector lenght - if options.auto_threshold: - meas_median = scipy.median(scipy.array([scipy.linalg.norm(v) for v in measurements])) + # estimate the noise threshold if not explicitly given + if noise_threshold <= 0: + # mean over all measurements (flattended array) as approx neutral value + neutral = scipy.mean(measurements) + # find the median of measurement vector length after subtracting approximate neutral + meas_median = scipy.median(scipy.array([scipy.linalg.norm(v - neutral) for v in measurements])) # set noise threshold to be below 10% of that noise_threshold = meas_median * 0.1 if options.verbose: @@ -113,7 +115,8 @@ def main(): print("remaining "+str(len(flt_meas))+" after filtering") if len(flt_meas) == 0: print("Error: found zero IMU_" + options.sensor + "_RAW measurements for aircraft with id " + options.ac_id + - " in log file after filtering!\nMaybe try the --auto_threshold option.") + " in log file after filtering with noise threshold of " + noise_threshold + + "!\nMaybe try specifying manually with the --noise_threshold option.") if options.plot: calibration_utils.plot_measurements(options.sensor, measurements) sys.exit(1) diff --git a/sw/tools/doxygen_gen/gen_messages_doc.py b/sw/tools/doxygen_gen/gen_messages_doc.py index ed3fc7fd80..3c091a3fb2 100755 --- a/sw/tools/doxygen_gen/gen_messages_doc.py +++ b/sw/tools/doxygen_gen/gen_messages_doc.py @@ -91,7 +91,7 @@ if __name__ == '__main__': usage = "Usage: %prog [options]" + "\n" + "Run %prog --help to list the options." parser = OptionParser(usage) parser.add_option("-f", "--file", dest="file", - help="messages file to read [default: PAPARAZZI_HOME/conf/messages.xml]", metavar="MESSAGES.XML") + help="messages file to read [default: PAPARAZZI_HOME/sw/ext/pprzlink/message_definitions/v1.0/messages.xml]", metavar="MESSAGES.XML") parser.add_option("-o", "--outputdir", dest="output_dir", help="write output to DIR [default: PAPARAZZI_HOME/doc/manual/generated]", metavar="DIR") parser.add_option("-p", "--parents", @@ -112,7 +112,7 @@ if __name__ == '__main__': if options.file: messages_file = options.file else: - messages_file = os.path.join(paparazzi_home, "conf/messages.xml") + messages_file = os.path.join(paparazzi_home, "sw/ext/pprzlink/message_definitions/v1.0/messages.xml") if not os.path.isfile(messages_file): print("Messages file " + messages_file + " not found.") sys.exit(1) diff --git a/sw/tools/find_free_msg_id.ml b/sw/tools/find_free_msg_id.ml index c0f1b11072..be85f9391a 100644 --- a/sw/tools/find_free_msg_id.ml +++ b/sw/tools/find_free_msg_id.ml @@ -32,7 +32,7 @@ (** FIXME: Get file names with Arg.parse *) let (//) = Filename.concat -let messages_xml = Env.paparazzi_home // "conf" // "messages.xml" +let messages_xml = Env.paparazzi_home // "var" // "messages.xml" let nb_msg = 255 diff --git a/sw/tools/generators/Makefile b/sw/tools/generators/Makefile index 33120e4ab3..caad70cbc3 100644 --- a/sw/tools/generators/Makefile +++ b/sw/tools/generators/Makefile @@ -26,9 +26,9 @@ include ../../Makefile.ocaml INCLUDES = PKG = -package pprz -LINKPKG = $(PKG) -linkpkg -dllpath-pkg pprz +LINKPKG = $(PKG) -linkpkg -dllpath-pkg pprz,pprzlink -all: gen_aircraft.out gen_airframe.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 +all: gen_aircraft.out gen_airframe.out gen_ubx.out gen_mtk.out gen_flight_plan.out gen_radio.out gen_periodic.out gen_settings.out gen_xsens.out gen_modules.out gen_autopilot.out gen_abi.out gen_srtm.out gen_flight_plan.out : gen_flight_plan.cmo $(LIBPPRZCMA) @echo OL $@ diff --git a/sw/tools/generators/gen_aircraft.ml b/sw/tools/generators/gen_aircraft.ml index 6ab3057ad2..2ad9df9a93 100644 --- a/sw/tools/generators/gen_aircraft.ml +++ b/sw/tools/generators/gen_aircraft.ml @@ -107,7 +107,7 @@ let module_xml2mk = fun f target m -> let section = let targets = Gen_common.targets_of_field section Env.default_module_targets in if Gen_common.test_targets target targets then section else Xml.Element ("makefile", [], []) - in + in Xml.iter (fun field -> match String.lowercase (Xml.tag field) with @@ -163,7 +163,7 @@ let subsystem_xml2mk = fun f firmware s -> fprintf f "\n# -subsystem: '%s'\n" name; let s_config, rest = ExtXml.partition_tag "configure" (Xml.children s) in let s_defines, _ = ExtXml.partition_tag "define" rest in - List.iter (configure_xml2mk f) s_config; + (*List.iter (configure_xml2mk f) s_config;*) List.iter (fun def -> define_xml2mk f def) s_defines; (* include subsystem *) (* TODO test if file exists with the generator ? *) let s_name = name ^ s_type ^ ".makefile" in @@ -174,6 +174,10 @@ let subsystem_xml2mk = fun f firmware s -> fprintf f "\tinclude $(CFG_SHARED)/%s\n" s_name; fprintf f "endif\n" +let subsystem_configure_xml2mk = fun f s -> + let s_config, _ = ExtXml.partition_tag "configure" (Xml.children s) in + List.iter (configure_xml2mk f) s_config + let mod_or_subsys_xml2mk = fun f global_targets firmware target xml -> try let m = Gen_common.get_module xml global_targets in @@ -208,7 +212,11 @@ let parse_firmware = fun makefile_ac ac_xml firmware -> end; List.iter (configure_xml2mk makefile_ac) config; List.iter (configure_xml2mk makefile_ac) t_config; - fprintf makefile_ac "include $(PAPARAZZI_SRC)/conf/boards/%s.makefile\n" (Xml.attrib target "board"); + List.iter (subsystem_configure_xml2mk makefile_ac) subsystems; + List.iter (subsystem_configure_xml2mk makefile_ac) t_subsystems; + List.iter (subsystem_configure_xml2mk makefile_ac) mods; + List.iter (subsystem_configure_xml2mk makefile_ac) t_mods; + fprintf makefile_ac "\ninclude $(PAPARAZZI_SRC)/conf/boards/%s.makefile\n" (Xml.attrib target "board"); fprintf makefile_ac "include $(PAPARAZZI_SRC)/conf/firmwares/%s.makefile\n" (Xml.attrib firmware "name"); List.iter (fun def -> define_xml2mk makefile_ac def) defines; List.iter (fun def -> define_xml2mk makefile_ac def) t_defines; @@ -329,7 +337,7 @@ let () = let configuration = make_element "configuration" [] - [ make_element "conf" [] [conf_aircraft]; Pprz.messages_xml () ] in + [ make_element "conf" [] [conf_aircraft]; PprzLink.messages_xml () ] in let conf_aircraft_file = aircraft_conf_dir // "conf_aircraft.xml" in let f = open_out conf_aircraft_file in Printf.fprintf f "%s\n" (ExtXml.to_string_fmt configuration); diff --git a/sw/tools/generators/gen_airframe.ml b/sw/tools/generators/gen_airframe.ml index d2fdbb48c7..45d5c84b97 100644 --- a/sw/tools/generators/gen_airframe.ml +++ b/sw/tools/generators/gen_airframe.ml @@ -86,14 +86,14 @@ let convert_value_with_code_unit_coef_of_xml = function xml -> (* if unit equals code unit, don't convert as that would always result in a float *) if u = cu then failwith "Not converting"; (* default value for code_unit is rad[/s] when unit is deg[/s] *) - let conv = try Pprz.scale_of_units u cu with - | Pprz.Unit_conversion_error s -> + let conv = try PprzLink.scale_of_units u cu with + | PprzLink.Unit_conversion_error s -> eprintf "Unit conversion error: %s\n%!" s; exit 1 - | Pprz.Unknown_conversion (su, scu) -> + | PprzLink.Unknown_conversion (su, scu) -> eprintf "Warning: unknown unit conversion: from %s to %s\n%!" su scu; failwith "Unknown unit conversion" - | Pprz.No_automatic_conversion _ | _ -> failwith "Unit conversion error" in + | PprzLink.No_automatic_conversion _ | _ -> failwith "Unit conversion error" in let v = try ExtXml.float_attrib xml "value" with _ -> prerr_endline (sprintf "Error: Unit conversion of parameter %s impossible because '%s' is not a float" (Xml.attrib xml "name") (Xml.attrib xml "value")); flush stderr; exit 1 in diff --git a/sw/tools/generators/gen_messages.ml b/sw/tools/generators/gen_messages.ml deleted file mode 100644 index 16a918ce5b..0000000000 --- a/sw/tools/generators/gen_messages.ml +++ /dev/null @@ -1,396 +0,0 @@ -(* - * XML preprocessing of messages.xml for downlink protocol - * - * Copyright (C) 2003-2008 ENAC, Pascal Brisset, 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. - * - *) - -open Printf - -type format = string - -type _type = - Basic of string - | Array of string * string - | FixedArray of string * string * int - -let c_type = fun format -> - match format with - "Float" -> "float" - | "Double" -> "double" - | "Int32" -> "int32_t" - | "Int16" -> "int16_t" - | "Int8" -> "int8_t" - | "Uint32" -> "uint32_t" - | "Uint16" -> "uint16_t" - | "Uint8" -> "uint8_t" - | "Char" -> "char" - | _ -> failwith (sprintf "gen_messages.c_type: unknown format '%s'" format) - -let dl_type = fun format -> - match format with - "Float" -> "DL_TYPE_FLOAT" - | "Double" -> "DL_TYPE_DOUBLE" - | "Int32" -> "DL_TYPE_INT32" - | "Int16" -> "DL_TYPE_INT16" - | "Int8" -> "DL_TYPE_INT8" - | "Uint32" -> "DL_TYPE_UINT32" - | "Uint16" -> "DL_TYPE_UINT16" - | "Uint8" -> "DL_TYPE_UINT8" - | "Char" -> "DL_TYPE_CHAR" - | _ -> failwith (sprintf "gen_messages.dl_type: unknown format '%s'" format) - -type field = _type * string * format option - -type fields = field list - -type message = { - name : string; - id : int; - period : float option; - fields : fields -} - -module Syntax = struct - (** Parse a type name and returns a _type value *) - let parse_type = fun t varname -> - try - let type_parts = Str.full_split (Str.regexp "[][]") t in - match type_parts with - | [Str.Text ty] -> Basic ty - | [Str.Text ty; Str.Delim "["; Str.Delim "]"] -> Array (ty, varname) - | [Str.Text ty; Str.Delim "["; Str.Text len ; Str.Delim "]"] -> FixedArray (ty, varname, int_of_string len) - | _ -> failwith "Gen_messages: not a valid field type" - with - | Failure fail -> failwith("Gen_messages: not a valid array length") - - let length_name = fun s -> "nb_"^s - - let assoc_types t = - try - List.assoc t Pprz.types - with - Not_found -> - failwith (sprintf "Error: '%s' unknown type" t) - - let rec sizeof = function - Basic t -> string_of_int (assoc_types t).Pprz.size - | Array (t, varname) -> sprintf "1+%s*%s" (length_name varname) (sizeof (Basic t)) - | FixedArray (t, varname, len) -> sprintf "0+%d*%s" len (sizeof (Basic t)) - - let rec nameof = function - Basic t -> String.capitalize t - | Array _ -> failwith "nameof" - | FixedArray _ -> failwith "nameof" - - (** Translates a "message" XML element into a value of the 'message' type *) - let struct_of_xml = fun xml -> - let name = ExtXml.attrib xml "name" - and id = ExtXml.int_attrib xml "id" - and period = try Some (ExtXml.float_attrib xml "period") with _ -> None - and fields = - List.map (fun field -> - let id = ExtXml.attrib field "name" - and type_name = ExtXml.attrib field "type" - and fmt = try Some (Xml.attrib field "format") with _ -> None in - let _type = parse_type type_name id in - (_type, id, fmt)) - (List.filter (fun t -> compare (Xml.tag t) "field" = 0) (Xml.children xml)) in - { id=id; name = name; period = period; fields = fields } - - let check_single_ids = fun msgs -> - let tab = Array.make 256 false - and last_id = ref 0 in - List.iter (fun msg -> - if tab.(msg.id) then - failwith (sprintf "Duplicated message id: %d" msg.id); - if msg.id < !last_id then - fprintf stderr "Warning: unsorted id: %d\n%!" msg.id; - last_id := msg.id; - tab.(msg.id) <- true) - msgs - - (** Translates one class of a XML message file into a list of messages *) - let read = fun filename class_ -> - let xml = Xml.parse_file filename in - try - let xml_class = ExtXml.child ~select:(fun x -> Xml.attrib x "name" = class_) xml "msg_class" in - let msgs = List.map struct_of_xml (Xml.children xml_class) in - check_single_ids msgs; - msgs - with - Not_found -> failwith (sprintf "No msg_class '%s' found" class_) -end (* module Suntax *) - - -(** Pretty printer of C macros for sending and parsing messages *) -module Gen_onboard = struct - let print_field = fun h (t, name, (_f: format option)) -> - match t with - Basic _ -> - fprintf h "\t trans->put_bytes(trans->impl, dev, %s, DL_FORMAT_SCALAR, %s, (void *) _%s);\n" (dl_type (Syntax.nameof t)) (Syntax.sizeof t) name - | Array (t, varname) -> - let _s = Syntax.sizeof (Basic t) in - fprintf h "\t trans->put_bytes(trans->impl, dev, DL_TYPE_ARRAY_LENGTH, DL_FORMAT_SCALAR, 1, (void *) &%s);\n" (Syntax.length_name varname); - fprintf h "\t trans->put_bytes(trans->impl, dev, %s, DL_FORMAT_ARRAY, %s * %s, (void *) _%s);\n" (dl_type (Syntax.nameof (Basic t))) (Syntax.sizeof (Basic t)) (Syntax.length_name varname) name - | FixedArray (t, varname, len) -> - let _s = Syntax.sizeof (Basic t) in - fprintf h "\t trans->put_bytes(trans->impl, dev, %s, DL_FORMAT_ARRAY, %s * %d, (void *) _%s);\n" (dl_type (Syntax.nameof (Basic t))) (Syntax.sizeof (Basic t)) len name - - let print_macro_param h = function - (Array _, s, _) -> fprintf h "%s, %s" (Syntax.length_name s) s - | (FixedArray _, s, _) -> fprintf h "%s" s - | (_, s, _) -> fprintf h "%s" s - - let print_macro_parameters h = function - [] -> () - | f::fields -> - fprintf h ", "; - print_macro_param h f; - List.iter (fun f -> fprintf h ", "; print_macro_param h f) fields - - let print_unused_param = fun unused -> - if unused then " __attribute__((unused))" else "" - - let print_fun_param ?(unused=false) h = function - (Array (t, _), s, _) -> fprintf h "uint8_t %s%s, %s *_%s%s" (Syntax.length_name s) (print_unused_param unused) (c_type (Syntax.nameof (Basic t))) s (print_unused_param unused) - | (FixedArray (t, _, _), s, _) -> fprintf h "%s *_%s%s" (c_type (Syntax.nameof (Basic t))) s (print_unused_param unused) - | (t, s, _) -> fprintf h "%s *_%s%s" (c_type (Syntax.nameof t)) s (print_unused_param unused) - - let print_function_parameters ?(unused=false) h = function - [] -> () - | f::fields -> - fprintf h ", "; - print_fun_param ~unused h f; - List.iter (fun f -> fprintf h ", "; print_fun_param ~unused h f) fields - - let rec size_fields = fun fields size -> - match fields with - [] -> size - | (t, _, _)::fields -> size_fields fields (size ^"+"^Syntax.sizeof t) - - let size_of_message = fun m -> size_fields m.fields "0" - - let estimated_size_of_message = fun m -> - try - List.fold_right - (fun (t, _, _) r -> int_of_string (Syntax.sizeof t)+r) - m.fields - 0 - with - Failure "int_of_string" -> 0 - - let print_downlink_macro = fun h {name=s; fields = fields} -> - (* Macros for backward compatibility *) - fprintf h "#define DOWNLINK_SEND_%s(_trans, _dev" s; - print_macro_parameters h fields; - fprintf h ") "; - fprintf h "pprz_msg_send_%s(&((_trans).trans_tx), &((_dev).device), AC_ID" s; - print_macro_parameters h fields; - fprintf h ")\n"; - (* Print message_send functions *) - fprintf h "static inline void pprz_msg_send_%s(struct transport_tx *trans, struct link_device *dev, uint8_t ac_id" s; - print_function_parameters h fields; - fprintf h ") {\n"; - let size = (size_fields fields "0") in - fprintf h "\tif (trans->check_available_space(trans->impl, dev, trans->size_of(trans->impl, %s +2 /* msg header overhead */))) {\n" size; - fprintf h "\t trans->count_bytes(trans->impl, dev, trans->size_of(trans->impl, %s +2 /* msg header overhead */));\n" size; - fprintf h "\t trans->start_message(trans->impl, dev, %s +2 /* msg header overhead */);\n" size; - fprintf h "\t trans->put_bytes(trans->impl, dev, DL_TYPE_UINT8, DL_FORMAT_SCALAR, 1, &ac_id);\n"; - fprintf h "\t trans->put_named_byte(trans->impl, dev, DL_TYPE_UINT8, DL_FORMAT_SCALAR, DL_%s, \"%s\");\n" s s; - List.iter (print_field h) fields; - fprintf h "\t trans->end_message(trans->impl, dev);\n"; - fprintf h "\t} else\n"; - fprintf h "\t trans->overrun(trans->impl, dev);\n"; - fprintf h "}\n\n" - - let print_null_downlink_macro = fun h {name=s; fields = fields} -> - fprintf h "#define DOWNLINK_SEND_%s(_trans, _dev" s; - print_macro_parameters h fields; - fprintf h ") {}\n"; - fprintf h "static inline void pprz_msg_send_%s(struct transport_tx *trans __attribute__((unused)), struct link_device *dev __attribute__((unused)), uint8_t ac_id __attribute__((unused))" s; - print_function_parameters ~unused:true h fields; - fprintf h ") {}\n" - - (** Prints the messages ids *) - let print_enum = fun h class_ messages -> - List.iter (fun m -> - if m.id < 0 || m.id > 255 then begin - fprintf stderr "Error: message %s has id %d but should be between 0 and 255\n" m.name m.id; exit 1; - end - else begin - fprintf h "#define DL_%s %d\n" m.name m.id; - fprintf h "#define PPRZ_MSG_ID_%s %d\n" m.name m.id; - end - ) messages; - fprintf h "#define DL_MSG_%s_NB %d\n\n" class_ (List.length messages) - - (** Prints the table of the messages lengths *) - let print_lengths_array = fun h class_ messages -> - let sizes = List.map (fun m -> (m.id, size_of_message m)) messages in - let max_id = List.fold_right (fun (id, _m) x -> max x id) sizes min_int in - let n = max_id + 1 in - fprintf h "#define MSG_%s_LENGTHS {" class_; - for i = 0 to n - 1 do - fprintf h "%s," (try "(2+" ^ List.assoc i sizes^")" with Not_found -> "0") - done; - fprintf h "}\n\n"; - - (* Print a comment with the actual size (when not variable) *) - fprintf h "/*\n Size for non variable messages\n"; - - let sizes = - List.map - (fun m -> (estimated_size_of_message m, m.name)) - messages in - let sizes = List.sort (fun (s1,_) (s2,_) -> compare s2 s1) sizes in - - List.iter - (fun (s, id) -> fprintf h "%2d : %s\n" s id) - sizes; - fprintf h "*/\n" - - (** Prints the macros required to send a message *) - let print_downlink_macros = fun h class_ messages -> - print_enum h class_ messages; - (*print_lengths_array h class_ messages;*) - List.iter (print_downlink_macro h) messages - - let print_null_downlink_macros = fun h messages -> - List.iter (print_null_downlink_macro h) messages - - (** Prints the macro to get access to the fields of a received message *) - let print_get_macros = fun h check_alignment message -> - let msg_name = message.name in - let offset = ref Pprz.offset_fields in - - (** Prints the macro for one field, using the global [offset] ref *) - let parse_field = fun (_type, field_name, _format) -> - if !offset < 0 then - failwith "FIXME: No field allowed after an array field (print_get_macros)"; - (** Converts bytes into the required type *) - let typed = fun o pprz_type -> (* o for offset *) - let size = pprz_type.Pprz.size in - if check_alignment && o mod (min size 4) <> 0 then - failwith (sprintf "Wrong alignment of field '%s' in message '%s" field_name msg_name); - - match size with - 1 -> sprintf "(%s)(*((uint8_t*)_payload+%d))" pprz_type.Pprz.inttype o - | 2 -> sprintf "(%s)(*((uint8_t*)_payload+%d)|*((uint8_t*)_payload+%d+1)<<8)" pprz_type.Pprz.inttype o o - | 4 when pprz_type.Pprz.inttype = "float" -> - sprintf "({ union { uint32_t u; float f; } _f; _f.u = (uint32_t)(*((uint8_t*)_payload+%d)|*((uint8_t*)_payload+%d+1)<<8|((uint32_t)*((uint8_t*)_payload+%d+2))<<16|((uint32_t)*((uint8_t*)_payload+%d+3))<<24); _f.f; })" o o o o - | 8 when pprz_type.Pprz.inttype = "double" -> - let s = ref (sprintf "*((uint8_t*)_payload+%d)" o) in - for i = 1 to 7 do - s := !s ^ sprintf "|((uint64_t)*((uint8_t*)_payload+%d+%d))<<%d" o i (8*i) - done; - - sprintf "({ union { uint64_t u; double f; } _f; _f.u = (uint64_t)(%s); Swap32IfBigEndian(_f.u); _f.f; })" !s - | 4 -> - sprintf "(%s)(*((uint8_t*)_payload+%d)|*((uint8_t*)_payload+%d+1)<<8|((uint32_t)*((uint8_t*)_payload+%d+2))<<16|((uint32_t)*((uint8_t*)_payload+%d+3))<<24)" pprz_type.Pprz.inttype o o o o - | 8 -> - let s = ref (sprintf "(%s)(*((uint8_t*)_payload+%d)" pprz_type.Pprz.inttype o) in - for i = 1 to 7 do - s := !s ^ sprintf "|((uint64_t)*((uint8_t*)_payload+%d+%d))<<%d" o i (8*i) - done; - sprintf "%s)" !s - | _ -> failwith "unexpected size in Gen_messages.print_get_macros" in - - (** To be an array or not to be an array: *) - match _type with - Basic t -> - let pprz_type = Syntax.assoc_types t in - fprintf h "#define DL_%s_%s(_payload) (%s)\n" msg_name field_name (typed !offset pprz_type); - offset := !offset + pprz_type.Pprz.size - - | Array (t, _varname) -> - (** The macro to access to the length of the array *) - fprintf h "#define DL_%s_%s_length(_payload) (%s)\n" msg_name field_name (typed !offset (Syntax.assoc_types "uint8")); - incr offset; - - (** The macro to access to the array itself *) - let pprz_type = Syntax.assoc_types t in - if check_alignment && !offset mod (min pprz_type.Pprz.size 4) <> 0 then - failwith (sprintf "Wrong alignment of field '%s' in message '%s" field_name msg_name); - - fprintf h "#define DL_%s_%s(_payload) ((%s*)(_payload+%d))\n" msg_name field_name pprz_type.Pprz.inttype !offset; - offset := -1 (** Mark for no more fields *) - | FixedArray (t, _varname, len) -> - (** The macro to access to the length of the array *) - fprintf h "#define DL_%s_%s_length(_payload) (%d)\n" msg_name field_name len; - (** The macro to access to the array itself *) - let pprz_type = Syntax.assoc_types t in - if check_alignment && !offset mod (min pprz_type.Pprz.size 4) <> 0 then - failwith (sprintf "Wrong alignment of field '%s' in message '%s" field_name msg_name); - - fprintf h "#define DL_%s_%s(_payload) ((%s*)(_payload+%d))\n" msg_name field_name pprz_type.Pprz.inttype !offset; - offset := !offset + (pprz_type.Pprz.size*len) - in - - fprintf h "\n"; - (** Do it for all the fields of the message *) - List.iter parse_field message.fields - -end (* module Gen_onboard *) - - -(********************* Main **************************************************) -let () = - if Array.length Sys.argv <> 3 then begin - failwith (sprintf "Usage: %s <.xml file> " Sys.argv.(0)) - end; - - let filename = Sys.argv.(1) - and class_name = Sys.argv.(2) in - - try - let messages = Syntax.read filename class_name in - - let h = stdout in - - Printf.fprintf h "/* Automatically generated by gen_messages from %s */\n" filename; - Printf.fprintf h "/* Version %s */\n" (Env.get_paparazzi_version ()); - Printf.fprintf h "/* Please DO NOT EDIT */\n"; - - Printf.fprintf h "/* Macros to send and receive messages of class %s */\n" class_name; - Printf.fprintf h "#ifndef _VAR_MESSAGES_%s_H_\n" class_name; - Printf.fprintf h "#define _VAR_MESSAGES_%s_H_\n" class_name; - Printf.fprintf h "#include \"subsystems/datalink/transport.h\"\n"; - Printf.fprintf h "#include \"mcu_periph/link_device.h\"\n"; - - (** Macros for airborne downlink (sending) *) - if class_name = "telemetry" then begin (** FIXME *) - Printf.fprintf h "#if DOWNLINK\n" - end; - Gen_onboard.print_downlink_macros h class_name messages; - if class_name = "telemetry" then begin - Printf.fprintf h "#else // DOWNLINK\n"; - Gen_onboard.print_null_downlink_macros h messages; - Printf.fprintf h "#endif // DOWNLINK\n" - end; - - (** Macros for airborne datalink (receiving) *) - if class_name = "datalink" || class_name = "intermcu" then - List.iter (Gen_onboard.print_get_macros h true) messages; - - Printf.fprintf h "#endif // _VAR_MESSAGES_%s_H_\n" class_name - - with - Xml.Error (msg, pos) -> failwith (sprintf "%s:%d : %s\n" filename (Xml.line pos) (Xml.error_msg msg)) diff --git a/sw/tools/generators/gen_modules.ml b/sw/tools/generators/gen_modules.ml index 0df7b88bc0..17c02951a5 100644 --- a/sw/tools/generators/gen_modules.ml +++ b/sw/tools/generators/gen_modules.ml @@ -261,7 +261,7 @@ let print_event_functions = fun modules -> lprintf out_h "}\n" let print_datalink_functions = fun modules -> - lprintf out_h "\n#include \"messages.h\"\n"; + lprintf out_h "\n#include \"pprzlink/messages.h\"\n"; lprintf out_h "#include \"generated/airframe.h\"\n"; lprintf out_h "static inline void modules_parse_datalink(uint8_t msg_id __attribute__ ((unused))) {\n"; right (); diff --git a/sw/tools/generators/gen_ubx.ml b/sw/tools/generators/gen_ubx.ml index 51a01df2ef..3eebb21dfc 100644 --- a/sw/tools/generators/gen_ubx.ml +++ b/sw/tools/generators/gen_ubx.ml @@ -159,7 +159,7 @@ let _ = fprintf out "/* Generated by gen_ubx from %s */\n" xml_file; fprintf out "/* Please DO NOT EDIT */\n\n"; - fprintf out "#include \"mcu_periph/link_device.h\"\n\n"; + fprintf out "#include \"pprzlink/pprzlink_device.h\"\n\n"; fprintf out "#include \"subsystems/gps/gps_ubx.h\"\n\n"; define "UBX_SYNC1" "0xB5"; diff --git a/sw/tools/tcp_aircraft_server/tcp_aircraft_server.py b/sw/tools/tcp_aircraft_server/tcp_aircraft_server.py index b9443d6f4a..46f50442cd 100755 --- a/sw/tools/tcp_aircraft_server/tcp_aircraft_server.py +++ b/sw/tools/tcp_aircraft_server/tcp_aircraft_server.py @@ -38,7 +38,7 @@ class Server(ivy.IvyServer): print("server listening on {:d}".format(tcp_port)) self.transp = phoenix.pprz_transport.Transport(check_crc=False, debug=False) - self.protocol = phoenix.messages.Protocol(path=path.join(home_dir, "conf/messages.xml"), debug=True) + self.protocol = phoenix.messages.Protocol(path=path.join(home_dir, "var/messages.xml"), debug=True) self.start(bus) GObject.timeout_add(500, self.periodic, priority=GObject.PRIORITY_HIGH)