diff --git a/.travis.yml b/.travis.yml index 05b49bdd62..011ebf7e6a 100644 --- a/.travis.yml +++ b/.travis.yml @@ -11,7 +11,7 @@ install: before_script: cd conf && ln -s conf_tests.xml conf.xml && cd .. script: - make - - make run_tests J=AUTO TEST_FILES=examples/01_compile_all_test_targets.t + - PAPARAZZI_SRC=$PWD PAPARAZZI_HOME=$PWD J=AUTO prove tests/examples/ notifications: webhooks: diff --git a/Makefile.ac b/Makefile.ac index a2bef0cdc1..7e8ec2960a 100644 --- a/Makefile.ac +++ b/Makefile.ac @@ -124,7 +124,7 @@ makefile_ac: $(MAKEFILE_AC) $(AIRFRAME_H) : $(CONF)/$(AIRFRAME_XML) $(CONF_XML) $(AIRCRAFT_MD5) $(Q)test -d $(AC_GENERATED) || mkdir -p $(AC_GENERATED) - @echo GENERATE $@ + @echo GENERATE $@ from $(AIRFRAME_XML) $(eval $@_TMP := $(shell $(MKTEMP))) $(Q)$(GENERATORS)/gen_airframe.out $(AC_ID) $(AIRCRAFT) $(MD5SUM) $< > $($@_TMP) $(Q)mv $($@_TMP) $@ @@ -133,7 +133,7 @@ $(AIRFRAME_H) : $(CONF)/$(AIRFRAME_XML) $(CONF_XML) $(AIRCRAFT_MD5) $(RADIO_H) : $(CONF)/$(RADIO) $(CONF_XML) $(GENERATORS)/gen_radio.out $(Q)test -d $(AC_GENERATED) || mkdir -p $(AC_GENERATED) - @echo GENERATE $@ + @echo GENERATE $@ from $(RADIO) $(eval $@_TMP := $(shell $(MKTEMP))) $(Q)$(GENERATORS)/gen_radio.out $< > $($@_TMP) $(Q)mv $($@_TMP) $@ @@ -142,7 +142,7 @@ $(RADIO_H) : $(CONF)/$(RADIO) $(CONF_XML) $(GENERATORS)/gen_radio.out $(PERIODIC_H) : $(CONF)/$(AIRFRAME_XML) $(MESSAGES_XML) $(CONF_XML) $(CONF)/$(TELEMETRY) $(MAKEFILE_AC) $(Q)test -d $(AC_GENERATED) || mkdir -p $(AC_GENERATED) - @echo GENERATE $@ + @echo GENERATE $@ from $(TELEMETRY) $(eval $@_TMP := $(shell $(MKTEMP))) $(Q)$(GENERATORS)/gen_periodic.out $(CONF)/$(AIRFRAME_XML) $(MESSAGES_XML) $(CONF)/$(TELEMETRY) $(TELEMETRY_FREQUENCY) $(SETTINGS_TELEMETRY) > $($@_TMP) $(Q)mv $($@_TMP) $@ @@ -151,7 +151,7 @@ $(PERIODIC_H) : $(CONF)/$(AIRFRAME_XML) $(MESSAGES_XML) $(CONF_XML) $(CONF)/$(TE $(FLIGHT_PLAN_H) : $(CONF)/$(FLIGHT_PLAN) $(CONF_XML) $(GENERATORS)/gen_flight_plan.out $(Q)test -d $(AC_GENERATED) || mkdir -p $(AC_GENERATED) - @echo GENERATE $@ + @echo GENERATE $@ from $(FLIGHT_PLAN) $(eval $@_TMP := $(shell $(MKTEMP))) $(Q)$(GENERATORS)/gen_flight_plan.out $< > $($@_TMP) $(Q)mv $($@_TMP) $@ @@ -159,7 +159,7 @@ $(FLIGHT_PLAN_H) : $(CONF)/$(FLIGHT_PLAN) $(CONF_XML) $(GENERATORS)/gen_flight_p $(Q)cp $< $(AIRCRAFT_CONF_DIR)/flight_plans $(FLIGHT_PLAN_XML) : $(CONF)/$(FLIGHT_PLAN) $(CONF_XML) $(GENERATORS)/gen_flight_plan.out - @echo GENERATE $@ + @echo GENERATE $@ from $(FLIGHT_PLAN) $(eval $@_TMP := $(shell $(MKTEMP))) $(Q)$(GENERATORS)/gen_flight_plan.out -dump $< > $($@_TMP) $(Q)mv $($@_TMP) $@ @@ -191,8 +191,10 @@ $(SETTINGS_MODULES) : $(MODULES_H) $(SETTINGS_TELEMETRY) : $(PERIODIC_H) %.ac_h : $(GENERATORS)/gen_aircraft.out - $(Q)if (expr "$(AIRCRAFT)"); then : ; else echo "AIRCRAFT undefined: type 'make AIRCRAFT=AircraftName ...'"; exit 1; fi - @echo BUILD $(AIRCRAFT), TARGET $* + $(Q)if (expr "$(AIRCRAFT)") > /dev/null; then : ; else echo "AIRCRAFT undefined: type 'make AIRCRAFT=AircraftName ...'"; exit 1; fi + @echo "#######################################" + @echo "# BUILD AIRCRAFT=$(AIRCRAFT), TARGET $*" + @echo "#######################################" $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) TARGET=$* Q=$(Q) $(GENERATORS)/gen_aircraft.out $(AIRCRAFT) %.compile: %.ac_h | print_version diff --git a/conf/Makefile.chibios-libopencm3 b/conf/Makefile.chibios-libopencm3 index d3b9ec0c82..e281206da7 100644 --- a/conf/Makefile.chibios-libopencm3 +++ b/conf/Makefile.chibios-libopencm3 @@ -283,7 +283,7 @@ DLIBS = -lm -lopencm3_stm32f4 # # List all user C define here, like -D_DEBUG=1 -UDEFS = $($(TARGET).CFLAGS) $(LOCAL_CFLAGS) -DUSE_OCM3_SYSTICK_INIT=0 +UDEFS = $($(TARGET).CFLAGS) $(USER_CFLAGS) -DUSE_OCM3_SYSTICK_INIT=0 # Define ASM defines here UADEFS = diff --git a/conf/Makefile.geode b/conf/Makefile.geode index dedaabf443..ce38405faa 100644 --- a/conf/Makefile.geode +++ b/conf/Makefile.geode @@ -46,7 +46,7 @@ CFLAGS += -mtune=geode # optimize for geode CFLAGS + = -Wstrict-prototypes -Wmissing-declarations CFLAGS += -Wmissing-prototypes -Wnested-externs CFLAGS += $(CSTANDARD) -CFLAGS += $($(TARGET).CFLAGS) $(LOCAL_CFLAGS) +CFLAGS += $($(TARGET).CFLAGS) $(USER_CFLAGS) #Additional libraries. MATH_LIB = -lm diff --git a/conf/Makefile.jsbsim b/conf/Makefile.jsbsim index 04044e7c27..b9dba82d4a 100644 --- a/conf/Makefile.jsbsim +++ b/conf/Makefile.jsbsim @@ -38,7 +38,7 @@ Q=@ # Compilation flags # -CFLAGS = -W -Wall $(INCLUDES) -I$(PAPARAZZI_SRC)/sw/airborne/$(SRC_ARCH) $($(TARGET).CFLAGS) $(LOCAL_CFLAGS) -O$(OPT) +CFLAGS = -W -Wall $(INCLUDES) -I$(PAPARAZZI_SRC)/sw/airborne/$(SRC_ARCH) $($(TARGET).CFLAGS) $(USER_CFLAGS) -O$(OPT) LDFLAGS = -lm $($(TARGET).LDFLAGS) diff --git a/conf/Makefile.lpc21 b/conf/Makefile.lpc21 index 473f0e06c6..117cf4607e 100644 --- a/conf/Makefile.lpc21 +++ b/conf/Makefile.lpc21 @@ -96,7 +96,7 @@ CFLAGS += -Wshadow CFLAGS += -Wnested-externs CFLAGS += $(CSTANDARD) -CFLAGS += $($(TARGET).CFLAGS) $(LOCAL_CFLAGS) +CFLAGS += $($(TARGET).CFLAGS) $(USER_CFLAGS) # Assembler flags. ASFLAGS = -Wa,-adhlns=$(OBJDIR)/$(notdir $(<:.S=.lst)) diff --git a/conf/Makefile.nps b/conf/Makefile.nps index 8e1d50063d..ca309ef70a 100644 --- a/conf/Makefile.nps +++ b/conf/Makefile.nps @@ -35,7 +35,7 @@ Q=@ CFLAGS = -W -Wall CFLAGS += $(INCLUDES) CFLAGS += $($(TARGET).CFLAGS) -CFLAGS += $(LOCAL_CFLAGS) +CFLAGS += $(USER_CFLAGS) CFLAGS += -O$(OPT) CFLAGS += -g CFLAGS += -std=gnu99 @@ -44,7 +44,7 @@ CFLAGS += $(shell pkg-config --cflags-only-I ivy-glib) CXXFLAGS = -W -Wall CXXFLAGS += $(INCLUDES) CXXFLAGS += $($(TARGET).CFLAGS) -CXXFLAGS += $(LOCAL_CFLAGS) +CXXFLAGS += $(USER_CFLAGS) CXXFLAGS += -O$(OPT) CXXFLAGS += -g CXXFLAGS += $(shell pkg-config --cflags-only-I ivy-glib) diff --git a/conf/Makefile.omap b/conf/Makefile.omap index 4e5fedcc8c..7b2e049a84 100644 --- a/conf/Makefile.omap +++ b/conf/Makefile.omap @@ -54,6 +54,7 @@ CFLAGS += -Wstrict-prototypes -Wmissing-declarations CFLAGS += -Wmissing-prototypes -Wnested-externs CFLAGS += $(CSTANDARD) CFLAGS += $($(TARGET).CFLAGS) +CFLAGS += $(USER_CFLAGS) LDFLAGS += -lm @@ -62,6 +63,7 @@ CXXFLAGS += -g -ffunction-sections -fdata-sections CXXFLAGS += -mfloat-abi=softfp -mtune=cortex-a8 -mfpu=vfp -march=armv7-a CXXFLAGS += -Wall -Wextra CXXFLAGS += $($(TARGET).CXXFLAGS) +CXXFLAGS += $(USER_CFLAGS) SRC_C_OMAP = $($(TARGET).srcs) OBJ_C_OMAP = $(SRC_C_OMAP:%.c=$(OBJDIR)/%.o) diff --git a/conf/Makefile.pentium-m b/conf/Makefile.pentium-m index 39793920df..ed9ee530d4 100644 --- a/conf/Makefile.pentium-m +++ b/conf/Makefile.pentium-m @@ -55,7 +55,7 @@ CFLAGS += -m32 CFLAGS += -Wstrict-prototypes -Wmissing-declarations CFLAGS += -Wmissing-prototypes -Wnested-externs CFLAGS += $(CSTANDARD) -CFLAGS += $($(TARGET).CFLAGS) $(LOCAL_CFLAGS) +CFLAGS += $($(TARGET).CFLAGS) $(USER_CFLAGS) #Additional libraries. MATH_LIB = -lm diff --git a/conf/Makefile.sim b/conf/Makefile.sim index b9cf412d4f..a6dbae0e19 100644 --- a/conf/Makefile.sim +++ b/conf/Makefile.sim @@ -51,7 +51,7 @@ INCLUDES += -I $(shell $(OCAMLC) -where) CFLAGS = -W -Wall CFLAGS += $(INCLUDES) CFLAGS += $($(TARGET).CFLAGS) -CFLAGS += $(LOCAL_CFLAGS) +CFLAGS += $(USER_CFLAGS) CFLAGS += -fPIC CFLAGS += -O$(OPT) CFLAGS += -g diff --git a/conf/Makefile.stm32 b/conf/Makefile.stm32 index 857d31941e..44e4feb67a 100644 --- a/conf/Makefile.stm32 +++ b/conf/Makefile.stm32 @@ -102,6 +102,8 @@ CFLAGS += -Wstrict-prototypes -Wmissing-prototypes CFLAGS += -Wshadow CFLAGS += -Wnested-externs +CFLAGS += $(USER_CFLAGS) + #CFLAGS += -fno-diagnostics-show-caret ifneq ($(ARCH_L), ) diff --git a/conf/airframes/TUDelft/conf.xml b/conf/airframes/TUDelft/conf.xml index 9308a0716b..3c8547adde 100644 --- a/conf/airframes/TUDelft/conf.xml +++ b/conf/airframes/TUDelft/conf.xml @@ -6,7 +6,8 @@ radio="radios/R6107SP_7ch.xml" telemetry="telemetry/default_fixedwing_imu.xml" flight_plan="flight_plans/versatile_airspeed.xml" - settings=" settings/fixedwing_basic.xml settings/control/ctl_energy.xml settings/estimation/ins_neutrals.xml settings/estimation/ac_char.xml" + settings="settings/fixedwing_basic.xml settings/control/ctl_energy.xml [settings/estimation/ins_neutrals.xml] settings/estimation/ac_char.xml" + settings_modules="modules/gps_ubx_ucenter.xml modules/light.xml modules/digital_cam.xml" gui_color="#ffffffffffff" /> diff --git a/conf/airframes/examples/demo.xml b/conf/airframes/examples/demo.xml index e704d9b1fb..e4578cdb43 100644 --- a/conf/airframes/examples/demo.xml +++ b/conf/airframes/examples/demo.xml @@ -65,7 +65,7 @@ demo4.srcs += $(SRC_ARCH)/armVIC.c demo4.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400 demo4.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c -demo4.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=UART0 +demo4.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=uart0 demo4.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c # @@ -88,7 +88,7 @@ demo5.srcs += $(SRC_ARCH)/armVIC.c demo5.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400 demo5.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c -demo5.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=UART0 +demo5.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=uart0 demo5.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c demo5.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=UART0 @@ -114,7 +114,7 @@ demo6.CFLAGS += -DUSE_USB_SERIAL demo6.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c demo6.srcs += $(SRC_ARCH)/usb_ser_hw.c -demo6.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=UsbS +demo6.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=usb_serial demo6.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c //demo6.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=UART0 @@ -137,7 +137,7 @@ test_spk.CFLAGS += -DUSE_LED test_spk.srcs += $(SRC_ARCH)/armVIC.c test_spk.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DUSE_USB_SERIAL -test_spk.CFLAGS += -DDOWNLINK_DEVICE=UsbS -DPPRZ_UART=UsbS -DDATALINK=PPRZ +test_spk.CFLAGS += -DDOWNLINK_DEVICE=usb_serial -DPPRZ_UART=UsbS -DDATALINK=PPRZ test_spk.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/usb_ser_hw.c subsystems/datalink/pprz_transport.c test_spk.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c test_spk.srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c diff --git a/conf/airframes/turntable.xml b/conf/airframes/turntable.xml index 35af58a2d8..67840a9275 100644 --- a/conf/airframes/turntable.xml +++ b/conf/airframes/turntable.xml @@ -26,7 +26,7 @@ main.srcs += mcu_periph/sys_time.c $(MB)/turntable_systime.c main.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B115200 main.srcs += mcu_periph/uart.c main.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c -main.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=UART0 +main.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=uart0 main.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c diff --git a/conf/airframes/turntable_usb.xml b/conf/airframes/turntable_usb.xml index b77b49cbf4..028c34b4d0 100644 --- a/conf/airframes/turntable_usb.xml +++ b/conf/airframes/turntable_usb.xml @@ -27,7 +27,7 @@ main.CFLAGS += -DUSE_USB_SERIAL main.srcs += $(SRC_ARCH)/usb_ser_hw.c main.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbinit.c main.srcs += $(SRC_ARCH)/lpcusb/usbcontrol.c $(SRC_ARCH)/lpcusb/usbstdreq.c -main.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=UsbS +main.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=usb_serial main.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c diff --git a/conf/airframes/wind_tunnel.xml b/conf/airframes/wind_tunnel.xml index a38885d6d5..73849c99d3 100644 --- a/conf/airframes/wind_tunnel.xml +++ b/conf/airframes/wind_tunnel.xml @@ -24,7 +24,7 @@ ap.srcs += $(SRC_ARCH)/armVIC.c ap.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400 ap.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c -ap.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=UART0 +ap.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=uart0 ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c ap.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=UART0 @@ -54,7 +54,7 @@ mb.srcs += $(SRC_ARCH)/armVIC.c mb.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400 mb.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c -mb.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=UART0 +mb.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=uart0 mb.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c mb.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=UART0 diff --git a/conf/firmwares/setup.makefile b/conf/firmwares/setup.makefile index 6ccc4d42e8..cb0827e31f 100644 --- a/conf/firmwares/setup.makefile +++ b/conf/firmwares/setup.makefile @@ -85,8 +85,9 @@ setup_actuators.CFLAGS += -DUSE_$(MODEM_PORT) setup_actuators.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) setup_actuators.srcs += mcu_periph/uart.c $(SRC_ARCH)/mcu_periph/uart_arch.c -setup_actuators.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=$(MODEM_PORT) -DPPRZ_UART=$(MODEM_PORT) -setup_actuators.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ +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=$(MODEM_PORT) +setup_actuators.CFLAGS += -DDOWNLINK_TRANSPORT=pprz_tp -DDATALINK=PPRZ setup_actuators.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c # we actually don't really use the generated periodic telemetry in this firmware, # but still needed to register e.g. the UART_ERRORS message #if DOWNLINK diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile index 5af5506c4e..6dfc581ea8 100644 --- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile +++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile @@ -183,8 +183,8 @@ sim.srcs += $(fbw_srcs) $(ap_srcs) sim.CFLAGS += -DSITL sim.srcs += $(SRC_ARCH)/sim_ap.c -sim.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_TRANSPORT=IvyTransport -sim.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/ivy_transport.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 sim.srcs += $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_adc_generic.c @@ -223,8 +223,8 @@ jsbsim.srcs += $(SIMDIR)/sim_ac_jsbsim.c $(SIMDIR)/sim_ac_fw.c $(SIMDIR)/sim_a jsbsim.CFLAGS += -I/usr/include $(shell pkg-config glib-2.0 --cflags) jsbsim.LDFLAGS += $(shell pkg-config glib-2.0 --libs) -lglibivy -lm $(shell pcre-config --libs) -jsbsim.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_TRANSPORT=IvyTransport -jsbsim.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/ivy_transport.c +jsbsim.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_TRANSPORT=ivy_tp -DDOWNLINK_DEVICE=ivy_tp +jsbsim.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c subsystems/datalink/ivy_transport.c jsbsim.srcs += $(SRC_ARCH)/jsbsim_hw.c $(SRC_ARCH)/jsbsim_ir.c $(SRC_ARCH)/jsbsim_gps.c $(SRC_ARCH)/jsbsim_ahrs.c $(SRC_ARCH)/jsbsim_transport.c diff --git a/conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile b/conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile index a529ca4778..a88d8a1bde 100644 --- a/conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile +++ b/conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile @@ -47,6 +47,6 @@ nps.srcs += $(NPSDIR)/nps_main.c \ $(NPSDIR)/nps_flightgear.c \ -nps.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_TRANSPORT=IvyTransport -nps.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/ivy_transport.c +nps.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_TRANSPORT=ivy_tp -DDOWNLINK_DEVICE=ivy_tp +nps.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c subsystems/datalink/ivy_transport.c diff --git a/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile b/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile index de5b2463b3..184f876c45 100644 --- a/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile +++ b/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile @@ -65,6 +65,6 @@ nps.srcs += $(NPSDIR)/nps_main.c \ nps.srcs += math/pprz_geodetic_wmm2010.c -nps.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_TRANSPORT=IvyTransport -nps.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/ivy_transport.c +nps.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_TRANSPORT=ivy_tp -DDOWNLINK_DEVICE=ivy_tp +nps.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c subsystems/datalink/ivy_transport.c diff --git a/conf/firmwares/subsystems/fixedwing/telemetry_superbitrf.makefile b/conf/firmwares/subsystems/fixedwing/telemetry_superbitrf.makefile index 6483a1bdf0..721c2cfb58 100644 --- a/conf/firmwares/subsystems/fixedwing/telemetry_superbitrf.makefile +++ b/conf/firmwares/subsystems/fixedwing/telemetry_superbitrf.makefile @@ -5,8 +5,8 @@ ap.CFLAGS += -DUSE_$(MODEM_PORT) ap.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) -ap.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_FBW_DEVICE=SuperbitRF -DDOWNLINK_AP_DEVICE=SuperbitRF -ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=SUPERBITRF +ap.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_FBW_DEVICE=superbitrf -DDOWNLINK_AP_DEVICE=superbitrf +ap.CFLAGS += -DDOWNLINK_TRANSPORT=pprz_tp -DDATALINK=SUPERBITRF #ap.CFLAGS += -DUSE_SUPERBITRF -DUSE_SPI2 -DUSE_SPI_SLAVE2 ap.srcs += peripherals/cyrf6936.c diff --git a/conf/firmwares/subsystems/fixedwing/telemetry_transparent.makefile b/conf/firmwares/subsystems/fixedwing/telemetry_transparent.makefile index 02069b3f63..69f6e6d3d8 100644 --- a/conf/firmwares/subsystems/fixedwing/telemetry_transparent.makefile +++ b/conf/firmwares/subsystems/fixedwing/telemetry_transparent.makefile @@ -1,9 +1,11 @@ # Hey Emacs, this is a -*- makefile -*- +PPRZ_MODEM_PORT_LOWER=$(shell echo $(MODEM_PORT) | tr A-Z a-z) + telemetry_CFLAGS = -DUSE_$(MODEM_PORT) telemetry_CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) -telemetry_CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=$(MODEM_PORT) -DPPRZ_UART=$(MODEM_PORT) -telemetry_CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ +telemetry_CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=$(PPRZ_MODEM_PORT_LOWER) -DPPRZ_UART=$(MODEM_PORT) +telemetry_CFLAGS += -DDOWNLINK_TRANSPORT=pprz_tp -DDATALINK=PPRZ telemetry_srcs = subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c ap.CFLAGS += $(telemetry_CFLAGS) diff --git a/conf/firmwares/subsystems/fixedwing/telemetry_transparent_usb.makefile b/conf/firmwares/subsystems/fixedwing/telemetry_transparent_usb.makefile index 420d549250..6f4a886bb3 100644 --- a/conf/firmwares/subsystems/fixedwing/telemetry_transparent_usb.makefile +++ b/conf/firmwares/subsystems/fixedwing/telemetry_transparent_usb.makefile @@ -4,8 +4,8 @@ #serial USB (e.g. /dev/ttyACM0) ifeq ($(ARCH), lpc21) -ap.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=UsbS -DPPRZ_UART=UsbS -ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ -DUSE_USB_SERIAL -DUSE_USB_HIGH_PCLK +ap.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=usb_serial -DPPRZ_UART=UsbS +ap.CFLAGS += -DDOWNLINK_TRANSPORT=pprz_tp -DDATALINK=PPRZ -DUSE_USB_SERIAL -DUSE_USB_HIGH_PCLK ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c ap.srcs += $(SRC_FIRMWARE)/datalink.c ap.srcs += $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c diff --git a/conf/firmwares/subsystems/fixedwing/telemetry_w5100.makefile b/conf/firmwares/subsystems/fixedwing/telemetry_w5100.makefile index a1859882ed..b7dcaa187d 100644 --- a/conf/firmwares/subsystems/fixedwing/telemetry_w5100.makefile +++ b/conf/firmwares/subsystems/fixedwing/telemetry_w5100.makefile @@ -7,10 +7,10 @@ W5100_SUBNET ?= "255,255,255,0" W5100_MULTICAST_IP ?= "224,1,1,11" W5100_MULTICAST_PORT ?= "1234" -ap.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=W5100 -ap.CFLAGS += -DDOWNLINK_TRANSPORT=W5100Transport -DDATALINK=W5100 +ap.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=chip0 +ap.CFLAGS += -DDOWNLINK_TRANSPORT=pprz_tp -DDATALINK=W5100 ap.CFLAGS += -DW5100_IP=$(W5100_IP) -DW5100_SUBNET=$(W5100_SUBNET) -DW5100_MULTICAST_IP=$(W5100_MULTICAST_IP) -DW5100_MULTICAST_PORT=$(W5100_MULTICAST_PORT) -ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/w5100.c +ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/w5100.c subsystems/datalink/pprz_transport.c ap.srcs += $(SRC_FIRMWARE)/datalink.c ifeq ($(ARCH), lpc21) diff --git a/conf/firmwares/subsystems/fixedwing/telemetry_xbee_api.makefile b/conf/firmwares/subsystems/fixedwing/telemetry_xbee_api.makefile index 563b107fd6..796f710b0d 100644 --- a/conf/firmwares/subsystems/fixedwing/telemetry_xbee_api.makefile +++ b/conf/firmwares/subsystems/fixedwing/telemetry_xbee_api.makefile @@ -2,10 +2,12 @@ # XBee modems in API mode +XBEE_MODEM_PORT_LOWER=$(shell echo $(MODEM_PORT) | tr A-Z a-z) + ap.CFLAGS += -DUSE_$(MODEM_PORT) ap.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) -DXBEE_BAUD=$(MODEM_BAUD) -ap.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=$(MODEM_PORT) -DXBEE_UART=$(MODEM_PORT) -ap.CFLAGS += -DDOWNLINK_TRANSPORT=XBeeTransport -DDATALINK=XBEE +ap.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=$(XBEE_MODEM_PORT_LOWER) -DXBEE_UART=$(MODEM_PORT) +ap.CFLAGS += -DDOWNLINK_TRANSPORT=xbee_tp -DDATALINK=XBEE ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/xbee.c ap.srcs += $(SRC_FIRMWARE)/datalink.c diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile index 7ba363fc4e..e19f469a33 100644 --- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile +++ b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile @@ -62,8 +62,8 @@ nps.srcs += $(NPSDIR)/nps_main.c \ # for geo mag calculation nps.srcs += math/pprz_geodetic_wmm2010.c -nps.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_TRANSPORT=IvyTransport -DDefaultPeriodic='&telemetry_Main' -nps.srcs += $(SRC_ARCH)/ivy_transport.c +nps.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_TRANSPORT=ivy_tp -DDOWNLINK_DEVICE=ivy_tp -DDefaultPeriodic='&telemetry_Main' +nps.srcs += subsystems/datalink/ivy_transport.c nps.srcs += subsystems/datalink/downlink.c subsystems/datalink/telemetry.c nps.srcs += $(SRC_FIRMWARE)/rotorcraft_telemetry.c nps.srcs += $(SRC_FIRMWARE)/datalink.c diff --git a/conf/firmwares/subsystems/rotorcraft/telemetry_superbitrf.makefile b/conf/firmwares/subsystems/rotorcraft/telemetry_superbitrf.makefile index 3ba030e418..33fb785a98 100644 --- a/conf/firmwares/subsystems/rotorcraft/telemetry_superbitrf.makefile +++ b/conf/firmwares/subsystems/rotorcraft/telemetry_superbitrf.makefile @@ -5,8 +5,8 @@ ap.CFLAGS += -DUSE_$(MODEM_PORT) ap.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) -ap.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=SuperbitRF -ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=SUPERBITRF +ap.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=superbitrf +ap.CFLAGS += -DDOWNLINK_TRANSPORT=pprz_tp -DDATALINK=SUPERBITRF ap.CFLAGS += -DDefaultPeriodic='&telemetry_Main' #ap.CFLAGS += -DUSE_SUPERBITRF -DUSE_SPI2 -DUSE_SPI_SLAVE2 diff --git a/conf/firmwares/subsystems/rotorcraft/telemetry_transparent.makefile b/conf/firmwares/subsystems/rotorcraft/telemetry_transparent.makefile index 974fe96422..43b827bd58 100644 --- a/conf/firmwares/subsystems/rotorcraft/telemetry_transparent.makefile +++ b/conf/firmwares/subsystems/rotorcraft/telemetry_transparent.makefile @@ -5,10 +5,12 @@ # MODEM_BAUD # +PPRZ_MODEM_PORT_LOWER=$(shell echo $(MODEM_PORT) | tr A-Z a-z) + ap.CFLAGS += -DUSE_$(MODEM_PORT) ap.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) -ap.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=$(MODEM_PORT) -DPPRZ_UART=$(MODEM_PORT) -ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ -DDefaultPeriodic='&telemetry_Main' +ap.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=$(PPRZ_MODEM_PORT_LOWER) -DPPRZ_UART=$(MODEM_PORT) +ap.CFLAGS += -DDOWNLINK_TRANSPORT=pprz_tp -DDATALINK=PPRZ -DDefaultPeriodic='&telemetry_Main' ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c subsystems/datalink/telemetry.c ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/rotorcraft_telemetry.c diff --git a/conf/firmwares/subsystems/rotorcraft/telemetry_transparent_udp.makefile b/conf/firmwares/subsystems/rotorcraft/telemetry_transparent_udp.makefile index bd64e2c418..7841fe35c0 100644 --- a/conf/firmwares/subsystems/rotorcraft/telemetry_transparent_udp.makefile +++ b/conf/firmwares/subsystems/rotorcraft/telemetry_transparent_udp.makefile @@ -8,11 +8,14 @@ MODEM_PORT_OUT ?= 4242 MODEM_PORT_IN ?= 4243 MODEM_BROADCAST ?= TRUE +UDP_MODEM_PORT_LOWER=$(shell echo $(MODEM_DEV) | tr A-Z a-z) + + MODEM_CFLAGS = -DUSE_$(MODEM_DEV) -D$(MODEM_DEV)_PORT_OUT=$(MODEM_PORT_OUT) -D$(MODEM_DEV)_PORT_IN=$(MODEM_PORT_IN) MODEM_CFLAGS += -D$(MODEM_DEV)_BROADCAST=$(MODEM_BROADCAST) -D$(MODEM_DEV)_HOST=$(MODEM_HOST) -TELEM_CFLAGS = -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=$(MODEM_DEV) -DPPRZ_UART=$(MODEM_DEV) -TELEM_CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ -DDefaultPeriodic='&telemetry_Main' +TELEM_CFLAGS = -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=$(UDP_MODEM_PORT_LOWER) -DPPRZ_UART=$(MODEM_DEV) +TELEM_CFLAGS += -DDOWNLINK_TRANSPORT=pprz_tp -DDATALINK=PPRZ -DDefaultPeriodic='&telemetry_Main' ap.CFLAGS += $(MODEM_CFLAGS) $(TELEM_CFLAGS) diff --git a/conf/firmwares/subsystems/rotorcraft/telemetry_transparent_usb.makefile b/conf/firmwares/subsystems/rotorcraft/telemetry_transparent_usb.makefile index 9dcd3bb043..065d599df1 100644 --- a/conf/firmwares/subsystems/rotorcraft/telemetry_transparent_usb.makefile +++ b/conf/firmwares/subsystems/rotorcraft/telemetry_transparent_usb.makefile @@ -2,8 +2,8 @@ #serial USB (e.g. /dev/ttyACM0) ifeq ($(ARCH), lpc21) -ap.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=UsbS -DPPRZ_UART=UsbS -ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ -DUSE_USB_SERIAL -DDefaultPeriodic='&telemetry_Main' +ap.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=usb_serial -DPPRZ_UART=UsbS +ap.CFLAGS += -DDOWNLINK_TRANSPORT=pprz_tp -DDATALINK=PPRZ -DUSE_USB_SERIAL -DDefaultPeriodic='&telemetry_Main' ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c subsystems/datalink/telemetry.c ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/rotorcraft_telemetry.c ap.srcs += $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c diff --git a/conf/firmwares/subsystems/rotorcraft/telemetry_xbee_api.makefile b/conf/firmwares/subsystems/rotorcraft/telemetry_xbee_api.makefile index b2998d97f3..457059a297 100644 --- a/conf/firmwares/subsystems/rotorcraft/telemetry_xbee_api.makefile +++ b/conf/firmwares/subsystems/rotorcraft/telemetry_xbee_api.makefile @@ -6,10 +6,12 @@ # MODEM_BAUD # +XBEE_MODEM_PORT_LOWER=$(shell echo $(MODEM_PORT) | tr A-Z a-z) + ap.CFLAGS += -DUSE_$(MODEM_PORT) ap.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) -DXBEE_BAUD=$(MODEM_BAUD) -ap.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=$(MODEM_PORT) -DXBEE_UART=$(MODEM_PORT) -ap.CFLAGS += -DDOWNLINK_TRANSPORT=XBeeTransport -DDATALINK=XBEE -DDefaultPeriodic='&telemetry_Main' +ap.CFLAGS += -DDOWNLINK -DPERIODIC_TELEMETRY -DDOWNLINK_DEVICE=$(XBEE_MODEM_PORT_LOWER) -DXBEE_UART=$(MODEM_PORT) +ap.CFLAGS += -DDOWNLINK_TRANSPORT=xbee_tp -DDATALINK=XBEE -DDefaultPeriodic='&telemetry_Main' ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/xbee.c subsystems/datalink/telemetry.c ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/rotorcraft_telemetry.c diff --git a/conf/firmwares/test_progs.makefile b/conf/firmwares/test_progs.makefile index 93a5999171..1299e96074 100644 --- a/conf/firmwares/test_progs.makefile +++ b/conf/firmwares/test_progs.makefile @@ -65,8 +65,9 @@ COMMON_TEST_SRCS += $(SRC_ARCH)/led_hw.c COMMON_TEST_SRCS += $(SRC_ARCH)/mcu_periph/gpio_arch.c endif +COMMON_TELEMETRY_MODEM_PORT_LOWER=$(shell echo $(MODEM_PORT) | tr A-Z a-z) COMMON_TELEMETRY_CFLAGS = -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) -COMMON_TELEMETRY_CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT) +COMMON_TELEMETRY_CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=pprz_tp -DDOWNLINK_DEVICE=$(COMMON_TELEMETRY_MODEM_PORT_LOWER) COMMON_TELEMETRY_CFLAGS += -DDefaultPeriodic='&telemetry_Main' COMMON_TELEMETRY_CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=$(MODEM_PORT) COMMON_TELEMETRY_SRCS = mcu_periph/uart.c diff --git a/conf/flight_plans/flight_plan.dtd b/conf/flight_plans/flight_plan.dtd index 33599ac043..68be3712bb 100644 --- a/conf/flight_plans/flight_plan.dtd +++ b/conf/flight_plans/flight_plan.dtd @@ -162,7 +162,9 @@ value CDATA #REQUIRED> +until CDATA #IMPLIED +loop CDATA #IMPLIED +break CDATA #IMPLIED> - + diff --git a/conf/modules/extra_dl.xml b/conf/modules/extra_dl.xml index ed4ed93c5d..7351d9f0b5 100644 --- a/conf/modules/extra_dl.xml +++ b/conf/modules/extra_dl.xml @@ -11,11 +11,15 @@ - + +EXTRA_DL_PORT_LOWER = $(shell echo $(EXTRA_DL_PORT) | tr A-Z a-z) + + + diff --git a/fix_code_style.sh b/fix_code_style.sh index 216be08413..8098f393bd 100755 --- a/fix_code_style.sh +++ b/fix_code_style.sh @@ -24,7 +24,7 @@ if [ $(bc <<< "$ASTYLE_VERSION >= 2.03") -eq 1 ]; then --unpad-paren \ --keep-one-line-blocks \ --keep-one-line-statements \ - --align-pointer=type \ + --align-pointer=name \ --suffix=none \ --lineend=linux \ --add-brackets \ @@ -42,7 +42,7 @@ else --unpad-paren \ --keep-one-line-blocks \ --keep-one-line-statements \ - --align-pointer=type \ + --align-pointer=name \ --suffix=none \ --lineend=linux \ --add-brackets \ diff --git a/sw/airborne/arch/lpc21/uart_tunnel.c b/sw/airborne/arch/lpc21/uart_tunnel.c index d8e22f8a60..be2bd5155f 100644 --- a/sw/airborne/arch/lpc21/uart_tunnel.c +++ b/sw/airborne/arch/lpc21/uart_tunnel.c @@ -11,7 +11,7 @@ #define TXD1_PIN 8 #define RXD1_PIN 9 -int main (int argc, char** argv) { +int main(int argc __attribute__((unused)), char** argv __attribute__((unused))) { int tx=0, rx=0; int tx_shadow=1, rx_shadow=1; mcu_init(); diff --git a/sw/airborne/arch/lpc21/usb_ser_hw.c b/sw/airborne/arch/lpc21/usb_ser_hw.c index f848426fb5..7a880dd408 100644 --- a/sw/airborne/arch/lpc21/usb_ser_hw.c +++ b/sw/airborne/arch/lpc21/usb_ser_hw.c @@ -434,7 +434,7 @@ int VCOM_check_available(void) @param [in] bEP @param [in] bEPStatus */ -static void BulkOut(U8 bEP, U8 bEPStatus) +static void BulkOut(U8 bEP, U8 bEPStatus __attribute__((unused))) { int i, iLen; @@ -463,7 +463,7 @@ static void BulkOut(U8 bEP, U8 bEPStatus) @param [in] bEP @param [in] bEPStatus */ -static void BulkIn(U8 bEP, U8 bEPStatus) +static void BulkIn(U8 bEP, U8 bEPStatus __attribute__((unused))) { int i, iLen; @@ -541,7 +541,7 @@ static void USBIntHandler(void) } -static void USBFrameHandler(U16 wFrame) +static void USBFrameHandler(U16 wFrame __attribute__((unused))) { if (fifo_avail(&txfifo) > 0) { // data available, enable NAK interrupt on bulk in @@ -549,6 +549,21 @@ static void USBFrameHandler(U16 wFrame) } } +// Periph with generic device API +struct usb_serial_periph usb_serial; + +// Functions for the generic device API +static int usb_serial_check_free_space(struct usb_serial_periph* p __attribute__((unused)), uint8_t len) +{ + return (int)VCOM_check_free_space(len); +} + +static void usb_serial_transmit(struct usb_serial_periph* p __attribute__((unused)), uint8_t byte) +{ + VCOM_putchar(byte); +} + +static void usb_serial_send(struct usb_serial_periph* p __attribute__((unused))) { } void VCOM_init(void) { // initialise stack @@ -588,6 +603,12 @@ void VCOM_init(void) { // connect to bus USBHwConnect(TRUE); + + // Configure generic device + usb_serial.device.periph = (void *)(&usb_serial); + usb_serial.device.check_free_space = (check_free_space_t) usb_serial_check_free_space; + usb_serial.device.transmit = (transmit_t) usb_serial_transmit; + usb_serial.device.send_message = (send_message_t) usb_serial_send; } diff --git a/sw/airborne/arch/sim/ivy_transport.c b/sw/airborne/arch/sim/ivy_transport.c deleted file mode 100644 index 4d8706dcd3..0000000000 --- a/sw/airborne/arch/sim/ivy_transport.c +++ /dev/null @@ -1,3 +0,0 @@ -char ivy_buf[256]; -char *ivy_p = ivy_buf; -int ivy_dl_enabled = 1; diff --git a/sw/airborne/arch/sim/ivy_transport.h b/sw/airborne/arch/sim/ivy_transport.h deleted file mode 100644 index d84ff19234..0000000000 --- a/sw/airborne/arch/sim/ivy_transport.h +++ /dev/null @@ -1,73 +0,0 @@ -#include -#include - -extern char ivy_buf[]; -extern char* ivy_p; -extern int ivy_dl_enabled; - -#define IvyTransportCheckFreeSpace(_dev,_) TRUE - -#define IvyTransportSizeOf(_dev, x) (x) - -#define IvyTransportHeader(_dev,len) ivy_p=ivy_buf; - -#define IvyTransportTrailer(_dev) { *(--ivy_p) = '\0'; if (ivy_dl_enabled) { IvySendMsg("%s",ivy_buf); } } - -#define IvyTransportPutUint8(_dev,x) { ivy_p += sprintf(ivy_p, "%u ", x); } -#define IvyTransportPutNamedUint8(_dev,_name, _x) { ivy_p += sprintf(ivy_p, "%s ", _name); } - -#define Space() ivy_p += sprintf(ivy_p, " "); -#define Comma() ivy_p += sprintf(ivy_p, ","); -#define DelimStart() ivy_p += sprintf(ivy_p, "|"); -#define DelimEnd() ivy_p += sprintf(ivy_p, "|"); - -#define IvyTransportPutcByAddr(_dev,x) ivy_p += sprintf(ivy_p, "%c", *x); -#define IvyTransportPutCharByAddr(_dev,x) IvyTransportPutcByAddr(_dev,x) Space() -#define IvyTransportPutUintByAddr(_dev,x) ivy_p += sprintf(ivy_p, "%u", *x); -#define IvyTransportPutUint8ByAddr(_dev,x) IvyTransportPutUintByAddr(_dev,x) Space() -#define IvyTransportPutUint16ByAddr(_dev,x) IvyTransportPutUintByAddr(_dev,x) Space() -#define IvyTransportPutUint32ByAddr(_dev,x) IvyTransportPutUintByAddr(_dev,x) Space() -#define IvyTransportPutUint64ByAddr(_dev,x) ivy_p += sprintf(ivy_p, "%llu", *x); Space() - -#define IvyTransportPutIntByAddr(_dev,x) ivy_p += sprintf(ivy_p, "%d", *x); -#define IvyTransportPutInt8ByAddr(_dev,x) IvyTransportPutIntByAddr(_dev,x) Space() -#define IvyTransportPutInt16ByAddr(_dev,x) IvyTransportPutIntByAddr(_dev,x) Space() -#define IvyTransportPutInt32ByAddr(_dev,x) IvyTransportPutIntByAddr(_dev,x) Space() -#define IvyTransportPutInt64ByAddr(_dev,x) ivy_p += sprintf(ivy_p, "%lld", *x); Space() - -#define IvyTransportPutOneFloatByAddr(_dev,x) ivy_p += sprintf(ivy_p, "%f", *x); -#define IvyTransportPutFloatByAddr(_dev,x) IvyTransportPutOneFloatByAddr(_dev,x) Space() -#define IvyTransportPutDoubleByAddr(_dev,x) IvyTransportPutOneFloatByAddr(_dev,x) Space() - -#define IvyTransportPutArray(_dev,_put, _n, _x) { \ - int __i; \ - DelimStart(); \ - for(__i = 0; __i < _n; __i++) { \ - _put(_dev,&_x[__i]); \ - Comma(); \ - } DelimEnd(); Space(); \ -} - -#define IvyTransportPutInt8Array(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutIntByAddr, _n, _x) -#define IvyTransportPutUint8Array(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutUintByAddr, _n, _x) -#define IvyTransportPutCharArray(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutcByAddr, _n, _x) -#define IvyTransportPutInt16Array(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutIntByAddr, _n, _x) -#define IvyTransportPutUint16Array(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutUintByAddr, _n, _x) -#define IvyTransportPutUint32Array(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutUintByAddr, _n, _x) -#define IvyTransportPutInt32Array(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutIntByAddr, _n, _x) -#define IvyTransportPutUint64Array(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutUintByAddr, _n, _x) -#define IvyTransportPutInt64Array(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutIntByAddr, _n, _x) -#define IvyTransportPutFloatArray(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutOneFloatByAddr, _n, _x) -#define IvyTransportPutDoubleArray(_dev,_n, _x) IvyTransportPutFloatArray(_dev,_n, _x) - -#define IvyTransportPutInt8FixedArray(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutIntByAddr, _n, _x) -#define IvyTransportPutUint8FixedArray(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutUintByAddr, _n, _x) -#define IvyTransportPutCharFixedArray(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutcByAddr, _n, _x) -#define IvyTransportPutInt16FixedArray(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutIntByAddr, _n, _x) -#define IvyTransportPutUint16FixedArray(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutUintByAddr, _n, _x) -#define IvyTransportPutUint32FixedArray(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutUintByAddr, _n, _x) -#define IvyTransportPutInt32FixedArray(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutIntByAddr, _n, _x) -#define IvyTransportPutUint64FixedArray(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutUintByAddr, _n, _x) -#define IvyTransportPutInt64FixedArray(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutIntByAddr, _n, _x) -#define IvyTransportPutFloatFixedArray(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutOneFloatByAddr, _n, _x) -#define IvyTransportPutDoubleFixedArray(_dev,_n, _x) IvyTransportPutFloatArray(_dev,_n, _x) diff --git a/sw/airborne/arch/sim/sim_ap.c b/sw/airborne/arch/sim/sim_ap.c index 9260564c69..dbc4dd7e28 100644 --- a/sw/airborne/arch/sim/sim_ap.c +++ b/sw/airborne/arch/sim/sim_ap.c @@ -110,7 +110,7 @@ value update_bat(value bat) { } value update_dl_status(value dl_enabled) { - ivy_dl_enabled = Int_val(dl_enabled); + ivy_tp.ivy_dl_enabled = Int_val(dl_enabled); return Val_unit; } diff --git a/sw/airborne/arch/stm32/mcu_arch.c b/sw/airborne/arch/stm32/mcu_arch.c index 717696224f..718f61cf51 100644 --- a/sw/airborne/arch/stm32/mcu_arch.c +++ b/sw/airborne/arch/stm32/mcu_arch.c @@ -39,23 +39,6 @@ #include "std.h" -#if defined(STM32F4) -/** 25MHz external clock to PLL it to 168MHz */ -const clock_scale_t hse_25mhz_3v3_168mhz = { /* 168MHz */ - .pllm = 25, - .plln = 336, - .pllp = 2, - .pllq = 7, - .hpre = RCC_CFGR_HPRE_DIV_NONE, - .ppre1 = RCC_CFGR_PPRE_DIV_4, - .ppre2 = RCC_CFGR_PPRE_DIV_2, - .flash_config = FLASH_ACR_ICE | FLASH_ACR_DCE | - FLASH_ACR_LATENCY_5WS, - .apb1_frequency = 42000000, - .apb2_frequency = 84000000, -}; -#endif - void mcu_arch_init(void) { #if LUFTBOOT PRINT_CONFIG_MSG("We are running luftboot, the interrupt vector is being relocated.") @@ -85,7 +68,7 @@ PRINT_CONFIG_MSG("Using 16MHz external clock to PLL it to 168MHz.") #elif EXT_CLK == 25000000 #if defined(STM32F4) PRINT_CONFIG_MSG("Using 25MHz external clock to PLL it to 168MHz.") - rcc_clock_setup_hse_3v3(&hse_25mhz_3v3_168mhz); + rcc_clock_setup_hse_3v3(&hse_25mhz_3v3[CLOCK_3V3_168MHZ]); #endif #else #error EXT_CLK is either set to an unsupported frequency or not defined at all. Please check! diff --git a/sw/airborne/arch/stm32/mcu_arch.h b/sw/airborne/arch/stm32/mcu_arch.h index 04d42da3ec..37fffacc69 100644 --- a/sw/airborne/arch/stm32/mcu_arch.h +++ b/sw/airborne/arch/stm32/mcu_arch.h @@ -31,7 +31,6 @@ #define STM32_MCU_ARCH_H #include "std.h" -#include extern void mcu_arch_init(void); @@ -50,14 +49,6 @@ extern void mcu_arch_init(void); #define mcu_int_enable() {} #define mcu_int_disable() {} -/** @todo: these should go into libopencm3 */ -#ifdef TIM9_BASE -#define TIM9 TIM9_BASE -#endif -#ifdef TIM12_BASE -#define TIM12 TIM12_BASE -#endif - uint32_t timer_get_frequency(uint32_t timer_peripheral); #endif /* STM32_MCU_ARCH_H */ diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c index 127754030b..cec1349723 100644 --- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c @@ -1003,12 +1003,7 @@ void i2c1_hw_init(void) { // enable peripheral i2c_peripheral_enable(I2C1); - /* - * XXX: there is a function to do that already in libopencm3 but I am not - * sure if it is correct, using direct register instead (esden) - */ - //i2c_set_own_7bit_slave_address(I2C1, 0); - I2C_OAR1(I2C1) = 0 | 0x4000; + i2c_set_own_7bit_slave_address(I2C1, 0); // enable error interrupts i2c_enable_interrupt(I2C1, I2C_CR2_ITERREN); @@ -1078,12 +1073,7 @@ void i2c2_hw_init(void) { // enable peripheral i2c_peripheral_enable(I2C2); - /* - * XXX: there is a function to do that already in libopencm3 but I am not - * sure if it is correct, using direct register instead (esden) - */ - //i2c_set_own_7bit_slave_address(I2C2, 0); - I2C_OAR1(I2C2) = 0 | 0x4000; + i2c_set_own_7bit_slave_address(I2C2, 0); // enable error interrupts i2c_enable_interrupt(I2C2, I2C_CR2_ITERREN); @@ -1153,12 +1143,7 @@ void i2c3_hw_init(void) { // enable peripheral i2c_peripheral_enable(I2C3); - /* - * XXX: there is a function to do that already in libopencm3 but I am not - * sure if it is correct, using direct register instead (esden) - */ - //i2c_set_own_7bit_slave_address(I2C3, 0); - I2C_OAR1(I2C3) = 0 | 0x4000; + i2c_set_own_7bit_slave_address(I2C3, 0); // enable error interrupts i2c_enable_interrupt(I2C3, I2C_CR2_ITERREN); diff --git a/sw/airborne/arch/stm32/peripherals/ms2100_arch.c b/sw/airborne/arch/stm32/peripherals/ms2100_arch.c index 4589b06628..51d31cd7ab 100644 --- a/sw/airborne/arch/stm32/peripherals/ms2100_arch.c +++ b/sw/airborne/arch/stm32/peripherals/ms2100_arch.c @@ -69,7 +69,7 @@ void ms2100_reset_cb( struct spi_transaction * t __attribute__ ((unused)) ) { int32_t end_cpu_ticks = systick_get_value() - dt_ticks; if (end_cpu_ticks < 0) end_cpu_ticks += systick_get_reload(); - while (systick_get_value() > end_cpu_ticks) + while (systick_get_value() > (uint32_t)end_cpu_ticks) ; Ms2100Reset(); diff --git a/sw/airborne/arch/stm32/subsystems/actuators/actuators_shared_arch.c b/sw/airborne/arch/stm32/subsystems/actuators/actuators_shared_arch.c index 42e15146e0..a7e07d1156 100644 --- a/sw/airborne/arch/stm32/subsystems/actuators/actuators_shared_arch.c +++ b/sw/airborne/arch/stm32/subsystems/actuators/actuators_shared_arch.c @@ -23,10 +23,11 @@ * STM32 PWM and dualPWM servos shared functions. */ -#include "subsystems/actuators/actuators_shared_arch.h" +#include "arch/stm32/subsystems/actuators/actuators_shared_arch.h" +#include // for timer_get_frequency -#include "mcu_arch.h" +#include "arch/stm32/mcu_arch.h" /** Set PWM channel configuration diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c index 1a2c808941..244ddbbed0 100644 --- a/sw/airborne/boards/ardrone/navdata.c +++ b/sw/airborne/boards/ardrone/navdata.c @@ -121,8 +121,8 @@ static void navdata_write(const uint8_t *buf, size_t count) #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_navdata(void) { - DOWNLINK_SEND_ARDRONE_NAVDATA(DefaultChannel, DefaultDevice, +static void send_navdata(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_ARDRONE_NAVDATA(trans, dev, AC_ID, &navdata.taille, &navdata.nu_trame, &navdata.ax, @@ -154,12 +154,12 @@ static void send_navdata(void) { &nav_port.checksum_errors); } -static void send_fliter_status(void) { +static void send_filter_status(struct transport_tx *trans, struct link_device *dev) { uint8_t mde = 3; if (ahrs.status == AHRS_UNINIT) mde = 2; if (imu_lost) mde = 5; uint16_t val = 0; - DOWNLINK_SEND_STATE_FILTER_STATUS(DefaultChannel, DefaultDevice, &mde, &val); + pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &val); } #endif @@ -231,7 +231,7 @@ bool_t navdata_init() #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "ARDRONE_NAVDATA", send_navdata); - register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_fliter_status); + register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status); #endif return TRUE; diff --git a/sw/airborne/firmwares/beth/overo_test_uart.c b/sw/airborne/firmwares/beth/overo_test_uart.c index e7ad7d3eb6..1cceade1cc 100644 --- a/sw/airborne/firmwares/beth/overo_test_uart.c +++ b/sw/airborne/firmwares/beth/overo_test_uart.c @@ -146,9 +146,9 @@ static void main_periodic(int my_sig_num) { #include "subsystems/datalink/downlink.h" #if 0 -uint8_t downlink_nb_ovrn; -uint16_t downlink_nb_bytes; -uint16_t downlink_nb_msgs; +uint8_t downlink.nb_ovrn; +uint16_t downlink.nb_bytes; +uint16_t downlink.nb_msgs; #define __Transport(dev, _x) dev##_x #define _Transport(dev, _x) __Transport(dev, _x) @@ -161,11 +161,11 @@ uint16_t downlink_nb_msgs; #define DownlinkPutUint8ByAddr(_chan, _x) Transport(_chan, PutUint8ByAddr(_x)) #define DownlinkPutUint8Array(_chan, _n, _x) Transport(_chan, PutUint8Array(_n, _x)) -#define DownlinkOverrun(_chan) downlink_nb_ovrn++; -#define DownlinkCountBytes(_chan, _n) downlink_nb_bytes += _n; +#define DownlinkOverrun(_chan) downlink.nb_ovrn++; +#define DownlinkCountBytes(_chan, _n) downlink.nb_bytes += _n; #define DownlinkStartMessage(_chan, _name, msg_id, payload_len) { \ - downlink_nb_msgs++; \ + downlink.nb_msgs++; \ Transport(_chan, Header(DownlinkIDsSize(_chan, payload_len))); \ Transport(_chan, PutUint8(AC_ID)); \ Transport(_chan, PutNamedUint8(_name, msg_id)); \ diff --git a/sw/airborne/firmwares/fixedwing/autopilot.c b/sw/airborne/firmwares/fixedwing/autopilot.c index db1abc313e..224b040384 100644 --- a/sw/airborne/firmwares/fixedwing/autopilot.c +++ b/sw/airborne/firmwares/fixedwing/autopilot.c @@ -56,87 +56,92 @@ bool_t gps_lost; bool_t power_switch; -static void send_alive(void) { - DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); +static void send_alive(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_ALIVE(trans, dev, AC_ID, 16, MD5SUM); } #if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS #include "rc_settings.h" -static void send_rc_settings(void) { +static void send_rc_settings(struct transport_tx *trans, struct link_device *dev) { if (!RcSettingsOff()) - DOWNLINK_SEND_SETTINGS(DefaultChannel, DefaultDevice, &slider_1_val, &slider_2_val); + pprz_msg_send_SETTINGS(trans, dev, AC_ID, &slider_1_val, &slider_2_val); } #else uint8_t rc_settings_mode = 0; #endif -void autopilot_send_mode(void) { - DOWNLINK_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice, +static void send_mode(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_PPRZ_MODE(trans, dev, AC_ID, &pprz_mode, &v_ctl_mode, &lateral_mode, &horizontal_mode, &rc_settings_mode, &mcu1_status); } -static void send_attitude(void) { +void autopilot_send_mode(void) { + // use default telemetry here + send_mode(&(DefaultChannel).trans_tx, &(DefaultDevice).device); +} + +static void send_attitude(struct transport_tx *trans, struct link_device *dev) { struct FloatEulers* att = stateGetNedToBodyEulers_f(); - DOWNLINK_SEND_ATTITUDE(DefaultChannel, DefaultDevice, + pprz_msg_send_ATTITUDE(trans, dev, AC_ID, &(att->phi), &(att->psi), &(att->theta)); }; -static void send_estimator(void) { - DOWNLINK_SEND_ESTIMATOR(DefaultChannel, DefaultDevice, +static void send_estimator(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_ESTIMATOR(trans, dev, AC_ID, &(stateGetPositionUtm_f()->alt), &(stateGetSpeedEnu_f()->z)); } -static void send_bat(void) { +static void send_bat(struct transport_tx *trans, struct link_device *dev) { int16_t amps = (int16_t) (current/10); int16_t e = energy; - DOWNLINK_SEND_BAT(DefaultChannel, DefaultDevice, + pprz_msg_send_BAT(trans, dev, AC_ID, &v_ctl_throttle_slewed, &vsupply, &s, - &autopilot_flight_time, &kill_throttle, + &autopilot_flight_time, (uint8_t*)(&kill_throttle), &block_time, &stage_time, &e); } -static void send_energy(void) { - const int16_t e = energy; - const float vsup = ((float)vsupply) / 10.0f; - const float curs = ((float)current) / 1000.0f; - const float power = vsup * curs; - DOWNLINK_SEND_ENERGY(DefaultChannel, DefaultDevice, &vsup, &curs, &e, &power); +static void send_energy(struct transport_tx *trans, struct link_device *dev) { + uint16_t e = energy; + float vsup = ((float)vsupply) / 10.0f; + float curs = ((float)current) / 1000.0f; + float power = vsup * curs; + pprz_msg_send_ENERGY(trans, dev, AC_ID, &vsup, &curs, &e, &power); } -static void send_dl_value(void) { - PeriodicSendDlValue(DefaultChannel, DefaultDevice); +static void send_dl_value(struct transport_tx *trans, struct link_device *dev) { + PeriodicSendDlValue(trans, dev); } // FIXME not the best place #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" #include CTRL_TYPE_H -static void send_desired(void) { +static void send_desired(struct transport_tx *trans, struct link_device *dev) { #ifndef USE_AIRSPEED float v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED; #endif - DOWNLINK_SEND_DESIRED(DefaultChannel, DefaultDevice, + pprz_msg_send_DESIRED(trans, dev, AC_ID, &h_ctl_roll_setpoint, &h_ctl_pitch_loop_setpoint, &h_ctl_course_setpoint, &desired_x, &desired_y, &v_ctl_altitude_setpoint, &v_ctl_climb_setpoint, &v_ctl_auto_airspeed_setpoint); } -static void send_airspeed(void) { +static void send_airspeed(struct transport_tx *trans __attribute__((unused)), struct link_device *dev __attribute__((unused))) { #ifdef MEASURE_AIRSPEED float* airspeed = stateGetAirspeed_f(); - DOWNLINK_SEND_AIRSPEED(DefaultChannel, DefaultDevice, airspeed, airspeed, airspeed, airspeed); + pprz_msg_send_AIRSPEED(trans, dev, AC_ID, airspeed, airspeed, airspeed, airspeed); #elif USE_AIRSPEED - DOWNLINK_SEND_AIRSPEED(DefaultChannel, DefaultDevice, + pprz_msg_send_AIRSPEED(trans, dev, AC_ID, stateGetAirspeed_f(), &v_ctl_auto_airspeed_setpoint, &v_ctl_auto_airspeed_controlled, &v_ctl_auto_groundspeed_setpoint); #endif } -static void send_downlink(void) { +static void send_downlink(struct transport_tx *trans, struct link_device *dev) { static uint16_t last; - uint16_t rate = (downlink_nb_bytes - last) / PERIOD_DOWNLINK_Ap_0; - last = downlink_nb_bytes; - DOWNLINK_SEND_DOWNLINK(DefaultChannel, DefaultDevice, - &downlink_nb_ovrn, &rate, &downlink_nb_msgs); + uint16_t rate = (downlink.nb_bytes - last) / PERIOD_DOWNLINK_Ap_0; + last = downlink.nb_bytes; + pprz_msg_send_DOWNLINK(trans, dev, AC_ID, + &downlink.nb_ovrn, &rate, &downlink.nb_msgs); } void autopilot_init(void) { @@ -157,7 +162,7 @@ void autopilot_init(void) { /* register some periodic message */ register_periodic_telemetry(DefaultPeriodic, "ALIVE", send_alive); - register_periodic_telemetry(DefaultPeriodic, "PPRZ_MODE", autopilot_send_mode); + register_periodic_telemetry(DefaultPeriodic, "PPRZ_MODE", send_mode); register_periodic_telemetry(DefaultPeriodic, "DOWNLINK", send_downlink); register_periodic_telemetry(DefaultPeriodic, "ATTITUDE", send_attitude); register_periodic_telemetry(DefaultPeriodic, "ESTIMATOR", send_estimator); diff --git a/sw/airborne/firmwares/fixedwing/datalink.c b/sw/airborne/firmwares/fixedwing/datalink.c index e47d4702f4..b37fd5b324 100644 --- a/sw/airborne/firmwares/fixedwing/datalink.c +++ b/sw/airborne/firmwares/fixedwing/datalink.c @@ -103,7 +103,7 @@ void dl_parse_msg(void) { #endif if (msg_id == DL_PING) { - DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice) + DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice); } else #ifdef TRAFFIC_INFO if (msg_id == DL_ACINFO && DL_ACINFO_ac_id(dl_buffer) != AC_ID) { @@ -139,7 +139,7 @@ void dl_parse_msg(void) { DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0); } else if (msg_id == DL_BLOCK && DL_BLOCK_ac_id(dl_buffer) == AC_ID) { nav_goto_block(DL_BLOCK_block_id(dl_buffer)); - SEND_NAVIGATION(DefaultChannel, DefaultDevice); + SEND_NAVIGATION(&(DefaultChannel).trans_tx, &(DefaultDevice).device); } else #endif /** NAV */ #ifdef WIND_INFO diff --git a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c index 6e61e12020..ed50e91934 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c +++ b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c @@ -137,15 +137,15 @@ float v_ctl_pitch_setpoint; ///////////// DEFAULT SETTINGS //////////////// #ifndef V_CTL_ALTITUDE_MAX_CLIMB #define V_CTL_ALTITUDE_MAX_CLIMB 2; -#warning "V_CTL_ALTITUDE_MAX_CLIMB not defined - default is 2m/s" +INFO("V_CTL_ALTITUDE_MAX_CLIMB not defined - default is 2m/s") #endif #ifndef STALL_AIRSPEED -#warning "No STALL_AIRSPEED defined. Using NOMINAL_AIRSPEED" +INFO("No STALL_AIRSPEED defined. Using NOMINAL_AIRSPEED") #define STALL_AIRSPEED NOMINAL_AIRSPEED #endif #ifndef V_CTL_GLIDE_RATIO #define V_CTL_GLIDE_RATIO 8. -#warning "V_CTL_GLIDE_RATIO not defined - default is 8." +INFO("V_CTL_GLIDE_RATIO not defined - default is 8.") #endif #ifndef AIRSPEED_SETPOINT_SLEW #define AIRSPEED_SETPOINT_SLEW 1 diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index de7c35106d..334ee68f5f 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -73,10 +73,9 @@ PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BO // datalink & telemetry #include "subsystems/datalink/datalink.h" +#include "subsystems/datalink/downlink.h" #include "subsystems/datalink/telemetry.h" #include "subsystems/settings.h" -#include "subsystems/datalink/xbee.h" -#include "subsystems/datalink/w5100.h" // modules & settings #include "generated/modules.h" @@ -147,12 +146,12 @@ static inline void on_mag_event( void ); volatile uint8_t ahrs_timeout_counter = 0; //FIXME not the correct place -static void send_filter_status(void) { +static void send_filter_status(struct transport_tx *trans, struct link_device *dev) { uint8_t mde = 3; if (ahrs.status == AHRS_UNINIT) mde = 2; if (ahrs_timeout_counter > 10) mde = 5; uint16_t val = 0; - DOWNLINK_SEND_STATE_FILTER_STATUS(DefaultChannel, DefaultDevice, &mde, &val); + pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &val); } #endif // USE_AHRS && USE_IMU @@ -252,14 +251,7 @@ void init_ap( void ) { /** - start interrupt task */ mcu_int_enable(); -#if defined DATALINK -#if DATALINK == XBEE - xbee_init(); -#endif -#if DATALINK == W5100 - w5100_init(); -#endif -#endif /* DATALINK */ + downlink_init(); #if defined AEROCOMM_DATA_PIN IO0DIR |= _BV(AEROCOMM_DATA_PIN); @@ -460,13 +452,14 @@ void reporting_task( void ) { /** initialisation phase during boot */ if (boot) { - DOWNLINK_SEND_BOOT(DefaultChannel, DefaultDevice, &version); + uint16_t non_const_version = version; + DOWNLINK_SEND_BOOT(DefaultChannel, DefaultDevice, &non_const_version); boot = FALSE; } /** then report periodicly */ else { //PeriodicSendAp(DefaultChannel, DefaultDevice); - periodic_telemetry_send_Ap(); + periodic_telemetry_send_Ap(&(DefaultChannel).trans_tx, &(DefaultDevice).device); } } @@ -515,7 +508,7 @@ void navigation_task( void ) { #endif #ifndef PERIOD_NAVIGATION_Ap_0 // If not sent periodically (in default 0 mode) - SEND_NAVIGATION(DefaultChannel, DefaultDevice); + SEND_NAVIGATION(&(DefaultChannel).trans_tx, &(DefaultDevice).device); #endif /* The nav task computes only nav_altitude. However, we are interested diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c index 26f8ee1a20..cb43ca4985 100644 --- a/sw/airborne/firmwares/fixedwing/main_fbw.c +++ b/sw/airborne/firmwares/fixedwing/main_fbw.c @@ -76,31 +76,31 @@ tid_t electrical_tid; ///< id for electrical_periodic() timer /********** PERIODIC MESSAGES ************************************************/ #if PERIODIC_TELEMETRY -static void send_commands(void) { - DOWNLINK_SEND_COMMANDS(DefaultChannel, DefaultDevice, COMMANDS_NB, commands); +static void send_commands(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_COMMANDS(trans, dev, AC_ID, COMMANDS_NB, commands); } #ifdef RADIO_CONTROL -static void send_fbw_status(void) { - DOWNLINK_SEND_FBW_STATUS(DefaultChannel, DefaultDevice, +static void send_fbw_status(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_FBW_STATUS(trans, dev, AC_ID, &(radio_control.status), &(radio_control.frame_rate), &fbw_mode, &electrical.vsupply, &electrical.current); } -static void send_rc(void) { - DOWNLINK_SEND_RC(DefaultChannel, DefaultDevice, RADIO_CONTROL_NB_CHANNEL, radio_control.values); +static void send_rc(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_RC(trans, dev, AC_ID, RADIO_CONTROL_NB_CHANNEL, radio_control.values); } #else -static void send_fbw_status(void) { +static void send_fbw_status(struct transport_tx *trans, struct link_device *dev) { uint8_t dummy = 0; - DOWNLINK_SEND_FBW_STATUS(DefaultChannel, DefaultDevice, + pprz_msg_send_FBW_STATUS(trans, dev, AC_ID, &dummy, &dummy, &fbw_mode, &electrical.vsupply, &electrical.current); } #endif #ifdef ACTUATORS -static void send_actuators(void) { - DOWNLINK_SEND_ACTUATORS(DefaultChannel, DefaultDevice , ACTUATORS_NB, actuators); +static void send_actuators(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_ACTUATORS(trans, dev, AC_ID , ACTUATORS_NB, actuators); } #endif @@ -335,7 +335,7 @@ void periodic_task_fbw( void ) { #endif #if PERIODIC_TELEMETRY - periodic_telemetry_send_Fbw(); + periodic_telemetry_send_Fbw(&(DefaultChannel).trans_tx, &(DefaultDevice).device); #endif } diff --git a/sw/airborne/firmwares/fixedwing/nav.c b/sw/airborne/firmwares/fixedwing/nav.c index ab22acac5a..799ac77277 100644 --- a/sw/airborne/firmwares/fixedwing/nav.c +++ b/sw/airborne/firmwares/fixedwing/nav.c @@ -451,45 +451,45 @@ void nav_periodic_task(void) { #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_nav_ref(void) { - DOWNLINK_SEND_NAVIGATION_REF(DefaultChannel, DefaultDevice, +static void send_nav_ref(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_NAVIGATION_REF(trans, dev, AC_ID, &nav_utm_east0, &nav_utm_north0, &nav_utm_zone0, &ground_alt); } -static void send_nav(void) { - SEND_NAVIGATION(DefaultChannel, DefaultDevice); +static void send_nav(struct transport_tx *trans, struct link_device *dev) { + SEND_NAVIGATION(trans, dev); } -static void send_wp_moved(void) { +static void send_wp_moved(struct transport_tx *trans, struct link_device *dev) { static uint8_t i; i++; if (i >= nb_waypoint) i = 0; - DownlinkSendWp(DefaultChannel, DefaultDevice, i); + DownlinkSendWp(trans, dev, i); } -bool_t DownlinkSendWpNr(int _wp) +bool_t DownlinkSendWpNr(uint8_t _wp) { - DownlinkSendWp(DefaultChannel, DefaultDevice, _wp); + DownlinkSendWp(&(DefaultChannel).trans_tx, &(DefaultDevice).device, _wp); return FALSE; } -static void send_circle(void) { +static void send_circle(struct transport_tx *trans, struct link_device *dev) { if (nav_in_circle) { - DOWNLINK_SEND_CIRCLE(DefaultChannel, DefaultDevice, + pprz_msg_send_CIRCLE(trans, dev, AC_ID, &nav_circle_x, &nav_circle_y, &nav_circle_radius); } } -static void send_segment(void) { +static void send_segment(struct transport_tx *trans, struct link_device *dev) { if (nav_in_segment) { - DOWNLINK_SEND_SEGMENT(DefaultChannel, DefaultDevice, + pprz_msg_send_SEGMENT(trans, dev, AC_ID, &nav_segment_x_1, &nav_segment_y_1, &nav_segment_x_2, &nav_segment_y_2); } } -static void send_survey(void) { +static void send_survey(struct transport_tx *trans, struct link_device *dev) { if (nav_survey_active) { - DOWNLINK_SEND_SURVEY(DefaultChannel, DefaultDevice, + pprz_msg_send_SURVEY(trans, dev, AC_ID, &nav_survey_east, &nav_survey_north, &nav_survey_west, &nav_survey_south); } } diff --git a/sw/airborne/firmwares/fixedwing/nav.h b/sw/airborne/firmwares/fixedwing/nav.h index cf7b70d9e2..4cbbfa4d3c 100644 --- a/sw/airborne/firmwares/fixedwing/nav.h +++ b/sw/airborne/firmwares/fixedwing/nav.h @@ -206,15 +206,15 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap struct EnuCoor_f* pos = stateGetPositionEnu_f(); \ float dist_wp = sqrtf(dist2_to_wp); \ float dist_home = sqrtf(dist2_to_home); \ - DOWNLINK_SEND_NAVIGATION(_trans, _dev, &nav_block, &nav_stage, &(pos->x), &(pos->y), &dist_wp, &dist_home, &_circle_count, &nav_oval_count); \ + pprz_msg_send_NAVIGATION(_trans, _dev, AC_ID, &nav_block, &nav_stage, &(pos->x), &(pos->y), &dist_wp, &dist_home, &_circle_count, &nav_oval_count); \ } -extern bool_t DownlinkSendWpNr(int _wp); +extern bool_t DownlinkSendWpNr(uint8_t _wp); #define DownlinkSendWp(_trans, _dev, i) { \ float x = nav_utm_east0 + waypoints[i].x; \ float y = nav_utm_north0 + waypoints[i].y; \ - DOWNLINK_SEND_WP_MOVED(_trans, _dev, &i, &x, &y, &(waypoints[i].a),&nav_utm_zone0); \ + pprz_msg_send_WP_MOVED(_trans, _dev, AC_ID, &i, &x, &y, &(waypoints[i].a),&nav_utm_zone0); \ } #endif /* NAV_H */ diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c index 61c9a08ffd..17824ba230 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c @@ -192,17 +192,17 @@ inline static void h_ctl_pitch_loop( void ); #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_calibration(void) { - DOWNLINK_SEND_CALIBRATION(DefaultChannel, DefaultDevice, &v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_submode); +static void send_calibration(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_CALIBRATION(trans, dev, AC_ID, &v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_submode); } -static void send_tune_roll(void) { - DOWNLINK_SEND_TUNE_ROLL(DefaultChannel, DefaultDevice, +static void send_tune_roll(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_TUNE_ROLL(trans, dev, AC_ID, &(stateGetBodyRates_f()->p), &(stateGetNedToBodyEulers_f()->phi), &h_ctl_roll_setpoint); } -static void send_ctl_a(void) { - DOWNLINK_SEND_H_CTL_A(DefaultChannel, DefaultDevice, +static void send_ctl_a(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_H_CTL_A(trans, dev, AC_ID, &h_ctl_roll_sum_err, &h_ctl_roll_setpoint, &h_ctl_ref.roll_angle, diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c index 9ddd5d606c..5f1ffc409c 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c @@ -115,8 +115,8 @@ static float nav_ratio; #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_calibration(void) { - DOWNLINK_SEND_CALIBRATION(DefaultChannel, DefaultDevice, &v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_submode); +static void send_calibration(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_CALIBRATION(trans, dev, AC_ID, &v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_submode); } #endif diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index 1173fd0da6..06e081532b 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -138,15 +138,15 @@ PRINT_CONFIG_MSG("Enabled UNLOCKED_HOME_MODE since MODE_AUTO2 is AP_MODE_HOME") #endif #endif -static void send_alive(void) { - DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); +static void send_alive(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_ALIVE(trans, dev, AC_ID, 16, MD5SUM); } #if USE_MOTOR_MIXING #include "subsystems/actuators/motor_mixing.h" #endif -static void send_status(void) { +static void send_status(struct transport_tx *trans, struct link_device *dev) { uint32_t imu_nb_err = 0; #if USE_MOTOR_MIXING uint8_t _motor_nb_err = motor_mixing.nb_saturation + motor_mixing.nb_failure * 10; @@ -159,7 +159,7 @@ static void send_status(void) { uint8_t fix = GPS_FIX_NONE; #endif uint16_t time_sec = sys_time.nb_sec; - DOWNLINK_SEND_ROTORCRAFT_STATUS(DefaultChannel, DefaultDevice, + pprz_msg_send_ROTORCRAFT_STATUS(trans, dev, AC_ID, &imu_nb_err, &_motor_nb_err, &radio_control.status, &radio_control.frame_rate, &fix, &autopilot_mode, @@ -168,17 +168,17 @@ static void send_status(void) { &electrical.vsupply, &time_sec); } -static void send_energy(void) { - const int16_t e = electrical.energy; - const float vsup = ((float)electrical.vsupply) / 10.0f; - const float curs = ((float)electrical.current) / 1000.0f; - const float power = vsup * curs; - DOWNLINK_SEND_ENERGY(DefaultChannel, DefaultDevice, &vsup, &curs, &e, &power); +static void send_energy(struct transport_tx *trans, struct link_device *dev) { + uint16_t e = electrical.energy; + float vsup = ((float)electrical.vsupply) / 10.0f; + float curs = ((float)electrical.current) / 1000.0f; + float power = vsup * curs; + pprz_msg_send_ENERGY(trans, dev, AC_ID, &vsup, &curs, &e, &power); } -static void send_fp(void) { +static void send_fp(struct transport_tx *trans, struct link_device *dev) { int32_t carrot_up = -guidance_v_z_sp; - DOWNLINK_SEND_ROTORCRAFT_FP(DefaultChannel, DefaultDevice, + pprz_msg_send_ROTORCRAFT_FP(trans, dev, AC_ID, &(stateGetPositionEnu_i()->x), &(stateGetPositionEnu_i()->y), &(stateGetPositionEnu_i()->z), @@ -197,17 +197,17 @@ static void send_fp(void) { } #ifdef RADIO_CONTROL -static void send_rc(void) { - DOWNLINK_SEND_RC(DefaultChannel, DefaultDevice, RADIO_CONTROL_NB_CHANNEL, radio_control.values); +static void send_rc(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_RC(trans, dev, AC_ID, RADIO_CONTROL_NB_CHANNEL, radio_control.values); } -static void send_rotorcraft_rc(void) { +static void send_rotorcraft_rc(struct transport_tx *trans, struct link_device *dev) { #ifdef RADIO_KILL_SWITCH int16_t _kill_switch = radio_control.values[RADIO_KILL_SWITCH]; #else int16_t _kill_switch = 42; #endif - DOWNLINK_SEND_ROTORCRAFT_RADIO_CONTROL(DefaultChannel, DefaultDevice, + pprz_msg_send_ROTORCRAFT_RADIO_CONTROL(trans, dev, AC_ID, &radio_control.values[RADIO_ROLL], &radio_control.values[RADIO_PITCH], &radio_control.values[RADIO_YAW], @@ -219,17 +219,17 @@ static void send_rotorcraft_rc(void) { #endif #ifdef ACTUATORS -static void send_actuators(void) { - DOWNLINK_SEND_ACTUATORS(DefaultChannel, DefaultDevice , ACTUATORS_NB, actuators); +static void send_actuators(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_ACTUATORS(trans, dev, AC_ID , ACTUATORS_NB, actuators); } #endif -static void send_dl_value(void) { - PeriodicSendDlValue(DefaultChannel, DefaultDevice); +static void send_dl_value(struct transport_tx *trans, struct link_device *dev) { + PeriodicSendDlValue(trans, dev); } -static void send_rotorcraft_cmd(void) { - DOWNLINK_SEND_ROTORCRAFT_CMD(DefaultChannel, DefaultDevice, +static void send_rotorcraft_cmd(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_ROTORCRAFT_CMD(trans, dev, AC_ID, &stabilization_cmd[COMMAND_ROLL], &stabilization_cmd[COMMAND_PITCH], &stabilization_cmd[COMMAND_YAW], diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index d501811885..3bc9287532 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -109,19 +109,19 @@ static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool_t in_flig #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_gh(void) { +static void send_gh(struct transport_tx *trans, struct link_device *dev) { struct NedCoor_i* pos = stateGetPositionNed_i(); - DOWNLINK_SEND_GUIDANCE_H_INT(DefaultChannel, DefaultDevice, + pprz_msg_send_GUIDANCE_H_INT(trans, dev, AC_ID, &guidance_h_pos_sp.x, &guidance_h_pos_sp.y, &guidance_h_pos_ref.x, &guidance_h_pos_ref.y, &(pos->x), &(pos->y)); } -static void send_hover_loop(void) { +static void send_hover_loop(struct transport_tx *trans, struct link_device *dev) { struct NedCoor_i* pos = stateGetPositionNed_i(); struct NedCoor_i* speed = stateGetSpeedNed_i(); struct NedCoor_i* accel = stateGetAccelNed_i(); - DOWNLINK_SEND_HOVER_LOOP(DefaultChannel, DefaultDevice, + pprz_msg_send_HOVER_LOOP(trans, dev, AC_ID, &guidance_h_pos_sp.x, &guidance_h_pos_sp.y, &(pos->x), &(pos->y), @@ -138,8 +138,8 @@ static void send_hover_loop(void) { &guidance_h_heading_sp); } -static void send_href(void) { - DOWNLINK_SEND_GUIDANCE_H_REF_INT(DefaultChannel, DefaultDevice, +static void send_href(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_GUIDANCE_H_REF_INT(trans, dev, AC_ID, &guidance_h_pos_sp.x, &guidance_h_pos_ref.x, &guidance_h_speed_sp.x, &guidance_h_speed_ref.x, &guidance_h_accel_ref.x, @@ -148,8 +148,8 @@ static void send_href(void) { &guidance_h_accel_ref.y); } -static void send_tune_hover(void) { - DOWNLINK_SEND_ROTORCRAFT_TUNE_HOVER(DefaultChannel, DefaultDevice, +static void send_tune_hover(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_ROTORCRAFT_TUNE_HOVER(trans, dev, AC_ID, &radio_control.values[RADIO_ROLL], &radio_control.values[RADIO_PITCH], &radio_control.values[RADIO_YAW], diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index 861606e290..44e9a36912 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -142,8 +142,8 @@ static void run_hover_loop(bool_t in_flight); #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_vert_loop(void) { - DOWNLINK_SEND_VERT_LOOP(DefaultChannel, DefaultDevice, +static void send_vert_loop(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_VERT_LOOP(trans, dev, AC_ID, &guidance_v_z_sp, &guidance_v_zd_sp, &(stateGetPositionNed_i()->z), &(stateGetSpeedNed_i()->z), @@ -159,8 +159,8 @@ static void send_vert_loop(void) { &guidance_v_delta_t); } -static void send_tune_vert(void) { - DOWNLINK_SEND_TUNE_VERT(DefaultChannel, DefaultDevice, +static void send_tune_vert(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_TUNE_VERT(trans, dev, AC_ID, &guidance_v_z_sp, &(stateGetPositionNed_i()->z), &guidance_v_z_ref, diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index acdb192ac2..80a6510ea1 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -41,8 +41,8 @@ #include "subsystems/datalink/telemetry.h" #include "subsystems/datalink/datalink.h" +#include "subsystems/datalink/downlink.h" #include "subsystems/settings.h" -#include "subsystems/datalink/xbee.h" #include "subsystems/commands.h" #include "subsystems/actuators.h" @@ -181,9 +181,7 @@ STATIC_INLINE void main_init( void ) { mcu_int_enable(); -#if DATALINK == XBEE - xbee_init(); -#endif + downlink_init(); // register the timers for the periodic functions main_periodic_tid = sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); @@ -234,7 +232,7 @@ STATIC_INLINE void main_periodic( void ) { } STATIC_INLINE void telemetry_periodic(void) { - periodic_telemetry_send_Main(); + periodic_telemetry_send_Main(&(DefaultChannel).trans_tx, &(DefaultDevice).device); } /** mode to enter when RC is lost while using a mode with RC input (not AP_MODE_NAV) */ diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index b9fc228c15..b99afe568a 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -106,10 +106,10 @@ static inline void nav_set_altitude( void ); #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_nav_status(void) { +static void send_nav_status(struct transport_tx *trans, struct link_device *dev) { float dist_home = sqrtf(dist2_to_home); float dist_wp = sqrtf(dist2_to_wp); - DOWNLINK_SEND_ROTORCRAFT_NAV_STATUS(DefaultChannel, DefaultDevice, + pprz_msg_send_ROTORCRAFT_NAV_STATUS(trans, dev, AC_ID, &block_time, &stage_time, &dist_home, &dist_wp, &nav_block, &nav_stage, @@ -119,20 +119,20 @@ static void send_nav_status(void) { float sy = POS_FLOAT_OF_BFP(nav_segment_start.y); float ex = POS_FLOAT_OF_BFP(nav_segment_end.x); float ey = POS_FLOAT_OF_BFP(nav_segment_end.y); - DOWNLINK_SEND_SEGMENT(DefaultChannel, DefaultDevice, &sx, &sy, &ex, &ey); + pprz_msg_send_SEGMENT(trans, dev, AC_ID, &sx, &sy, &ex, &ey); } else if (horizontal_mode == HORIZONTAL_MODE_CIRCLE) { float cx = POS_FLOAT_OF_BFP(nav_circle_center.x); float cy = POS_FLOAT_OF_BFP(nav_circle_center.y); float r = POS_FLOAT_OF_BFP(nav_circle_radius); - DOWNLINK_SEND_CIRCLE(DefaultChannel, DefaultDevice, &cx, &cy, &r); + pprz_msg_send_CIRCLE(trans, dev, AC_ID, &cx, &cy, &r); } } -static void send_wp_moved(void) { +static void send_wp_moved(struct transport_tx *trans, struct link_device *dev) { static uint8_t i; i++; if (i >= nb_waypoint) i = 0; - DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, + pprz_msg_send_WP_MOVED_ENU(trans, dev, AC_ID, &i, &(waypoints[i].x), &(waypoints[i].y), diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c index d1d9763120..1f19c09559 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c @@ -44,11 +44,11 @@ float stabilization_att_ff_cmd[COMMANDS_NB]; #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_att(void) { +static void send_att(struct transport_tx *trans, struct link_device *dev) { struct FloatRates* body_rate = stateGetBodyRates_f(); struct FloatEulers* att = stateGetNedToBodyEulers_f(); float foo = 0.0; - DOWNLINK_SEND_STAB_ATTITUDE_FLOAT(DefaultChannel, DefaultDevice, + pprz_msg_send_STAB_ATTITUDE_FLOAT(trans, dev, AC_ID, &(body_rate->p), &(body_rate->q), &(body_rate->r), &(att->phi), &(att->theta), &(att->psi), &stab_att_sp_euler.phi, @@ -69,8 +69,8 @@ static void send_att(void) { &foo, &foo, &foo); } -static void send_att_ref(void) { - DOWNLINK_SEND_STAB_ATTITUDE_REF_FLOAT(DefaultChannel, DefaultDevice, +static void send_att_ref(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_STAB_ATTITUDE_REF_FLOAT(trans, dev, AC_ID, &stab_att_sp_euler.phi, &stab_att_sp_euler.theta, &stab_att_sp_euler.psi, diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c index 04e8a09401..03731bf2a9 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c @@ -65,10 +65,10 @@ static inline void reset_psi_ref_from_body(void) { #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_att(void) { +static void send_att(struct transport_tx *trans, struct link_device *dev) { struct Int32Rates* body_rate = stateGetBodyRates_i(); struct Int32Eulers* att = stateGetNedToBodyEulers_i(); - DOWNLINK_SEND_STAB_ATTITUDE_INT(DefaultChannel, DefaultDevice, + pprz_msg_send_STAB_ATTITUDE_INT(trans, dev, AC_ID, &(body_rate->p), &(body_rate->q), &(body_rate->r), &(att->phi), &(att->theta), &(att->psi), &stab_att_sp_euler.phi, @@ -88,8 +88,8 @@ static void send_att(void) { &stabilization_cmd[COMMAND_YAW]); } -static void send_att_ref(void) { - DOWNLINK_SEND_STAB_ATTITUDE_REF_INT(DefaultChannel, DefaultDevice, +static void send_att_ref(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_STAB_ATTITUDE_REF_INT(trans, dev, AC_ID, &stab_att_sp_euler.phi, &stab_att_sp_euler.theta, &stab_att_sp_euler.psi, diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c index 48ed47932f..4d296a1f21 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c @@ -30,6 +30,7 @@ #include "state.h" #include "firmwares/rotorcraft/stabilization.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h" #include "paparazzi.h" diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c index a881c75197..b7542fd772 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -95,10 +95,10 @@ static const float psi_ddgain_surface[] = STABILIZATION_ATTITUDE_PSI_DDGAIN_SURF #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_att(void) { +static void send_att(struct transport_tx *trans, struct link_device *dev) { struct FloatRates* body_rate = stateGetBodyRates_f(); struct FloatEulers* att = stateGetNedToBodyEulers_f(); - DOWNLINK_SEND_STAB_ATTITUDE_FLOAT(DefaultChannel, DefaultDevice, + pprz_msg_send_STAB_ATTITUDE_FLOAT(trans, dev, AC_ID, &(body_rate->p), &(body_rate->q), &(body_rate->r), &(att->phi), &(att->theta), &(att->psi), &stab_att_sp_euler.phi, @@ -119,8 +119,8 @@ static void send_att(void) { &body_rate_d.p, &body_rate_d.q, &body_rate_d.r); } -static void send_att_ref(void) { - DOWNLINK_SEND_STAB_ATTITUDE_REF_FLOAT(DefaultChannel, DefaultDevice, +static void send_att_ref(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_STAB_ATTITUDE_REF_FLOAT(trans, dev, AC_ID, &stab_att_sp_euler.phi, &stab_att_sp_euler.theta, &stab_att_sp_euler.psi, diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c index 145d902815..a8db35f28b 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -69,10 +69,10 @@ int32_t stabilization_att_ff_cmd[COMMANDS_NB]; #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_att(void) { //FIXME really use this message here ? +static void send_att(struct transport_tx *trans, struct link_device *dev) { //FIXME really use this message here ? struct Int32Rates* body_rate = stateGetBodyRates_i(); struct Int32Eulers* att = stateGetNedToBodyEulers_i(); - DOWNLINK_SEND_STAB_ATTITUDE_INT(DefaultChannel, DefaultDevice, + pprz_msg_send_STAB_ATTITUDE_INT(trans, dev, AC_ID, &(body_rate->p), &(body_rate->q), &(body_rate->r), &(att->phi), &(att->theta), &(att->psi), &stab_att_sp_euler.phi, @@ -92,8 +92,8 @@ static void send_att(void) { //FIXME really use this message here ? &stabilization_cmd[COMMAND_YAW]); } -static void send_att_ref(void) { - DOWNLINK_SEND_STAB_ATTITUDE_REF_INT(DefaultChannel, DefaultDevice, +static void send_att_ref(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_STAB_ATTITUDE_REF_INT(trans, dev, AC_ID, &stab_att_sp_euler.phi, &stab_att_sp_euler.theta, &stab_att_sp_euler.psi, @@ -108,9 +108,9 @@ static void send_att_ref(void) { &stab_att_ref_accel.r); } -static void send_ahrs_ref_quat(void) { +static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev) { struct Int32Quat* quat = stateGetNedToBodyQuat_i(); - DOWNLINK_SEND_AHRS_REF_QUAT(DefaultChannel, DefaultDevice, + pprz_msg_send_AHRS_REF_QUAT(trans, dev, AC_ID, &stab_att_ref_quat.qi, &stab_att_ref_quat.qx, &stab_att_ref_quat.qy, diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c index 191fa668e2..1b40f8445e 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c @@ -118,8 +118,8 @@ struct Int32Rates stabilization_rate_ff_cmd; #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_rate(void) { - DOWNLINK_SEND_RATE_LOOP(DefaultChannel, DefaultDevice, +static void send_rate(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_RATE_LOOP(trans, dev, AC_ID, &stabilization_rate_sp.p, &stabilization_rate_sp.q, &stabilization_rate_sp.r, diff --git a/sw/airborne/fms/udp_transport.h b/sw/airborne/fms/udp_transport.h index 2de1a8ef9f..1d73e771e8 100644 --- a/sw/airborne/fms/udp_transport.h +++ b/sw/airborne/fms/udp_transport.h @@ -35,10 +35,10 @@ extern uint8_t udpt_ck_a, udpt_ck_b; if (udpt_tx_buf_idx) { \ int len; \ len = network_write(network, updt_tx_buf, udpt_tx_buf_idx); \ - downlink_nb_bytes += udpt_tx_buf_idx; \ - downlink_nb_msgs++; \ + downlink.nb_bytes += udpt_tx_buf_idx; \ + downlink.nb_msgs++; \ if (len != udpt_tx_buf_idx) \ - downlink_nb_ovrn++; \ + downlink.nb_ovrn++; \ udpt_tx_buf_idx = 0; \ } \ } diff --git a/sw/airborne/link_mcu_can.c b/sw/airborne/link_mcu_can.c index 1880815492..a457f6c704 100644 --- a/sw/airborne/link_mcu_can.c +++ b/sw/airborne/link_mcu_can.c @@ -184,12 +184,12 @@ struct link_mcu_msg link_mcu_from_fbw_msg; #define RC_REALLY_LOST 2 -static void send_commands(void) +static void send_commands(struct transport_tx *trans, struct link_device *dev) { - DOWNLINK_SEND_COMMANDS(DefaultChannel, DefaultDevice, COMMANDS_NB, ap_state->commands); + pprz_msg_send_COMMANDS(trans, dev, AC_ID, COMMANDS_NB, ap_state->commands); } -static void send_fbw_status(void) +static void send_fbw_status(struct transport_tx *trans, struct link_device *dev) { uint8_t rc_status = 0; uint8_t fbw_status = 0; @@ -206,7 +206,7 @@ static void send_fbw_status(void) } else { rc_status = RC_LOST; } - DOWNLINK_SEND_FBW_STATUS(DefaultChannel, DefaultDevice, + pprz_msg_send_FBW_STATUS(trans, dev, AC_ID, &(rc_status), &(fbw_state->ppm_cpt), &(fbw_status), &(fbw_state->vsupply), &(fbw_state->current)); } #endif diff --git a/sw/airborne/link_mcu_spi.c b/sw/airborne/link_mcu_spi.c index 750436958f..87efc22a5e 100644 --- a/sw/airborne/link_mcu_spi.c +++ b/sw/airborne/link_mcu_spi.c @@ -106,9 +106,9 @@ uint8_t link_mcu_fbw_nb_err; #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_debug_link(void) { +static void send_debug_link(struct transport_tx *trans, struct link_device *dev) { uint8_t mcu1_ppm_cpt_foo = 0; //FIXME - DOWNLINK_SEND_DEBUG_MCU_LINK(DefaultChannel, DefaultDevice, + pprz_msg_send_DEBUG_MCU_LINK(trans, dev, AC_ID, &link_mcu_nb_err, &link_mcu_fbw_nb_err, &mcu1_ppm_cpt_foo); } #endif diff --git a/sw/airborne/link_mcu_usart.c b/sw/airborne/link_mcu_usart.c index 07052ed9ce..da5502ff40 100644 --- a/sw/airborne/link_mcu_usart.c +++ b/sw/airborne/link_mcu_usart.c @@ -254,12 +254,12 @@ inline void parse_mavpilot_msg( void ); #define RC_REALLY_LOST 2 -static void send_commands(void) { - DOWNLINK_SEND_COMMANDS(DefaultChannel, DefaultDevice, COMMANDS_NB, ap_state->commands); +static void send_commands(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_COMMANDS(trans, dev, AC_ID, COMMANDS_NB, ap_state->commands); } -static void send_fbw_status(void) { +static void send_fbw_status(struct transport_tx *trans, struct link_device *dev) { uint8_t rc_status = 0; uint8_t fbw_status = 0; if (bit_is_set(fbw_state->status, STATUS_MODE_AUTO)) @@ -272,7 +272,7 @@ static void send_fbw_status(void) { rc_status = RC_OK; else rc_status = RC_LOST; - DOWNLINK_SEND_FBW_STATUS(DefaultChannel, DefaultDevice, + pprz_msg_send_FBW_STATUS(trans, dev, AC_ID, &(rc_status), &(fbw_state->ppm_cpt), &(fbw_status), &(fbw_state->vsupply), &(fbw_state->current)); } #endif diff --git a/sw/airborne/mcu_periph/i2c.c b/sw/airborne/mcu_periph/i2c.c index aa5c0f37d0..ef2c6db820 100644 --- a/sw/airborne/mcu_periph/i2c.c +++ b/sw/airborne/mcu_periph/i2c.c @@ -36,7 +36,7 @@ struct i2c_periph i2c0; #if PERIODIC_TELEMETRY -static void send_i2c0_err(void) { +static void send_i2c0_err(struct transport_tx *trans, struct link_device *dev) { uint16_t i2c0_queue_full_cnt = i2c0.errors->queue_full_cnt; uint16_t i2c0_ack_fail_cnt = i2c0.errors->ack_fail_cnt; uint16_t i2c0_miss_start_stop_cnt = i2c0.errors->miss_start_stop_cnt; @@ -47,8 +47,8 @@ static void send_i2c0_err(void) { uint16_t i2c0_smbus_alert_cnt = i2c0.errors->smbus_alert_cnt; uint16_t i2c0_unexpected_event_cnt = i2c0.errors->unexpected_event_cnt; uint32_t i2c0_last_unexpected_event = i2c0.errors->last_unexpected_event; - const uint8_t _bus0 = 0; - DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice, + uint8_t _bus0 = 0; + pprz_msg_send_I2C_ERRORS(trans, dev, AC_ID, &i2c0_queue_full_cnt, &i2c0_ack_fail_cnt, &i2c0_miss_start_stop_cnt, @@ -76,7 +76,7 @@ void i2c0_init(void) { struct i2c_periph i2c1; #if PERIODIC_TELEMETRY -static void send_i2c1_err(void) { +static void send_i2c1_err(struct transport_tx *trans, struct link_device *dev) { uint16_t i2c1_queue_full_cnt = i2c1.errors->queue_full_cnt; uint16_t i2c1_ack_fail_cnt = i2c1.errors->ack_fail_cnt; uint16_t i2c1_miss_start_stop_cnt = i2c1.errors->miss_start_stop_cnt; @@ -87,8 +87,8 @@ static void send_i2c1_err(void) { uint16_t i2c1_smbus_alert_cnt = i2c1.errors->smbus_alert_cnt; uint16_t i2c1_unexpected_event_cnt = i2c1.errors->unexpected_event_cnt; uint32_t i2c1_last_unexpected_event = i2c1.errors->last_unexpected_event; - const uint8_t _bus1 = 1; - DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice, + uint8_t _bus1 = 1; + pprz_msg_send_I2C_ERRORS(trans, dev, AC_ID, &i2c1_queue_full_cnt, &i2c1_ack_fail_cnt, &i2c1_miss_start_stop_cnt, @@ -116,7 +116,7 @@ void i2c1_init(void) { struct i2c_periph i2c2; #if PERIODIC_TELEMETRY -static void send_i2c2_err(void) { +static void send_i2c2_err(struct transport_tx *trans, struct link_device *dev) { uint16_t i2c2_queue_full_cnt = i2c2.errors->queue_full_cnt; uint16_t i2c2_ack_fail_cnt = i2c2.errors->ack_fail_cnt; uint16_t i2c2_miss_start_stop_cnt = i2c2.errors->miss_start_stop_cnt; @@ -127,8 +127,8 @@ static void send_i2c2_err(void) { uint16_t i2c2_smbus_alert_cnt = i2c2.errors->smbus_alert_cnt; uint16_t i2c2_unexpected_event_cnt = i2c2.errors->unexpected_event_cnt; uint32_t i2c2_last_unexpected_event = i2c2.errors->last_unexpected_event; - const uint8_t _bus2 = 2; - DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice, + uint8_t _bus2 = 2; + pprz_msg_send_I2C_ERRORS(trans, dev, AC_ID, &i2c2_queue_full_cnt, &i2c2_ack_fail_cnt, &i2c2_miss_start_stop_cnt, @@ -160,7 +160,7 @@ void i2c3_init(void) { } #if PERIODIC_TELEMETRY -static void send_i2c3_err(void) { +static void send_i2c3_err(struct transport_tx *trans, struct link_device *dev) { uint16_t i2c3_queue_full_cnt = i2c3.errors->queue_full_cnt; uint16_t i2c3_ack_fail_cnt = i2c3.errors->ack_fail_cnt; uint16_t i2c3_miss_start_stop_cnt = i2c3.errors->miss_start_stop_cnt; @@ -171,8 +171,8 @@ static void send_i2c3_err(void) { uint16_t i2c3_smbus_alert_cnt = i2c3.errors->smbus_alert_cnt; uint16_t i2c3_unexpected_event_cnt = i2c3.errors->unexpected_event_cnt; uint32_t i2c3_last_unexpected_event = i2c3.errors->last_unexpected_event; - const uint8_t _bus3 = 3; - DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice, + uint8_t _bus3 = 3; + pprz_msg_send_I2C_ERRORS(trans, dev, AC_ID, &i2c3_queue_full_cnt, &i2c3_ack_fail_cnt, &i2c3_miss_start_stop_cnt, @@ -190,27 +190,27 @@ static void send_i2c3_err(void) { #endif /* USE_I2C3 */ #if PERIODIC_TELEMETRY -static void send_i2c_err(void) { +static void send_i2c_err(struct transport_tx *trans __attribute__((unused)), struct link_device *dev __attribute__((unused))) { static uint8_t _i2c_nb_cnt = 0; switch (_i2c_nb_cnt) { case 0: #if USE_I2C0 - send_i2c0_err(); + send_i2c0_err(trans, dev); #endif break; case 1: #if USE_I2C1 - send_i2c1_err(); + send_i2c1_err(trans, dev); #endif break; case 2: #if USE_I2C2 - send_i2c2_err(); + send_i2c2_err(trans, dev); #endif break; case 3: #if USE_I2C3 - send_i2c3_err(); + send_i2c3_err(trans, dev); #endif break; default: diff --git a/sw/airborne/mcu_periph/link_device.h b/sw/airborne/mcu_periph/link_device.h new file mode 100644 index 0000000000..04d2186479 --- /dev/null +++ b/sw/airborne/mcu_periph/link_device.h @@ -0,0 +1,50 @@ +/* + * 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 (*transmit_t)(void *, uint8_t); +typedef void (*send_message_t)(void *); + +/** Device structure + */ +struct link_device { + check_free_space_t check_free_space; ///< check if transmit buffer is not full + transmit_t transmit; ///< transmit one byte + send_message_t send_message; ///< send completed buffer + void *periph; ///< pointer to parent implementation +}; + +#endif // LINK_DEVICE_H + diff --git a/sw/airborne/mcu_periph/uart.c b/sw/airborne/mcu_periph/uart.c index 0c3dfc0e7e..ffb926d1f1 100644 --- a/sw/airborne/mcu_periph/uart.c +++ b/sw/airborne/mcu_periph/uart.c @@ -30,12 +30,12 @@ struct uart_periph uart0; #if PERIODIC_TELEMETRY -static void send_uart0_err(void) { +static void send_uart0_err(struct transport_tx *trans, struct link_device *dev) { uint16_t ore = uart0.ore; uint16_t ne_err = uart0.ne_err; uint16_t fe_err = uart0.fe_err; - const uint8_t _bus0 = 0; - DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, + uint8_t _bus0 = 0; + pprz_msg_send_UART_ERRORS(trans, dev, AC_ID, &ore, &ne_err, &fe_err, &_bus0); } #endif @@ -46,12 +46,12 @@ static void send_uart0_err(void) { struct uart_periph uart1; #if PERIODIC_TELEMETRY -static void send_uart1_err(void) { +static void send_uart1_err(struct transport_tx *trans, struct link_device *dev) { uint16_t ore = uart1.ore; uint16_t ne_err = uart1.ne_err; uint16_t fe_err = uart1.fe_err; - const uint8_t _bus1 = 1; - DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, + uint8_t _bus1 = 1; + pprz_msg_send_UART_ERRORS(trans, dev, AC_ID, &ore, &ne_err, &fe_err, &_bus1); } #endif @@ -62,12 +62,12 @@ static void send_uart1_err(void) { struct uart_periph uart2; #if PERIODIC_TELEMETRY -static void send_uart2_err(void) { +static void send_uart2_err(struct transport_tx *trans, struct link_device *dev) { uint16_t ore = uart2.ore; uint16_t ne_err = uart2.ne_err; uint16_t fe_err = uart2.fe_err; - const uint8_t _bus2 = 2; - DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, + uint8_t _bus2 = 2; + pprz_msg_send_UART_ERRORS(trans, dev, AC_ID, &ore, &ne_err, &fe_err, &_bus2); } #endif @@ -78,12 +78,12 @@ static void send_uart2_err(void) { struct uart_periph uart3; #if PERIODIC_TELEMETRY -static void send_uart3_err(void) { +static void send_uart3_err(struct transport_tx *trans, struct link_device *dev) { uint16_t ore = uart3.ore; uint16_t ne_err = uart3.ne_err; uint16_t fe_err = uart3.fe_err; - const uint8_t _bus3 = 3; - DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, + uint8_t _bus3 = 3; + pprz_msg_send_UART_ERRORS(trans, dev, AC_ID, &ore, &ne_err, &fe_err, &_bus3); } #endif @@ -94,12 +94,12 @@ static void send_uart3_err(void) { struct uart_periph uart4; #if PERIODIC_TELEMETRY -static void send_uart4_err(void) { +static void send_uart4_err(struct transport_tx *trans, struct link_device *dev) { uint16_t ore = uart4.ore; uint16_t ne_err = uart4.ne_err; uint16_t fe_err = uart4.fe_err; - const uint8_t _bus4 = 4; - DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, + uint8_t _bus4 = 4; + pprz_msg_send_UART_ERRORS(trans, dev, AC_ID, &ore, &ne_err, &fe_err, &_bus4); } #endif @@ -110,12 +110,12 @@ static void send_uart4_err(void) { struct uart_periph uart5; #if PERIODIC_TELEMETRY -static void send_uart5_err(void) { +static void send_uart5_err(struct transport_tx *trans, struct link_device *dev) { uint16_t ore = uart5.ore; uint16_t ne_err = uart5.ne_err; uint16_t fe_err = uart5.fe_err; - const uint8_t _bus5 = 5; - DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, + uint8_t _bus5 = 5; + pprz_msg_send_UART_ERRORS(trans, dev, AC_ID, &ore, &ne_err, &fe_err, &_bus5); } #endif @@ -126,12 +126,12 @@ static void send_uart5_err(void) { struct uart_periph uart6; #if PERIODIC_TELEMETRY -static void send_uart6_err(void) { - const uint8_t _bus6 = 6; +static void send_uart6_err(struct transport_tx *trans, struct link_device *dev) { uint16_t ore = uart6.ore; uint16_t ne_err = uart6.ne_err; uint16_t fe_err = uart6.fe_err; - DOWNLINK_SEND_UART_ERRORS(DefaultChannel, DefaultDevice, + uint8_t _bus6 = 6; + pprz_msg_send_UART_ERRORS(trans, dev, AC_ID, &ore, &ne_err, &fe_err, &_bus6); } #endif @@ -139,36 +139,36 @@ static void send_uart6_err(void) { #endif #if PERIODIC_TELEMETRY -static void send_uart_err(void) { +static void send_uart_err(struct transport_tx *trans, struct link_device *dev) { static uint8_t uart_nb_cnt = 0; switch (uart_nb_cnt) { #if USE_UART0 case 0: - send_uart0_err(); break; + send_uart0_err(trans, dev); break; #endif #if USE_UART1 case 1: - send_uart1_err(); break; + send_uart1_err(trans, dev); break; #endif #if USE_UART2 case 2: - send_uart2_err(); break; + send_uart2_err(trans, dev); break; #endif #if USE_UART3 case 3: - send_uart3_err(); break; + send_uart3_err(trans, dev); break; #endif #if USE_UART4 case 4: - send_uart4_err(); break; + send_uart4_err(trans, dev); break; #endif #if USE_UART5 case 5: - send_uart5_err(); break; + send_uart5_err(trans, dev); break; #endif #if USE_UART6 case 6: - send_uart6_err(); break; + send_uart6_err(trans, dev); break; #endif default: break; } @@ -178,6 +178,8 @@ static void send_uart_err(void) { } #endif +static void null_function(struct uart_periph* p __attribute__((unused))) {} + void uart_periph_init(struct uart_periph* p) { p->rx_insert_idx = 0; p->rx_extract_idx = 0; @@ -187,6 +189,10 @@ void uart_periph_init(struct uart_periph* p) { p->ore = 0; p->ne_err = 0; p->fe_err = 0; + p->device.periph = (void *)p; + p->device.check_free_space = (check_free_space_t)uart_check_free_space; + p->device.transmit = (transmit_t)uart_transmit; + p->device.send_message = (send_message_t)null_function; #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 83cee5bdd5..3c970f1c5b 100644 --- a/sw/airborne/mcu_periph/uart.h +++ b/sw/airborne/mcu_periph/uart.h @@ -29,6 +29,7 @@ #define MCU_PERIPH_UART_H #include "mcu_periph/uart_arch.h" +#include "mcu_periph/link_device.h" #include "std.h" #define UART_RX_BUFFER_SIZE 128 @@ -69,6 +70,8 @@ struct uart_periph { volatile uint16_t ore; ///< overrun error counter volatile uint16_t ne_err; ///< noise error counter volatile uint16_t fe_err; ///< framing error counter + /** Generic device interface */ + struct link_device device; }; diff --git a/sw/airborne/mcu_periph/udp.c b/sw/airborne/mcu_periph/udp.c index f747cb3054..69b45dfcef 100644 --- a/sw/airborne/mcu_periph/udp.c +++ b/sw/airborne/mcu_periph/udp.c @@ -32,6 +32,10 @@ void udp_periph_init(struct udp_periph* p, char* host, int port_out, int port_in p->rx_insert_idx = 0; p->rx_extract_idx = 0; p->tx_insert_idx = 0; + p->device.periph = (void *)p; + p->device.check_free_space = (check_free_space_t) udp_check_free_space; + p->device.transmit = (transmit_t) udp_transmit; + p->device.send_message = (send_message_t) udp_send_message; // Arch dependent initialization udp_arch_periph_init(p, host, port_out, port_in, broadcast); diff --git a/sw/airborne/mcu_periph/udp.h b/sw/airborne/mcu_periph/udp.h index 8c874302cb..5495ab621a 100644 --- a/sw/airborne/mcu_periph/udp.h +++ b/sw/airborne/mcu_periph/udp.h @@ -30,6 +30,7 @@ #include "std.h" #include "mcu_periph/udp_arch.h" +#include "mcu_periph/link_device.h" #define UDP_RX_BUFFER_SIZE 256 #define UDP_TX_BUFFER_SIZE 256 @@ -44,6 +45,8 @@ struct udp_periph { uint16_t tx_insert_idx; /** UDP network */ void* network; + /** Generic device interface */ + struct link_device device; }; extern void udp_periph_init(struct udp_periph* p, char* host, int port_out, int port_in, bool_t broadcast); diff --git a/sw/airborne/mcu_periph/usb_serial.h b/sw/airborne/mcu_periph/usb_serial.h index 996da6bb4d..cc37f3df2d 100644 --- a/sw/airborne/mcu_periph/usb_serial.h +++ b/sw/airborne/mcu_periph/usb_serial.h @@ -30,8 +30,16 @@ #include #include "std.h" +#include "mcu_periph/link_device.h" //#include "usb_serial_hw.h" +struct usb_serial_periph { + /** Generic device interface */ + struct link_device device; +}; + +extern struct usb_serial_periph usb_serial; + void VCOM_init(void); int VCOM_putchar(int c); int VCOM_getchar(void); diff --git a/sw/airborne/modules/air_data/air_data.c b/sw/airborne/modules/air_data/air_data.c index e387ab9239..e7576d4a46 100644 --- a/sw/airborne/modules/air_data/air_data.c +++ b/sw/airborne/modules/air_data/air_data.c @@ -138,27 +138,27 @@ static void temperature_cb(uint8_t __attribute__((unused)) sender_id, const floa #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_baro_raw(void) +static void send_baro_raw(struct transport_tx *trans, struct link_device *dev) { - DOWNLINK_SEND_BARO_RAW(DefaultChannel, DefaultDevice, + pprz_msg_send_BARO_RAW(trans, dev, AC_ID, &air_data.pressure, &air_data.differential); } -static void send_air_data(void) +static void send_air_data(struct transport_tx *trans, struct link_device *dev) { - DOWNLINK_SEND_AIR_DATA(DefaultChannel, DefaultDevice, + pprz_msg_send_AIR_DATA(trans, dev, AC_ID, &air_data.pressure, &air_data.differential, &air_data.temperature, &air_data.qnh, &air_data.amsl_baro, &air_data.airspeed, &air_data.tas_factor); } -static void send_amsl(void) +static void send_amsl(struct transport_tx *trans, struct link_device *dev) { const float MeterPerFeet = 0.3048; float amsl_baro_ft = air_data.amsl_baro / MeterPerFeet; float amsl_gps_ft = stateGetPositionLla_f()->alt / MeterPerFeet; - DOWNLINK_SEND_AMSL(DefaultChannel, DefaultDevice, &amsl_baro_ft, &amsl_gps_ft); + pprz_msg_send_AMSL(trans, dev, AC_ID, &amsl_baro_ft, &amsl_gps_ft); } #endif diff --git a/sw/airborne/modules/cam_control/cam.c b/sw/airborne/modules/cam_control/cam.c index 9893253d9c..41a994dab1 100644 --- a/sw/airborne/modules/cam_control/cam.c +++ b/sw/airborne/modules/cam_control/cam.c @@ -100,13 +100,13 @@ void cam_target(void); void cam_waypoint_target(void); void cam_ac_target(void); -static void send_cam(void) { +static void send_cam(struct transport_tx *trans, struct link_device *dev) { SEND_CAM(DefaultChannel, DefaultDevice); } #ifdef SHOW_CAM_COORDINATES -static void send_cam_point(void) { - DOWNLINK_SEND_CAM_POINT(DefaultChannel, DefaultDevice, +static void send_cam_point(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_CAM_POINT(trans, dev, AC_ID, &cam_point_distance_from_home, &cam_point_lat, &cam_point_lon); } #endif diff --git a/sw/airborne/modules/cam_control/rotorcraft_cam.c b/sw/airborne/modules/cam_control/rotorcraft_cam.c index 7adb4e491f..a801d69198 100644 --- a/sw/airborne/modules/cam_control/rotorcraft_cam.c +++ b/sw/airborne/modules/cam_control/rotorcraft_cam.c @@ -88,8 +88,8 @@ int16_t rotorcraft_cam_pan; #define ROTORCRAFT_CAM_PAN_MIN 0 #define ROTORCRAFT_CAM_PAN_MAX INT32_ANGLE_2_PI -static void send_cam(void) { - DOWNLINK_SEND_ROTORCRAFT_CAM(DefaultChannel, DefaultDevice, +static void send_cam(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_ROTORCRAFT_CAM(trans, dev, AC_ID, &rotorcraft_cam_tilt,&rotorcraft_cam_pan); } @@ -156,7 +156,7 @@ void rotorcraft_cam_periodic(void) { nav_heading = rotorcraft_cam_pan; #if ROTORCRAFT_CAM_USE_TILT_ANGLES int32_t dist, height; - INT32_VECT2_NORM(dist, diff); + dist = INT32_VECT2_NORM(diff); height = (waypoints[ROTORCRAFT_CAM_TRACK_WP].z - stateGetPositionEnu_i()->z) >> INT32_POS_FRAC; rotorcraft_cam_tilt = int32_atan2(height, dist); Bound(rotorcraft_cam_tilt, CAM_TA_MIN, CAM_TA_MAX); diff --git a/sw/airborne/modules/datalink/mavlink_decoder.h b/sw/airborne/modules/datalink/mavlink_decoder.h index 8f2622cafc..4fab5ccb0b 100644 --- a/sw/airborne/modules/datalink/mavlink_decoder.h +++ b/sw/airborne/modules/datalink/mavlink_decoder.h @@ -139,7 +139,7 @@ struct mavlink_msg_req { */ struct mavlink_transport { // generic interface - struct transport trans; + struct transport_rx trans; // specific mavlink transport variables mavlink_parse_state status; uint8_t payload_idx; diff --git a/sw/airborne/modules/digital_cam/dc.c b/sw/airborne/modules/digital_cam/dc.c index 4ee8fd2786..b6d20c6c6d 100644 --- a/sw/airborne/modules/digital_cam/dc.c +++ b/sw/airborne/modules/digital_cam/dc.c @@ -117,8 +117,9 @@ void dc_send_shot_position(void) uint8_t dc_info(void) { #ifdef DOWNLINK_SEND_DC_INFO float course = DegOfRad(stateGetNedToBodyEulers_f()->psi); + int16_t mode = dc_autoshoot; DOWNLINK_SEND_DC_INFO(DefaultChannel, DefaultDevice, - &dc_autoshoot, + &mode, &stateGetPositionEnu_f()->x, &stateGetPositionEnu_f()->y, &course, diff --git a/sw/airborne/modules/digital_cam/dc.h b/sw/airborne/modules/digital_cam/dc.h index 3b59a89da1..b575688b2b 100644 --- a/sw/airborne/modules/digital_cam/dc.h +++ b/sw/airborne/modules/digital_cam/dc.h @@ -187,7 +187,7 @@ extern uint8_t dc_circle(float interval, float start); * If both 'x' and 'y' are DC_IGNORE the current position * will be used as reference point. * - * @param interval minimum angle between shots in deg + * @param interval distance between shots in meters * @param x x coordinate of reference point (or special DC_IGNORE) * @param y y coordinate of reference point (or special DC_IGNORE) * @return zero diff --git a/sw/airborne/modules/digital_cam/hackhd.h b/sw/airborne/modules/digital_cam/hackhd.h index cfe0f2415e..93ba55f71c 100644 --- a/sw/airborne/modules/digital_cam/hackhd.h +++ b/sw/airborne/modules/digital_cam/hackhd.h @@ -58,7 +58,7 @@ enum hackhd_status { struct HackHD { enum hackhd_status status; uint32_t timer; - uint32_t photo_nr; + int16_t photo_nr; uint32_t autoshoot; struct EnuCoor_f last_shot_pos; uint32_t log_delay; diff --git a/sw/airborne/modules/digital_cam/uart_cam_ctrl.c b/sw/airborne/modules/digital_cam/uart_cam_ctrl.c index dc53aadd49..88da7a4b64 100644 --- a/sw/airborne/modules/digital_cam/uart_cam_ctrl.c +++ b/sw/airborne/modules/digital_cam/uart_cam_ctrl.c @@ -96,7 +96,7 @@ void digital_cam_uart_event(void) } #if PERIODIC_TELEMETRY -static void send_thumbnails(void) +static void send_thumbnails(struct transport_tx *trans, struct link_device *dev) { static int cnt = 0; if (digital_cam_uart_thumbnails > 0) { @@ -107,7 +107,7 @@ static void send_thumbnails(void) return; } } - DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, THUMB_MSG_SIZE, thumbs[thumb_pointer]); + pprz_msg_send_PAYLOAD(trans, dev, AC_ID, THUMB_MSG_SIZE, thumbs[thumb_pointer]); // Update the write/read pointer: if we receive a new thumb part, that will be sent, otherwise the oldest infor is repeated thumb_pointer++; diff --git a/sw/airborne/modules/meteo/meteo_france_DAQ.c b/sw/airborne/modules/meteo/meteo_france_DAQ.c index 8ce5a0fca6..7c89c13c55 100644 --- a/sw/airborne/modules/meteo/meteo_france_DAQ.c +++ b/sw/airborne/modules/meteo/meteo_france_DAQ.c @@ -66,7 +66,7 @@ void init_mf_daq(void) { void mf_daq_send_state(void) { // Send aircraft state to DAQ board - DOWNLINK_SEND_MF_DAQ_STATE(PprzTransport, EXTRA_PPRZ_UART, + DOWNLINK_SEND_MF_DAQ_STATE(pprz_tp, EXTRA_DOWNLINK_DEVICE, &autopilot_flight_time, &stateGetBodyRates_f()->p, &stateGetBodyRates_f()->q, @@ -96,14 +96,14 @@ void mf_daq_send_report(void) { if (pprzLogFile.fs != NULL) { if (log_started == FALSE) { // Log MD5SUM once - DOWNLINK_SEND_ALIVE(PprzLogTransport, SDLOG, 16, MD5SUM); + DOWNLINK_SEND_ALIVE(pprzlog_tp, chibios_sdlog, 16, MD5SUM); log_started = TRUE; } // Log GPS for time reference uint8_t foo = 0; int16_t climb = -gps.ned_vel.z; int16_t course = (DegOfRad(gps.course)/((int32_t)1e6)); - DOWNLINK_SEND_GPS(PprzLogTransport, SDLOG, &gps.fix, + DOWNLINK_SEND_GPS(pprzlog_tp, chibios_sdlog, &gps.fix, &gps.utm_pos.east, &gps.utm_pos.north, &course, &gps.hmsl, &gps.gspeed, &climb, &gps.week, &gps.tow, &gps.utm_pos.zone, &foo); @@ -118,8 +118,8 @@ void parse_mf_daq_msg(void) { memcpy(mf_daq.values, DL_PAYLOAD_FLOAT_values(dl_buffer), mf_daq.nb * sizeof(float)); // Log on SD card if (log_started) { - DOWNLINK_SEND_PAYLOAD_FLOAT(PprzLogTransport, SDLOG, mf_daq.nb, mf_daq.values); - DOWNLINK_SEND_MF_DAQ_STATE(PprzLogTransport, SDLOG, + DOWNLINK_SEND_PAYLOAD_FLOAT(pprzlog_tp, chibios_sdlog, mf_daq.nb, mf_daq.values); + DOWNLINK_SEND_MF_DAQ_STATE(pprzlog_tp, chibios_sdlog, &autopilot_flight_time, &stateGetBodyRates_f()->p, &stateGetBodyRates_f()->q, diff --git a/sw/airborne/modules/nav/nav_catapult.c b/sw/airborne/modules/nav/nav_catapult.c index e29c88c8a4..679284baa0 100644 --- a/sw/airborne/modules/nav/nav_catapult.c +++ b/sw/airborne/modules/nav/nav_catapult.c @@ -197,7 +197,7 @@ bool_t nav_catapult_run(uint8_t _to, uint8_t _climb) WaypointX(_climb) = nav_catapult_x + (dir_x / dir_L) * 300; WaypointY(_climb) = nav_catapult_y + (dir_y / dir_L) * 300; - DownlinkSendWp(DefaultChannel, DefaultDevice, _climb); + DownlinkSendWp(&(DefaultChannel).trans_tx, &(DefaultDevice).device, _climb); } diff --git a/sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c b/sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c index 5f37925c72..1bf4ff0c41 100644 --- a/sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c +++ b/sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c @@ -139,7 +139,7 @@ static Butterworth2LowPass ms45xx_filter; static void ms45xx_downlink(void) { - DOWNLINK_SEND_AIRSPEED_MS45XX(DefaultChannel, DefaultDevice, + pprz_msg_send_AIRSPEED_MS45XX(trans, dev, AC_ID, &ms45xx.diff_pressure, &ms45xx.temperature, &ms45xx.airspeed); } diff --git a/sw/airborne/modules/sensors/temp_adc.c b/sw/airborne/modules/sensors/temp_adc.c index 3c5e6863da..2a1c04f729 100644 --- a/sw/airborne/modules/sensors/temp_adc.c +++ b/sw/airborne/modules/sensors/temp_adc.c @@ -100,7 +100,7 @@ float calc_lm35(int16_t raw_temp) static void temp_adc_downlink(void) { - DOWNLINK_SEND_TEMP_ADC(DefaultChannel, DefaultDevice, &temp_c1, &temp_c2, &temp_c3); + pprz_msg_send_TEMP_ADC(trans, dev, AC_ID, &temp_c1, &temp_c2, &temp_c3); } void temp_adc_init(void) diff --git a/sw/airborne/subsystems/ahrs/ahrs_aligner.c b/sw/airborne/subsystems/ahrs/ahrs_aligner.c index 476d3b5a42..050354515d 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_aligner.c +++ b/sw/airborne/subsystems/ahrs/ahrs_aligner.c @@ -47,8 +47,8 @@ static uint32_t samples_idx; #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_aligner(void) { - DOWNLINK_SEND_FILTER_ALIGNER(DefaultChannel, DefaultDevice, +static void send_aligner(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_FILTER_ALIGNER(trans, dev, AC_ID, &ahrs_aligner.lp_gyro.p, &ahrs_aligner.lp_gyro.q, &ahrs_aligner.lp_gyro.r, diff --git a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c index 9fe5a08c50..6592385cd3 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c +++ b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c @@ -48,8 +48,8 @@ unsigned char buffer[4096]; //Packet buffer #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_ahrs_ad2(void) { - DOWNLINK_SEND_AHRS_ARDRONE2(DefaultChannel, DefaultDevice, +static void send_ahrs_ad2(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_AHRS_ARDRONE2(trans, dev, AC_ID, &ahrs_ardrone2.state, &ahrs_ardrone2.control_state, &ahrs_ardrone2.eulers.phi, diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c index bf477b2e04..835fb2558b 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c @@ -150,13 +150,13 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id, #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_att(void) { +static void send_att(struct transport_tx *trans, struct link_device *dev) { struct FloatEulers ltp_to_imu_euler; float_eulers_of_quat(<p_to_imu_euler, &ahrs_fc.ltp_to_imu_quat); struct Int32Eulers euler_i; EULERS_BFP_OF_REAL(euler_i, ltp_to_imu_euler); struct Int32Eulers* eulers_body = stateGetNedToBodyEulers_i(); - DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice, + pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID, &euler_i.phi, &euler_i.theta, &euler_i.psi, @@ -165,16 +165,16 @@ static void send_att(void) { &(eulers_body->psi)); } -static void send_geo_mag(void) { - DOWNLINK_SEND_GEO_MAG(DefaultChannel, DefaultDevice, +static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_GEO_MAG(trans, dev, AC_ID, &ahrs_fc.mag_h.x, &ahrs_fc.mag_h.y, &ahrs_fc.mag_h.z); } // TODO convert from float to int if we really need this one /* -static void send_rmat(void) { +static void send_rmat(struct transport_tx *trans, struct link_device *dev) { struct Int32RMat* att_rmat = stateGetNedToBodyRMat_i(); - DOWNLINK_SEND_AHRS_RMAT(DefaultChannel, DefaultDevice, + pprz_msg_send_AHRS_RMAT(trans, dev, AC_ID, &ahrs_fc.ltp_to_imu_rmat.m[0], &ahrs_fc.ltp_to_imu_rmat.m[1], &ahrs_fc.ltp_to_imu_rmat.m[2], diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c index b0e1d5bae6..8e2320e041 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c @@ -63,8 +63,8 @@ struct AhrsMlkf ahrs_mlkf; #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_geo_mag(void) { - DOWNLINK_SEND_GEO_MAG(DefaultChannel, DefaultDevice, +static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_GEO_MAG(trans, dev, AC_ID, &ahrs_mlkf.mag_h.x, &ahrs_mlkf.mag_h.y, &ahrs_mlkf.mag_h.z); } #endif diff --git a/sw/airborne/subsystems/ahrs/ahrs_gx3.c b/sw/airborne/subsystems/ahrs/ahrs_gx3.c index ce69bf3650..aa6499eddf 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_gx3.c +++ b/sw/airborne/subsystems/ahrs/ahrs_gx3.c @@ -83,8 +83,8 @@ void ahrs_align(void) { #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static send_gx3(void) { - DOWNLINK_SEND_GX3_INFO(DefaultChannel, DefaultDevice, +static send_gx3(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_GX3_INFO(trans, dev, AC_ID, &ahrs_impl.gx3_freq, &ahrs_impl.gx3_packet.chksm_error, &ahrs_impl.gx3_packet.hdr_error, diff --git a/sw/airborne/subsystems/ahrs/ahrs_infrared.c b/sw/airborne/subsystems/ahrs/ahrs_infrared.c index ed897f0670..12b5ffa9e8 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_infrared.c +++ b/sw/airborne/subsystems/ahrs/ahrs_infrared.c @@ -59,16 +59,16 @@ static void gyro_cb(uint8_t sender_id __attribute__((unused)), #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_infrared(void) { - DOWNLINK_SEND_IR_SENSORS(DefaultChannel, DefaultDevice, +static void send_infrared(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_IR_SENSORS(trans, dev, AC_ID, &infrared.value.ir1, &infrared.value.ir2, &infrared.pitch, &infrared.roll, &infrared.top); } -static void send_status(void) { +static void send_status(struct transport_tx *trans, struct link_device *dev) { uint16_t contrast = abs(infrared.roll) + abs(infrared.pitch) + abs(infrared.top); uint8_t mde = 3; if (contrast < 50) mde = 7; - DOWNLINK_SEND_STATE_FILTER_STATUS(DefaultChannel, DefaultDevice, &mde, &contrast); + pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &contrast); } #endif diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c index 9b2eb38140..0a5a5596c0 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c @@ -120,8 +120,8 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id, #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_filter(void) { - DOWNLINK_SEND_FILTER(DefaultChannel, DefaultDevice, +static void send_filter(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_FILTER(trans, dev, AC_ID, &ahrs_ice.ltp_to_imu_euler.phi, &ahrs_ice.ltp_to_imu_euler.theta, &ahrs_ice.ltp_to_imu_euler.psi, @@ -139,9 +139,9 @@ static void send_filter(void) { &ahrs_ice.gyro_bias.r); } -static void send_euler(void) { +static void send_euler(struct transport_tx *trans, struct link_device *dev) { struct Int32Eulers* eulers = stateGetNedToBodyEulers_i(); - DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice, + pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID, &ahrs_ice.ltp_to_imu_euler.phi, &ahrs_ice.ltp_to_imu_euler.theta, &ahrs_ice.ltp_to_imu_euler.psi, @@ -150,8 +150,8 @@ static void send_euler(void) { &(eulers->psi)); } -static void send_bias(void) { - DOWNLINK_SEND_AHRS_GYRO_BIAS_INT(DefaultChannel, DefaultDevice, +static void send_bias(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_AHRS_GYRO_BIAS_INT(trans, dev, AC_ID, &ahrs_ice.gyro_bias.p, &ahrs_ice.gyro_bias.q, &ahrs_ice.gyro_bias.r); } #endif diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index 1f6a1f12ec..14cd568c35 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -170,9 +170,9 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id, #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_quat(void) { +static void send_quat(struct transport_tx *trans, struct link_device *dev) { struct Int32Quat* quat = stateGetNedToBodyQuat_i(); - DOWNLINK_SEND_AHRS_QUAT_INT(DefaultChannel, DefaultDevice, + pprz_msg_send_AHRS_QUAT_INT(trans, dev, AC_ID, &ahrs_icq.weight, &ahrs_icq.ltp_to_imu_quat.qi, &ahrs_icq.ltp_to_imu_quat.qx, @@ -184,11 +184,11 @@ static void send_quat(void) { &(quat->qz)); } -static void send_euler(void) { +static void send_euler(struct transport_tx *trans, struct link_device *dev) { struct Int32Eulers ltp_to_imu_euler; int32_eulers_of_quat(<p_to_imu_euler, &ahrs_icq.ltp_to_imu_quat); struct Int32Eulers* eulers = stateGetNedToBodyEulers_i(); - DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice, + pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID, <p_to_imu_euler.phi, <p_to_imu_euler.theta, <p_to_imu_euler.psi, @@ -197,17 +197,17 @@ static void send_euler(void) { &(eulers->psi)); } -static void send_bias(void) { - DOWNLINK_SEND_AHRS_GYRO_BIAS_INT(DefaultChannel, DefaultDevice, +static void send_bias(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_AHRS_GYRO_BIAS_INT(trans, dev, AC_ID, &ahrs_icq.gyro_bias.p, &ahrs_icq.gyro_bias.q, &ahrs_icq.gyro_bias.r); } -static void send_geo_mag(void) { +static void send_geo_mag(struct transport_tx *trans, struct link_device *dev) { struct FloatVect3 h_float; h_float.x = MAG_FLOAT_OF_BFP(ahrs_icq.mag_h.x); h_float.y = MAG_FLOAT_OF_BFP(ahrs_icq.mag_h.y); h_float.z = MAG_FLOAT_OF_BFP(ahrs_icq.mag_h.z); - DOWNLINK_SEND_GEO_MAG(DefaultChannel, DefaultDevice, + pprz_msg_send_GEO_MAG(trans, dev, AC_ID, &h_float.x, &h_float.y, &h_float.z); } #endif diff --git a/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.c b/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.c index ec0e601b61..f8bd288963 100644 --- a/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.c +++ b/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.c @@ -51,6 +51,8 @@ static const char PROCESS_LOG_NAME[] = "processLog_"; FIL processLogFile = {0}; #endif +struct chibios_sdlog chibios_sdlog; + static WORKING_AREA(waThdBatterySurvey, 4096); static void launchBatterySurveyThread (void) { @@ -60,12 +62,31 @@ static void launchBatterySurveyThread (void) } +// Functions for the generic device API +static int sdlog_check_free_space(struct chibios_sdlog* p __attribute__((unused)), uint8_t len __attribute__((unused))) +{ + return TRUE; +} + +static void sdlog_transmit(struct chibios_sdlog* p __attribute__((unused)), uint8_t byte) +{ + sdLogWriteByte(&pprzLogFile, byte); +} + +static void sdlog_send(struct chibios_sdlog* p __attribute__((unused))) { } + bool_t chibios_logInit(const bool_t binaryFile) { nvicSetSystemHandlerPriority(HANDLER_PENDSV, CORTEX_PRIORITY_MASK(15)); + // Configure generic device + chibios_sdlog.device.periph = (void *)(&chibios_sdlog); + chibios_sdlog.device.check_free_space = (check_free_space_t) sdlog_check_free_space; + chibios_sdlog.device.transmit = (transmit_t) sdlog_transmit; + chibios_sdlog.device.send_message = (send_message_t) sdlog_send; + if (sdLogInit (NULL) != SDLOG_OK) goto error; diff --git a/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.h b/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.h index 465fe14ba0..03526c39fd 100644 --- a/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.h +++ b/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.h @@ -29,6 +29,7 @@ #define CHIBIOS_SDLOG_H #include "ff.h" +#include "mcu_periph/link_device.h" /* what to be done : @@ -47,6 +48,13 @@ extern FIL processLogFile; extern bool_t chibios_logInit(const bool_t binaryFile); extern void chibios_logFinish(void); +struct chibios_sdlog { + /** Generic device interface */ + struct link_device device; +}; + +extern struct chibios_sdlog chibios_sdlog; + /** Paparazzi datalink API */ #define SDLOGCheckFreeSpace(_x) (true) #define SDLOGTransmit(_x) sdLogWriteByte(&pprzLogFile, _x) diff --git a/sw/airborne/subsystems/datalink/datalink.h b/sw/airborne/subsystems/datalink/datalink.h index cdef69a225..1ec00307ef 100644 --- a/sw/airborne/subsystems/datalink/datalink.h +++ b/sw/airborne/subsystems/datalink/datalink.h @@ -83,7 +83,7 @@ EXTERN void dl_parse_msg(void); #elif defined DATALINK && DATALINK == W5100 #define DatalinkEvent() { \ - W5100CheckAndParse(W5100, w5100_tp); \ + W5100CheckAndParse(W5100, pprz_tp); \ DlCheckAndParse(); \ } diff --git a/sw/airborne/subsystems/datalink/downlink.c b/sw/airborne/subsystems/datalink/downlink.c index ac628f2ce4..c4fbbe041c 100644 --- a/sw/airborne/subsystems/datalink/downlink.c +++ b/sw/airborne/subsystems/datalink/downlink.c @@ -26,8 +26,31 @@ */ -#include "std.h" +#include "subsystems/datalink/downlink.h" + +struct downlink downlink; + +void downlink_init(void) +{ + downlink.nb_ovrn = 0; + downlink.nb_bytes = 0; + downlink.nb_msgs = 0; + +#if defined DATALINK +#if DATALINK == PPRZ + pprz_transport_init(); +#endif +#if DATALINK == XBEE + xbee_init(); +#endif +#if DATALINK == W5100 + w5100_init(); +#endif +#endif + +#if SITL + ivy_transport_init(); +#endif + +} -uint8_t downlink_nb_ovrn; -uint16_t downlink_nb_bytes; -uint16_t downlink_nb_msgs; diff --git a/sw/airborne/subsystems/datalink/downlink.h b/sw/airborne/subsystems/datalink/downlink.h index a888c9f353..4837c6b28a 100644 --- a/sw/airborne/subsystems/datalink/downlink.h +++ b/sw/airborne/subsystems/datalink/downlink.h @@ -93,76 +93,16 @@ #define DefaultDevice DOWNLINK_DEVICE #endif -/** Counter of messages not sent because of unavailibity of the output buffer*/ -extern uint8_t downlink_nb_ovrn; -extern uint16_t downlink_nb_bytes; -extern uint16_t downlink_nb_msgs; +/** Downlink structure */ +struct downlink { + uint8_t nb_ovrn; ///< Counter of messages not sent because of unavailibity of the output buffer + uint16_t nb_bytes; ///< Number of bytes send over telemetry + uint16_t nb_msgs; ///< Number of messages send over telemetry +}; -/* Transport macros - * - * call transport functions from channel - */ -#define __Transport(dev, _x) dev##_x -#define _Transport(dev, _x) __Transport(dev, _x) -#define Transport(_chan, _fun) _Transport(_chan, _fun) +extern struct downlink downlink; - -/** Set of macros for generated code (messages.h) from messages.xml */ -/** 2 = ac_id + msg_id */ -#define DownlinkIDsSize(_trans, _dev, _x) (_x+2) -#define DownlinkSizeOf(_trans, _dev, _x) Transport(_trans, SizeOf(_dev, DownlinkIDsSize(_trans, _dev, _x))) - -#define DownlinkCheckFreeSpace(_trans, _dev, _x) Transport(_trans, CheckFreeSpace(_dev, (uint8_t)(_x))) - -#define DownlinkPutUint8(_trans, _dev, _x) Transport(_trans, PutUint8(_dev, _x)) - -#define DownlinkPutInt8ByAddr(_trans, _dev, _x) Transport(_trans, PutInt8ByAddr(_dev, _x)) -#define DownlinkPutUint8ByAddr(_trans, _dev, _x) Transport(_trans, PutUint8ByAddr(_dev, _x)) -#define DownlinkPutInt16ByAddr(_trans, _dev, _x) Transport(_trans, PutInt16ByAddr(_dev, _x)) -#define DownlinkPutUint16ByAddr(_trans, _dev, _x) Transport(_trans, PutUint16ByAddr(_dev, _x)) -#define DownlinkPutInt32ByAddr(_trans, _dev, _x) Transport(_trans, PutInt32ByAddr(_dev, _x)) -#define DownlinkPutUint32ByAddr(_trans, _dev, _x) Transport(_trans, PutUint32ByAddr(_dev, _x)) -#define DownlinkPutFloatByAddr(_trans, _dev, _x) Transport(_trans, PutFloatByAddr(_dev, _x)) - -#define DownlinkPutDoubleByAddr(_trans, _dev, _x) Transport(_trans, PutDoubleByAddr(_dev, _x)) -#define DownlinkPutUint64ByAddr(_trans, _dev, _x) Transport(_trans, PutUint64ByAddr(_dev, _x)) -#define DownlinkPutInt64ByAddr(_trans, _dev, _x) Transport(_trans, PutInt64ByAddr(_dev, _x)) -#define DownlinkPutCharByAddr(_trans, _dev, _x) Transport(_trans, PutCharByAddr(_dev, _x)) - -#define DownlinkPutFloatArray(_trans, _dev, _n, _x) Transport(_trans, PutFloatArray(_dev, _n, _x)) -#define DownlinkPutDoubleArray(_trans, _dev, _n, _x) Transport(_trans, PutDoubleArray(_dev, _n, _x)) -#define DownlinkPutInt16Array(_trans, _dev, _n, _x) Transport(_trans, PutInt16Array(_dev, _n, _x)) -#define DownlinkPutUint16Array(_trans, _dev, _n, _x) Transport(_trans, PutUint16Array(_dev, _n, _x)) -#define DownlinkPutInt32Array(_trans, _dev, _n, _x) Transport(_trans, PutInt32Array(_dev, _n, _x)) -#define DownlinkPutUint32Array(_trans, _dev, _n, _x) Transport(_trans, PutUint32Array(_dev, _n, _x)) -#define DownlinkPutInt64Array(_trans, _dev, _n, _x) Transport(_trans, PutInt64Array(_dev, _n, _x)) -#define DownlinkPutUint64Array(_trans, _dev, _n, _x) Transport(_trans, PutUint64Array(_dev, _n, _x)) -#define DownlinkPutInt8Array(_trans, _dev, _n, _x) Transport(_trans, PutInt8Array(_dev, _n, _x)) -#define DownlinkPutUint8Array(_trans, _dev, _n, _x) Transport(_trans, PutUint8Array(_dev, _n, _x)) -#define DownlinkPutCharArray(_trans, _dev, _n, _x) Transport(_trans, PutCharArray(_dev, _n, _x)) - -#define DownlinkPutFloatFixedArray(_trans, _dev, _n, _x) Transport(_trans, PutFloatFixedArray(_dev, _n, _x)) -#define DownlinkPutDoubleFixedArray(_trans, _dev, _n, _x) Transport(_trans, PutDoubleFixedArray(_dev, _n, _x)) -#define DownlinkPutInt16FixedArray(_trans, _dev, _n, _x) Transport(_trans, PutInt16FixedArray(_dev, _n, _x)) -#define DownlinkPutUint16FixedArray(_trans, _dev, _n, _x) Transport(_trans, PutUint16FixedArray(_dev, _n, _x)) -#define DownlinkPutInt32FixedArray(_trans, _dev, _n, _x) Transport(_trans, PutInt32FixedArray(_dev, _n, _x)) -#define DownlinkPutUint32FixedArray(_trans, _dev, _n, _x) Transport(_trans, PutUint32FixedArray(_dev, _n, _x)) -#define DownlinkPutInt64FixedArray(_trans, _dev, _n, _x) Transport(_trans, PutInt64FixedArray(_dev, _n, _x)) -#define DownlinkPutUint64FixedArray(_trans, _dev, _n, _x) Transport(_trans, PutUint64FixedArray(_dev, _n, _x)) -#define DownlinkPutInt8FixedArray(_trans, _dev, _n, _x) Transport(_trans, PutInt8FixedArray(_dev, _n, _x)) -#define DownlinkPutUint8FixedArray(_trans, _dev, _n, _x) Transport(_trans, PutUint8FixedArray(_dev, _n, _x)) -#define DownlinkPutCharFixedArray(_trans, _dev, _n, _x) Transport(_trans, PutCharFixedArray(_dev, _n, _x)) - -#define DownlinkOverrun(_trans, _dev) downlink_nb_ovrn++; -#define DownlinkCountBytes(_trans, _dev, _n) downlink_nb_bytes += _n; - -#define DownlinkStartMessage(_trans, _dev, _name, msg_id, payload_len) { \ - downlink_nb_msgs++; \ - Transport(_trans, Header(_dev, DownlinkIDsSize(_trans, _dev, payload_len))); \ - Transport(_trans, PutUint8(_dev, AC_ID)); \ - Transport(_trans, PutNamedUint8(_dev, _name, msg_id)); \ -} - -#define DownlinkEndMessage(_trans, _dev) Transport(_trans, Trailer(_dev)) +// Init function +extern void downlink_init(void); #endif /* DOWNLINK_H */ diff --git a/sw/airborne/subsystems/datalink/ivy_transport.c b/sw/airborne/subsystems/datalink/ivy_transport.c new file mode 100644 index 0000000000..d8188d5eed --- /dev/null +++ b/sw/airborne/subsystems/datalink/ivy_transport.c @@ -0,0 +1,186 @@ +/* + * 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 for arrays + if (format == DL_FORMAT_ARRAY) { + 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, space otherwise + if (format == DL_FORMAT_ARRAY) { + trans->ivy_p += sprintf(trans->ivy_p, ","); + } else { + trans->ivy_p += sprintf(trans->ivy_p, " "); + } + } + + // End delimiter for arrays + if (format == DL_FORMAT_ARRAY) { + 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 __attribute__((unused))) +{ + *(--trans->ivy_p) = '\0'; + if (trans->ivy_dl_enabled) { + IvySendMsg("%s", trans->ivy_buf); + } +} + +static void overrun(struct ivy_transport *trans __attribute__((unused)), struct link_device *dev __attribute__((unused))) +{ + downlink.nb_ovrn++; +} + +static void count_bytes(struct ivy_transport *trans __attribute__((unused)), struct link_device *dev __attribute__((unused)), uint8_t bytes) +{ + downlink.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))) {} + +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.transmit = (transmit_t) transmit; + ivy_tp.device.send_message = (send_message_t) send_message; + 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 new file mode 100644 index 0000000000..dae014a7ed --- /dev/null +++ b/sw/airborne/subsystems/datalink/ivy_transport.h @@ -0,0 +1,53 @@ +/* + * 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 index 05ac097bd3..0cc85af9f3 100644 --- a/sw/airborne/subsystems/datalink/pprz_transport.c +++ b/sw/airborne/subsystems/datalink/pprz_transport.c @@ -1,5 +1,6 @@ /* * Copyright (C) 2006 Pascal Brisset, Antoine Drouin + * Copyright (C) 2014 Gautier Hattenberger * * This file is part of paparazzi. * @@ -14,19 +15,110 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . * */ +/** + * @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 -uint8_t ck_a, ck_b; - 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->transmit(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) +{ + downlink.nb_msgs++; + dev->transmit(dev->periph, STX); + const uint8_t msg_len = size_of(trans, payload_len); + dev->transmit(dev->periph, msg_len); + trans->ck_a_tx = msg_len; + trans->ck_b_tx = msg_len; +} + +static void end_message(struct pprz_transport *trans, struct link_device *dev) +{ + dev->transmit(dev->periph, trans->ck_a_tx); + dev->transmit(dev->periph, trans->ck_b_tx); + dev->send_message(dev); +} + +static void overrun(struct pprz_transport *trans __attribute__((unused)), struct link_device *dev __attribute__((unused))) +{ + downlink.nb_ovrn++; +} + +static void count_bytes(struct pprz_transport *trans __attribute__((unused)), struct link_device *dev __attribute__((unused)), uint8_t bytes) +{ + downlink.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, bytes); +} + +void pprz_transport_init(void) +{ + pprz_tp.status = UNINIT; + pprz_tp.trans_rx.msg_received = FALSE; + pprz_tp.trans_tx.size_of = (size_of_t) size_of; + pprz_tp.trans_tx.check_available_space = (check_available_space_t) check_available_space; + pprz_tp.trans_tx.put_bytes = (put_bytes_t) put_bytes; + pprz_tp.trans_tx.put_named_byte = (put_named_byte_t) put_named_byte; + pprz_tp.trans_tx.start_message = (start_message_t) start_message; + pprz_tp.trans_tx.end_message = (end_message_t) end_message; + pprz_tp.trans_tx.overrun = (overrun_t) overrun; + pprz_tp.trans_tx.count_bytes = (count_bytes_t) count_bytes; + pprz_tp.trans_tx.impl = (void *)(&pprz_tp); +} + diff --git a/sw/airborne/subsystems/datalink/pprz_transport.h b/sw/airborne/subsystems/datalink/pprz_transport.h index f04261c6cc..7a97da6600 100644 --- a/sw/airborne/subsystems/datalink/pprz_transport.h +++ b/sw/airborne/subsystems/datalink/pprz_transport.h @@ -1,5 +1,6 @@ /* * Copyright (C) 2003 Pascal Brisset, Antoine Drouin + * Copyright (C) 2014 Gautier Hattenberger * * This file is part of paparazzi. * @@ -14,9 +15,8 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . * */ @@ -52,146 +52,10 @@ #endif /* PPRZ Transport - * downlink macros */ -extern uint8_t ck_a, ck_b; #define STX 0x99 -/** 4 = STX + len + ck_a + ck_b */ -#define PprzTransportSizeOf(_dev, _payload) (_payload+4) - -#define PprzTransportCheckFreeSpace(_dev, _x) TransportLink(_dev, CheckFreeSpace(_x)) -#define PprzTransportPut1Byte(_dev, _x) TransportLink(_dev, Transmit(_x)) -#define PprzTransportSendMessage(_dev) TransportLink(_dev, SendMessage()) - -#define PprzTransportHeader(_dev, payload_len) { \ - PprzTransportPut1Byte(_dev, STX); \ - uint8_t msg_len = PprzTransportSizeOf(_dev, payload_len); \ - PprzTransportPut1Byte(_dev, msg_len); \ - ck_a = msg_len; ck_b = msg_len; \ -} - -#define PprzTransportTrailer(_dev) { \ - PprzTransportPut1Byte(_dev, ck_a); \ - PprzTransportPut1Byte(_dev, ck_b); \ - PprzTransportSendMessage(_dev); \ -} - -#define PprzTransportPutUint8(_dev, _byte) { \ - ck_a += _byte; \ - ck_b += ck_a; \ - PprzTransportPut1Byte(_dev, _byte); \ - } - -#define PprzTransportPutNamedUint8(_dev, _name, _byte) PprzTransportPutUint8(_dev, _byte) - -#define PprzTransportPut1ByteByAddr(_dev, _byte) { \ - uint8_t _x = *(_byte); \ - PprzTransportPutUint8(_dev, _x); \ - } - -#define PprzTransportPut2ByteByAddr(_dev, _byte) { \ - PprzTransportPut1ByteByAddr(_dev, _byte); \ - PprzTransportPut1ByteByAddr(_dev, (const uint8_t*)_byte+1); \ - } - -#define PprzTransportPut4ByteByAddr(_dev, _byte) { \ - PprzTransportPut2ByteByAddr(_dev, _byte); \ - PprzTransportPut2ByteByAddr(_dev, (const uint8_t*)_byte+2); \ - } - -#ifdef __IEEE_BIG_ENDIAN /* From machine/ieeefp.h */ -#define PprzTransportPutDoubleByAddr(_dev, _byte) { \ - PprzTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4); \ - PprzTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte); \ - } -#define PprzTransportPutUint64ByAddr(_dev, _byte) { \ - PprzTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4); \ - PprzTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte); \ - } -#define PprzTransportPutInt64ByAddr(_dev, _byte) { \ - PprzTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4); \ - PprzTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte); \ - } -#else -#define PprzTransportPutDoubleByAddr(_dev, _byte) { \ - PprzTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte); \ - PprzTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4); \ - } -#define PprzTransportPutUint64ByAddr(_dev, _byte) { \ - PprzTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte); \ - PprzTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4); \ - } -#define PprzTransportPutInt64ByAddr(_dev, _byte) { \ - PprzTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte); \ - PprzTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4); \ - } -#endif - - -#define PprzTransportPutInt8ByAddr(_dev, _x) PprzTransportPut1ByteByAddr(_dev, _x) -#define PprzTransportPutUint8ByAddr(_dev, _x) PprzTransportPut1ByteByAddr(_dev, (const uint8_t*)_x) -#define PprzTransportPutInt16ByAddr(_dev, _x) PprzTransportPut2ByteByAddr(_dev, (const uint8_t*)_x) -#define PprzTransportPutUint16ByAddr(_dev, _x) PprzTransportPut2ByteByAddr(_dev, (const uint8_t*)_x) -#define PprzTransportPutInt32ByAddr(_dev, _x) PprzTransportPut4ByteByAddr(_dev, (const uint8_t*)_x) -#define PprzTransportPutUint32ByAddr(_dev, _x) PprzTransportPut4ByteByAddr(_dev, (const uint8_t*)_x) -#define PprzTransportPutFloatByAddr(_dev, _x) PprzTransportPut4ByteByAddr(_dev, (const uint8_t*)_x) -#define PprzTransportPutCharByAddr(_dev, _x) PprzTransportPut1ByteByAddr(_dev, (const uint8_t*)_x) - -#define PprzTransportPutArray(_dev, _put, _n, _x) { \ - uint8_t _i; \ - PprzTransportPutUint8(_dev, _n); \ - for(_i = 0; _i < _n; _i++) { \ - _put(_dev, &_x[_i]); \ - } \ -} - -#define PprzTransportPutInt8Array(_dev, _n, _x) PprzTransportPutArray(_dev, PprzTransportPutInt8ByAddr, _n, _x) -#define PprzTransportPutUint8Array(_dev, _n, _x) PprzTransportPutArray(_dev, PprzTransportPutUint8ByAddr, _n, _x) - -#define PprzTransportPutCharArray(_dev, _n, _x) PprzTransportPutArray(_dev, PprzTransportPutCharByAddr, _n, _x) - -#define PprzTransportPutInt16Array(_dev, _n, _x) PprzTransportPutArray(_dev, PprzTransportPutInt16ByAddr, _n, _x) -#define PprzTransportPutUint16Array(_dev, _n, _x) PprzTransportPutArray(_dev, PprzTransportPutUint16ByAddr, _n, _x) - -#define PprzTransportPutInt32Array(_dev, _n, _x) PprzTransportPutArray(_dev, PprzTransportPutInt32ByAddr, _n, _x) -#define PprzTransportPutUint32Array(_dev, _n, _x) PprzTransportPutArray(_dev, PprzTransportPutUint32ByAddr, _n, _x) - -#define PprzTransportPutFloatArray(_dev, _n, _x) PprzTransportPutArray(_dev, PprzTransportPutFloatByAddr, _n, _x) - -#define PprzTransportPutInt64Array(_dev, _n, _x) PprzTransportPutArray(_dev, PprzTransportPutInt64ByAddr, _n, _x) -#define PprzTransportPutUint64Array(_dev, _n, _x) PprzTransportPutArray(_dev, PprzTransportPutUint64ByAddr, _n, _x) - -#define PprzTransportPutDoubleArray(_dev, _n, _x) PprzTransportPutArray(_dev, PprzTransportPutDoubleByAddr, _n, _x) - -#define PprzTransportPutFixedArray(_dev, _put, _n, _x) { \ - uint8_t _i; \ - for(_i = 0; _i < _n; _i++) { \ - _put(_dev, &_x[_i]); \ - } \ -} - -#define PprzTransportPutInt8FixedArray(_dev, _n, _x) PprzTransportPutFixedArray(_dev, PprzTransportPutInt8ByAddr, _n, _x) -#define PprzTransportPutUint8FixedArray(_dev, _n, _x) PprzTransportPutFixedArray(_dev, PprzTransportPutUint8ByAddr, _n, _x) - -#define PprzTransportPutCharFixedArray(_dev, _n, _x) PprzTransportPutFixedArray(_dev, PprzTransportPutCharByAddr, _n, _x) - -#define PprzTransportPutInt16FixedArray(_dev, _n, _x) PprzTransportPutFixedArray(_dev, PprzTransportPutInt16ByAddr, _n, _x) -#define PprzTransportPutUint16FixedArray(_dev, _n, _x) PprzTransportPutFixedArray(_dev, PprzTransportPutUint16ByAddr, _n, _x) - -#define PprzTransportPutInt32FixedArray(_dev, _n, _x) PprzTransportPutFixedArray(_dev, PprzTransportPutInt32ByAddr, _n, _x) -#define PprzTransportPutUint32FixedArray(_dev, _n, _x) PprzTransportPutFixedArray(_dev, PprzTransportPutUint32ByAddr, _n, _x) - -#define PprzTransportPutFloatFixedArray(_dev, _n, _x) PprzTransportPutFixedArray(_dev, PprzTransportPutFloatByAddr, _n, _x) - -#define PprzTransportPutInt64FixedArray(_dev, _n, _x) PprzTransportPutFixedArray(_dev, PprzTransportPutInt64ByAddr, _n, _x) -#define PprzTransportPutUint64FixedArray(_dev, _n, _x) PprzTransportPutFixedArray(_dev, PprzTransportPutUint64ByAddr, _n, _x) - -#define PprzTransportPutDoubleFixedArray(_dev, _n, _x) PprzTransportPutFixedArray(_dev, PprzTransportPutDoubleByAddr, _n, _x) - -/** Receiving pprz messages */ - // PPRZ parsing state machine #define UNINIT 0 #define GOT_STX 1 @@ -200,16 +64,23 @@ extern uint8_t ck_a, ck_b; #define GOT_CRC1 4 struct pprz_transport { - // generic interface - struct transport trans; - // specific pprz transport variables + // generic reception interface + struct transport_rx trans_rx; + // specific pprz transport_rx variables uint8_t status; uint8_t payload_idx; - uint8_t ck_a, ck_b; + 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(void); + static inline void parse_pprz(struct pprz_transport * t, uint8_t c ) { switch (t->status) { case UNINIT: @@ -217,38 +88,38 @@ static inline void parse_pprz(struct pprz_transport * t, uint8_t c ) { t->status++; break; case GOT_STX: - if (t->trans.msg_received) { - t->trans.ovrn++; + if (t->trans_rx.msg_received) { + t->trans_rx.ovrn++; goto error; } - t->trans.payload_len = c-4; /* Counting STX, LENGTH and CRC1 and CRC2 */ - t->ck_a = t->ck_b = c; + 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.payload[t->payload_idx] = c; - t->ck_a += c; t->ck_b += t->ck_a; + 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.payload_len) + if (t->payload_idx == t->trans_rx.payload_len) t->status++; break; case GOT_PAYLOAD: - if (c != t->ck_a) + if (c != t->ck_a_rx) goto error; t->status++; break; case GOT_CRC1: - if (c != t->ck_b) + if (c != t->ck_b_rx) goto error; - t->trans.msg_received = TRUE; + t->trans_rx.msg_received = TRUE; goto restart; default: goto error; } return; error: - t->trans.error++; + t->trans_rx.error++; restart: t->status = UNINIT; return; @@ -256,20 +127,26 @@ static inline void parse_pprz(struct pprz_transport * t, uint8_t c ) { static inline void pprz_parse_payload(struct pprz_transport * t) { uint8_t i; - for(i = 0; i < t->trans.payload_len; i++) - dl_buffer[i] = t->trans.payload[i]; + for(i = 0; i < t->trans_rx.payload_len; i++) + dl_buffer[i] = t->trans_rx.payload[i]; dl_msg_available = TRUE; } #define PprzBuffer(_dev) TransportLink(_dev,ChAvailable()) -#define ReadPprzBuffer(_dev,_trans) { while (TransportLink(_dev,ChAvailable())&&!(_trans.trans.msg_received)) parse_pprz(&(_trans),TransportLink(_dev,Getch())); } + +#define ReadPprzBuffer(_dev,_trans) { \ + while (TransportLink(_dev,ChAvailable()) && !(_trans.trans_rx.msg_received)) { \ + parse_pprz(&(_trans),TransportLink(_dev,Getch())); \ + } \ +} + #define PprzCheckAndParse(_dev,_trans) { \ if (PprzBuffer(_dev)) { \ ReadPprzBuffer(_dev,_trans); \ - if (_trans.trans.msg_received) { \ + if (_trans.trans_rx.msg_received) { \ pprz_parse_payload(&(_trans)); \ - _trans.trans.msg_received = FALSE; \ + _trans.trans_rx.msg_received = FALSE; \ } \ } \ } diff --git a/sw/airborne/subsystems/datalink/pprzlog_transport.c b/sw/airborne/subsystems/datalink/pprzlog_transport.c index 3b539c2660..23f42b2e7a 100644 --- a/sw/airborne/subsystems/datalink/pprzlog_transport.c +++ b/sw/airborne/subsystems/datalink/pprzlog_transport.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2014 Gautier Hattenberger + * Copyright (C) 2014 Gautier Hattenberger * * This file is part of paparazzi. * @@ -14,14 +14,104 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . + * + */ + +/** + * @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" -uint8_t log_ck; +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->transmit(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->transmit(dev->periph, STX_LOG); + const uint8_t msg_len = size_of(trans, payload_len); + trans->ck = 0; + put_1byte(trans, dev, msg_len); + 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->transmit(dev->periph, trans->ck); + dev->send_message(dev); +} + +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, 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 index 4883bb38a3..ef96526f48 100644 --- a/sw/airborne/subsystems/datalink/pprzlog_transport.h +++ b/sw/airborne/subsystems/datalink/pprzlog_transport.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2014 Gautier Hattenberger + * Copyright (C) 2014 Gautier Hattenberger * * This file is part of paparazzi. * @@ -14,31 +14,15 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . * */ /** * @file subsystems/datalink/pprzlog_transport.h * - * 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]) + * Protocol for on-board data logger with timestamp * */ @@ -46,143 +30,16 @@ #define PPRZLOG_TRANSPORT_H #include "mcu_periph/sys_time.h" +#include "subsystems/datalink/transport.h" -extern uint8_t log_ck; - -#define STX_LOG 0x99 - -#define PprzLogTransportSizeOf(_dev, _payload) (_payload) - -#define PprzLogTransportCheckFreeSpace(_dev, _x) TransportLink(_dev, CheckFreeSpace(_x)) -#define PprzLogTransportPut1Byte(_dev, _x) TransportLink(_dev, Transmit(_x)) -#define PprzLogTransportSendMessage(_dev) TransportLink(_dev, SendMessage()) - -#define PprzLogTransportHeader(_dev, payload_len) { \ - PprzLogTransportPut1Byte(_dev, STX_LOG); \ - uint8_t msg_len = PprzLogTransportSizeOf(_dev, payload_len); \ - PprzLogTransportPut1Byte(_dev, msg_len); \ - log_ck = msg_len; \ - PprzLogTransportPutUint8(_dev, 0); \ - uint32_t ts = get_sys_time_usec()/100; \ - PprzLogTransportPut4ByteByAddr(_dev, &ts); \ -} - -#define PprzLogTransportTrailer(_dev) { \ - PprzLogTransportPut1Byte(_dev, log_ck); \ - PprzLogTransportSendMessage(_dev); \ -} - -#define PprzLogTransportPutUint8(_dev, _byte) { \ - log_ck += _byte; \ - PprzLogTransportPut1Byte(_dev, _byte); \ - } - -#define PprzLogTransportPutNamedUint8(_dev, _name, _byte) PprzLogTransportPutUint8(_dev, _byte) - -#define PprzLogTransportPut1ByteByAddr(_dev, _byte) { \ - uint8_t _x = *(_byte); \ - PprzLogTransportPutUint8(_dev, _x); \ - } - -#define PprzLogTransportPut2ByteByAddr(_dev, _byte) { \ - PprzLogTransportPut1ByteByAddr(_dev, _byte); \ - PprzLogTransportPut1ByteByAddr(_dev, (const uint8_t*)_byte+1); \ - } - -#define PprzLogTransportPut4ByteByAddr(_dev, _byte) { \ - PprzLogTransportPut2ByteByAddr(_dev, _byte); \ - PprzLogTransportPut2ByteByAddr(_dev, (const uint8_t*)_byte+2); \ - } - -#ifdef __IEEE_BIG_ENDIAN /* From machine/ieeefp.h */ -#define PprzLogTransportPutDoubleByAddr(_dev, _byte) { \ - PprzLogTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4); \ - PprzLogTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte); \ - } -#define PprzLogTransportPutUint64ByAddr(_dev, _byte) { \ - PprzLogTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4); \ - PprzLogTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte); \ - } -#define PprzLogTransportPutInt64ByAddr(_dev, _byte) { \ - PprzLogTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4); \ - PprzLogTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte); \ - } -#else -#define PprzLogTransportPutDoubleByAddr(_dev, _byte) { \ - PprzLogTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte); \ - PprzLogTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4); \ - } -#define PprzLogTransportPutUint64ByAddr(_dev, _byte) { \ - PprzLogTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte); \ - PprzLogTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4); \ - } -#define PprzLogTransportPutInt64ByAddr(_dev, _byte) { \ - PprzLogTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte); \ - PprzLogTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4); \ - } -#endif - - -#define PprzLogTransportPutInt8ByAddr(_dev, _x) PprzLogTransportPut1ByteByAddr(_dev, _x) -#define PprzLogTransportPutUint8ByAddr(_dev, _x) PprzLogTransportPut1ByteByAddr(_dev, (const uint8_t*)_x) -#define PprzLogTransportPutInt16ByAddr(_dev, _x) PprzLogTransportPut2ByteByAddr(_dev, (const uint8_t*)_x) -#define PprzLogTransportPutUint16ByAddr(_dev, _x) PprzLogTransportPut2ByteByAddr(_dev, (const uint8_t*)_x) -#define PprzLogTransportPutInt32ByAddr(_dev, _x) PprzLogTransportPut4ByteByAddr(_dev, (const uint8_t*)_x) -#define PprzLogTransportPutUint32ByAddr(_dev, _x) PprzLogTransportPut4ByteByAddr(_dev, (const uint8_t*)_x) -#define PprzLogTransportPutFloatByAddr(_dev, _x) PprzLogTransportPut4ByteByAddr(_dev, (const uint8_t*)_x) -#define PprzLogTransportPutCharByAddr(_dev, _x) PprzLogTransportPut1ByteByAddr(_dev, (const uint8_t*)_x) - -#define PprzLogTransportPutArray(_dev, _put, _n, _x) { \ - uint8_t _i; \ - PprzLogTransportPutUint8(_dev, _n); \ - for(_i = 0; _i < _n; _i++) { \ - _put(_dev, &_x[_i]); \ - } \ -} - -#define PprzLogTransportPutInt8Array(_dev, _n, _x) PprzLogTransportPutArray(_dev, PprzLogTransportPutInt8ByAddr, _n, _x) -#define PprzLogTransportPutUint8Array(_dev, _n, _x) PprzLogTransportPutArray(_dev, PprzLogTransportPutUint8ByAddr, _n, _x) - -#define PprzLogTransportPutCharArray(_dev, _n, _x) PprzLogTransportPutArray(_dev, PprzLogTransportPutCharByAddr, _n, _x) - -#define PprzLogTransportPutInt16Array(_dev, _n, _x) PprzLogTransportPutArray(_dev, PprzLogTransportPutInt16ByAddr, _n, _x) -#define PprzLogTransportPutUint16Array(_dev, _n, _x) PprzLogTransportPutArray(_dev, PprzLogTransportPutUint16ByAddr, _n, _x) - -#define PprzLogTransportPutInt32Array(_dev, _n, _x) PprzLogTransportPutArray(_dev, PprzLogTransportPutInt32ByAddr, _n, _x) -#define PprzLogTransportPutUint32Array(_dev, _n, _x) PprzLogTransportPutArray(_dev, PprzLogTransportPutUint32ByAddr, _n, _x) - -#define PprzLogTransportPutFloatArray(_dev, _n, _x) PprzLogTransportPutArray(_dev, PprzLogTransportPutFloatByAddr, _n, _x) - -#define PprzLogTransportPutInt64Array(_dev, _n, _x) PprzLogTransportPutArray(_dev, PprzLogTransportPutInt64ByAddr, _n, _x) -#define PprzLogTransportPutUint64Array(_dev, _n, _x) PprzLogTransportPutArray(_dev, PprzLogTransportPutUint64ByAddr, _n, _x) - -#define PprzLogTransportPutDoubleArray(_dev, _n, _x) PprzLogTransportPutArray(_dev, PprzLogTransportPutDoubleByAddr, _n, _x) - -#define PprzLogTransportPutFixedArray(_dev, _put, _n, _x) { \ - uint8_t _i; \ - for(_i = 0; _i < _n; _i++) { \ - _put(_dev, &_x[_i]); \ - } \ -} - -#define PprzLogTransportPutInt8FixedArray(_dev, _n, _x) PprzLogTransportPutFixedArray(_dev, PprzLogTransportPutInt8ByAddr, _n, _x) -#define PprzLogTransportPutUint8FixedArray(_dev, _n, _x) PprzLogTransportPutFixedArray(_dev, PprzLogTransportPutUint8ByAddr, _n, _x) - -#define PprzLogTransportPutCharFixedArray(_dev, _n, _x) PprzLogTransportPutFixedArray(_dev, PprzLogTransportPutCharByAddr, _n, _x) - -#define PprzLogTransportPutInt16FixedArray(_dev, _n, _x) PprzLogTransportPutFixedArray(_dev, PprzLogTransportPutInt16ByAddr, _n, _x) -#define PprzLogTransportPutUint16FixedArray(_dev, _n, _x) PprzLogTransportPutFixedArray(_dev, PprzLogTransportPutUint16ByAddr, _n, _x) - -#define PprzLogTransportPutInt32FixedArray(_dev, _n, _x) PprzLogTransportPutFixedArray(_dev, PprzLogTransportPutInt32ByAddr, _n, _x) -#define PprzLogTransportPutUint32FixedArray(_dev, _n, _x) PprzLogTransportPutFixedArray(_dev, PprzLogTransportPutUint32ByAddr, _n, _x) - -#define PprzLogTransportPutFloatFixedArray(_dev, _n, _x) PprzLogTransportPutFixedArray(_dev, PprzLogTransportPutFloatByAddr, _n, _x) - -#define PprzLogTransportPutInt64FixedArray(_dev, _n, _x) PprzLogTransportPutFixedArray(_dev, PprzLogTransportPutInt64ByAddr, _n, _x) -#define PprzLogTransportPutUint64FixedArray(_dev, _n, _x) PprzLogTransportPutFixedArray(_dev, PprzLogTransportPutUint64ByAddr, _n, _x) - -#define PprzLogTransportPutDoubleFixedArray(_dev, _n, _x) PprzLogTransportPutFixedArray(_dev, PprzLogTransportPutDoubleByAddr, _n, _x) +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; #endif diff --git a/sw/airborne/subsystems/datalink/superbitrf.c b/sw/airborne/subsystems/datalink/superbitrf.c index d6af4d24ac..760594bf4e 100644 --- a/sw/airborne/subsystems/datalink/superbitrf.c +++ b/sw/airborne/subsystems/datalink/superbitrf.c @@ -178,10 +178,12 @@ static const uint8_t pn_bind[] = { 0x98, 0x88, 0x1B, 0xE4, 0x30, 0x79, 0x03, 0x8 #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_superbit(void) { - DOWNLINK_SEND_SUPERBITRF(DefaultChannel, DefaultDevice, - &superbitrf.status, - &superbitrf.cyrf6936.status, +static void send_superbit(struct transport_tx *trans, struct link_device *dev) { + uint8_t status = superbitrf.status; + uint8_t cyrf6936_status = superbitrf.cyrf6936.status; + pprz_msg_send_SUPERBITRF(trans, dev, AC_ID, + &status, + &cyrf6936_status, &superbitrf.irq_count, &superbitrf.rx_packet_count, &superbitrf.tx_packet_count, @@ -197,6 +199,20 @@ static void send_superbit(void) { } #endif +// Functions for the generic device API +static int superbitrf_check_free_space(struct SuperbitRF* p __attribute__((unused)), uint8_t len __attribute__((unused))) +{ + return (((superbitrf.tx_insert_idx+1) %128) != superbitrf.tx_extract_idx); +} + +static void superbitrf_transmit(struct SuperbitRF* p __attribute__((unused)), uint8_t byte) +{ + superbitrf.tx_buffer[superbitrf.tx_insert_idx] = byte; + superbitrf.tx_insert_idx = (superbitrf.tx_insert_idx+1) %128; +} + +static void superbitrf_send(struct SuperbitRF* p __attribute__((unused))) { } + /** * Initialize the superbitrf */ @@ -216,6 +232,12 @@ void superbitrf_init(void) { superbitrf.num_channels = 0; superbitrf.protocol = 0; + // Configure generic device + superbitrf.device.periph = (void *)(&superbitrf); + superbitrf.device.check_free_space = (check_free_space_t) superbitrf_check_free_space; + superbitrf.device.transmit = (transmit_t) superbitrf_transmit; + superbitrf.device.send_message = (send_message_t) superbitrf_send; + // Initialize the binding pin gpio_setup_input(SPEKTRUM_BIND_PIN_PORT, SPEKTRUM_BIND_PIN); @@ -794,9 +816,9 @@ static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, ui parse_pprz(&superbitrf.rx_transport, packet[i]); // When we have a full message - if (superbitrf.rx_transport.trans.msg_received) { + if (superbitrf.rx_transport.trans_rx.msg_received) { pprz_parse_payload(&superbitrf.rx_transport); - superbitrf.rx_transport.trans.msg_received = FALSE; + superbitrf.rx_transport.trans_rx.msg_received = FALSE; } } } @@ -853,9 +875,9 @@ static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, ui parse_pprz(&superbitrf.rx_transport, packet[i]); // When we have a full message - if (superbitrf.rx_transport.trans.msg_received) { + if (superbitrf.rx_transport.trans_rx.msg_received) { pprz_parse_payload(&superbitrf.rx_transport); - superbitrf.rx_transport.trans.msg_received = FALSE; + superbitrf.rx_transport.trans_rx.msg_received = FALSE; } } } @@ -937,9 +959,9 @@ static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, ui parse_pprz(&superbitrf.rx_transport, packet[i]); // When we have a full message - if (superbitrf.rx_transport.trans.msg_received) { + if (superbitrf.rx_transport.trans_rx.msg_received) { pprz_parse_payload(&superbitrf.rx_transport); - superbitrf.rx_transport.trans.msg_received = FALSE; + 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 41f5ad1a3e..1bba6ffd7c 100644 --- a/sw/airborne/subsystems/datalink/superbitrf.h +++ b/sw/airborne/subsystems/datalink/superbitrf.h @@ -29,6 +29,7 @@ #include "mcu_periph/gpio.h" #include "peripherals/cyrf6936.h" +#include "mcu_periph/link_device.h" #include "subsystems/datalink/datalink.h" #include "subsystems/datalink/pprz_transport.h" @@ -110,6 +111,9 @@ struct SuperbitRF { uint8_t tx_buffer[128]; /**< The transmit buffer */ uint8_t tx_insert_idx; /**< The transmit buffer insert index */ uint8_t tx_extract_idx; /**< The transmit buffer extract index */ + + /** Generic device interface */ + struct link_device device; }; /* The superbitrf functions and structures */ diff --git a/sw/airborne/subsystems/datalink/telemetry_common.h b/sw/airborne/subsystems/datalink/telemetry_common.h index 96cf09dc46..ab107ddbc0 100644 --- a/sw/airborne/subsystems/datalink/telemetry_common.h +++ b/sw/airborne/subsystems/datalink/telemetry_common.h @@ -31,10 +31,12 @@ #include #include "std.h" +#include "mcu_periph/link_device.h" +#include "subsystems/datalink/transport.h" /** Telemetry callback definition */ -typedef void (*telemetry_cb)(void); +typedef void (*telemetry_cb)(struct transport_tx *trans, struct link_device *dev); /** Telemetry header */ diff --git a/sw/airborne/subsystems/datalink/transport.h b/sw/airborne/subsystems/datalink/transport.h index 09d42ab980..d3dc1a4da0 100644 --- a/sw/airborne/subsystems/datalink/transport.h +++ b/sw/airborne/subsystems/datalink/transport.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2011 Gautier Hattenberger + * Copyright (C) 2011-2014 Gautier Hattenberger * * This file is part of paparazzi. * @@ -14,13 +14,12 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . * */ -/** \file transport.h +/** \file subsystems/datalink/transport.h * generic transport header */ @@ -28,23 +27,20 @@ #define TRANSPORT_H #include +#include "mcu_periph/link_device.h" #include "std.h" #ifndef TRANSPORT_PAYLOAD_LEN #define TRANSPORT_PAYLOAD_LEN 256 #endif -/** Generic transport header +/** Generic reception transport header */ -struct transport { - // payload buffer - uint8_t payload[TRANSPORT_PAYLOAD_LEN]; - // payload length - volatile uint8_t payload_len; - // message received flag - volatile bool_t msg_received; - // overrun and error flags - uint8_t ovrn, error; +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 }; /** Transport link macros @@ -56,5 +52,59 @@ struct transport { #define _TransportLink(dev, _x) __TransportLink(dev, _x) #define TransportLink(_dev, _x) _TransportLink(_dev, _x) + +/** 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/w5100.c b/sw/airborne/subsystems/datalink/w5100.c index 3e767869bd..7fd5d2925e 100644 --- a/sw/airborne/subsystems/datalink/w5100.c +++ b/sw/airborne/subsystems/datalink/w5100.c @@ -28,7 +28,7 @@ #include "mcu_periph/sys_time.h" #include "subsystems/datalink/w5100.h" #include "mcu_periph/spi.h" -#inculde "mcu_periph/gpio.h" +#include "mcu_periph/gpio.h" #define TXBUF_BASE 0x4000 #define RXBUF_BASE 0x6000 @@ -106,7 +106,6 @@ uint16_t RBASE[SOCKETS]; // Rx buffer base address static const uint16_t SSIZE = 2048; // Max Tx buffer size static const uint16_t RSIZE = 2048; // Max Rx buffer size -struct w5100_transport w5100_tp; struct spi_transaction w5100_spi; static void w5100_close_socket( uint8_t _s ); @@ -166,6 +165,11 @@ static inline uint16_t w5100_sock_get16( uint8_t _sock, uint16_t _reg) { return res; } +// Functions for the generic device API +static int true_function(struct w5100_periph* p __attribute__((unused)), uint8_t len __attribute__((unused))) { return TRUE; } +static void dev_transmit(struct w5100_periph* p __attribute__((unused)), uint8_t byte) { w5100_transmit(byte); } +static void dev_send(struct w5100_periph* p __attribute__((unused))) { w5100_send(); } + void w5100_init( void ) { // configure the SPI bus. @@ -232,6 +236,12 @@ void w5100_init( void ) { // make dest zero and configure socket to receive data dest[ 0 ] = 0x00; configure_socket( CMD_SOCKET, 0, dport, dport, dest ); + + // Configure generic device + chip0.device.periph = (void *)(&chip0); + chip0.device.check_free_space = (check_free_space_t) true_function; + chip0.device.transmit = (transmit_t) dev_transmit; + chip0.device.send_message = (send_message_t) dev_send; } void w5100_transmit( uint8_t data ) { @@ -341,7 +351,7 @@ bool_t w5100_ch_available() { return FALSE; } -uint16_t w5100_receive( uint8_t *buf, uint16_t len ) { +uint16_t w5100_receive( uint8_t *buf, uint16_t len __attribute__((unused))) { uint8_t head[8]; uint16_t data_len=0; uint16_t ptr=0; @@ -366,7 +376,7 @@ uint16_t w5100_receive( uint8_t *buf, uint16_t len ) { return data_len; } -static void w5100_read_data( uint8_t s, volatile uint8_t *src, volatile uint8_t *dst, uint16_t len ) { +static void w5100_read_data( uint8_t s __attribute__((unused)), volatile uint8_t *src, volatile uint8_t *dst, uint16_t len ) { uint16_t size; uint16_t src_mask; uint16_t src_ptr; diff --git a/sw/airborne/subsystems/datalink/w5100.h b/sw/airborne/subsystems/datalink/w5100.h index 392fe9a37d..4ee53fcee6 100644 --- a/sw/airborne/subsystems/datalink/w5100.h +++ b/sw/airborne/subsystems/datalink/w5100.h @@ -1,5 +1,6 @@ /* * Copyright (C) 2012 Gerard Toonstra + * Copyright (C) 2014 Gautier Hattenberger * * This file is part of paparazzi. * @@ -14,9 +15,8 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . * */ @@ -25,18 +25,15 @@ * W5100 ethernet chip I/O */ -#ifndef W5100_TELEM_H -#define W5100_TELEM_H +#ifndef W5100_H +#define W5100_H -#include "subsystems/datalink/datalink.h" +#include "mcu_periph/link_device.h" #include "generated/airframe.h" -#include "subsystems/datalink/transport.h" - #define W5100_RX_BUFFER_SIZE 80 #define W5100_TX_BUFFER_SIZE 80 #define W5100_BUFFER_NUM 2 -#define STX 0x99 enum W5100Status { W5100StatusUninit, @@ -58,15 +55,15 @@ struct w5100_periph { volatile uint8_t work_tx[4]; volatile uint8_t work_rx[4]; uint8_t tx_running; + /** Generic device interface */ + struct link_device device; }; extern uint8_t w5100_rx_buf[W5100_RX_BUFFER_SIZE]; extern struct w5100_periph chip0; -//extern uint8_t ck_a, ck_b; void w5100_init( void ); - void w5100_transmit( uint8_t data ); uint16_t w5100_receive( uint8_t *buf, uint16_t len ); void w5100_send( void ); @@ -84,221 +81,19 @@ bool_t w5100_ch_available( void ); #define W5100TxRunning chip0.tx_running #define W5100SetBaudrate(_b) w5100_set_baudrate(_b) -#define W5100TransportSizeOf(_dev, _x) (_x+4) -#define W5100TransportCheckFreeSpace(_dev, x) TransportLink(_dev, CheckFreeSpace(x)) -#define W5100TransportPut1Byte(_dev, x) TransportLink(_dev, Transmit(x)) -#define W5100TransportSendMessage(_dev) TransportLink(_dev, SendMessage()) -#define W5100TransportPutUint8(_dev, _byte) { \ - ck_a += _byte; \ - ck_b += ck_a; \ - W5100TransportPut1Byte(_dev, _byte); \ - } +// 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" -#define W5100TransportPut1ByteByAddr(_dev, _byte) { \ - uint8_t _x = *(_byte); \ - W5100TransportPutUint8(_dev, _x); \ - } - -#define W5100TransportPut2Bytes(_dev, _x) { \ - uint16_t x16 = _x; \ - W5100TransportPut1Byte(_dev, x16>>8); \ - W5100TransportPut1Byte(_dev, x16 & 0xff); \ - } - -#define W5100TransportPut2ByteByAddr(_dev, _byte) { \ - W5100TransportPut1ByteByAddr(_dev, _byte); \ - W5100TransportPut1ByteByAddr(_dev, (const uint8_t*)_byte+1); \ - } - -#define W5100TransportPut4ByteByAddr(_dev, _byte) { \ - W5100TransportPut2ByteByAddr(_dev, _byte); \ - W5100TransportPut2ByteByAddr(_dev, (const uint8_t*)_byte+2); \ - } - -#ifdef __IEEE_BIG_ENDIAN // From machine/ieeefp.h -#define W5100TransportPutDoubleByAddr(_dev, _byte) { \ - W5100TransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4); \ - W5100TransportPut4ByteByAddr(_dev, (const uint8_t*)_byte); \ - } -#define W5100TransportPutUint64ByAddr(_dev, _byte) { \ - W5100TransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4); \ - W5100TransportPut4ByteByAddr(_dev, (const uint8_t*)_byte); \ - } -#define W5100TransportPutInt64ByAddr(_dev, _byte) { \ - W5100TransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4); \ - W5100TransportPut4ByteByAddr(_dev, (const uint8_t*)_byte); \ - } -#else -#define W5100TransportPutDoubleByAddr(_dev, _byte) { \ - W5100TransportPut4ByteByAddr(_dev, (const uint8_t*)_byte); \ - W5100TransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4); \ - } -#define W5100TransportPutUint64ByAddr(_dev, _byte) { \ - W5100TransportPut4ByteByAddr(_dev, (const uint8_t*)_byte); \ - W5100TransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4); \ - } -#define W5100TransportPutInt64ByAddr(_dev, _byte) { \ - W5100TransportPut4ByteByAddr(_dev, (const uint8_t*)_byte); \ - W5100TransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4); \ - } -#endif - - -#define W5100TransportPutInt8ByAddr(_dev, _x) W5100TransportPut1ByteByAddr(_dev, _x) -#define W5100TransportPutUint8ByAddr(_dev, _x) W5100TransportPut1ByteByAddr(_dev, (const uint8_t*)_x) -#define W5100TransportPutInt16ByAddr(_dev, _x) W5100TransportPut2ByteByAddr(_dev, (const uint8_t*)_x) -#define W5100TransportPutUint16ByAddr(_dev, _x) W5100TransportPut2ByteByAddr(_dev, (const uint8_t*)_x) -#define W5100TransportPutInt32ByAddr(_dev, _x) W5100TransportPut4ByteByAddr(_dev, (const uint8_t*)_x) -#define W5100TransportPutUint32ByAddr(_dev, _x) W5100TransportPut4ByteByAddr(_dev, (const uint8_t*)_x) -#define W5100TransportPutFloatByAddr(_dev, _x) W5100TransportPut4ByteByAddr(_dev, (const uint8_t*)_x) -#define W5100TransportPutNamedUint8(_dev, _name, _byte) W5100TransportPutUint8(_dev, _byte) - -#define W5100TransportPutArray(_dev, _put, _n, _x) { \ - uint8_t _i; \ - W5100TransportPutUint8(_dev, _n); \ - for(_i = 0; _i < _n; _i++) { \ - _put(_dev, &_x[_i]); \ - } \ - } - -#define W5100TransportPutInt8Array(_dev, _n, _x) W5100TransportPutArray(_dev, W5100TransportPutInt8ByAddr, _n, _x) -#define W5100TransportPutUint8Array(_dev, _n, _x) W5100TransportPutArray(_dev, W5100TransportPutUint8ByAddr, _n, _x) - -#define W5100TransportPutCharArray(_dev, _n, _x) W5100TransportPutArray(_dev, W5100TransportPutCharByAddr, _n, _x) - -#define W5100TransportPutInt16Array(_dev, _n, _x) W5100TransportPutArray(_dev, W5100TransportPutInt16ByAddr, _n, _x) -#define W5100TransportPutUint16Array(_dev, _n, _x) W5100TransportPutArray(_dev, W5100TransportPutUint16ByAddr, _n, _x) - -#define W5100TransportPutInt32Array(_dev, _n, _x) W5100TransportPutArray(_dev, W5100TransportPutInt32ByAddr, _n, _x) -#define W5100TransportPutUint32Array(_dev, _n, _x) W5100TransportPutArray(_dev, W5100TransportPutUint32ByAddr, _n, _x) - -#define W5100TransportPutFloatArray(_dev, _n, _x) W5100TransportPutArray(_dev, W5100TransportPutFloatByAddr, _n, _x) - -#define W5100TransportPutInt64Array(_dev, _n, _x) W5100TransportPutArray(_dev, W5100TransportPutInt64ByAddr, _n, _x) -#define W5100TransportPutUint64Array(_dev, _n, _x) W5100TransportPutArray(_dev, W5100TransportPutUint64ByAddr, _n, _x) - -#define W5100TransportPutDoubleArray(_dev, _n, _x) W5100TransportPutArray(_dev, W5100TransportPutDoubleByAddr, _n, _x) - - -#define W5100TransportPutFixedArray(_dev, _put, _n, _x) { \ - uint8_t _i; \ - for(_i = 0; _i < _n; _i++) { \ - _put(_dev, &_x[_i]); \ - } \ - } - -#define W5100TransportPutInt8FixedArray(_dev, _n, _x) W5100TransportPutFixedArray(_dev, W5100TransportPutInt8ByAddr, _n, _x) -#define W5100TransportPutUint8FixedArray(_dev, _n, _x) W5100TransportPutFixedArray(_dev, W5100TransportPutUint8ByAddr, _n, _x) - -#define W5100TransportPutCharFixedArray(_dev, _n, _x) W5100TransportPutFixedArray(_dev, W5100TransportPutCharByAddr, _n, _x) - -#define W5100TransportPutInt16FixedArray(_dev, _n, _x) W5100TransportPutFixedArray(_dev, W5100TransportPutInt16ByAddr, _n, _x) -#define W5100TransportPutUint16FixedArray(_dev, _n, _x) W5100TransportPutFixedArray(_dev, W5100TransportPutUint16ByAddr, _n, _x) - -#define W5100TransportPutInt32FixedArray(_dev, _n, _x) W5100TransportPutFixedArray(_dev, W5100TransportPutInt32ByAddr, _n, _x) -#define W5100TransportPutUint32FixedArray(_dev, _n, _x) W5100TransportPutFixedArray(_dev, W5100TransportPutUint32ByAddr, _n, _x) - -#define W5100TransportPutFloatFixedArray(_dev, _n, _x) W5100TransportPutFixedArray(_dev, W5100TransportPutFloatByAddr, _n, _x) - -#define W5100TransportPutInt64FixedArray(_dev, _n, _x) W5100TransportPutFixedArray(_dev, W5100TransportPutInt64ByAddr, _n, _x) -#define W5100TransportPutUint64FixedArray(_dev, _n, _x) W5100TransportPutFixedArray(_dev, W5100TransportPutUint64ByAddr, _n, _x) - -#define W5100TransportPutDoubleFixedArray(_dev, _n, _x) W5100TransportPutFixedArray(_dev, W5100TransportPutDoubleByAddr, _n, _x) - - -#define W5100TransportHeader(_dev, payload_len) { \ - W5100TransportPut1Byte(_dev, STX); \ - uint8_t msg_len = W5100TransportSizeOf(_dev, payload_len); \ - W5100TransportPut1Byte(_dev, msg_len); \ - ck_a = msg_len; ck_b = msg_len; \ - } - -#define W5100TransportTrailer(_dev) { \ - W5100TransportPut1Byte(_dev, ck_a); \ - W5100TransportPut1Byte(_dev, ck_b); \ - W5100TransportSendMessage(_dev); \ - } - - -/** Receiving pprz messages */ - -// PPRZ parsing state machine -#define UNINIT 0 -#define GOT_STX 1 -#define GOT_LENGTH 2 -#define GOT_PAYLOAD 3 -#define GOT_CRC1 4 - -struct w5100_transport { - // generic interface - struct transport trans; - // specific pprz transport variables - uint8_t status; - uint8_t payload_idx; - uint8_t ck_a, ck_b; -}; - -extern struct w5100_transport w5100_tp; - -static inline void parse_w5100(struct w5100_transport * t, uint8_t c ) { - switch (t->status) { - case UNINIT: - if (c == STX) - t->status++; - break; - case GOT_STX: - if (t->trans.msg_received) { - t->trans.ovrn++; - goto error; - } - t->trans.payload_len = c-4; /* Counting STX, LENGTH and CRC1 and CRC2 */ - t->ck_a = t->ck_b = c; - t->status++; - t->payload_idx = 0; - break; - case GOT_LENGTH: - t->trans.payload[t->payload_idx] = c; - t->ck_a += c; t->ck_b += t->ck_a; - t->payload_idx++; - if (t->payload_idx == t->trans.payload_len) - t->status++; - break; - case GOT_PAYLOAD: - if (c != t->ck_a) - goto error; - t->status++; - break; - case GOT_CRC1: - if (c != t->ck_b) - goto error; - t->trans.msg_received = TRUE; - goto restart; - default: - goto error; - } - return; - error: - t->trans.error++; - restart: - t->status = UNINIT; - return; -} - -static inline void w5100_parse_payload(struct w5100_transport * t) { - uint8_t i; - for(i = 0; i < t->trans.payload_len; i++) - dl_buffer[i] = t->trans.payload[i]; - dl_msg_available = TRUE; -} - -static inline void w5100_read_buffer( struct w5100_transport *t ) { +static inline void w5100_read_buffer( struct pprz_transport *t ) { while ( w5100_ch_available() ) { w5100_receive( w5100_rx_buf, W5100_RX_BUFFER_SIZE ); int c = 0; do { - parse_w5100( t, w5100_rx_buf[ c++ ] ); - } while ( ( t->status != UNINIT ) && !(t->trans.msg_received) ); + parse_pprz( t, w5100_rx_buf[ c++ ] ); + } while ( ( t->status != UNINIT ) && !(t->trans_rx.msg_received) ); } } @@ -307,13 +102,13 @@ static inline void w5100_read_buffer( struct w5100_transport *t ) { #define W5100CheckAndParse(_dev,_trans) { \ if (W5100Buffer(_dev)) { \ w5100_read_buffer( &(_trans) ); \ - if (_trans.trans.msg_received) { \ - w5100_parse_payload(&(_trans)); \ - _trans.trans.msg_received = FALSE; \ + if (_trans.trans_rx.msg_received) { \ + pprz_parse_payload(&(_trans)); \ + _trans.trans_rx.msg_received = FALSE; \ } \ } \ } -#endif /* W5100_TELEM_H */ +#endif /* W5100_H */ diff --git a/sw/airborne/subsystems/datalink/xbee.c b/sw/airborne/subsystems/datalink/xbee.c index 4c89b86915..e26fe7c1b5 100644 --- a/sw/airborne/subsystems/datalink/xbee.c +++ b/sw/airborne/subsystems/datalink/xbee.c @@ -1,5 +1,6 @@ /* * Copyright (C) 2006 Pascal Brisset, Antoine Drouin + * Copyright (C) 2014 Gautier Hattenberger * * This file is part of paparazzi. * @@ -14,22 +15,34 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . * */ +/** + * @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" #ifdef SIM_UART #include "sim_uart.h" #endif -uint8_t xbee_cs; -uint8_t xbee_rssi; +/** 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; @@ -38,6 +51,68 @@ struct xbee_transport xbee_tp; #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->transmit(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) +{ + downlink.nb_msgs++; + dev->transmit(dev->periph, XBEE_START); + const uint16_t len = payload_len + XBEE_API_OVERHEAD; + dev->transmit(dev->periph, (len >> 8)); + dev->transmit(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); +} + +static void end_message(struct xbee_transport *trans, struct link_device *dev) +{ + trans->cs_tx = 0xff - trans->cs_tx; + dev->transmit(dev->periph, trans->cs_tx); + dev->send_message(dev); +} + +static void overrun(struct xbee_transport *trans __attribute__((unused)), struct link_device *dev __attribute__((unused))) +{ + downlink.nb_ovrn++; +} + +static void count_bytes(struct xbee_transport *trans __attribute__((unused)), struct link_device *dev __attribute__((unused)), uint8_t bytes) +{ + downlink.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, bytes); +} + static uint8_t xbee_text_reply_is_ok(void) { char c[2]; @@ -86,7 +161,16 @@ static uint8_t xbee_try_to_enter_api(void) { void xbee_init( void ) { xbee_tp.status = XBEE_UNINIT; - xbee_tp.trans.msg_received = FALSE; + 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); // Empty buffer before init process while (TransportLink(XBEE_UART,ChAvailable())) diff --git a/sw/airborne/subsystems/datalink/xbee.h b/sw/airborne/subsystems/datalink/xbee.h index a0db46f8b3..5dba67f93c 100644 --- a/sw/airborne/subsystems/datalink/xbee.h +++ b/sw/airborne/subsystems/datalink/xbee.h @@ -1,5 +1,6 @@ /* * Copyright (C) 2006 Pascal Brisset, Antoine Drouin + * Copyright (C) 2014 Gautier Hattenberger * * This file is part of paparazzi. * @@ -14,184 +15,35 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . * */ -/* Maxstream XBee serial input and output */ +/** + * @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 -#include "subsystems/datalink/transport.h" -/** Constants for the API protocol */ #define XBEE_START 0x7e -#define TX_OPTIONS 0x00 -#define NO_FRAME_ID 0 - -/** Ground station address */ -#define GROUND_STATION_ADDR 0x100 - -extern uint8_t xbee_cs; -extern uint8_t xbee_rssi; /** Initialisation in API mode and setting of the local address * FIXME: busy wait */ -#define XBEE_MY_ADDR AC_ID void xbee_init( void ); -/* 5 = Start + len_msb + len_lsb + API_id + checksum */ -#define XBeeAPISizeOf(_dev, _x) (_x+5) - -#define XBeeTransportCheckFreeSpace(_dev, x) TransportLink(_dev, CheckFreeSpace(x)) -#define XBeeTransportPut1Byte(_dev, x) TransportLink(_dev, Transmit(x)) -#define XBeeTransportSendMessage(_dev) TransportLink(_dev, SendMessage()) - -#define XBeeTransportPutUint8(_dev, _x) { \ - xbee_cs += _x; \ - XBeeTransportPut1Byte(_dev, _x); \ -} - -#define XBeeTransportPut1ByteByAddr(_dev, _byte) { \ - uint8_t _x = *(_byte); \ - XBeeTransportPutUint8(_dev, _x); \ - } - -#define XBeeTransportPut2Bytes(_dev, _x) { \ - uint16_t x16 = _x; \ - XBeeTransportPut1Byte(_dev, x16>>8); \ - XBeeTransportPut1Byte(_dev, x16 & 0xff); \ -} - -#define XBeeTransportPut2ByteByAddr(_dev, _byte) { \ - XBeeTransportPut1ByteByAddr(_dev, _byte); \ - XBeeTransportPut1ByteByAddr(_dev, (const uint8_t*)_byte+1); \ - } - -#define XBeeTransportPut4ByteByAddr(_dev, _byte) { \ - XBeeTransportPut2ByteByAddr(_dev, _byte); \ - XBeeTransportPut2ByteByAddr(_dev, (const uint8_t*)_byte+2); \ - } - -#ifdef __IEEE_BIG_ENDIAN /* From machine/ieeefp.h */ -#define XBeeTransportPutDoubleByAddr(_dev, _byte) { \ - XBeeTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4); \ - XBeeTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte); \ - } -#define XBeeTransportPutUint64ByAddr(_dev, _byte) { \ - XBeeTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4); \ - XBeeTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte); \ - } -#define XBeeTransportPutInt64ByAddr(_dev, _byte) { \ - XBeeTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4); \ - XBeeTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte); \ - } -#else -#define XBeeTransportPutDoubleByAddr(_dev, _byte) { \ - XBeeTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte); \ - XBeeTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4); \ - } -#define XBeeTransportPutUint64ByAddr(_dev, _byte) { \ - XBeeTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte); \ - XBeeTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4); \ - } -#define XBeeTransportPutInt64ByAddr(_dev, _byte) { \ - XBeeTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte); \ - XBeeTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4); \ - } -#endif - - -#define XBeeTransportPutInt8ByAddr(_dev, _x) XBeeTransportPut1ByteByAddr(_dev, _x) -#define XBeeTransportPutUint8ByAddr(_dev, _x) XBeeTransportPut1ByteByAddr(_dev, (const uint8_t*)_x) -#define XBeeTransportPutInt16ByAddr(_dev, _x) XBeeTransportPut2ByteByAddr(_dev, (const uint8_t*)_x) -#define XBeeTransportPutUint16ByAddr(_dev, _x) XBeeTransportPut2ByteByAddr(_dev, (const uint8_t*)_x) -#define XBeeTransportPutInt32ByAddr(_dev, _x) XBeeTransportPut4ByteByAddr(_dev, (const uint8_t*)_x) -#define XBeeTransportPutUint32ByAddr(_dev, _x) XBeeTransportPut4ByteByAddr(_dev, (const uint8_t*)_x) -#define XBeeTransportPutFloatByAddr(_dev, _x) XBeeTransportPut4ByteByAddr(_dev, (const uint8_t*)_x) -#define XBeeTransportPutNamedUint8(_dev, _name, _byte) XBeeTransportPutUint8(_dev, _byte) -#define XBeeTransportPutCharByAddr(_dev, _x) XBeeTransportPut1ByteByAddr(_dev, (const uint8_t*)_x) - -#define XBeeTransportPutArray(_dev, _put, _n, _x) { \ - uint8_t _i; \ - XBeeTransportPutUint8(_dev, _n); \ - for(_i = 0; _i < _n; _i++) { \ - _put(_dev, &_x[_i]); \ - } \ -} - -#define XBeeTransportPutInt8Array(_dev, _n, _x) XBeeTransportPutArray(_dev, XBeeTransportPutInt8ByAddr, _n, _x) -#define XBeeTransportPutUint8Array(_dev, _n, _x) XBeeTransportPutArray(_dev, XBeeTransportPutUint8ByAddr, _n, _x) - -#define XBeeTransportPutCharArray(_dev, _n, _x) XBeeTransportPutArray(_dev, XBeeTransportPutCharByAddr, _n, _x) - -#define XBeeTransportPutInt16Array(_dev, _n, _x) XBeeTransportPutArray(_dev, XBeeTransportPutInt16ByAddr, _n, _x) -#define XBeeTransportPutUint16Array(_dev, _n, _x) XBeeTransportPutArray(_dev, XBeeTransportPutUint16ByAddr, _n, _x) - -#define XBeeTransportPutInt32Array(_dev, _n, _x) XBeeTransportPutArray(_dev, XBeeTransportPutInt32ByAddr, _n, _x) -#define XBeeTransportPutUint32Array(_dev, _n, _x) XBeeTransportPutArray(_dev, XBeeTransportPutUint32ByAddr, _n, _x) - -#define XBeeTransportPutFloatArray(_dev, _n, _x) XBeeTransportPutArray(_dev, XBeeTransportPutFloatByAddr, _n, _x) - -#define XBeeTransportPutInt64Array(_dev, _n, _x) XBeeTransportPutArray(_dev, XBeeTransportPutInt64ByAddr, _n, _x) -#define XBeeTransportPutUint64Array(_dev, _n, _x) XBeeTransportPutArray(_dev, XBeeTransportPutUint64ByAddr, _n, _x) - -#define XBeeTransportPutDoubleArray(_dev, _n, _x) XBeeTransportPutArray(_dev, XBeeTransportPutDoubleByAddr, _n, _x) - - -#define XBeeTransportPutFixedArray(_dev, _put, _n, _x) { \ - uint8_t _i; \ - for(_i = 0; _i < _n; _i++) { \ - _put(_dev, &_x[_i]); \ - } \ -} - -#define XBeeTransportPutInt8FixedArray(_dev, _n, _x) XBeeTransportPutFixedArray(_dev, XBeeTransportPutInt8ByAddr, _n, _x) -#define XBeeTransportPutUint8FixedArray(_dev, _n, _x) XBeeTransportPutFixedArray(_dev, XBeeTransportPutUint8ByAddr, _n, _x) - -#define XBeeTransportPutCharFixedArray(_dev, _n, _x) XBeeTransportPutFixedArray(_dev, XBeeTransportPutCharByAddr, _n, _x) - -#define XBeeTransportPutInt16FixedArray(_dev, _n, _x) XBeeTransportPutFixedArray(_dev, XBeeTransportPutInt16ByAddr, _n, _x) -#define XBeeTransportPutUint16FixedArray(_dev, _n, _x) XBeeTransportPutFixedArray(_dev, XBeeTransportPutUint16ByAddr, _n, _x) - -#define XBeeTransportPutInt32FixedArray(_dev, _n, _x) XBeeTransportPutFixedArray(_dev, XBeeTransportPutInt32ByAddr, _n, _x) -#define XBeeTransportPutUint32FixedArray(_dev, _n, _x) XBeeTransportPutFixedArray(_dev, XBeeTransportPutUint32ByAddr, _n, _x) - -#define XBeeTransportPutFloatFixedArray(_dev, _n, _x) XBeeTransportPutFixedArray(_dev, XBeeTransportPutFloatByAddr, _n, _x) - -#define XBeeTransportPutInt64FixedArray(_dev, _n, _x) XBeeTransportPutFixedArray(_dev, XBeeTransportPutInt64ByAddr, _n, _x) -#define XBeeTransportPutUint64FixedArray(_dev, _n, _x) XBeeTransportPutFixedArray(_dev, XBeeTransportPutUint64ByAddr, _n, _x) - -#define XBeeTransportPutDoubleFixedArray(_dev, _n, _x) XBeeTransportPutFixedArray(_dev, XBeeTransportPutDoubleByAddr, _n, _x) - - -#define XBeeTransportHeader(_dev, _len) { \ - XBeeTransportPut1Byte(_dev, XBEE_START); \ - uint8_t payload_len = XBeeAPISizeOf(_dev, _len); \ - XBeeTransportPut2Bytes(_dev, payload_len); \ - xbee_cs = 0; \ - XBeeTransportPutTXHeader(_dev); \ -} - -#define XBeeTransportTrailer(_dev) { \ - xbee_cs = 0xff - xbee_cs; \ - XBeeTransportPut1Byte(_dev, xbee_cs); \ - XBeeTransportSendMessage(_dev) \ -} - - - /** Status of the API packet receiver automata */ #define XBEE_UNINIT 0 #define XBEE_GOT_START 1 @@ -200,12 +52,17 @@ void xbee_init( void ); #define XBEE_GOT_PAYLOAD 4 struct xbee_transport { - // generic interface - struct transport trans; - // specific pprz transport variables + // generic reception interface + struct transport_rx trans_rx; + // specific xbee transport variables uint8_t status; uint8_t payload_idx; - uint8_t cs; + 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; @@ -218,30 +75,30 @@ static inline void parse_xbee( struct xbee_transport * t, uint8_t c ) { t->status++; break; case XBEE_GOT_START: - if (t->trans.msg_received) { - t->trans.ovrn++; + if (t->trans_rx.msg_received) { + t->trans_rx.ovrn++; goto error; } - t->trans.payload_len = c<<8; + t->trans_rx.payload_len = c<<8; t->status++; break; case XBEE_GOT_LENGTH_MSB: - t->trans.payload_len |= c; + t->trans_rx.payload_len |= c; t->status++; t->payload_idx = 0; - t->cs=0; + t->cs_rx=0; break; case XBEE_GOT_LENGTH_LSB: - t->trans.payload[t->payload_idx] = c; - t->cs += c; + t->trans_rx.payload[t->payload_idx] = c; + t->cs_rx += c; t->payload_idx++; - if (t->payload_idx == t->trans.payload_len) + if (t->payload_idx == t->trans_rx.payload_len) t->status++; break; case XBEE_GOT_PAYLOAD: - if (c + t->cs != 0xff) + if (c + t->cs_rx != 0xff) goto error; - t->trans.msg_received = TRUE; + t->trans_rx.msg_received = TRUE; goto restart; break; default: @@ -249,7 +106,7 @@ static inline void parse_xbee( struct xbee_transport * t, uint8_t c ) { } return; error: - t->trans.error++; + t->trans_rx.error++; restart: t->status = XBEE_UNINIT; return; @@ -257,13 +114,13 @@ static inline void parse_xbee( struct xbee_transport * t, uint8_t c ) { /** 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.payload[0]) { + 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.payload); + XbeeGetRSSI(t->trans_rx.payload); uint8_t i; - for(i = XBEE_RFDATA_OFFSET; i < t->trans.payload_len; i++) - dl_buffer[i-XBEE_RFDATA_OFFSET] = t->trans.payload[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: @@ -272,13 +129,13 @@ static inline void xbee_parse_payload(struct xbee_transport * t) { } #define XBeeBuffer(_dev) TransportLink(_dev,ChAvailable()) -#define ReadXBeeBuffer(_dev,_trans) { while (TransportLink(_dev,ChAvailable())&&!(_trans.trans.msg_received)) parse_xbee(&(_trans),TransportLink(_dev,Getch())); } +#define ReadXBeeBuffer(_dev,_trans) { while (TransportLink(_dev,ChAvailable())&&!(_trans.trans_rx.msg_received)) parse_xbee(&(_trans),TransportLink(_dev,Getch())); } #define XBeeCheckAndParse(_dev,_trans) { \ if (XBeeBuffer(_dev)) { \ ReadXBeeBuffer(_dev,_trans); \ - if (_trans.trans.msg_received) { \ + if (_trans.trans_rx.msg_received) { \ xbee_parse_payload(&(_trans)); \ - _trans.trans.msg_received = FALSE; \ + _trans.trans_rx.msg_received = FALSE; \ } \ } \ } diff --git a/sw/airborne/subsystems/datalink/xbee24.h b/sw/airborne/subsystems/datalink/xbee24.h index 219a555159..50daa6a7cf 100644 --- a/sw/airborne/subsystems/datalink/xbee24.h +++ b/sw/airborne/subsystems/datalink/xbee24.h @@ -1,5 +1,6 @@ /* * Copyright (C) 2009 ENAC, Pascal Brisset + * Copyright (C) 2014 Gautier Hattenberger * * This file is part of paparazzi. * @@ -14,13 +15,15 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . * */ -/* Configuration for 2.4GHz "series 1" and 900MHz modules */ +/** + * @file subsystems/datalink/xbee24.h + * Configuration for 2.4GHz "series 1" and 900MHz modules + */ #ifndef XBEE24_H #define XBEE24_H @@ -29,17 +32,15 @@ #define XBEE_RX_ID 0x81 /* 16 bits address */ #define XBEE_RFDATA_OFFSET 5 -#define XBeeTransportPutTXHeader(_dev) { \ - XBeeTransportPutUint8(_dev, XBEE_TX_ID); \ - XBeeTransportPutUint8(_dev, NO_FRAME_ID); \ - XBeeTransportPutUint8(_dev, GROUND_STATION_ADDR >> 8); \ - XBeeTransportPutUint8(_dev, GROUND_STATION_ADDR & 0xff); \ - XBeeTransportPutUint8(_dev, TX_OPTIONS); \ +#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 \ } -/* 4 = frame_id + addr_msb + addr_lsb + options */ -#define XBeeTransportSizeOf(_dev, _x) XBeeAPISizeOf(_dev, _x+4) - -#define XbeeGetRSSI(_payload) { xbee_rssi = _payload[3]; } +#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 index 506d5788ac..ad199aa5d7 100644 --- a/sw/airborne/subsystems/datalink/xbee868.h +++ b/sw/airborne/subsystems/datalink/xbee868.h @@ -1,5 +1,6 @@ /* * Copyright (C) 2009 ENAC, Pascal Brisset + * Copyright (C) 2014 Gautier Hattenberger * * This file is part of paparazzi. * @@ -14,13 +15,15 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . * */ -/* Configuration for 868MHz modules */ +/** + * @file subsystems/datalink/xbee868.h + * Configuration for 868MHz modules + */ #ifndef XBEE868_H #define XBEE868_H @@ -29,26 +32,24 @@ #define XBEE_RX_ID 0x90 #define XBEE_RFDATA_OFFSET 12 -#define XBeeTransportPutTXHeader(_dev) { \ - XBeeTransportPutUint8(_dev, XBEE_TX_ID); \ - XBeeTransportPutUint8(_dev, NO_FRAME_ID); \ - XBeeTransportPutUint8(_dev, 0x00); \ - XBeeTransportPutUint8(_dev, 0x00); \ - XBeeTransportPutUint8(_dev, 0x00); \ - XBeeTransportPutUint8(_dev, 0x00); \ - XBeeTransportPutUint8(_dev, 0x00); \ - XBeeTransportPutUint8(_dev, 0x00); \ - XBeeTransportPutUint8(_dev, GROUND_STATION_ADDR >> 8); \ - XBeeTransportPutUint8(_dev, GROUND_STATION_ADDR & 0xff); \ - XBeeTransportPutUint8(_dev, 0xff); \ - XBeeTransportPutUint8(_dev, 0xfe); \ - XBeeTransportPutUint8(_dev, 0x00); \ - XBeeTransportPutUint8(_dev, TX_OPTIONS); \ +#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 \ } -/* 13 = frame_id + addr==8 + 3 + options */ -#define XBeeTransportSizeOf(_dev, _x) XBeeAPISizeOf(_dev, _x+13) - #define XbeeGetRSSI(_payload) {} #endif // XBEE868_H diff --git a/sw/airborne/subsystems/gps.c b/sw/airborne/subsystems/gps.c index f2dc8659a3..a7587bc2dd 100644 --- a/sw/airborne/subsystems/gps.c +++ b/sw/airborne/subsystems/gps.c @@ -45,9 +45,9 @@ struct GpsTimeSync gps_time_sync; #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_svinfo(uint8_t svid) { +static void send_svinfo(struct transport_tx *trans, struct link_device *dev, uint8_t svid) { if (svid < GPS_NB_CHANNELS) { - DOWNLINK_SEND_SVINFO(DefaultChannel, DefaultDevice, &svid, + pprz_msg_send_SVINFO(trans, dev, AC_ID, &svid, &gps.svinfos[svid].svid, &gps.svinfos[svid].flags, &gps.svinfos[svid].qi, &gps.svinfos[svid].cno, &gps.svinfos[svid].elev, &gps.svinfos[svid].azim); @@ -55,17 +55,17 @@ static void send_svinfo(uint8_t svid) { } /** send SVINFO message if there is information for satellite with svid */ -static inline void send_svinfo_available(uint8_t svid) { +static inline void send_svinfo_available(struct transport_tx *trans, struct link_device *dev, uint8_t svid) { if (gps.svinfos[svid].cno > 0) { - send_svinfo(svid); + send_svinfo(trans, dev, svid); } } -static void send_gps(void) { +static void send_gps(struct transport_tx *trans, struct link_device *dev) { static uint8_t i; int16_t climb = -gps.ned_vel.z; int16_t course = (DegOfRad(gps.course)/((int32_t)1e6)); - DOWNLINK_SEND_GPS(DefaultChannel, DefaultDevice, &gps.fix, + pprz_msg_send_GPS(trans, dev, AC_ID, &gps.fix, &gps.utm_pos.east, &gps.utm_pos.north, &course, &gps.hmsl, &gps.gspeed, &climb, &gps.week, &gps.tow, &gps.utm_pos.zone, &i); @@ -75,15 +75,15 @@ static void send_gps(void) { if ((gps.fix != GPS_FIX_3D) && (i >= gps.nb_channels)) i = 0; if (i >= gps.nb_channels * 2) i = 0; if (i < gps.nb_channels && ((gps.fix != GPS_FIX_3D) || (gps.svinfos[i].cno > 0))) { - send_svinfo(i); + send_svinfo(trans, dev, i); } i++; } -static void send_gps_int(void) { +static void send_gps_int(struct transport_tx *trans, struct link_device *dev) { static uint8_t i; static uint8_t last_cnos[GPS_NB_CHANNELS]; - DOWNLINK_SEND_GPS_INT(DefaultChannel, DefaultDevice, + pprz_msg_send_GPS_INT(trans, dev, AC_ID, &gps.ecef_pos.x, &gps.ecef_pos.y, &gps.ecef_pos.z, &gps.lla_pos.lat, &gps.lla_pos.lon, &gps.lla_pos.alt, &gps.hmsl, @@ -96,25 +96,25 @@ static void send_gps_int(void) { // send SVINFO for available satellites that have new data if (i == gps.nb_channels) i = 0; if (i < gps.nb_channels && gps.svinfos[i].cno != last_cnos[i]) { - send_svinfo_available(i); + send_svinfo_available(trans, dev, i); last_cnos[i] = gps.svinfos[i].cno; } i++; } -static void send_gps_lla(void) { +static void send_gps_lla(struct transport_tx *trans, struct link_device *dev) { uint8_t err = 0; int16_t climb = -gps.ned_vel.z; int16_t course = (DegOfRad(gps.course)/((int32_t)1e6)); - DOWNLINK_SEND_GPS_LLA(DefaultChannel, DefaultDevice, + pprz_msg_send_GPS_LLA(trans, dev, AC_ID, &gps.lla_pos.lat, &gps.lla_pos.lon, &gps.lla_pos.alt, &course, &gps.gspeed, &climb, &gps.week, &gps.tow, &gps.fix, &err); } -static void send_gps_sol(void) { - DOWNLINK_SEND_GPS_SOL(DefaultChannel, DefaultDevice, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv); +static void send_gps_sol(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_GPS_SOL(trans, dev, AC_ID, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv); } #endif diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h index d121c0b898..dd388fba25 100644 --- a/sw/airborne/subsystems/gps.h +++ b/sw/airborne/subsystems/gps.h @@ -67,8 +67,8 @@ struct GpsState { int32_t hmsl; ///< height above mean sea level in mm struct EcefCoor_i ecef_vel; ///< speed ECEF in cm/s struct NedCoor_i ned_vel; ///< speed NED in cm/s - int16_t gspeed; ///< norm of 2d ground speed in cm/s - int16_t speed_3d; ///< norm of 3d speed in cm/s + uint16_t gspeed; ///< norm of 2d ground speed in cm/s + uint16_t speed_3d; ///< norm of 3d speed in cm/s int32_t course; ///< GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north) uint32_t pacc; ///< position accuracy in cm uint32_t sacc; ///< speed accuracy in cm/s @@ -76,7 +76,7 @@ struct GpsState { uint16_t pdop; ///< position dilution of precision scaled by 100 uint8_t num_sv; ///< number of sat in fix uint8_t fix; ///< status of fix - int16_t week; ///< GPS week + uint16_t week; ///< GPS week uint32_t tow; ///< GPS time of week in ms uint8_t nb_channels; ///< Number of scanned satellites diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c index 488fb73973..ee2fdcd097 100644 --- a/sw/airborne/subsystems/imu.c +++ b/sw/airborne/subsystems/imu.c @@ -44,66 +44,66 @@ #if USE_IMU_FLOAT -static void send_accel(void) { - DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, DefaultDevice, +static void send_accel(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID, &imuf.accel.x, &imuf.accel.y, &imuf.accel.z); } -static void send_gyro(void) { - DOWNLINK_SEND_IMU_GYRO(DefaultChannel, DefaultDevice, +static void send_gyro(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_IMU_GYRO(trans, dev, AC_ID, &imuf.gyro.p, &imuf.gyro.q, &imuf.gyro.r); } #else // !USE_IMU_FLOAT -static void send_accel_raw(void) { - DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, +static void send_accel_raw(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_IMU_ACCEL_RAW(trans, dev, AC_ID, &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z); } -static void send_accel_scaled(void) { - DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, DefaultDevice, +static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_IMU_ACCEL_SCALED(trans, dev, AC_ID, &imu.accel.x, &imu.accel.y, &imu.accel.z); } -static void send_accel(void) { +static void send_accel(struct transport_tx *trans, struct link_device *dev) { struct FloatVect3 accel_float; ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); - DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, DefaultDevice, + pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID, &accel_float.x, &accel_float.y, &accel_float.z); } -static void send_gyro_raw(void) { - DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, +static void send_gyro_raw(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_IMU_GYRO_RAW(trans, dev, AC_ID, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r); } -static void send_gyro_scaled(void) { - DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, DefaultDevice, +static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_IMU_GYRO_SCALED(trans, dev, AC_ID, &imu.gyro.p, &imu.gyro.q, &imu.gyro.r); } -static void send_gyro(void) { +static void send_gyro(struct transport_tx *trans, struct link_device *dev) { struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); - DOWNLINK_SEND_IMU_GYRO(DefaultChannel, DefaultDevice, + pprz_msg_send_IMU_GYRO(trans, dev, AC_ID, &gyro_float.p, &gyro_float.q, &gyro_float.r); } -static void send_mag_raw(void) { - DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, +static void send_mag_raw(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_IMU_MAG_RAW(trans, dev, AC_ID, &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z); } -static void send_mag_scaled(void) { - DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, DefaultDevice, +static void send_mag_scaled(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_IMU_MAG_SCALED(trans, dev, AC_ID, &imu.mag.x, &imu.mag.y, &imu.mag.z); } -static void send_mag(void) { +static void send_mag(struct transport_tx *trans, struct link_device *dev) { struct FloatVect3 mag_float; MAGS_FLOAT_OF_BFP(mag_float, imu.mag); - DOWNLINK_SEND_IMU_MAG(DefaultChannel, DefaultDevice, + pprz_msg_send_IMU_MAG(trans, dev, AC_ID, &mag_float.x, &mag_float.y, &mag_float.z); } #endif // !USE_IMU_FLOAT diff --git a/sw/airborne/subsystems/ins/hf_float.c b/sw/airborne/subsystems/ins/hf_float.c index 600b5bda99..0d185eb92e 100644 --- a/sw/airborne/subsystems/ins/hf_float.c +++ b/sw/airborne/subsystems/ins/hf_float.c @@ -229,8 +229,8 @@ static void b2_hff_update_ydot(struct HfilterFloat* hff_work, float vel, float R #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_hff(void) { - DOWNLINK_SEND_HFF(DefaultChannel, DefaultDevice, +static void send_hff(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_HFF(trans, dev, AC_ID, &b2_hff_state.x, &b2_hff_state.y, &b2_hff_state.xdot, @@ -239,8 +239,8 @@ static void send_hff(void) { &b2_hff_state.ydotdot); } -static void send_hff_debug(void) { - DOWNLINK_SEND_HFF_DBG(DefaultChannel, DefaultDevice, +static void send_hff_debug(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_HFF_DBG(trans, dev, AC_ID, &b2_hff_x_meas, &b2_hff_y_meas, &b2_hff_xd_meas, @@ -252,8 +252,8 @@ static void send_hff_debug(void) { } #ifdef GPS_LAG -static void send_hff_gps(void) { - DOWNLINK_SEND_HFF_GPS(DefaultChannel, DefaultDevice, +static void send_hff_gps(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_HFF_GPS(trans, dev, AC_ID, &(b2_hff_rb_last->lag_counter), &lag_counter_err, &save_counter); diff --git a/sw/airborne/subsystems/ins/ins_ardrone2.c b/sw/airborne/subsystems/ins/ins_ardrone2.c index 6abdec9947..43a6bb589a 100644 --- a/sw/airborne/subsystems/ins/ins_ardrone2.c +++ b/sw/airborne/subsystems/ins/ins_ardrone2.c @@ -92,8 +92,12 @@ void ins_reset_altitude_ref( void ) { void ins_propagate(float __attribute__((unused)) dt) { /* untilt accels and speeds */ - float_rmat_transp_vmult(&ins_impl.ltp_accel, stateGetNedToBodyRMat_f(), &ahrs_ardrone2.accel); - float_rmat_transp_vmult(&ins_impl.ltp_speed, stateGetNedToBodyRMat_f(), &ahrs_ardrone2.speed); + float_rmat_transp_vmult((struct FloatVect3*)&ins_impl.ltp_accel, + stateGetNedToBodyRMat_f(), + (struct FloatVect3*)&ahrs_ardrone2.accel); + float_rmat_transp_vmult((struct FloatVect3*)&ins_impl.ltp_speed, + stateGetNedToBodyRMat_f(), + (struct FloatVect3*)&ahrs_ardrone2.speed); //Add g to the accelerations ins_impl.ltp_accel.z += 9.81; diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c index 9a4e054acd..07e06165c8 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.c +++ b/sw/airborne/subsystems/ins/ins_float_invariant.c @@ -62,10 +62,10 @@ #if !INS_UPDATE_FW_ESTIMATOR && PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_ins_ref(void) { +static void send_ins_ref(struct transport_tx *trans, struct link_device *dev) { float foo = 0.; if (state.ned_initialized_i) { - DOWNLINK_SEND_INS_REF(DefaultChannel, DefaultDevice, + pprz_msg_send_INS_REF(trans, dev, AC_ID, &state.ned_origin_i.ecef.x, &state.ned_origin_i.ecef.y, &state.ned_origin_i.ecef.z, &state.ned_origin_i.lla.lat, &state.ned_origin_i.lla.lon, &state.ned_origin_i.lla.alt, &state.ned_origin_i.hmsl, &foo); @@ -396,7 +396,7 @@ void ahrs_propagate(float dt) { struct FloatEulers eulers; FLOAT_EULERS_OF_QUAT(eulers, ins_impl.state.quat); RunOnceEvery(3,{ - DOWNLINK_SEND_INV_FILTER(DefaultChannel, DefaultDevice, + pprz_msg_send_INV_FILTER(trans, dev, AC_ID, &ins_impl.state.quat.qi, &eulers.phi, &eulers.theta, diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough.c b/sw/airborne/subsystems/ins/ins_gps_passthrough.c index c7384c286a..77d5b82c5e 100644 --- a/sw/airborne/subsystems/ins/ins_gps_passthrough.c +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough.c @@ -58,23 +58,23 @@ struct InsGpsPassthrough ins_impl; #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_ins(void) { - DOWNLINK_SEND_INS(DefaultChannel, DefaultDevice, +static void send_ins(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_INS(trans, dev, AC_ID, &ins_impl.ltp_pos.x, &ins_impl.ltp_pos.y, &ins_impl.ltp_pos.z, &ins_impl.ltp_speed.x, &ins_impl.ltp_speed.y, &ins_impl.ltp_speed.z, &ins_impl.ltp_accel.x, &ins_impl.ltp_accel.y, &ins_impl.ltp_accel.z); } -static void send_ins_z(void) { +static void send_ins_z(struct transport_tx *trans, struct link_device *dev) { static const float fake_baro_z = 0.0; - DOWNLINK_SEND_INS_Z(DefaultChannel, DefaultDevice, + pprz_msg_send_INS_Z(trans, dev, AC_ID, &fake_baro_z, &ins_impl.ltp_pos.z, &ins_impl.ltp_speed.z, &ins_impl.ltp_accel.z); } -static void send_ins_ref(void) { +static void send_ins_ref(struct transport_tx *trans, struct link_device *dev) { static const float fake_qfe = 0.0; if (ins_impl.ltp_initialized) { - DOWNLINK_SEND_INS_REF(DefaultChannel, DefaultDevice, + pprz_msg_send_INS_REF(trans, dev, AC_ID, &ins_impl.ltp_def.ecef.x, &ins_impl.ltp_def.ecef.y, &ins_impl.ltp_def.ecef.z, &ins_impl.ltp_def.lla.lat, &ins_impl.ltp_def.lla.lon, &ins_impl.ltp_def.lla.alt, &ins_impl.ltp_def.hmsl, &fake_qfe); diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index 425e4709ab..abf1560556 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -121,21 +121,21 @@ struct InsInt ins_impl; #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_ins(void) { - DOWNLINK_SEND_INS(DefaultChannel, DefaultDevice, +static void send_ins(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_INS(trans, dev, AC_ID, &ins_impl.ltp_pos.x, &ins_impl.ltp_pos.y, &ins_impl.ltp_pos.z, &ins_impl.ltp_speed.x, &ins_impl.ltp_speed.y, &ins_impl.ltp_speed.z, &ins_impl.ltp_accel.x, &ins_impl.ltp_accel.y, &ins_impl.ltp_accel.z); } -static void send_ins_z(void) { - DOWNLINK_SEND_INS_Z(DefaultChannel, DefaultDevice, +static void send_ins_z(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_INS_Z(trans, dev, AC_ID, &ins_impl.baro_z, &ins_impl.ltp_pos.z, &ins_impl.ltp_speed.z, &ins_impl.ltp_accel.z); } -static void send_ins_ref(void) { +static void send_ins_ref(struct transport_tx *trans, struct link_device *dev) { if (ins_impl.ltp_initialized) { - DOWNLINK_SEND_INS_REF(DefaultChannel, DefaultDevice, + pprz_msg_send_INS_REF(trans, dev, AC_ID, &ins_impl.ltp_def.ecef.x, &ins_impl.ltp_def.ecef.y, &ins_impl.ltp_def.ecef.z, &ins_impl.ltp_def.lla.lat, &ins_impl.ltp_def.lla.lon, &ins_impl.ltp_def.lla.alt, &ins_impl.ltp_def.hmsl, &ins_impl.qfe); diff --git a/sw/airborne/subsystems/ins/vf_extended_float.c b/sw/airborne/subsystems/ins/vf_extended_float.c index f6cea52eb4..2397ed0d22 100644 --- a/sw/airborne/subsystems/ins/vf_extended_float.c +++ b/sw/airborne/subsystems/ins/vf_extended_float.c @@ -69,8 +69,8 @@ struct VffExtended vff; #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_vffe(void) { - DOWNLINK_SEND_VFF_EXTENDED(DefaultChannel, DefaultDevice, +static void send_vffe(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_VFF_EXTENDED(trans, dev, AC_ID, &vff.z_meas, &vff.z_meas_baro, &vff.z, &vff.zdot, &vff.zdotdot, &vff.bias, &vff.offset); @@ -150,7 +150,7 @@ void vff_propagate(float accel, float dt) { vff.P[3][3] = FPF33 + Qoffoff; #if DEBUG_VFF_EXTENDED - RunOnceEvery(10, send_vffe()); + RunOnceEvery(10, send_vffe(&(DefaultChannel).trans_tx, &(DefaultDevice).device)); #endif } diff --git a/sw/airborne/subsystems/ins/vf_float.c b/sw/airborne/subsystems/ins/vf_float.c index 88e3acf845..fe75917f9a 100644 --- a/sw/airborne/subsystems/ins/vf_float.c +++ b/sw/airborne/subsystems/ins/vf_float.c @@ -62,8 +62,8 @@ struct Vff vff; #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_vff(void) { - DOWNLINK_SEND_VFF(DefaultChannel, DefaultDevice, +static void send_vff(struct transport_tx *trans, struct link_device *dev) { + pprz_msg_send_VFF(trans, dev, AC_ID, &vff.z_meas, &vff.z, &vff.zdot, &vff.bias, &vff.P[0][0], &vff.P[1][1], &vff.P[2][2]); } diff --git a/sw/airborne/subsystems/radio_control/ppm.c b/sw/airborne/subsystems/radio_control/ppm.c index 31b8ff05e3..5dc5effbba 100644 --- a/sw/airborne/subsystems/radio_control/ppm.c +++ b/sw/airborne/subsystems/radio_control/ppm.c @@ -58,13 +58,13 @@ static bool_t ppm_data_valid; #include "subsystems/datalink/telemetry.h" -static void send_ppm(void) +static void send_ppm(struct transport_tx *trans, struct link_device *dev) { uint16_t ppm_pulses_usec[RADIO_CONTROL_NB_CHANNEL]; for (int i = 0; i < RADIO_CONTROL_NB_CHANNEL; i++) { ppm_pulses_usec[i] = USEC_OF_RC_PPM_TICKS(ppm_pulses[i]); } - DOWNLINK_SEND_PPM(DefaultChannel, DefaultDevice, + pprz_msg_send_PPM(trans, dev, AC_ID, &radio_control.frame_rate, PPM_NB_CHANNEL, ppm_pulses_usec); } #endif diff --git a/sw/airborne/subsystems/radio_control/sbus.c b/sw/airborne/subsystems/radio_control/sbus.c index 59db21d0bd..adc9cbd724 100644 --- a/sw/airborne/subsystems/radio_control/sbus.c +++ b/sw/airborne/subsystems/radio_control/sbus.c @@ -42,10 +42,10 @@ struct Sbus sbus; #include "subsystems/datalink/telemetry.h" -static void send_sbus(void) +static void send_sbus(struct transport_tx *trans, struct link_device *dev) { // Using PPM message - DOWNLINK_SEND_PPM(DefaultChannel, DefaultDevice, + pprz_msg_send_PPM(trans, dev, AC_ID, &radio_control.frame_rate, SBUS_NB_CHANNEL, sbus.ppm); } #endif diff --git a/sw/airborne/subsystems/radio_control/sbus_dual.c b/sw/airborne/subsystems/radio_control/sbus_dual.c index 4af7ce06c3..9be43cfc6b 100644 --- a/sw/airborne/subsystems/radio_control/sbus_dual.c +++ b/sw/airborne/subsystems/radio_control/sbus_dual.c @@ -45,10 +45,10 @@ struct Sbus sbus1, sbus2; #include "subsystems/datalink/telemetry.h" -static void send_sbus(void) +static void send_sbus(struct transport_tx *trans, struct link_device *dev) { // Using PPM message - DOWNLINK_SEND_PPM(DefaultChannel, DefaultDevice, + pprz_msg_send_PPM(trans, dev, AC_ID, &radio_control.frame_rate, SBUS_NB_CHANNEL, sbus1.ppm); } #endif diff --git a/sw/airborne/test/mcu_periph/test_adc.c b/sw/airborne/test/mcu_periph/test_adc.c index 473058bc7d..f446d48acf 100644 --- a/sw/airborne/test/mcu_periph/test_adc.c +++ b/sw/airborne/test/mcu_periph/test_adc.c @@ -85,7 +85,7 @@ int main( void ) { static inline void main_periodic_task( void ) { RunOnceEvery(100, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);}); - RunOnceEvery(100, {DOWNLINK_SEND_TIME(DefaultChannel, DefaultDevice, &sys_time.nb_sec);}); + RunOnceEvery(100, {uint32_t sec = sys_time.nb_sec; DOWNLINK_SEND_TIME(DefaultChannel, DefaultDevice, &sec);}); LED_PERIODIC(); uint16_t values[NB_ADC]; diff --git a/sw/airborne/test/mcu_periph/test_sys_time_usleep.c b/sw/airborne/test/mcu_periph/test_sys_time_usleep.c index cda5dbd4eb..887758ed0a 100644 --- a/sw/airborne/test/mcu_periph/test_sys_time_usleep.c +++ b/sw/airborne/test/mcu_periph/test_sys_time_usleep.c @@ -67,7 +67,7 @@ static inline void main_periodic_15(void) { /* * Called from the systime interrupt handler */ -static inline void main_periodic_05(uint8_t id) { +static inline void main_periodic_05(uint8_t id __attribute__((unused))) { #ifdef LED_RED LED_TOGGLE(LED_RED); #endif diff --git a/sw/airborne/test/peripherals/test_ms2100.c b/sw/airborne/test/peripherals/test_ms2100.c index b7a750a6ba..51059fa4a4 100644 --- a/sw/airborne/test/peripherals/test_ms2100.c +++ b/sw/airborne/test/peripherals/test_ms2100.c @@ -57,7 +57,8 @@ static inline void main_init( void ) { static inline void main_periodic_task( void ) { RunOnceEvery(10, { - DOWNLINK_SEND_BOOT(DefaultChannel, DefaultDevice, &sys_time.nb_sec); + uint16_t foo = sys_time.nb_sec; + DOWNLINK_SEND_BOOT(DefaultChannel, DefaultDevice, &foo); LED_TOGGLE(2); LED_PERIODIC(); }); @@ -70,10 +71,11 @@ static inline void main_event_task( void ) { ms2100_event(&ms2100); if (ms2100.status == MS2100_DATA_AVAILABLE) { RunOnceEvery(10, { + int32_t mag_x = ms2100.data.vect.x; + int32_t mag_y = ms2100.data.vect.y; + int32_t mag_z = ms2100.data.vect.z; DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, - &ms2100.data.vect.x, - &ms2100.data.vect.y, - &ms2100.data.vect.z); + &mag_x, &mag_y, &mag_z); }); ms2100.status = MS2100_IDLE; } diff --git a/sw/airborne/test/subsystems/test_radio_control.c b/sw/airborne/test/subsystems/test_radio_control.c index 44cab29a20..9963615f15 100644 --- a/sw/airborne/test/subsystems/test_radio_control.c +++ b/sw/airborne/test/subsystems/test_radio_control.c @@ -57,8 +57,9 @@ extern uint32_t debug_len; static inline void main_periodic_task( void ) { RunOnceEvery(51, { - /*LED_TOGGLE(2);*/ - DOWNLINK_SEND_TIME(DefaultChannel, DefaultDevice, &sys_time.nb_sec); + /*LED_TOGGLE(2);*/ + uint32_t sec = sys_time.nb_sec; + DOWNLINK_SEND_TIME(DefaultChannel, DefaultDevice, &sec); }); RunOnceEvery(10, {radio_control_periodic_task();}); diff --git a/sw/airborne/test/subsystems/test_settings.c b/sw/airborne/test/subsystems/test_settings.c index 99e94c04dd..25e2474265 100644 --- a/sw/airborne/test/subsystems/test_settings.c +++ b/sw/airborne/test/subsystems/test_settings.c @@ -68,7 +68,7 @@ static inline void main_periodic( void ) { RunOnceEvery(100, { DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); - PeriodicSendDlValue(DefaultChannel, DefaultDevice); + PeriodicSendDlValue(&(DefaultChannel).trans_tx, &(DefaultDevice).device); }); } diff --git a/sw/airborne/test/test_baro_board.c b/sw/airborne/test/test_baro_board.c index 133c368470..ac244154be 100644 --- a/sw/airborne/test/test_baro_board.c +++ b/sw/airborne/test/test_baro_board.c @@ -76,8 +76,9 @@ int main(void) { } static void pressure_abs_cb(uint8_t __attribute__((unused)) sender_id, const float * pressure) { + float p = *pressure; float foo = 42.; - DOWNLINK_SEND_BARO_RAW(DefaultChannel, DefaultDevice, pressure, &foo); + DOWNLINK_SEND_BARO_RAW(DefaultChannel, DefaultDevice, &p, &foo); } static inline void main_init( void ) { diff --git a/sw/ext/libopencm3 b/sw/ext/libopencm3 index 283d8cc7d2..1420be5577 160000 --- a/sw/ext/libopencm3 +++ b/sw/ext/libopencm3 @@ -1 +1 @@ -Subproject commit 283d8cc7d2acbfc873897e886658364b12fc5e3b +Subproject commit 1420be55777189fd25f06ffde174c8abe6882186 diff --git a/sw/ground_segment/cockpit/gcs.ml b/sw/ground_segment/cockpit/gcs.ml index 44ee2e936f..5a1e998612 100644 --- a/sw/ground_segment/cockpit/gcs.ml +++ b/sw/ground_segment/cockpit/gcs.ml @@ -171,9 +171,9 @@ module TodoList = struct end -(************ Google, OSM Maps handling *****************************************) +(************ Maps handling (Google, OSM, MS, etc.) ***********************************) module GM = struct - (** Fill the visible background with Google, OSM tiles *) + (** Fill the visible background with map tiles *) let zoomlevel = ref 20 let fill_tiles = fun geomap -> match geomap#georef with @@ -187,7 +187,7 @@ module GM = struct auto := x; update geomap - (** Creates a calibrated map from the Google, OSM tiles (selected region) *) + (** Creates a calibrated map from the map tiles (selected region) *) let map_from_tiles = fun (geomap:G.widget) () -> match geomap#region with None -> GToolbox.message_box "Error" "Select a region (shift-left drag)" @@ -267,7 +267,7 @@ let any_event = fun (_geomap:G.widget) _ev -> false let button_press = fun (geomap:G.widget) ev -> let state = GdkEvent.Button.state ev in if GdkEvent.Button.button ev = 3 then begin - (** Display a tile from Google Maps or IGN *) + (** Display a map tile from map provider (Google, OSC, ..) or IGN *) let xc = GdkEvent.Button.x ev and yc = GdkEvent.Button.y ev in let (xw,yw) = geomap#window_to_world xc yc in @@ -354,7 +354,7 @@ let options = "-layout", Arg.Set_string layout_file, (sprintf " GUI layout. Default: %s" !layout_file); "-m", Arg.String (fun x -> map_files := x :: !map_files), "Map XML description file"; "-maximize", Arg.Set maximize, "Maximize window"; - "-mercator", Arg.Unit (fun () -> projection:=G.Mercator),"Switch to (Google Maps) Mercator projection, default"; + "-mercator", Arg.Unit (fun () -> projection:=G.Mercator),"Switch to Mercator projection, default"; "-mplayer", Arg.Set_string mplayer, "Launch mplayer with the given argument as X plugin"; "-no_alarm", Arg.Set no_alarm, "Disables alarm page"; "-maps_no_http", Arg.Unit (fun () -> Gm.set_policy Gm.NoHttp), "Switch off downloading of maps, always use cached maps"; @@ -433,7 +433,7 @@ let create_geomap = fun switch_fullscreen editor_frame -> group := menu_item#group) Gm.policies; - (* Google fill menu entry and toolbar button *) + (* Map tiles fill menu entry and toolbar button *) let callback = fun _ -> GM.fill_tiles geomap in ignore (map_menu_fact#add_item "Maps Fill" ~key:GdkKeysyms._G ~callback); let b = GButton.button ~packing:geomap#toolbar#add () in @@ -441,14 +441,14 @@ let create_geomap = fun switch_fullscreen editor_frame -> let pixbuf = GdkPixbuf.from_file (Env.gcs_icons_path // "googleearth.png") in ignore (GMisc.image ~pixbuf ~packing:b#add ()); let tooltips = GData.tooltips () in - tooltips#set_tip b#coerce ~text:"Google maps fill"; + tooltips#set_tip b#coerce ~text:"Fill current view with background map tiles"; ignore (map_menu_fact#add_check_item "Maps Auto" ~active:!GM.auto ~callback:(GM.active_auto geomap)); ignore (map_menu_fact#add_item "Map of Region" ~key:GdkKeysyms._R ~callback:(map_from_region geomap)); ignore (map_menu_fact#add_item "Dump map of Tiles" ~key:GdkKeysyms._T ~callback:(GM.map_from_tiles geomap)); ignore (map_menu_fact#add_item "Load sector" ~callback:(Sectors.load geomap)); - (** Connect Google Maps display to view change *) + (** Connect Maps display to view change *) geomap#connect_view (fun () -> GM.update geomap); if !auto_ortho then geomap#connect_view (fun () -> fill_ortho geomap); diff --git a/sw/lib/ocaml/pprz.ml b/sw/lib/ocaml/pprz.ml index a43150db0e..8191b9889e 100644 --- a/sw/lib/ocaml/pprz.ml +++ b/sw/lib/ocaml/pprz.ml @@ -526,7 +526,7 @@ module PprzTransportBase(SubType:TRANSPORT_TYPE) = struct 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\n" ck_a (Char.code msg.[l-2])); + 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 -> diff --git a/sw/logalizer/play_core.ml b/sw/logalizer/play_core.ml index 382cccc1c6..88a9c5d9f1 100644 --- a/sw/logalizer/play_core.ml +++ b/sw/logalizer/play_core.ml @@ -149,21 +149,27 @@ let index_of_time = fun log t -> let run = fun serial_port log adj i0 speed no_gui -> let rec loop = fun i -> let (t, ac, m) = log.(i) in - Ivy.send (Printf.sprintf "replay%s %s" ac m); - Ivy.send (Printf.sprintf "time%s %f" ac t); - begin - match serial_port with - None -> () - | Some channel -> - 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 - Debug.call 'o' (fun f -> fprintf f "%s\n" (Debug.xprint buf)); - fprintf channel "%s%!" buf - with - _ -> () - end; + (* extract message name *) + let name = Str.string_before m (Str.search_forward (Str.regexp " ") m 0) in + (* continue if message is in telemetry class *) + begin try + let _ = Tm_Pprz.message_of_name name in + Ivy.send (Printf.sprintf "replay%s %s" ac m); + Ivy.send (Printf.sprintf "time%s %f" ac t); + begin + match serial_port with + None -> () + | Some channel -> + 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 + Debug.call 'o' (fun f -> fprintf f "%s\n" (Debug.xprint buf)); + fprintf channel "%s%!" buf + with + _ -> () + end; + with _ -> (); end; adj#set_value t; if i + 1 < Array.length log then begin let dt = time_of log.(i+1) -. t in diff --git a/sw/simulator/nps/nps_ivy_common.c b/sw/simulator/nps/nps_ivy_common.c index 32e5b65eb8..5ef6f19d7b 100644 --- a/sw/simulator/nps/nps_ivy_common.c +++ b/sw/simulator/nps/nps_ivy_common.c @@ -1,6 +1,7 @@ #include "nps_ivy.h" #include +#include #include #include diff --git a/sw/simulator/nps/nps_ivy_rotorcraft.c b/sw/simulator/nps/nps_ivy_rotorcraft.c index fcb0b99964..90b89eeb3a 100644 --- a/sw/simulator/nps/nps_ivy_rotorcraft.c +++ b/sw/simulator/nps/nps_ivy_rotorcraft.c @@ -1,6 +1,7 @@ #include "nps_ivy.h" #include +#include #include #include "generated/airframe.h" diff --git a/sw/supervision/paparazzicenter.ml b/sw/supervision/paparazzicenter.ml index 8f682b1b48..e699147bcb 100644 --- a/sw/supervision/paparazzicenter.ml +++ b/sw/supervision/paparazzicenter.ml @@ -204,10 +204,10 @@ let () = let buffer = GText.buffer ~tag_table () in gui#console#set_buffer buffer; - let errors = "red", ["error"; "no such file"; "undefined reference"; "failure"; "multiple definition"] + let errors = "red", ["error:"; "error "; "no such file"; "undefined reference"; "failure"; "multiple definition"] and warnings = "orange", ["warning"] and info = "green", ["pragma message"] - and version = "cyan", ["paparazzi version"] in + and version = "cyan", ["paparazzi version"; "build aircraft"] in let color_regexps = List.map (fun (color, strings) -> diff --git a/sw/tools/generators/gen_airframe.ml b/sw/tools/generators/gen_airframe.ml index cea04221e3..7c5b12ecf1 100644 --- a/sw/tools/generators/gen_airframe.ml +++ b/sw/tools/generators/gen_airframe.ml @@ -319,7 +319,7 @@ let _ = and md5sum = Sys.argv.(3) in try let xml = start_and_begin xml_file h_name in - Xml2h.warning ("AIRFRAME MODEL: "^ ac_name); + (* Xml2h.warning ("AIRFRAME MODEL: "^ ac_name); *) define_string "AIRFRAME_NAME" ac_name; define "AC_ID" ac_id; define "MD5SUM" (sprintf "((uint8_t*)\"%s\")" (hex_to_bin md5sum)); diff --git a/sw/tools/generators/gen_flight_plan.ml b/sw/tools/generators/gen_flight_plan.ml index 4300d1d88d..0b02de8c13 100644 --- a/sw/tools/generators/gen_flight_plan.ml +++ b/sw/tools/generators/gen_flight_plan.ml @@ -494,16 +494,38 @@ let rec print_stage = fun index_of_waypoints x -> | "call" -> stage (); let statement = ExtXml.attrib x "fun" in - lprintf "if (! (%s))\n" statement; - lprintf " NextStageAndBreak();\n"; - begin - try - let c = parsed_attrib x "until" in - lprintf "if (%s) NextStageAndBreak();\n" c - with - ExtXml.Error _ -> () - end; - lprintf "break;\n" + (* by default, function is called while returning TRUE *) + (* otherwise, function is called once and returned value is ignored *) + let loop = String.uppercase (ExtXml.attrib_or_default x "loop" "TRUE") in + (* be default, go to next stage immediately *) + let break = String.uppercase (ExtXml.attrib_or_default x "break" "FALSE") in + begin match loop with + | "TRUE" -> + lprintf "if (! (%s)) {\n" statement; + begin match break with + | "TRUE" -> lprintf " NextStageAndBreak();\n"; + | "FALSE" -> lprintf " NextStage();\n"; + | _ -> failwith "FP: 'call' break attribute must be TRUE or FALSE"; + end; + lprintf "} else {\n"; + begin + try + let c = parsed_attrib x "until" in + lprintf " if (%s) NextStageAndBreak();\n" c + with + ExtXml.Error _ -> () + end; + lprintf " break;\n"; + lprintf "}\n" + | "FALSE" -> + lprintf "%s\n" statement; + begin match break with + | "TRUE" -> lprintf "NextStageAndBreak();\n"; + | "FALSE" -> lprintf "NextStage();\n"; + | _ -> failwith "FP: 'call' break attribute must be TRUE or FALSE"; + end; + | _ -> failwith "FP: 'call' loop attribute must be TRUE or FALSE" + end | "survey_rectangle" -> let grid = parsed_attrib x "grid" and wp1 = get_index_waypoint (ExtXml.attrib x "wp1") index_of_waypoints @@ -775,7 +797,7 @@ let () = end; let name = ExtXml.attrib xml "name" in - Xml2h.warning ("FLIGHT PLAN: "^name); + (* Xml2h.warning ("FLIGHT PLAN: "^name); *) Xml2h.define_string "FLIGHT_PLAN_NAME" name; let get_float = fun x -> float_attrib xml x in diff --git a/sw/tools/generators/gen_messages.ml b/sw/tools/generators/gen_messages.ml index 065ca8f429..f3438fe682 100644 --- a/sw/tools/generators/gen_messages.ml +++ b/sw/tools/generators/gen_messages.ml @@ -31,6 +31,32 @@ type _type = | 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 @@ -119,24 +145,38 @@ module Gen_onboard = struct let print_field = fun h (t, name, (_f: format option)) -> match t with Basic _ -> - fprintf h "\t DownlinkPut%sByAddr(_trans, _dev, (%s)); \\\n" (Syntax.nameof t) name + 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 DownlinkPut%sArray(_trans, _dev, %s, %s); \\\n" (Syntax.nameof (Basic t)) (Syntax.length_name varname) name + 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 DownlinkPut%sFixedArray(_trans, _dev, %d, %s); \\\n" (Syntax.nameof (Basic t)) len name + 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_parameter h = function + 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 -> - print_parameter h f; - List.iter (fun f -> fprintf h ", "; print_parameter h f) fields + fprintf h ", "; + print_macro_param h f; + List.iter (fun f -> fprintf h ", "; print_macro_param h f) fields + + let print_fun_param h = function + (Array (t, _), s, _) -> fprintf h "uint8_t %s, %s *_%s" (Syntax.length_name s) (c_type (Syntax.nameof (Basic t))) s + | (FixedArray (t, _, _), s, _) -> fprintf h "%s *_%s" (c_type (Syntax.nameof (Basic t))) s + | (t, s, _) -> fprintf h "%s *_%s" (c_type (Syntax.nameof t)) s + + let print_function_parameters h = function + [] -> () + | f::fields -> + fprintf h ", "; + print_fun_param h f; + List.iter (fun f -> fprintf h ", "; print_fun_param h f) fields let rec size_fields = fun fields size -> match fields with @@ -155,28 +195,35 @@ module Gen_onboard = struct Failure "int_of_string" -> 0 let print_downlink_macro = fun h {name=s; fields = fields} -> - if List.length fields > 0 then begin - fprintf h "#define DOWNLINK_SEND_%s(_trans, _dev, " s; - end else - fprintf h "#define DOWNLINK_SEND_%s(_trans, _dev " s; + (* Macros for backward compatibility *) + fprintf h "#define DOWNLINK_SEND_%s(_trans, _dev" s; print_macro_parameters h fields; - fprintf h "){ \\\n"; + 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 (DownlinkCheckFreeSpace(_trans, _dev, DownlinkSizeOf(_trans, _dev, %s))) {\\\n" size; - fprintf h "\t DownlinkCountBytes(_trans, _dev, DownlinkSizeOf(_trans, _dev, %s)); \\\n" size; - fprintf h "\t DownlinkStartMessage(_trans, _dev, \"%s\", DL_%s, %s) \\\n" s s size; + 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 DownlinkEndMessage(_trans, _dev ) \\\n"; - fprintf h "\t} else \\\n"; - fprintf h "\t DownlinkOverrun(_trans, _dev ); \\\n"; + 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} -> - if List.length fields > 0 then begin - fprintf h "#define DOWNLINK_SEND_%s(_trans, _dev, " s; - end else - fprintf h "#define DOWNLINK_SEND_%s(_trans, _dev " s; + fprintf h "#define DOWNLINK_SEND_%s(_trans, _dev" s; print_macro_parameters h fields; + fprintf h ") {}\n"; + fprintf h "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" (** Prints the messages ids *) @@ -217,7 +264,7 @@ module Gen_onboard = struct (** 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; + (*print_lengths_array h class_ messages;*) List.iter (print_downlink_macro h) messages let print_null_downlink_macros = fun h messages -> @@ -316,6 +363,10 @@ let () = 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 *) @@ -330,7 +381,9 @@ let () = (** Macros for airborne datalink (receiving) *) let check_alignment = class_name <> "telemetry" in - List.iter (Gen_onboard.print_get_macros h check_alignment) messages + List.iter (Gen_onboard.print_get_macros h check_alignment) 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_periodic.ml b/sw/tools/generators/gen_periodic.ml index 124e58eed8..365575c06d 100644 --- a/sw/tools/generators/gen_periodic.ml +++ b/sw/tools/generators/gen_periodic.ml @@ -84,7 +84,7 @@ let output_modes = fun out_h process_name modes freq modules -> right (); lprintf out_h "if (telemetry_%s.msgs[TELEMETRY_%s_MSG_%s_ID].cb != NULL)\n" process_name (String.uppercase process_name) message_name; right (); - lprintf out_h "telemetry_%s.msgs[TELEMETRY_%s_MSG_%s_ID].cb();\n" process_name (String.uppercase process_name) message_name; + lprintf out_h "telemetry_%s.msgs[TELEMETRY_%s_MSG_%s_ID].cb(trans, dev);\n" process_name (String.uppercase process_name) message_name; left (); fprintf out_h "#if USE_PERIODIC_TELEMETRY_REPORT\n"; lprintf out_h "else periodic_telemetry_err_report(TELEMETRY_PROCESS_%s, telemetry_mode_%s, TELEMETRY_%s_MSG_%s_ID);\n" process_name process_name (String.uppercase process_name) message_name; @@ -212,7 +212,7 @@ let _ = fprintf out_h "extern struct pprz_telemetry telemetry_%s;\n" process_name; fprintf out_h "#endif /* PERIODIC_C_%s */\n" (String.uppercase process_name); - lprintf out_h "static inline void periodic_telemetry_send_%s(void) { /* %dHz */\n" process_name freq; (*TODO pass transport+device *) + lprintf out_h "static inline void periodic_telemetry_send_%s(struct transport_tx *trans __attribute__((unused)), struct link_device *dev __attribute__((unused))) { /* %dHz */\n" process_name freq; (*TODO pass transport+device with correct types *) right (); output_modes out_h process_name modes freq modules_name; left (); diff --git a/sw/tools/generators/gen_radio.ml b/sw/tools/generators/gen_radio.ml index 5d48af1a8b..3ae0f28886 100644 --- a/sw/tools/generators/gen_radio.ml +++ b/sw/tools/generators/gen_radio.ml @@ -145,7 +145,7 @@ let _ = nl (); let channels = Xml.children xml in let n = ExtXml.attrib xml "name" in - Xml2h.warning ("RADIO MODEL: "^n); + (* Xml2h.warning ("RADIO MODEL: "^n); *) define_string "RADIO_NAME" n; nl (); (*define "RADIO_CONTROL_NB_CHANNEL" (string_of_int (List.length channels));*) diff --git a/sw/tools/generators/gen_settings.ml b/sw/tools/generators/gen_settings.ml index 374ba664bb..6296e05c64 100644 --- a/sw/tools/generators/gen_settings.ml +++ b/sw/tools/generators/gen_settings.ml @@ -124,7 +124,7 @@ let print_dl_settings = fun settings -> lprintf "default: var = 0.; break;\\\n"; left (); lprintf "}\\\n"; - lprintf "DOWNLINK_SEND_DL_VALUE(_trans, _dev, &i, &var);\\\n"; + lprintf "pprz_msg_send_DL_VALUE(_trans, _dev, AC_ID, &i, &var);\\\n"; lprintf "i++;\\\n"; left () end; diff --git a/tests/examples/01_compile_all_test_targets.t b/tests/examples/01_compile_all_aircrafts.t similarity index 55% rename from tests/examples/01_compile_all_test_targets.t rename to tests/examples/01_compile_all_aircrafts.t index 4fcff50f08..9c14937f01 100644 --- a/tests/examples/01_compile_all_test_targets.t +++ b/tests/examples/01_compile_all_aircrafts.t @@ -1,5 +1,24 @@ #!/usr/bin/perl -w +# +# Reads conf/conf.xml (can be symlink to e.g. conf/conf_tests.xml) +# and compiles all targets of all aircrafts. +# +# Mandatory environment variables: +# PAPARAZZI_SRC : path to paparazzi source directory +# PAPARAZZI_HOME : path to paparazz home directory containing the conf +# +# optional environment variables: +# TEST_VERBOSE : set to 1 to print the compile output even if there was no error +# +# environment variables passed on to make: +# J=AUTO : detect number of CPUs to set jobs for parallel compilation +# +# Example on how to test compile all aircrafts/targets in your current conf.xml +# with parallel compilation and treating all warnings as errors: +# J=AUTO USER_CFLAGS=-Werror prove tests/examples +# + use Test::More; use lib "$ENV{'PAPARAZZI_SRC'}/tests/lib"; use XML::Simple; @@ -9,28 +28,28 @@ use Config; $|++; my $xmlSimple = XML::Simple->new(ForceArray => 1); -my $examples = $xmlSimple->XMLin("$ENV{'PAPARAZZI_SRC'}/conf/conf.xml"); +my $conf = $xmlSimple->XMLin("$ENV{'PAPARAZZI_HOME'}/conf/conf.xml"); -ok(1, "Parsed the tests configuration file"); -foreach my $example (sort keys%{$examples->{'aircraft'}}) +ok(1, "Parsed the configuration file"); +foreach my $aircraft (sort keys%{$conf->{'aircraft'}}) { - my $airframe = $examples->{'aircraft'}->{$example}->{'airframe'}; - my $airframe_config = $xmlSimple->XMLin("$ENV{'PAPARAZZI_SRC'}/conf/$airframe"); + my $airframe = $conf->{'aircraft'}->{$aircraft}->{'airframe'}; + my $airframe_config = $xmlSimple->XMLin("$ENV{'PAPARAZZI_HOME'}/conf/$airframe"); foreach my $process (sort keys %{$airframe_config->{'firmware'}}) { - #warn "EX: [$example] ". Dumper($airframe_config->{'firmware'}->{$process}->{'target'}); + #warn "EX: [$aircraft] ". Dumper($airframe_config->{'firmware'}->{$process}->{'target'}); foreach my $target (sort keys %{$airframe_config->{'firmware'}->{$process}->{'target'}}) { - #warn "EXAMPLE: [$example] TARGET: [$target]\n"; - my $make_options = "AIRCRAFT=$example clean_ac $target.compile"; + #warn "AIRCRAFT: [$aircraft] TARGET: [$target]\n"; + my $make_options = "AIRCRAFT=$aircraft clean_ac $target.compile"; my ($exit_status, $output) = run_program( - "Attempting to build the firmware $target for the aircraft $example.", + "Attempting to build the firmware $target for the aircraft $aircraft.", $ENV{'PAPARAZZI_SRC'}, "make $make_options", $ENV{'TEST_VERBOSE'},1); # print output if it failed and we didn't already print it in verbose mode warn "$output\n" if $exit_status && !$ENV{'TEST_VERBOSE'}; - ok($exit_status == 0, "Compile aircraft: $example, target: $target"); + ok($exit_status == 0, "Compile aircraft: $aircraft, target: $target"); } } } @@ -68,13 +87,14 @@ sub run_program my $exit_status = $?/256; unless ($exit_status == 0) { + my $err_msg = "\nError: The command \"". $prog->last_command() ."\" failed to complete successfully. Exit status: $exit_status\n"; if ($dont_fail_on_error) { - warn "\nError: The command \"". $prog->last_command() ."\" failed to complete successfully. Exit status: $exit_status\n"; + warn $err_msg; } else { - die "\nError: The command \"". $prog->last_command() ."\" failed to complete successfully. Exit status: $exit_status\n"; + die $err_msg; } } my $output_string = join "\n", @output;