diff --git a/conf/airframes/examples/demo.xml b/conf/airframes/examples/demo.xml index d4998a85c6..8a25c9bccc 100644 --- a/conf/airframes/examples/demo.xml +++ b/conf/airframes/examples/demo.xml @@ -91,7 +91,7 @@ demo5.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c 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 +demo5.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=uart0 # @@ -117,7 +117,7 @@ demo6.srcs += $(SRC_ARCH)/usb_ser_hw.c 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 +//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=usb_serial -DPPRZ_UART=UsbS -DDATALINK=PPRZ +test_spk.CFLAGS += -DDOWNLINK_DEVICE=usb_serial -DPPRZ_UART=usb_serial -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/wind_tunnel.xml b/conf/airframes/wind_tunnel.xml index 1d82b0e1ea..ec6a0c15ae 100644 --- a/conf/airframes/wind_tunnel.xml +++ b/conf/airframes/wind_tunnel.xml @@ -27,7 +27,7 @@ ap.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c 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 +ap.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=uart0 #ap.srcs += $(BOOZ)/booz_datalink.c ap.srcs += $(WIND_TUNNEL)/wt_servo.c @@ -57,7 +57,7 @@ mb.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c 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 +mb.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=uart0 mb.srcs += $(WIND_TUNNEL)/wt_servo.c diff --git a/conf/firmwares/setup.makefile b/conf/firmwares/setup.makefile index c234518ab4..7cf27a2132 100644 --- a/conf/firmwares/setup.makefile +++ b/conf/firmwares/setup.makefile @@ -108,7 +108,7 @@ setup_actuators.srcs += mcu_periph/uart.c $(SRC_ARCH)/mcu_periph/uart_arch.c setup_actuators.CFLAGS += -DUSE_$(MODEM_PORT) setup_actuators.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) SETUP_ACTUATORS_MODEM_PORT_LOWER=$(shell echo $(MODEM_PORT) | tr A-Z a-z) -setup_actuators.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=$(SETUP_ACTUATORS_MODEM_PORT_LOWER) -DPPRZ_UART=$(MODEM_PORT) +setup_actuators.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=$(SETUP_ACTUATORS_MODEM_PORT_LOWER) -DPPRZ_UART=$(SETUP_ACTUATORS_MODEM_PORT_LOWER) setup_actuators.CFLAGS += -DDOWNLINK_TRANSPORT=pprz_tp -DDATALINK=PPRZ setup_actuators.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c diff --git a/conf/firmwares/subsystems/fixedwing/fbw_datalink.makefile b/conf/firmwares/subsystems/fixedwing/fbw_datalink.makefile index 4983c1fab2..ecb94029fe 100644 --- a/conf/firmwares/subsystems/fixedwing/fbw_datalink.makefile +++ b/conf/firmwares/subsystems/fixedwing/fbw_datalink.makefile @@ -1,5 +1,8 @@ +FBW_MODEM_PORT_LOWER=$(shell echo $(MODEM_PORT) | tr A-Z a-z) +FBW_AP_PORT_LOWER=$(shell echo $(AUTOPILOT_PORT) | tr A-Z a-z) + fbw.srcs += firmwares/fixedwing/fbw_datalink.c fbw.CFLAGS += -DFBW_DATALINK -fbw.CFLAGS += -DMODEM_LINK=$(MODEM_PORT) -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) -fbw.CFLAGS += -DAUTOPILOT_LINK=$(AUTOPILOT_PORT) -DUSE_$(AUTOPILOT_PORT) -D$(AUTOPILOT_PORT)_BAUD=$(MODEM_BAUD) +fbw.CFLAGS += -DMODEM_LINK=$(FBW_MODEM_PORT_LOWER) -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) +fbw.CFLAGS += -DAUTOPILOT_LINK=$(FBW_AP_PORT_LOWER) -DUSE_$(AUTOPILOT_PORT) -D$(AUTOPILOT_PORT)_BAUD=$(MODEM_BAUD) diff --git a/conf/firmwares/subsystems/fixedwing/gps_furuno.makefile b/conf/firmwares/subsystems/fixedwing/gps_furuno.makefile index 3a49318971..a76f040d8c 100644 --- a/conf/firmwares/subsystems/fixedwing/gps_furuno.makefile +++ b/conf/firmwares/subsystems/fixedwing/gps_furuno.makefile @@ -3,9 +3,10 @@ # Furuno NMEA GPS unit GPS_LED ?= none +FURUNO_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z) ap.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG -ap.CFLAGS += -DGPS_LINK=$(GPS_PORT) +ap.CFLAGS += -DGPS_LINK=$(FURUNO_GPS_PORT_LOWER) ap.CFLAGS += -DUSE_$(GPS_PORT) ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD) ap.CFLAGS += -DNMEA_PARSE_PROP diff --git a/conf/firmwares/subsystems/fixedwing/gps_mediatek_diy.makefile b/conf/firmwares/subsystems/fixedwing/gps_mediatek_diy.makefile index 96f08a7fd4..bbff58464c 100644 --- a/conf/firmwares/subsystems/fixedwing/gps_mediatek_diy.makefile +++ b/conf/firmwares/subsystems/fixedwing/gps_mediatek_diy.makefile @@ -3,9 +3,10 @@ # Mediatek MT3329, DIYDrones V1.4/1.6 protocol GPS_LED ?= none +MTK_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z) ap.CFLAGS += -DUSE_GPS -DGPS_CONFIGURE -DGPS_USE_LATLONG -ap.CFLAGS += -DGPS_LINK=$(GPS_PORT) +ap.CFLAGS += -DGPS_LINK=$(MTK_GPS_PORT_LOWER) ap.CFLAGS += -DUSE_$(GPS_PORT) ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD) diff --git a/conf/firmwares/subsystems/fixedwing/gps_nmea.makefile b/conf/firmwares/subsystems/fixedwing/gps_nmea.makefile index 8fa7a5eaf3..1656ef6667 100644 --- a/conf/firmwares/subsystems/fixedwing/gps_nmea.makefile +++ b/conf/firmwares/subsystems/fixedwing/gps_nmea.makefile @@ -3,9 +3,10 @@ # NMEA GPS unit GPS_LED ?= none +NMEA_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z) ap.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG -ap.CFLAGS += -DGPS_LINK=$(GPS_PORT) +ap.CFLAGS += -DGPS_LINK=$(NMEA_GPS_PORT_LOWER) ap.CFLAGS += -DUSE_$(GPS_PORT) ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD) diff --git a/conf/firmwares/subsystems/fixedwing/gps_skytraq.makefile b/conf/firmwares/subsystems/fixedwing/gps_skytraq.makefile index e5bd7870ad..2a3858fa3f 100644 --- a/conf/firmwares/subsystems/fixedwing/gps_skytraq.makefile +++ b/conf/firmwares/subsystems/fixedwing/gps_skytraq.makefile @@ -1,9 +1,10 @@ # Hey Emacs, this is a -*- makefile -*- GPS_LED ?= none +SKYTRAQ_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z) ap.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG -ap.CFLAGS += -DGPS_LINK=$(GPS_PORT) +ap.CFLAGS += -DGPS_LINK=$(SKYTRAQ_GPS_PORT_LOWER) ap.CFLAGS += -DUSE_$(GPS_PORT) ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD) diff --git a/conf/firmwares/subsystems/fixedwing/gps_ublox.makefile b/conf/firmwares/subsystems/fixedwing/gps_ublox.makefile index cc524852ca..225e53d289 100644 --- a/conf/firmwares/subsystems/fixedwing/gps_ublox.makefile +++ b/conf/firmwares/subsystems/fixedwing/gps_ublox.makefile @@ -2,9 +2,10 @@ # UBlox LEA 5H GPS_LED ?= none +UBX_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z) ap.CFLAGS += -DUSE_GPS -DUBX -DGPS_USE_LATLONG -ap.CFLAGS += -DGPS_LINK=$(GPS_PORT) +ap.CFLAGS += -DGPS_LINK=$(UBX_GPS_PORT_LOWER) ap.CFLAGS += -DUSE_$(GPS_PORT) ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD) diff --git a/conf/firmwares/subsystems/fixedwing/gps_ublox_utm.makefile b/conf/firmwares/subsystems/fixedwing/gps_ublox_utm.makefile index 0a5864e226..aaca0cb830 100644 --- a/conf/firmwares/subsystems/fixedwing/gps_ublox_utm.makefile +++ b/conf/firmwares/subsystems/fixedwing/gps_ublox_utm.makefile @@ -2,9 +2,10 @@ # UBlox LEA 4P GPS_LED ?= none +UBX_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z) ap.CFLAGS += -DUSE_GPS -DUBX -ap.CFLAGS += -DGPS_LINK=$(GPS_PORT) +ap.CFLAGS += -DGPS_LINK=$(UBX_GPS_PORT_LOWER) ap.CFLAGS += -DUSE_$(GPS_PORT) ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD) diff --git a/conf/firmwares/subsystems/fixedwing/imu_xsens.makefile b/conf/firmwares/subsystems/fixedwing/imu_xsens.makefile index f63b94aad6..b18ab3bd5f 100644 --- a/conf/firmwares/subsystems/fixedwing/imu_xsens.makefile +++ b/conf/firmwares/subsystems/fixedwing/imu_xsens.makefile @@ -63,6 +63,6 @@ ifndef XSENS_UART_BAUD endif ap.CFLAGS += -DUSE_UART$(XSENS_UART_NR) -ap.CFLAGS += -DINS_LINK=UART$(XSENS_UART_NR) +ap.CFLAGS += -DINS_LINK=uart$(XSENS_UART_NR) ap.CFLAGS += -DUART$(XSENS_UART_NR)_BAUD=$(XSENS_UART_BAUD) ap.CFLAGS += -DXSENS_OUTPUT_MODE=0x1836 diff --git a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile b/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile index f6134183ce..3207698cea 100644 --- a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile +++ b/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile @@ -25,7 +25,7 @@ endif #B115200 ap.CFLAGS += -DUSE_UART$(XSENS_UART_NR) -ap.CFLAGS += -DINS_LINK=UART$(XSENS_UART_NR) +ap.CFLAGS += -DINS_LINK=uart$(XSENS_UART_NR) ap.CFLAGS += -DUART$(XSENS_UART_NR)_BAUD=$(XSENS_UART_BAUD) ap.CFLAGS += -DXSENS_OUTPUT_MODE=0x1836 ap.srcs += $(SRC_SUBSYSTEMS)/ins.c diff --git a/conf/firmwares/subsystems/fixedwing/ins_xsens700.makefile b/conf/firmwares/subsystems/fixedwing/ins_xsens700.makefile index 061ed2742d..b114444d17 100644 --- a/conf/firmwares/subsystems/fixedwing/ins_xsens700.makefile +++ b/conf/firmwares/subsystems/fixedwing/ins_xsens700.makefile @@ -21,7 +21,7 @@ ifndef XSENS_UART_BAUD endif ap.CFLAGS += -DUSE_UART$(XSENS_UART_NR) -ap.CFLAGS += -DINS_LINK=UART$(XSENS_UART_NR) +ap.CFLAGS += -DINS_LINK=uart$(XSENS_UART_NR) ap.CFLAGS += -DUART$(XSENS_UART_NR)_BAUD=$(XSENS_UART_BAUD) ap.CFLAGS += -DXSENS_OUTPUT_MODE=0x1836 ap.srcs += $(SRC_SUBSYSTEMS)/ins.c diff --git a/conf/firmwares/subsystems/fixedwing/intermcu_uart.makefile b/conf/firmwares/subsystems/fixedwing/intermcu_uart.makefile index 0430a139cb..7bb2ce981b 100644 --- a/conf/firmwares/subsystems/fixedwing/intermcu_uart.makefile +++ b/conf/firmwares/subsystems/fixedwing/intermcu_uart.makefile @@ -8,15 +8,13 @@ $(error Using intermcu via UART, so dual mcu with separate fbw. Please add - + diff --git a/conf/modules/ahrs_chimu_uart.xml b/conf/modules/ahrs_chimu_uart.xml index 78767fbfd3..15536a758f 100644 --- a/conf/modules/ahrs_chimu_uart.xml +++ b/conf/modules/ahrs_chimu_uart.xml @@ -17,7 +17,7 @@ - + diff --git a/conf/modules/airspeed_otf.xml b/conf/modules/airspeed_otf.xml index 6935d2f3a4..28229c085d 100644 --- a/conf/modules/airspeed_otf.xml +++ b/conf/modules/airspeed_otf.xml @@ -22,9 +22,12 @@ + + OTF_UART_LOWER=$(shell echo $(OTF_UART) | tr A-Z a-z) + - + diff --git a/conf/modules/direct_memory_logger.xml b/conf/modules/direct_memory_logger.xml index 4f59643617..adadd1acd4 100644 --- a/conf/modules/direct_memory_logger.xml +++ b/conf/modules/direct_memory_logger.xml @@ -48,7 +48,7 @@ - + diff --git a/conf/modules/extra_dl.xml b/conf/modules/extra_dl.xml index e03ed7ef73..21ced1bc68 100644 --- a/conf/modules/extra_dl.xml +++ b/conf/modules/extra_dl.xml @@ -16,7 +16,6 @@ EXTRA_DL_PORT_LOWER = $(shell echo $(EXTRA_DL_PORT) | tr A-Z a-z) - diff --git a/conf/modules/mavlink.xml b/conf/modules/mavlink.xml index 2a0dae6be4..0c5ee3dcbe 100644 --- a/conf/modules/mavlink.xml +++ b/conf/modules/mavlink.xml @@ -19,18 +19,19 @@ MAVLINK_PORT ?= UART1 MAVLINK_PORT_UPPER=$(shell echo $(MAVLINK_PORT) | tr a-z A-Z) + MAVLINK_PORT_LOWER=$(shell echo $(MAVLINK_PORT) | tr A-Z a-z) ifeq ($(MAVLINK_PORT), UsbS) ap.CFLAGS += -DUSE_USB_SERIAL ap.srcs += $(SRC_ARCH)/usb_ser_hw.c - ap.CFLAGS += -DMAVLINK_DEV=UsbS + ap.CFLAGS += -DMAVLINK_DEV=usb_serial else ifeq ($(findstring UDP,$(MAVLINK_PORT)), UDP) include $(CFG_SHARED)/udp.makefile - $(TARGET).CFLAGS += -DMAVLINK_DEV=$(MAVLINK_PORT_UPPER) + $(TARGET).CFLAGS += -DMAVLINK_DEV=$(MAVLINK_PORT_LOWER) $(TARGET).CFLAGS += -DUSE_$(MAVLINK_PORT_UPPER) else MAVLINK_BAUD ?= B57600 - ap.CFLAGS += -DMAVLINK_DEV=$(MAVLINK_PORT_UPPER) + ap.CFLAGS += -DMAVLINK_DEV=$(MAVLINK_PORT_LOWER) ap.CFLAGS += -DUSE_$(MAVLINK_PORT_UPPER) ap.CFLAGS += -D$(MAVLINK_PORT_UPPER)_BAUD=$(MAVLINK_BAUD) endif diff --git a/conf/modules/mavlink_decoder.xml b/conf/modules/mavlink_decoder.xml index fc85851331..18182e7c27 100644 --- a/conf/modules/mavlink_decoder.xml +++ b/conf/modules/mavlink_decoder.xml @@ -11,7 +11,10 @@ - + + MAVLINK_PORT_LOWER=$(shell echo $(MAVLINK_PORT) | tr A-Z a-z) + + diff --git a/sw/airborne/arch/linux/mcu_periph/uart_arch.c b/sw/airborne/arch/linux/mcu_periph/uart_arch.c index e159a55e5b..972a2ae7a2 100644 --- a/sw/airborne/arch/linux/mcu_periph/uart_arch.c +++ b/sw/airborne/arch/linux/mcu_periph/uart_arch.c @@ -42,6 +42,8 @@ void uart_periph_set_baudrate(struct uart_periph *periph, uint32_t baud) { + periph->baudrate = baud; + struct SerialPort *port; // close serial port if already open if (periph->reg_addr != NULL) { diff --git a/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.c b/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.c index 1c202e2969..528a344ef8 100644 --- a/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.c +++ b/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.c @@ -38,6 +38,7 @@ #include "pprz_debug.h" #include "armVIC.h" +struct spi_slave_hs spi_slave_hs; /* High Speed SPI Slave Circular Buffer */ uint16_t spi_slave_hs_rx_insert_idx, spi_slave_hs_rx_extract_idx; @@ -103,6 +104,36 @@ static void SSP_ISR(void) __attribute__((naked)); #endif +// Functions for the generic device API +static int spi_slave_hs_check_free_space(struct spi_slave_hs *p __attribute__((unused)), uint8_t len __attribute__((unused))) +{ + return TRUE; +} + +static void spi_slave_hs_transmit(struct spi_slave_hs *p __attribute__((unused)), uint8_t byte) +{ + uint8_t temp = (spi_slave_hs_tx_insert_idx + 1) % SPI_SLAVE_HS_TX_BUFFER_SIZE; + if (temp != spi_slave_hs_tx_extract_idx) /* there is room left */ + { + spi_slave_hs_tx_buffer[spi_slave_hs_tx_insert_idx] = byte; + spi_slave_hs_tx_insert_idx = temp; + } +} + +static void spi_slave_hs_send(struct spi_slave_hs *p __attribute__((unused))) { } + +static int spi_slave_hs_char_available(struct spi_slave_hs *p __attribute__((unused))) +{ + return spi_slave_hs_rx_insert_idx != spi_slave_hs_rx_extract_idx; +} + +static uint8_t spi_slave_hs_getch(struct spi_slave_hs *p __attribute__((unused))) +{ + uint8_t ret = spi_slave_hs_rx_buffer[spi_slave_hs_rx_extract_idx]; + spi_slave_hs_rx_extract_idx = (spi_slave_hs_rx_extract_idx + 1)%SPI_SLAVE_HS_RX_BUFFER_SIZE; + return ret; +} + void spi_slave_hs_init(void) { @@ -129,6 +160,14 @@ void spi_slave_hs_init(void) // Enable Receive interrupt SetBit(SSPIMSC, RXIM); + // Configure generic device + spi_slave_hs.device.periph = (void *)(&spi_slave_hs); + spi_slave_hs.device.check_free_space = (check_free_space_t) spi_slave_hs_check_free_space; + spi_slave_hs.device.put_byte = (put_byte_t) spi_slave_hs_transmit; + spi_slave_hs.device.send_message = (send_message_t) spi_slave_hs_send; + spi_slave_hs.device.char_available = (char_available_t) spi_slave_hs_char_available; + spi_slave_hs.device.get_byte = (get_byte_t) spi_slave_hs_getch; + } /* diff --git a/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.h b/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.h index 957931742c..f695de20e9 100644 --- a/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.h +++ b/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.h @@ -32,7 +32,14 @@ #define SPI_SLAVE_HS_ARCH_H #include "std.h" +#include "mcu_periph/link_device.h" +struct spi_slave_hs { + /** Generic device interface */ + struct link_device device; +}; + +extern struct spi_slave_hs spi_slave_hs; #define SpiEnable() { \ SetBit(SSPCR1, SSE); \ @@ -48,29 +55,11 @@ extern uint16_t spi_slave_hs_rx_insert_idx, spi_slave_hs_rx_extract_idx; extern uint8_t spi_slave_hs_rx_buffer[SPI_SLAVE_HS_RX_BUFFER_SIZE]; -#define SpiSlaveChAvailable() (spi_slave_hs_rx_insert_idx != spi_slave_hs_rx_extract_idx) - -#define SpiSlaveGetch() ({\ - uint8_t ret = spi_slave_hs_rx_buffer[spi_slave_hs_rx_extract_idx]; \ - spi_slave_hs_rx_extract_idx = (spi_slave_hs_rx_extract_idx + 1)%SPI_SLAVE_HS_RX_BUFFER_SIZE; \ - ret; \ - }) #define SPI_SLAVE_HS_TX_BUFFER_SIZE 64 extern uint8_t spi_slave_hs_tx_insert_idx, spi_slave_hs_tx_extract_idx; extern uint8_t spi_slave_hs_tx_buffer[SPI_SLAVE_HS_TX_BUFFER_SIZE]; -#define SpiSlaveTransmit(data) {\ - uint8_t temp = (spi_slave_hs_tx_insert_idx + 1) % SPI_SLAVE_HS_TX_BUFFER_SIZE; \ - if (temp != spi_slave_hs_tx_extract_idx) /* there is room left */ \ - { \ - spi_slave_hs_tx_buffer[spi_slave_hs_tx_insert_idx] = (uint8_t)data; \ - spi_slave_hs_tx_insert_idx = temp; \ - } \ - } - - - #endif diff --git a/sw/airborne/arch/lpc21/mcu_periph/uart_arch.c b/sw/airborne/arch/lpc21/mcu_periph/uart_arch.c index 4abae2266c..d13e104888 100644 --- a/sw/airborne/arch/lpc21/mcu_periph/uart_arch.c +++ b/sw/airborne/arch/lpc21/mcu_periph/uart_arch.c @@ -48,6 +48,8 @@ static inline void uart_enable_interrupts(struct uart_periph *p) static inline void uart_set_baudrate(struct uart_periph *p, uint32_t baud) { + p->baudrate = baud; + /* calculate the baudrate */ uint32_t _baud_reg_val = (uint16_t)((PCLK / (((float)baud) * 16.0)) + 0.5); /* select divisor latches */ diff --git a/sw/airborne/arch/lpc21/usb_ser_hw.c b/sw/airborne/arch/lpc21/usb_ser_hw.c index 0e6a4373e8..973db68984 100644 --- a/sw/airborne/arch/lpc21/usb_ser_hw.c +++ b/sw/airborne/arch/lpc21/usb_ser_hw.c @@ -565,6 +565,16 @@ void VCOM_event(void) {} // Empty for lpc21 void VCOM_send_message(void) {} +static int usb_serial_char_available(struct usb_serial_periph *p __attribute__((unused))) +{ + return VCOM_check_available(); +} + +static uint8_t usb_serial_getch(struct usb_serial_periph *p __attribute__((unused))) +{ + return (uint8_t)(VCOM_getchar()); +} + void VCOM_init(void) { // initialise stack @@ -608,6 +618,8 @@ void VCOM_init(void) // 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.put_byte = (put_byte_t) usb_serial_transmit; usb_serial.device.send_message = (send_message_t) usb_serial_send; + usb_serial.device.char_available = (char_available_t) usb_serial_char_available; + usb_serial.device.get_byte = (get_byte_t) usb_serial_getch; } diff --git a/sw/airborne/arch/sim/sim_ap.c b/sw/airborne/arch/sim/sim_ap.c index d9ea5e32b7..970f5da101 100644 --- a/sw/airborne/arch/sim/sim_ap.c +++ b/sw/airborne/arch/sim/sim_ap.c @@ -20,7 +20,6 @@ #include "firmwares/fixedwing/guidance/guidance_v.h" #include "subsystems/commands.h" #include "firmwares/fixedwing/main_ap.h" -#include "sim_uart.h" #include "subsystems/datalink/datalink.h" #include "subsystems/datalink/telemetry.h" #include "generated/flight_plan.h" @@ -100,26 +99,6 @@ value sim_init(value unit) { init_fbw(); init_ap(); -#ifdef SIM_UART - /* open named pipe */ - char link_pipe_name[128]; -#ifdef SIM_XBEE - sprintf(link_pipe_name, "/tmp/pprz_xbee"); -#else - sprintf(link_pipe_name, "/tmp/pprz_link_%d", AC_ID); -#endif - struct stat st; - if (stat(link_pipe_name, &st)) { - if (mkfifo(link_pipe_name, 0644) == -1) { - perror("make pipe"); - exit(10); - } - } - if (!(pipe_stream = fopen(link_pipe_name, "w"))) { - perror("open pipe"); - exit(10); - } -#endif return unit; } diff --git a/sw/airborne/arch/sim/sim_uart.c b/sw/airborne/arch/sim/sim_uart.c deleted file mode 100644 index 01e0eed92e..0000000000 --- a/sw/airborne/arch/sim/sim_uart.c +++ /dev/null @@ -1,3 +0,0 @@ -#include "sim_uart.h" - -FILE *pipe_stream; diff --git a/sw/airborne/arch/sim/sim_uart.h b/sw/airborne/arch/sim/sim_uart.h deleted file mode 100644 index ed6d23ce16..0000000000 --- a/sw/airborne/arch/sim/sim_uart.h +++ /dev/null @@ -1,14 +0,0 @@ -/** \file sim_uart.h - * \brief Simulation of uart transmission on an Unix pipe - */ - -#include - -extern FILE *pipe_stream; - -#define SimUartCheckFreeSpace(_) TRUE - -#define SimUartTransmit(_x) fputc(_x, pipe_stream) -#define SimUartPrintString(_s) fputs(_s, pipe_stream) -#define SimUartSendMessage() fflush(pipe_stream); -#define SimUartPrintHex16(c) _PrintHex16(SimUartTransmit, c) diff --git a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c index ac6ecbbe14..e679592836 100644 --- a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c @@ -41,6 +41,7 @@ void uart_periph_set_baudrate(struct uart_periph *p, uint32_t baud) { + p->baudrate = baud; /* Configure USART baudrate */ usart_set_baudrate((uint32_t)p->reg_addr, baud); diff --git a/sw/airborne/arch/stm32/usb_ser_hw.c b/sw/airborne/arch/stm32/usb_ser_hw.c index 0ad2afc1f3..b89a19bf8c 100644 --- a/sw/airborne/arch/stm32/usb_ser_hw.c +++ b/sw/airborne/arch/stm32/usb_ser_hw.c @@ -486,6 +486,16 @@ static void usb_serial_send(struct usb_serial_periph *p __attribute__((unused))) VCOM_send_message(); } +static int usb_serial_char_available(struct usb_serial_periph *p __attribute__((unused))) +{ + return VCOM_check_available(); +} + +static uint8_t usb_serial_getch(struct usb_serial_periph *p __attribute__((unused))) +{ + return (uint8_t)(VCOM_getchar()); +} + void VCOM_init(void) { // initialise fifos @@ -518,6 +528,8 @@ void VCOM_init(void) // 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.put_byte = (put_byte_t) usb_serial_transmit; usb_serial.device.send_message = (send_message_t) usb_serial_send; + usb_serial.device.char_available = (char_available_t) usb_serial_char_available; + usb_serial.device.get_byte = (get_byte_t) usb_serial_getch; } diff --git a/sw/airborne/firmwares/fixedwing/fbw_datalink.c b/sw/airborne/firmwares/fixedwing/fbw_datalink.c index d09a3fa017..dfb5d6b7d0 100644 --- a/sw/airborne/firmwares/fixedwing/fbw_datalink.c +++ b/sw/airborne/firmwares/fixedwing/fbw_datalink.c @@ -31,38 +31,19 @@ #include "led.h" -#define __ModemLink(dev, _x) dev##_x -#define _ModemLink(dev, _x) __ModemLink(dev, _x) -#define ModemLink(_x) _ModemLink(MODEM_LINK, _x) -#define ModemBuffer() ModemLink(ChAvailable()) - - -#define __AutopilotLink(dev, _x) dev##_x -#define _AutopilotLink(dev, _x) __AutopilotLink(dev, _x) -#define AutopilotLink(_x) _AutopilotLink(AUTOPILOT_LINK, _x) - -#define AutopilotBuffer() AutopilotLink(ChAvailable()) +#define ModemLinkDevice (&(MODEM_LINK).device) +#define AutopilotLinkDevice (&(AUTOPILOT_LINK).device) static inline void autopilot_parse(char c) { - ModemLink(Transmit(c)); + ModemLinkDevice->put_byte(ModemLinkDevice->periph, c); } static inline void modem_parse(char c) { - AutopilotLink(Transmit(c)); + AutopilotLinkDevice->put_byte(AutopilotLinkDevice->periph, c); } -#define ReadAutopilotBuffer() { \ - while (AutopilotLink(ChAvailable())) \ - autopilot_parse(AutopilotLink(Getch())); \ - } - -#define ReadModemBuffer() { \ - while (ModemLink(ChAvailable())) \ - modem_parse(ModemLink(Getch())); \ - } - void fbw_datalink_periodic(void) { #ifdef MODEM_LINK_LED @@ -76,16 +57,19 @@ void fbw_datalink_periodic(void) void fbw_datalink_event(void) { #ifdef MODEM_LINK_LED - if (ModemLink(ChAvailable())) { + if (ModemLinkDevice->char_available(ModemLinkDevice->periph)) { LED_ON(MODEM_LINK_LED); } #endif #ifdef AUTOPILOT_LINK_LED - if (AutopilotLink(ChAvailable())) { + if (AutopilotLinkDevice->char_available(AutopilotLinkDevice->periph)) { LED_ON(AUTOPILOT_LINK_LED); } #endif - ReadModemBuffer(); - ReadAutopilotBuffer(); + while (ModemLinkDevice->char_available(ModemLinkDevice->periph)) + modem_parse(ModemLinkDevice->get_byte(ModemLinkDevice->periph)); + + while (AutopilotLinkDevice->char_available(AutopilotLinkDevice->periph)) + autopilot_parse(AutopilotLinkDevice->get_byte(AutopilotLinkDevice->periph)); } diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c index bd7083314c..97072979dc 100644 --- a/sw/airborne/firmwares/fixedwing/main_fbw.c +++ b/sw/airborne/firmwares/fixedwing/main_fbw.c @@ -331,12 +331,7 @@ void periodic_task_fbw(void) } #endif -#ifdef MCU_UART_LINK - inter_mcu_fill_fbw_state(); - link_mcu_periodic_task(); -#endif - -#ifdef MCU_CAN_LINK +#if defined MCU_UART_LINK || defined MCU_CAN_LINK inter_mcu_fill_fbw_state(); link_mcu_periodic_task(); #endif diff --git a/sw/airborne/link_mcu_usart.c b/sw/airborne/link_mcu_usart.c index d4a212be14..c42f03537a 100644 --- a/sw/airborne/link_mcu_usart.c +++ b/sw/airborne/link_mcu_usart.c @@ -29,16 +29,12 @@ ////////////////////////////////////////////////////////////////////////////////////////////// // LINK -#define __InterMcuLink(dev, _x) dev##_x -#define _InterMcuLink(dev, _x) __InterMcuLink(dev, _x) -#define InterMcuLink(_x) _InterMcuLink(INTERMCU_LINK, _x) - -#define InterMcuBuffer() InterMcuLink(ChAvailable()) - -#define InterMcuUartSend1(c) InterMcuLink(Transmit(c)) -#define InterMcuUartSetBaudrate(_a) InterMcuLink(SetBaudrate(_a)) -#define InterMcuUartRunning InterMcuLink(TxRunning) -#define InterMcuUartSendMessage InterMcuLink(SendMessage) +// Use uart interface directly +#define InterMcuBuffer() uart_char_available(&(INTERMCU_LINK)) +#define InterMcuUartSend1(c) uart_transmit(&(INTERMCU_LINK), c) +#define InterMcuUartSetBaudrate(_a) uart_periph_set_baudrate(&(INTERMCU_LINK), _a) +#define InterMcuUartSendMessage() {} +#define InterMcuUartGetch() uart_getch(&(INTERMCU_LINK)) ////////////////////////////////////////////////////////////////////////////////////////////// // PROTOCOL @@ -379,8 +375,8 @@ void link_mcu_event_task(void) { /* A message has been received */ if (InterMcuBuffer()) { - while (InterMcuLink(ChAvailable())) { - intermcu_parse(InterMcuLink(Getch())); + while (InterMcuBuffer()) { + intermcu_parse(InterMcuUartGetch()); if (intermcu_data.msg_available) { parse_mavpilot_msg(); intermcu_data.msg_available = FALSE; diff --git a/sw/airborne/mcu_periph/link_device.h b/sw/airborne/mcu_periph/link_device.h index 04d2186479..e282686d79 100644 --- a/sw/airborne/mcu_periph/link_device.h +++ b/sw/airborne/mcu_periph/link_device.h @@ -34,15 +34,19 @@ * to store in the device structure */ typedef int (*check_free_space_t)(void *, uint8_t); -typedef void (*transmit_t)(void *, uint8_t); +typedef void (*put_byte_t)(void *, uint8_t); typedef void (*send_message_t)(void *); +typedef int (*char_available_t)(void *); +typedef uint8_t (*get_byte_t)(void *); /** Device structure */ struct link_device { check_free_space_t check_free_space; ///< check if transmit buffer is not full - transmit_t transmit; ///< transmit one byte + put_byte_t put_byte; ///< put one byte send_message_t send_message; ///< send completed buffer + char_available_t char_available; ///< check if a new character is available + get_byte_t get_byte; ///< get a new char void *periph; ///< pointer to parent implementation }; diff --git a/sw/airborne/mcu_periph/spi.h b/sw/airborne/mcu_periph/spi.h index 29c940552c..b09b222828 100644 --- a/sw/airborne/mcu_periph/spi.h +++ b/sw/airborne/mcu_periph/spi.h @@ -365,6 +365,7 @@ extern bool_t spi_slave_wait(struct spi_periph *p); /** @}*/ #if SPI_SLAVE_HS +#include "mcu_periph/spi_slave_hs_arch.h" extern void spi_slave_hs_init(void); #endif diff --git a/sw/airborne/mcu_periph/uart.c b/sw/airborne/mcu_periph/uart.c index 7cb034166e..7e1aa83750 100644 --- a/sw/airborne/mcu_periph/uart.c +++ b/sw/airborne/mcu_periph/uart.c @@ -201,8 +201,10 @@ void uart_periph_init(struct uart_periph *p) 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.put_byte = (put_byte_t)uart_transmit; p->device.send_message = (send_message_t)null_function; + p->device.char_available = (char_available_t)uart_char_available; + p->device.get_byte = (get_byte_t)uart_getch; #if PERIODIC_TELEMETRY // the first to register do it for the others @@ -226,7 +228,17 @@ uint8_t uart_getch(struct uart_periph *p) return ret; } +uint16_t uart_char_available(struct uart_periph *p) +{ + int16_t available = p->rx_insert_idx - p->rx_extract_idx; + if (available < 0) { + available += UART_RX_BUFFER_SIZE; + } + return (uint16_t)available; +} + void WEAK uart_event(void) { } + diff --git a/sw/airborne/mcu_periph/uart.h b/sw/airborne/mcu_periph/uart.h index b4c5ca0919..36834d8d3a 100644 --- a/sw/airborne/mcu_periph/uart.h +++ b/sw/airborne/mcu_periph/uart.h @@ -65,6 +65,8 @@ struct uart_periph { uint8_t tx_running; /** UART Register */ void *reg_addr; + /** UART Baudrate */ + int baudrate; /** UART Dev (linux) */ char dev[UART_DEV_NAME_SIZE]; volatile uint16_t ore; ///< overrun error counter @@ -88,126 +90,42 @@ extern void uart_event(void); * Check UART for available chars in receive buffer. * @return number of chars in the buffer */ -static inline uint16_t uart_char_available(struct uart_periph *p) -{ - int16_t available = p->rx_insert_idx - p->rx_extract_idx; - if (available < 0) { - available += UART_RX_BUFFER_SIZE; - } - return (uint16_t)available; -} +extern uint16_t uart_char_available(struct uart_periph *p); #if USE_UART0 extern struct uart_periph uart0; extern void uart0_init(void); - -#define UART0Init() uart_periph_init(&uart0) -#define UART0CheckFreeSpace(_x) uart_check_free_space(&uart0, _x) -#define UART0Transmit(_x) uart_transmit(&uart0, _x) -#define UART0SendMessage() {} -#define UART0ChAvailable() uart_char_available(&uart0) -#define UART0Getch() uart_getch(&uart0) -#define UART0TxRunning uart0.tx_running -#define UART0SetBaudrate(_b) uart_periph_set_baudrate(&uart0, _b) -#define UART0SetBitsStopParity(_b, _s, _p) uart_periph_set_bits_stop_parity(&uart0, _b, _s, _p) - #endif // USE_UART0 #if USE_UART1 extern struct uart_periph uart1; extern void uart1_init(void); - -#define UART1Init() uart_periph_init(&uart1) -#define UART1CheckFreeSpace(_x) uart_check_free_space(&uart1, _x) -#define UART1Transmit(_x) uart_transmit(&uart1, _x) -#define UART1SendMessage() {} -#define UART1ChAvailable() uart_char_available(&uart1) -#define UART1Getch() uart_getch(&uart1) -#define UART1TxRunning uart1.tx_running -#define UART1SetBaudrate(_b) uart_periph_set_baudrate(&uart1, _b) -#define UART1SetBitsStopParity(_b, _s, _p) uart_periph_set_bits_stop_parity(&uart1, _b, _s, _p) - #endif // USE_UART1 #if USE_UART2 extern struct uart_periph uart2; extern void uart2_init(void); - -#define UART2Init() uart_periph_init(&uart2) -#define UART2CheckFreeSpace(_x) uart_check_free_space(&uart2, _x) -#define UART2Transmit(_x) uart_transmit(&uart2, _x) -#define UART2SendMessage() {} -#define UART2ChAvailable() uart_char_available(&uart2) -#define UART2Getch() uart_getch(&uart2) -#define UART2TxRunning uart2.tx_running -#define UART2SetBaudrate(_b) uart_periph_set_baudrate(&uart2, _b) -#define UART2SetBitsStopParity(_b, _s, _p) uart_periph_set_bits_stop_parity(&uart2, _b, _s, _p) - #endif // USE_UART2 #if USE_UART3 extern struct uart_periph uart3; extern void uart3_init(void); - -#define UART3Init() uart_periph_init(&uart3) -#define UART3CheckFreeSpace(_x) uart_check_free_space(&uart3, _x) -#define UART3Transmit(_x) uart_transmit(&uart3, _x) -#define UART3SendMessage() {} -#define UART3ChAvailable() uart_char_available(&uart3) -#define UART3Getch() uart_getch(&uart3) -#define UART3TxRunning uart3.tx_running -#define UART3SetBaudrate(_b) uart_periph_set_baudrate(&uart3, _b) -#define UART3SetBitsStopParity(_b, _s, _p) uart_periph_set_bits_stop_parity(&uart3, _b, _s, _p) - #endif // USE_UART3 #if USE_UART4 extern struct uart_periph uart4; extern void uart4_init(void); - -#define UART4Init() uart_periph_init(&uart4) -#define UART4CheckFreeSpace(_x) uart_check_free_space(&uart4, _x) -#define UART4Transmit(_x) uart_transmit(&uart4, _x) -#define UART4SendMessage() {} -#define UART4ChAvailable() uart_char_available(&uart4) -#define UART4Getch() uart_getch(&uart4) -#define UART4TxRunning uart4.tx_running -#define UART4SetBaudrate(_b) uart_periph_set_baudrate(&uart4, _b) -#define UART4SetBitsStopParity(_b, _s, _p) uart_periph_set_bits_stop_parity(&uart4, _b, _s, _p) - #endif // USE_UART4 #if USE_UART5 extern struct uart_periph uart5; extern void uart5_init(void); - -#define UART5Init() uart_periph_init(&uart5) -#define UART5CheckFreeSpace(_x) uart_check_free_space(&uart5, _x) -#define UART5Transmit(_x) uart_transmit(&uart5, _x) -#define UART5SendMessage() {} -#define UART5ChAvailable() uart_char_available(&uart5) -#define UART5Getch() uart_getch(&uart5) -#define UART5TxRunning uart5.tx_running -#define UART5SetBaudrate(_b) uart_periph_set_baudrate(&uart5, _b) -#define UART5SetBitsStopParity(_b, _s, _p) uart_periph_set_bits_stop_parity(&uart5, _b, _s, _p) - #endif // USE_UART5 #if USE_UART6 extern struct uart_periph uart6; extern void uart6_init(void); - -#define UART6Init() uart_periph_init(&uart6) -#define UART6CheckFreeSpace(_x) uart_check_free_space(&uart6, _x) -#define UART6Transmit(_x) uart_transmit(&uart6, _x) -#define UART6SendMessage() {} -#define UART6ChAvailable() uart_char_available(&uart6) -#define UART6Getch() uart_getch(&uart6) -#define UART6TxRunning uart6.tx_running -#define UART6SetBaudrate(_b) uart_periph_set_baudrate(&uart6, _b) -#define UART6SetBitsStopParity(_b, _s, _p) uart_periph_set_bits_stop_parity(&uart6, _b, _s, _p) - #endif // USE_UART6 #endif /* MCU_PERIPH_UART_H */ diff --git a/sw/airborne/mcu_periph/udp.c b/sw/airborne/mcu_periph/udp.c index 0552b39290..d953a7de11 100644 --- a/sw/airborne/mcu_periph/udp.c +++ b/sw/airborne/mcu_periph/udp.c @@ -62,8 +62,10 @@ void udp_periph_init(struct udp_periph *p, char *host, int port_out, int port_in 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.put_byte = (put_byte_t) udp_transmit; p->device.send_message = (send_message_t) udp_send_message; + p->device.char_available = (char_available_t) udp_char_available; + p->device.get_byte = (get_byte_t) udp_getch; // 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 3024b8a8e2..21a06ef319 100644 --- a/sw/airborne/mcu_periph/udp.h +++ b/sw/airborne/mcu_periph/udp.h @@ -80,11 +80,6 @@ extern struct udp_periph udp0; #endif #define UDP0Init() udp_periph_init(&udp0, UDP0_HOST, UDP0_PORT_OUT, UDP0_PORT_IN, UDP0_BROADCAST) -#define UDP0CheckFreeSpace(_x) udp_check_free_space(&udp0, _x) -#define UDP0Transmit(_x) udp_transmit(&udp0, _x) -#define UDP0SendMessage() udp_send_message(&udp0) -#define UDP0ChAvailable() udp_char_available(&udp0) -#define UDP0Getch() udp_getch(&udp0) #endif // USE_UDP0 #if USE_UDP1 @@ -107,11 +102,6 @@ extern struct udp_periph udp1; #endif #define UDP1Init() udp_periph_init(&udp1, UDP1_HOST, UDP1_PORT_OUT, UDP1_PORT_IN, UDP1_BROADCAST) -#define UDP1CheckFreeSpace(_x) udp_check_free_space(&udp1, _x) -#define UDP1Transmit(_x) udp_transmit(&udp1, _x) -#define UDP1SendMessage() udp_send_message(&udp1) -#define UDP1ChAvailable() udp_char_available(&udp1) -#define UDP1Getch() udp_getch(&udp1) #endif // USE_UDP1 #if USE_UDP2 @@ -134,11 +124,6 @@ extern struct udp_periph udp2; #endif #define UDP2Init() udp_periph_init(&udp2, UDP2_HOST, UDP2_PORT_OUT, UDP2_PORT_IN, UDP2_BROADCAST) -#define UDP2CheckFreeSpace(_x) udp_check_free_space(&udp2, _x) -#define UDP2Transmit(_x) udp_transmit(&udp2, _x) -#define UDP2SendMessage() udp_send_message(&udp2) -#define UDP2ChAvailable() udp_char_available(&udp2) -#define UDP2Getch() udp_getch(&udp2) #endif // USE_UDP2 #endif /* MCU_PERIPH_UDP_H */ diff --git a/sw/airborne/mcu_periph/usb_serial.h b/sw/airborne/mcu_periph/usb_serial.h index ad54a885df..3d7ab5eb27 100644 --- a/sw/airborne/mcu_periph/usb_serial.h +++ b/sw/airborne/mcu_periph/usb_serial.h @@ -49,15 +49,4 @@ void VCOM_allow_linecoding(uint8_t mode); void VCOM_send_message(void); void VCOM_event(void); -/* - * Macros can be used in subsystems that normally work with serial ports - * e.g. use UsbS instead of UART1 - */ -#define UsbSInit() VCOM_init() -#define UsbSCheckFreeSpace(_x) VCOM_check_free_space(_x) -#define UsbSTransmit(_x) VCOM_putchar(_x) -#define UsbSSendMessage() VCOM_send_message() -#define UsbSGetch() VCOM_getchar() -#define UsbSChAvailable() VCOM_check_available() - #endif /* USB_S_H */ diff --git a/sw/airborne/modules/datalink/extra_pprz_dl.h b/sw/airborne/modules/datalink/extra_pprz_dl.h index 08e5a34497..0515a8aea6 100644 --- a/sw/airborne/modules/datalink/extra_pprz_dl.h +++ b/sw/airborne/modules/datalink/extra_pprz_dl.h @@ -37,7 +37,7 @@ extern struct pprz_transport extra_pprz_tp; /* Datalink Event */ #define ExtraDatalinkEvent() { \ - PprzCheckAndParse(EXTRA_PPRZ_UART, extra_pprz_tp); \ + PprzCheckAndParse(EXTRA_DOWNLINK_DEVICE, extra_pprz_tp); \ DlCheckAndParse(); \ } diff --git a/sw/airborne/modules/datalink/mavlink.c b/sw/airborne/modules/datalink/mavlink.c index f6f807152e..9e329b9753 100644 --- a/sw/airborne/modules/datalink/mavlink.c +++ b/sw/airborne/modules/datalink/mavlink.c @@ -143,8 +143,8 @@ void mavlink_event(void) mavlink_status_t status; // Check uplink - while (MAVLink(ChAvailable())) { - char test = MAVLink(Getch()); + while (MAVLinkChAvailable()) { + char test = MAVLinkGetch(); // When we receive a message if (mavlink_parse_char(MAVLINK_COMM_0, test, &msg, &status)) { @@ -158,7 +158,7 @@ void mavlink_event(void) mavlink_msg_request_data_stream_decode(&msg, &cmd); mavlink_msg_data_stream_send(MAVLINK_COMM_0, cmd.req_stream_id, 0, 0); - MAVLink(SendMessage()); + MAVLinkSendMessage(); break; } @@ -200,7 +200,7 @@ void mavlink_event(void) MAV_PARAM_TYPE_REAL32, NB_SETTING, cmd.param_index); - MAVLink(SendMessage()); + MAVLinkSendMessage(); break; } @@ -227,7 +227,7 @@ void mavlink_event(void) MAV_PARAM_TYPE_REAL32, NB_SETTING, idx); - MAVLink(SendMessage()); + MAVLinkSendMessage(); } } } @@ -244,7 +244,7 @@ void mavlink_event(void) msg.sysid, msg.compid, NB_WAYPOINT); - MAVLink(SendMessage()); + MAVLinkSendMessage(); } } break; @@ -268,7 +268,7 @@ void mavlink_event(void) WaypointX(req.seq), WaypointY(req.seq), WaypointAlt(req.seq)); - MAVLink(SendMessage()); + MAVLinkSendMessage(); } } } @@ -311,7 +311,7 @@ static inline void mavlink_send_heartbeat(void) mav_mode, 0, // custom_mode mav_state); - MAVLink(SendMessage()); + MAVLinkSendMessage(); } /** @@ -333,7 +333,7 @@ static inline void mavlink_send_sys_status(void) 0, // Autopilot specific error 2 0, // Autopilot specific error 3 0); // Autopilot specific error 4 - MAVLink(SendMessage()); + MAVLinkSendMessage(); } /** @@ -349,7 +349,7 @@ static inline void mavlink_send_attitude(void) stateGetBodyRates_f()->p, // p stateGetBodyRates_f()->q, // q stateGetBodyRates_f()->r); // r - MAVLink(SendMessage()); + MAVLinkSendMessage(); } static inline void mavlink_send_local_position_ned(void) @@ -362,7 +362,7 @@ static inline void mavlink_send_local_position_ned(void) stateGetSpeedNed_f()->x, stateGetSpeedNed_f()->y, stateGetSpeedNed_f()->z); - MAVLink(SendMessage()); + MAVLinkSendMessage(); } static inline void mavlink_send_global_position_int(void) @@ -384,7 +384,7 @@ static inline void mavlink_send_global_position_int(void) stateGetSpeedNed_f()->y * 100, stateGetSpeedNed_f()->z * 100, compass_heading); - MAVLink(SendMessage()); + MAVLinkSendMessage(); } static inline void mavlink_send_gps_global_origin(void) @@ -394,7 +394,7 @@ static inline void mavlink_send_gps_global_origin(void) state.ned_origin_i.lla.lat, state.ned_origin_i.lla.lon, state.ned_origin_i.hmsl); - MAVLink(SendMessage()); + MAVLinkSendMessage(); } } @@ -413,7 +413,7 @@ static inline void mavlink_send_params(void) MAV_PARAM_TYPE_REAL32, NB_SETTING, mavlink_params_idx); - MAVLink(SendMessage()); + MAVLinkSendMessage(); mavlink_params_idx++; } diff --git a/sw/airborne/modules/datalink/mavlink.h b/sw/airborne/modules/datalink/mavlink.h index 5621f684df..ac4c0145f3 100644 --- a/sw/airborne/modules/datalink/mavlink.h +++ b/sw/airborne/modules/datalink/mavlink.h @@ -46,15 +46,17 @@ extern mavlink_system_t mavlink_system; #ifndef MAVLINK_DEV -#define MAVLINK_DEV UART1 +#define MAVLINK_DEV uart1 #endif /* * The MAVLink link description */ -#define __MAVLink(dev, _x) dev##_x -#define _MAVLink(dev, _x) __MAVLink(dev, _x) -#define MAVLink(_x) _MAVLink(MAVLINK_DEV, _x) +#define MAVLinkDev (&(MAVLINK_DEV).device) +#define MAVLinkTransmit(c) MAVLinkDev->put_byte(MAVLinkDev->periph, c) +#define MAVLinkChAvailable() MAVLinkDev->char_available(MAVLinkDev->periph) +#define MAVLinkGetch() MAVLinkDev->get_byte(MAVLinkDev->periph) +#define MAVLinkSendMessage() MAVLinkDev->send_message(MAVLinkDev->periph) /** * Module functions @@ -72,7 +74,7 @@ void mavlink_event(void); static inline void comm_send_ch(mavlink_channel_t chan __attribute__((unused)), uint8_t ch) { // Send bytes - MAVLink(Transmit(ch)); + MAVLinkTransmit(ch); } #endif // DATALINK_MAVLINK_H diff --git a/sw/airborne/modules/datalink/mavlink_decoder.h b/sw/airborne/modules/datalink/mavlink_decoder.h index 2eda365779..ee0970d256 100644 --- a/sw/airborne/modules/datalink/mavlink_decoder.h +++ b/sw/airborne/modules/datalink/mavlink_decoder.h @@ -251,22 +251,21 @@ static inline void mavlink_parse_payload(struct mavlink_transport *t) } } - -#define MavlinkBuffer(_dev) TransportLink(_dev,ChAvailable()) -#define ReadMavlinkBuffer(_dev,_trans) { while (TransportLink(_dev,ChAvailable())&&!(_trans.trans.msg_received)) parse_mavlink(&(_trans),TransportLink(_dev,Getch())); } - -#define MavlinkCheckAndParse(_dev,_trans) { \ - if (MavlinkBuffer(_dev)) { \ - ReadMavlinkBuffer(_dev,_trans); \ - if (_trans.trans.msg_received) { \ - mavlink_parse_payload(&(_trans)); \ - _trans.trans.msg_received = FALSE; \ - } \ - } \ +static inline void mavlink_check_and_parse(struct link_device *dev, struct mavlink_transport *trans) +{ + if (dev->char_available(dev->periph)) { + while (dev->char_available(dev->periph) && !trans->trans.msg_received) { + parse_mavlink(trans, dev->get_byte(dev->periph)); + } + if (trans->trans.msg_received) { + mavlink_parse_payload(trans); + trans->trans.msg_received = FALSE; + } } +} /* Datalink Event Macro */ -#define MavlinkDatalinkEvent() MavlinkCheckAndParse(MAVLINK_UART, mavlink_tp) +#define MavlinkDatalinkEvent() mavlink_check_and_parse(&(MAVLINK_UART).device, &mavlink_tp) #endif /* MAVLINK_DECODER_H */ diff --git a/sw/airborne/modules/digital_cam/catia/protocol.c b/sw/airborne/modules/digital_cam/catia/protocol.c index 778412190d..647ae72e02 100644 --- a/sw/airborne/modules/digital_cam/catia/protocol.c +++ b/sw/airborne/modules/digital_cam/catia/protocol.c @@ -75,28 +75,3 @@ restart: t->status = UNINIT; return; } - - -/* -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]; - 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 PprzCheckAndParse(_dev,_trans) { \ - if (PprzBuffer(_dev)) { \ - ReadPprzBuffer(_dev,_trans); \ - if (_trans.trans.msg_received) { \ - pprz_parse_payload(&(_trans)); \ - _trans.trans.msg_received = FALSE; \ - } \ - } \ -} -*/ - - diff --git a/sw/airborne/modules/digital_cam/catia/protocol.h b/sw/airborne/modules/digital_cam/catia/protocol.h index 0b976d179b..8c06ff1cc8 100644 --- a/sw/airborne/modules/digital_cam/catia/protocol.h +++ b/sw/airborne/modules/digital_cam/catia/protocol.h @@ -102,20 +102,20 @@ extern uint8_t mora_ck_a, mora_ck_b; #define MoraPutUint8( _byte) { \ mora_ck_a += _byte; \ mora_ck_b += mora_ck_a; \ - CameraLink(Transmit(_byte)); \ + CameraLinkTransmit(_byte); \ } #define MoraHeader(msg_id, payload_len) { \ - CameraLink(Transmit(STX)); \ + CameraLinkTransmit(STX); \ uint8_t msg_len = MoraSizeOf( payload_len); \ - CameraLink(Transmit(msg_len)); \ + CameraLinkTransmit(msg_len); \ mora_ck_a = msg_len; mora_ck_b = msg_len; \ MoraPutUint8(msg_id); \ } #define MoraTrailer() { \ - CameraLink(Transmit(mora_ck_a)); \ - CameraLink(Transmit(mora_ck_b)); \ + CameraLinkTransmit(mora_ck_a); \ + CameraLinkTransmit(mora_ck_b); \ } #define MoraPut1ByteByAddr( _byte) { \ diff --git a/sw/airborne/modules/digital_cam/catia/std.h b/sw/airborne/modules/digital_cam/catia/std.h index 340a5a46c2..d7e635559c 100644 --- a/sw/airborne/modules/digital_cam/catia/std.h +++ b/sw/airborne/modules/digital_cam/catia/std.h @@ -4,12 +4,9 @@ #define TRUE (1==1) #define FALSE (1==0) -#define Transmit(_x) { \ +#define CameraLinkTransmit(_x) { \ uint8_t _tmp = _x; \ write(fd,&_tmp,1); \ } -#define CameraLink(_x) _x - - #endif diff --git a/sw/airborne/modules/digital_cam/uart_cam_ctrl.c b/sw/airborne/modules/digital_cam/uart_cam_ctrl.c index ea7df75918..1d397f34c1 100644 --- a/sw/airborne/modules/digital_cam/uart_cam_ctrl.c +++ b/sw/airborne/modules/digital_cam/uart_cam_ctrl.c @@ -45,11 +45,10 @@ #include "state.h" -#define __CameraLink(dev, _x) dev##_x -#define _CameraLink(dev, _x) __CameraLink(dev, _x) -#define CameraLink(_x) _CameraLink(CAMERA_LINK, _x) - -#define CameraBuffer() CameraLink(ChAvailable()) +#define CameraLinkDev (&(CAMERA_LINK).device) +#define CameraLinkTransmit(c) CameraLinkDev->put_byte(CameraLinkDev->periph, c) +#define CameraLinkChAvailable() CameraLinkDev->check_available(CameraLinkDev->periph) +#define CameraLinkGetch() CameraLinkGetch->get_byte(CameraLinkDev->periph) union dc_shot_union dc_shot_msg; union mora_status_union mora_status_msg; @@ -63,15 +62,15 @@ static uint8_t thumb_pointer = 0; -#define ReadCameraBuffer() { \ - while (CameraLink(ChAvailable())) \ - digital_cam_uart_parse(CameraLink(Getch())); \ +#define ReadCameraBuffer() { \ + while (CameraLinkChAvailable()) \ + digital_cam_uart_parse(CameraLinkGetch()); \ } void digital_cam_uart_event(void) { - while (CameraLink(ChAvailable())) { - parse_mora(&mora_protocol, CameraLink(Getch())); + while (CameraLinkChAvailable()) { + parse_mora(&mora_protocol, CameraLinkGetch()); if (mora_protocol.msg_received) { switch (mora_protocol.msg_id) { case MORA_STATUS: diff --git a/sw/airborne/modules/gps/gps_ubx_ucenter.c b/sw/airborne/modules/gps/gps_ubx_ucenter.c index b7cfce8a4d..f72cc0d9a3 100644 --- a/sw/airborne/modules/gps/gps_ubx_ucenter.c +++ b/sw/airborne/modules/gps/gps_ubx_ucenter.c @@ -27,8 +27,9 @@ * */ -#include "gps_ubx_ucenter.h" +#include "modules/gps/gps_ubx_ucenter.h" #include "subsystems/gps/gps_ubx.h" +#include "ubx_protocol.h" #include "subsystems/datalink/downlink.h" #include @@ -81,6 +82,8 @@ void gps_ubx_ucenter_init(void) for (int i = 0; i < GPS_UBX_UCENTER_CONFIG_STEPS; i++) { gps_ubx_ucenter.replies[i] = 0; } + + gps_ubx_ucenter.dev = &(GPS_LINK).device; } @@ -191,7 +194,7 @@ void gps_ubx_ucenter_event(void) */ static inline void gps_ubx_ucenter_config_port_poll(void) { - UbxSend_CFG_PRT_POLL(); + UbxSend_CFG_PRT_POLL(gps_ubx_ucenter.dev); } /** @@ -208,7 +211,7 @@ static inline void gps_ubx_ucenter_config_port_poll(void) */ static inline void gps_ubx_ucenter_enable_msg(uint8_t class, uint8_t id, uint8_t rate) { - UbxSend_CFG_MSG(class, id, rate); + UbxSend_CFG_MSG(gps_ubx_ucenter.dev, class, id, rate); } /** @@ -229,7 +232,7 @@ static bool_t gps_ubx_ucenter_autobaud(uint8_t nr) break; case 2: gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; - GpsUartSetBaudrate(B38400); // Try the most common first? + uart_periph_set_baudrate(&(GPS_LINK), B38400); // Try the most common first? gps_ubx_ucenter_config_port_poll(); break; case 3: @@ -238,7 +241,7 @@ static bool_t gps_ubx_ucenter_autobaud(uint8_t nr) return FALSE; } gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; - GpsUartSetBaudrate(B9600); // Maybe the factory default? + uart_periph_set_baudrate(&(GPS_LINK), B9600); // Maybe the factory default? gps_ubx_ucenter_config_port_poll(); break; case 4: @@ -247,7 +250,7 @@ static bool_t gps_ubx_ucenter_autobaud(uint8_t nr) return FALSE; } gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; - GpsUartSetBaudrate(B57600); // The high-rate default? + uart_periph_set_baudrate(&(GPS_LINK), B57600); // The high-rate default? gps_ubx_ucenter_config_port_poll(); break; case 5: @@ -256,7 +259,7 @@ static bool_t gps_ubx_ucenter_autobaud(uint8_t nr) return FALSE; } gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; - GpsUartSetBaudrate(B4800); // Default NMEA baudrate? + uart_periph_set_baudrate(&(GPS_LINK), B4800); // Default NMEA baudrate? gps_ubx_ucenter_config_port_poll(); break; case 6: @@ -265,7 +268,7 @@ static bool_t gps_ubx_ucenter_autobaud(uint8_t nr) return FALSE; } gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; - GpsUartSetBaudrate(B115200); // Last possible option for ublox + uart_periph_set_baudrate(&(GPS_LINK), B115200); // Last possible option for ublox gps_ubx_ucenter_config_port_poll(); break; case 7: @@ -277,7 +280,7 @@ static bool_t gps_ubx_ucenter_autobaud(uint8_t nr) // Autoconfig Failed... let's setup the failsafe baudrate // Should we try even a different baudrate? gps_ubx_ucenter.baud_init = 0; // Set as zero to indicate that we couldn't verify the baudrate - GpsUartSetBaudrate(B9600); + uart_periph_set_baudrate(&(GPS_LINK), B9600); return FALSE; default: break; @@ -330,12 +333,15 @@ static inline void gps_ubx_ucenter_config_nav(void) { //New ublox firmware v5 or higher uses CFG_NAV5 message, CFG_NAV is no longer available if (gps_ubx_ucenter.sw_ver_h < 5 && gps_ubx_ucenter.hw_ver_h < 6) { - UbxSend_CFG_NAV(NAV_DYN_AIRBORNE_2G, 3, 16, 24, 20, 5, 0, 0x3C, 0x3C, 0x14, 0x03E8 , 0x0000, 0x0, 0x17, 0x00FA, 0x00FA, - 0x0064, 0x012C, 0x000F, 0x00, 0x00); + UbxSend_CFG_NAV(gps_ubx_ucenter.dev, + NAV_DYN_AIRBORNE_2G, 3, 16, 24, 20, 5, 0, 0x3C, + 0x3C, 0x14, 0x03E8 , 0x0000, 0x0, 0x17, 0x00FA, 0x00FA, + 0x0064, 0x012C, 0x000F, 0x00, 0x00); } else { - UbxSend_CFG_NAV5(NAV5_MASK, GPS_UBX_NAV5_DYNAMICS, NAV5_3D_ONLY, IGNORED, IGNORED, NAV5_DEFAULT_MIN_ELEV, RESERVED, - NAV5_DEFAULT_PDOP_MASK, NAV5_DEFAULT_TDOP_MASK, NAV5_DEFAULT_P_ACC, NAV5_DEFAULT_T_ACC, - NAV5_DEFAULT_STATIC_HOLD_THRES, RESERVED, RESERVED, RESERVED, RESERVED); + UbxSend_CFG_NAV5(gps_ubx_ucenter.dev, + NAV5_MASK, GPS_UBX_NAV5_DYNAMICS, NAV5_3D_ONLY, IGNORED, IGNORED, NAV5_DEFAULT_MIN_ELEV, RESERVED, + NAV5_DEFAULT_PDOP_MASK, NAV5_DEFAULT_TDOP_MASK, NAV5_DEFAULT_P_ACC, NAV5_DEFAULT_T_ACC, + NAV5_DEFAULT_STATIC_HOLD_THRES, RESERVED, RESERVED, RESERVED, RESERVED); } } @@ -362,9 +368,7 @@ static inline void gps_ubx_ucenter_config_nav(void) #define GPS_PORT_SPI 0x04 #define GPS_PORT_RESERVED 0x05 -#define __UBX_GPS_BAUD(_u) _u##_BAUD -#define _UBX_GPS_BAUD(_u) __UBX_GPS_BAUD(_u) -#define UBX_GPS_BAUD _UBX_GPS_BAUD(GPS_LINK) +#define UBX_GPS_BAUD (GPS_LINK).baudrate #ifndef GPS_UBX_UCENTER_RATE #define GPS_UBX_UCENTER_RATE 0x00FA // In milliseconds. 0x00FA = 250ms = 4Hz @@ -376,7 +380,7 @@ static inline void gps_ubx_ucenter_config_port(void) // I2C Interface case GPS_PORT_DDC: #ifdef GPS_I2C - UbxSend_CFG_PRT(gps_ubx_ucenter.port_id, 0x0, 0x0, GPS_I2C_SLAVE_ADDR, 0x0, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0); + UbxSend_CFG_PRT(gps_ubx_ucenter.dev, gps_ubx_ucenter.port_id, 0x0, 0x0, GPS_I2C_SLAVE_ADDR, 0x0, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0); #else DEBUG_PRINT("WARNING: Please include the gps_i2c module.\n"); #endif @@ -384,12 +388,16 @@ static inline void gps_ubx_ucenter_config_port(void) // UART Interface case GPS_PORT_UART1: case GPS_PORT_UART2: - UbxSend_CFG_PRT(gps_ubx_ucenter.port_id, 0x0, 0x0, UBX_UART_MODE_MASK, UART_SPEED(UBX_GPS_BAUD), UBX_PROTO_MASK, - UBX_PROTO_MASK, 0x0, 0x0); + UbxSend_CFG_PRT(gps_ubx_ucenter.dev, + gps_ubx_ucenter.port_id, 0x0, 0x0, + UBX_UART_MODE_MASK, UART_SPEED(UBX_GPS_BAUD), UBX_PROTO_MASK, + UBX_PROTO_MASK, 0x0, 0x0); break; // USB Interface case GPS_PORT_USB: - UbxSend_CFG_PRT(gps_ubx_ucenter.port_id, 0x0, 0x0, 0x0, 0x0, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0); + UbxSend_CFG_PRT(gps_ubx_ucenter.dev, + gps_ubx_ucenter.port_id, 0x0, 0x0, 0x0, 0x0, + UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0); break; case GPS_PORT_SPI: DEBUG_PRINT("WARNING: ublox SPI port is currently not supported.\n"); @@ -413,7 +421,7 @@ static inline void gps_ubx_ucenter_config_port(void) static inline void gps_ubx_ucenter_config_sbas(void) { // Since March 2nd 2011 EGNOS is released for aviation purposes - UbxSend_CFG_SBAS(GPS_SBAS_ENABLED, GPS_SBAS_RANGING | GPS_SBAS_CORRECTIONS | GPS_SBAS_INTEGRITY, GPS_SBAS_MAX_SBAS, + UbxSend_CFG_SBAS(gps_ubx_ucenter.dev, GPS_SBAS_ENABLED, GPS_SBAS_RANGING | GPS_SBAS_CORRECTIONS | GPS_SBAS_INTEGRITY, GPS_SBAS_MAX_SBAS, GPS_SBAS_AUTOSCAN, GPS_SBAS_AUTOSCAN); //UbxSend_CFG_SBAS(0x00, 0x00, 0x00, 0x00, 0x00); } @@ -444,9 +452,9 @@ static bool_t gps_ubx_ucenter_configure(uint8_t nr) } #endif // Now the GPS baudrate should have changed - GpsUartSetBaudrate(UBX_GPS_BAUD); + uart_periph_set_baudrate(&(GPS_LINK), UBX_GPS_BAUD); gps_ubx_ucenter.baud_run = UART_SPEED(UBX_GPS_BAUD); - UbxSend_MON_GET_VER(); + UbxSend_MON_GET_VER(gps_ubx_ucenter.dev); break; case 2: case 3: @@ -503,7 +511,7 @@ static bool_t gps_ubx_ucenter_configure(uint8_t nr) break; case 14: // Poll Navigation/Measurement Rate Settings - UbxSend_CFG_RATE(GPS_UBX_UCENTER_RATE, 0x0001, 0x0000); + UbxSend_CFG_RATE(gps_ubx_ucenter.dev, GPS_UBX_UCENTER_RATE, 0x0001, 0x0000); break; case 15: // Raw Measurement Data @@ -519,7 +527,7 @@ static bool_t gps_ubx_ucenter_configure(uint8_t nr) break; case 17: // Try to save on non-ROM devices... - UbxSend_CFG_CFG(0x00000000, 0xffffffff, 0x00000000); + UbxSend_CFG_CFG(gps_ubx_ucenter.dev, 0x00000000, 0xffffffff, 0x00000000); break; case 18: #if DEBUG_GPS_UBX_UCENTER @@ -538,3 +546,4 @@ static bool_t gps_ubx_ucenter_configure(uint8_t nr) gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; return TRUE; // Continue, except for the last case } + diff --git a/sw/airborne/modules/gps/gps_ubx_ucenter.h b/sw/airborne/modules/gps/gps_ubx_ucenter.h index e068e15cbb..6d00bb94f6 100644 --- a/sw/airborne/modules/gps/gps_ubx_ucenter.h +++ b/sw/airborne/modules/gps/gps_ubx_ucenter.h @@ -29,6 +29,7 @@ #define GPS_UBX_UCENTER_H #include "std.h" +#include "mcu_periph/link_device.h" /** U-Center Variables */ #define GPS_UBX_UCENTER_CONFIG_STEPS 19 @@ -51,6 +52,9 @@ struct gps_ubx_ucenter_struct { uint8_t port_id; char replies[GPS_UBX_UCENTER_CONFIG_STEPS]; + + // Gps device + struct link_device *dev; }; extern struct gps_ubx_ucenter_struct gps_ubx_ucenter; diff --git a/sw/airborne/modules/gsm/gsm.c b/sw/airborne/modules/gsm/gsm.c index bd5fc57b0a..c89dd3a55b 100644 --- a/sw/airborne/modules/gsm/gsm.c +++ b/sw/airborne/modules/gsm/gsm.c @@ -58,9 +58,7 @@ Receiving: #include "gsm.h" #include "mcu_periph/uart.h" #include "std.h" -#include "mcu_periph/uart.h" #include "subsystems/datalink/downlink.h" -#include "ap_subsystems/datalink/downlink.h" #include "subsystems/gps.h" #include "autopilot.h" //#include "subsystems/navigation/common_nav.h" //why is should this be needed? @@ -72,14 +70,13 @@ Receiving: #define GSM_MAX_PAYLOAD 160 -#define __GSMLink(dev, _x) dev##_x -#define _GSMLink(dev, _x) __GSMLink(dev, _x) -#define GSMLink(_x) _GSMLink(GSM_LINK, _x) +#define GSMLinkDev (&(GSM_LINK).device) -#define GSMBuffer() GSMLink(ChAvailable()) -#define ReadGSMBuffer() { while (GSMLink(ChAvailable())&&!gsm_line_received) gsm_parse(GSMLink(Getch())); } +#define GSMLinkChAvailable() GSMLinkDev->check_available(GSMLinkDev->periph) +#define GSMLinkTransmit(_c) GSMLinkDev->put_byte(GSMLinkDev->periph, _c) +#define GSMLinkGetch() GSMLinkDev->get_byte(GSMLinkDev->periph) +#define ReadGSMBuffer() { while (GSMLinkChAvailable&&!gsm_line_received) gsm_parse(GSMLinkGetch()); } -#define GSMTransmit(_c) GSMLink(Transmit(_c)) #define CTRLZ 0x1A #define GSM_ORIGIN_MAXLEN 32 @@ -179,7 +176,7 @@ void gsm_init_report(void) /* Second call */ void gsm_event(void) { - if (GSMBuffer()) { + if (GSMLinkChAvailable()) { ReadGSMBuffer(); } diff --git a/sw/airborne/modules/ins/ahrs_chimu_spi.c b/sw/airborne/modules/ins/ahrs_chimu_spi.c index 40b705d18c..161312858e 100644 --- a/sw/airborne/modules/ins/ahrs_chimu_spi.c +++ b/sw/airborne/modules/ins/ahrs_chimu_spi.c @@ -81,8 +81,9 @@ void ahrs_chimu_init(void) void parse_ins_msg(void) { - while (InsLink(ChAvailable())) { - uint8_t ch = InsLink(Getch()); + struct link_device *dev = InsLinkDevice; + while (dev->char_available(dev->periph)) { + uint8_t ch = dev->get_byte(dev->periph); if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) { RunOnceEvery(25, LED_TOGGLE(3)); diff --git a/sw/airborne/modules/ins/ahrs_chimu_uart.c b/sw/airborne/modules/ins/ahrs_chimu_uart.c index b993ee3f92..8bd7045279 100644 --- a/sw/airborne/modules/ins/ahrs_chimu_uart.c +++ b/sw/airborne/modules/ins/ahrs_chimu_uart.c @@ -72,8 +72,9 @@ void ahrs_chimu_init(void) void parse_ins_msg(void) { - while (InsLink(ChAvailable())) { - uint8_t ch = InsLink(Getch()); + struct link_device *dev = InsLinkDevice; + while (dev->char_available(dev->periph)) { + uint8_t ch = dev->get_byte(dev->periph); if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) { if (CHIMU_DATA.m_MsgID == 0x03) { diff --git a/sw/airborne/modules/ins/ins_module.h b/sw/airborne/modules/ins/ins_module.h index ac71638289..3256e26626 100644 --- a/sw/airborne/modules/ins/ins_module.h +++ b/sw/airborne/modules/ins/ins_module.h @@ -71,33 +71,34 @@ void handle_ins_msg(void); void parse_ins_msg(void); void parse_ins_buffer(uint8_t); +#include "mcu_periph/link_device.h" + +#define InsLinkDevice (&((INS_LINK).device)) + #ifndef SITL #include "mcu_periph/uart.h" +#include "mcu_periph/spi.h" -#define __InsLink(dev, _x) dev##_x -#define _InsLink(dev, _x) __InsLink(dev, _x) -#define InsLink(_x) _InsLink(INS_LINK, _x) - -#define InsBuffer() InsLink(ChAvailable()) -#define ReadInsBuffer() { while (InsLink(ChAvailable())&&!ins_msg_received) parse_ins_buffer(InsLink(Getch())); } -#define InsSend1(c) InsLink(Transmit(c)) +#define InsSend1(c) InsLinkDevice->put_byte(InsLinkDevice->periph, c) #define InsUartSend1(c) InsSend1(c) #define InsSend(_dat,_len) { for (uint8_t i = 0; i< (_len); i++) InsSend1(_dat[i]); }; -#define InsUartSetBaudrate(_b) InsLink(SetBaudrate(_b)) -#define InsUartRunning InsLink(TxRunning) +#define InsUartSetBaudrate(_b) uart_periph_set_baudrate(INS_LINK, _b) #endif /** !SITL */ -#define InsEventCheckAndHandle(handler) { \ - if (InsBuffer()) { \ - ReadInsBuffer(); \ - } \ - if (ins_msg_received) { \ - parse_ins_msg(); \ - handler; \ - ins_msg_received = FALSE; \ - } \ +static inline void ins_event_check_and_handle(void (* handler)(void)) +{ + struct link_device *dev = InsLinkDevice; + if (dev->char_available(dev->periph)) { + while (dev->char_available(dev->periph) && !ins_msg_received) { + parse_ins_buffer(dev->get_byte(dev->periph)); + } } - + if (ins_msg_received) { + parse_ins_msg(); + handler(); + ins_msg_received = FALSE; + } +} #endif /* INS_MODULE_H */ diff --git a/sw/airborne/modules/ins/ins_xsens.h b/sw/airborne/modules/ins/ins_xsens.h index b662360ae0..ca0c46595f 100644 --- a/sw/airborne/modules/ins/ins_xsens.h +++ b/sw/airborne/modules/ins/ins_xsens.h @@ -80,7 +80,7 @@ extern struct ImuXsens imu_xsens; /* use Xsens as a full INS solution */ #if USE_INS_MODULE #define InsEvent(_ins_handler) { \ - InsEventCheckAndHandle(handle_ins_msg()) \ + ins_event_check_and_handle(handle_ins_msg); \ } #endif diff --git a/sw/airborne/modules/loggers/direct_memory_logger.c b/sw/airborne/modules/loggers/direct_memory_logger.c index 3d0dbcc6b8..4004025bfb 100644 --- a/sw/airborne/modules/loggers/direct_memory_logger.c +++ b/sw/airborne/modules/loggers/direct_memory_logger.c @@ -38,10 +38,6 @@ static int32_t seq_in_array(uint8_t *array, uint16_t array_size, uint8_t *sequen static uint8_t start_log_sequence[6] = {0xAA, 0x55, 0xFF, 0x00, 0x55, 0xAA}; static uint8_t stop_log_sequence[6] = {0xFF, 0x00, 0x55, 0xAA, 0x10, 0xFF}; -// The logging output connection -#define __DMLoggerLink(dev, _x) dev##_x -#define _DMLoggerLink(dev, _x) __DMLoggerLink(dev, _x) -#define DMLoggerLink(_x) _DMLoggerLink(DM_LOG_UART, _x) // Logging struct struct LogStruct { @@ -149,7 +145,7 @@ void direct_memory_logger_periodic(void) dml.status = DML_READING; case DML_READING: - if (DMLoggerLink(TxRunning) || dml.sst.status != SST25VFXXXX_IDLE) { + if (DM_LOG_UART.tx_running || dml.sst.status != SST25VFXXXX_IDLE) { break; } @@ -163,7 +159,7 @@ void direct_memory_logger_periodic(void) } for (i = 5; i < end_idx; i++) { - DMLoggerLink(Transmit(dml.buffer[i])); + uart_transmit(&DM_LOG_UART, dml.buffer[i]); } // Read next bytes diff --git a/sw/airborne/modules/max3100/max3100_hw.c b/sw/airborne/modules/max3100/max3100_hw.c deleted file mode 100644 index d4becb7f27..0000000000 --- a/sw/airborne/modules/max3100/max3100_hw.c +++ /dev/null @@ -1,173 +0,0 @@ -/* - * Copyright (C) 2009 ENAC - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#include "LPC21xx.h" -#include "armVIC.h" -#include "max3100_hw.h" - -#include "subsystems/datalink/downlink.h" -#include "mcu_periph/uart.h" - - -uint8_t volatile max3100_status; -bool volatile max3100_data_available; -bool volatile max3100_transmit_buffer_empty; - -uint8_t volatile max3100_tx_insert_idx, max3100_tx_extract_idx; -uint8_t volatile max3100_rx_insert_idx, max3100_rx_extract_idx; - -uint8_t volatile max3100_tx_buf[MAX3100_TX_BUF_LEN]; -uint8_t volatile max3100_rx_buf[MAX3100_RX_BUF_LEN]; - - -bool read_bytes = false; - - -static void EXTINT_ISR(void) __attribute__((naked)); -static void SPI1_ISR(void) __attribute__((naked)); - -#define PINSEL1_SCK (2 << 2) -#define PINSEL1_MISO (2 << 4) -#define PINSEL1_MOSI (2 << 6) -#define PINSEL1_SSEL (2 << 8) - -/* SSPCR0 settings */ -#define SSP_DSS 0x0F << 0 /* data size : 16 bits */ -// #define SSP_DSS 0x07 << 0 /* data size : 8 bits */ -#define SSP_FRF 0x00 << 4 /* frame format : SPI */ -#define SSP_CPOL 0x00 << 6 /* clock polarity : idle low */ -#define SSP_CPHA 0x00 << 7 /* clock phase : 0 */ -#define SSP_SCR 0x0F << 8 /* serial clock rate : 29.3kHz, SSP input clock / 16 */ - -/* SSPCR1 settings */ -#define SSP_LBM 0x00 << 0 /* loopback mode : disabled */ -#define SSP_SSE 0x00 << 1 /* SSP enable : disabled */ -#define SSP_MS 0x00 << 2 /* master slave mode : master */ -#define SSP_SOD 0x00 << 3 /* slave output disable : don't care when master */ - -#ifndef SSPCPSR_VAL -#define SSPCPSR_VAL 0x04 -#endif - -#warning "This driver should be updated to use the new SPI peripheral" - -#ifndef SPI1_VIC_SLOT -#define SPI1_VIC_SLOT 7 -#endif - - -void max3100_init(void) -{ - max3100_status = MAX3100_STATUS_IDLE; - max3100_data_available = false; - max3100_transmit_buffer_empty = true; - max3100_tx_insert_idx = 0; - max3100_tx_extract_idx = 0; - max3100_rx_insert_idx = 0; - max3100_rx_extract_idx = 0; - - /* setup pins for SSP (SCK, MISO, MOSI) */ - PINSEL1 |= PINSEL1_SCK | PINSEL1_MISO | PINSEL1_MOSI; - - /* setup SSP */ - SSPCR0 = SSP_DSS | SSP_FRF | SSP_CPOL | SSP_CPHA | SSP_SCR; - SSPCR1 = SSP_LBM | SSP_MS | SSP_SOD; - SSPCPSR = SSPCPSR_VAL; /* Prescaler */ - - - /* From arm7/max1167_hw.c */ - - /* SS pin is output */ - SetBit(MAX3100_SS_IODIR, MAX3100_SS_PIN); - /* unselected max3100 */ - Max3100Unselect(); - - /* connect extint (IRQ) */ - MAX3100_IRQ_PINSEL |= MAX3100_IRQ_PINSEL_VAL << MAX3100_IRQ_PINSEL_BIT; - /* extint is edge trigered */ - SetBit(EXTMODE, MAX3100_IRQ_EINT); - /* extint is trigered on falling edge */ - ClearBit(EXTPOLAR, MAX3100_IRQ_EINT); - /* clear pending extint0 before enabling interrupts */ - SetBit(EXTINT, MAX3100_IRQ_EINT); - - /* Configure interrupt vector for external pin interrupt */ - VICIntSelect &= ~VIC_BIT(MAX3100_VIC_EINT); // EXTINT selected as IRQ - VICIntEnable = VIC_BIT(MAX3100_VIC_EINT); // EXTINT interrupt enabled - VICVectCntl8 = VIC_ENABLE | MAX3100_VIC_EINT; - VICVectAddr8 = (uint32_t)EXTINT_ISR; // address of the ISR - - /* Configure interrupt vector for SPI */ - VICIntSelect &= ~VIC_BIT(VIC_SPI1); /* SPI1 selected as IRQ */ - VICIntEnable = VIC_BIT(VIC_SPI1); /* SPI1 interrupt enabled */ - _VIC_CNTL(SPI1_VIC_SLOT) = VIC_ENABLE | VIC_SPI1; - _VIC_CNTL(SPI1_VIC_SLOT) = (uint32_t)SPI1_ISR; /* address of the ISR */ - - /* Write configuration */ - //Max3100TransmitConf(MAX3100_BAUD_RATE | MAX3100_BIT_NOT_TM); - Max3100TransmitConf(MAX3100_BAUD_RATE | MAX3100_BIT_NOT_RM | MAX3100_BIT_NOT_TM); -} - - -/******* External interrupt: Data input available ***********/ -void EXTINT_ISR(void) -{ - ISR_ENTRY(); - - max3100_data_available = true; - - SetBit(EXTINT, MAX3100_IRQ_EINT); /* clear extint */ - VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */ - - ISR_EXIT(); -} - -void SPI1_ISR(void) -{ - ISR_ENTRY(); - - while (bit_is_set(SSPSR, RNE)) { - uint16_t data = SSPDR; - - if (bit_is_set(data, MAX3100_R_BIT)) { /* Data available */ - max3100_rx_buf[max3100_rx_insert_idx] = data & 0xff; - max3100_rx_insert_idx++; // automatic overflow because len=256 - read_bytes = true; - } - if (bit_is_set(data, MAX3100_T_BIT) && (max3100_status == MAX3100_STATUS_READING)) { /* transmit buffer empty */ - max3100_transmit_buffer_empty = true; - } - } - SpiClearRti(); /* clear interrupt */ - SpiDisableRti(); - SpiDisable(); - Max3100Unselect(); - max3100_status = MAX3100_STATUS_IDLE; - - VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */ - ISR_EXIT(); -} - -void max3100_debug(void) -{ - /*** DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice, 16, max3100_rx_buf); ***/ -} diff --git a/sw/airborne/modules/max3100/max3100_hw.h b/sw/airborne/modules/max3100/max3100_hw.h deleted file mode 100644 index bd73b15ecd..0000000000 --- a/sw/airborne/modules/max3100/max3100_hw.h +++ /dev/null @@ -1,172 +0,0 @@ -#ifndef MAX3100_H -#define MAX3100_H - -#include -#include "std.h" -#include "mcu_periph/spi_arch.h" -#include "led.h" - -/* Pin configuration for max3100 IRQ */ -// #define MAX3100_IRQ_PIN 7 -#if MAX3100_IRQ_PIN == 7 -#define MAX3100_IRQ_PINSEL PINSEL0 -#define MAX3100_IRQ_PINSEL_BIT 14 -#define MAX3100_IRQ_PINSEL_VAL 0x3 -#define MAX3100_IRQ_EINT 2 -#define MAX3100_VIC_EINT VIC_EINT2 -#elif MAX3100_IRQ_PIN == 16 -#define MAX3100_IRQ_PINSEL PINSEL1 -#define MAX3100_IRQ_PINSEL_BIT 0 -#define MAX3100_IRQ_PINSEL_VAL 0x1 -#define MAX3100_IRQ_EINT 0 -#define MAX3100_VIC_EINT VIC_EINT0 -#else -#error "Define MAX3100_IRQ_PIN" -#endif - -#define MAX3100_SS_PORT 0 -#define MAX3100_SS_PIN 20 - -#define MAX3100_IO__(port, reg) IO ## port ## reg -#define MAX3100_IO_(port, reg) MAX3100_IO__(port, reg) - -#define MAX3100_SS_IOCLR MAX3100_IO_(MAX3100_SS_PORT, CLR) -#define MAX3100_SS_IODIR MAX3100_IO_(MAX3100_SS_PORT, DIR) -#define MAX3100_SS_IOSET MAX3100_IO_(MAX3100_SS_PORT, SET) - -/** Max3100 protocol status */ -#define MAX3100_STATUS_IDLE 0 -#define MAX3100_STATUS_WRITING 1 -#define MAX3100_STATUS_READING 2 - -extern volatile uint8_t max3100_status; -extern volatile bool max3100_data_available; -extern volatile bool max3100_transmit_buffer_empty; // Max3100 ready to receive data on SPI - -/** I/O Buffers */ -#define MAX3100_TX_BUF_LEN 256 -#define MAX3100_RX_BUF_LEN 256 - -extern volatile uint8_t max3100_tx_insert_idx, max3100_tx_extract_idx; -extern volatile uint8_t max3100_rx_insert_idx, max3100_rx_extract_idx; - -extern volatile uint8_t max3100_tx_buf[MAX3100_TX_BUF_LEN]; -extern volatile uint8_t max3100_rx_buf[MAX3100_RX_BUF_LEN]; - -extern volatile uint8_t read_byte1, read_byte2; -extern bool read_bytes; - -#define Max3100Select() { \ - SetBit(MAX3100_SS_IOCLR, MAX3100_SS_PIN); \ - } - -#define Max3100Unselect() { \ - SetBit(MAX3100_SS_IOSET, MAX3100_SS_PIN); \ - } - -#define MAX3100_WRITE_CONF ((1U<<15) | (1U<<14)) -#define MAX3100_READ_CONF ((0U<<15) | (1U<<14)) -#define MAX3100_WRITE_DATA ((1U<<15) | (0U<<14)) -#define MAX3100_READ_DATA ((0U<<15) | (0U<<14)) - -/* Datasheet page 12 */ -#if MAX3100_FOSC == 1843200 -#define MAX3100_B115200 0x0 -#define MAX3100_B57600 0x1 -#define MAX3100_B19200 0x9 -#define MAX3100_B9600 0xA -#elif MAX3100_FOSC == 3686400 -#define MAX3100_B9600 0xB -#else -#error "MAX3100_FOSC must be defined to 1843200 or 3686400" -#endif - -#define MAX3100_BIT_NOT_RM (1U<<10) -#define MAX3100_BIT_NOT_TM (1U<<11) -#define MAX3100_BIT_NOT_FEN (1U<<13) -#define MAX3100_T_BIT 14 -#define MAX3100_R_BIT 15 - -/** Like UART macros */ -#define UART3100Init() {} /* Already initialized as a module */ -#define UART3100CheckFreeSpace(_len) (((int16_t)max3100_tx_extract_idx - max3100_tx_insert_idx + MAX3100_TX_BUF_LEN - 1) % MAX3100_TX_BUF_LEN >= _len) - -#define UART3100Transmit(_x) { max3100_putchar(_x); } -#define UART3100SendMessage() {} -#define UART3100Getch() ({\ - uint8_t ret = max3100_rx_buf[max3100_rx_extract_idx]; \ - max3100_rx_extract_idx++; /* Since size=256 */ \ - ret; \ - }) - -#define UART3100ChAvailable() (max3100_rx_extract_idx != max3100_rx_insert_idx) - -static inline void max3100_transmit(uint16_t data) -{ - Max3100Select(); - SpiClearRti(); - SpiEnableRti(); /* enable rx fifo time out */ - SpiEnable(); - // SSPDR = data >> 8; - // SSPDR = data & 0xff; - SSPDR = data; -} - -#define Max3100TransmitConf(_conf) max3100_transmit((_conf) | MAX3100_WRITE_CONF) -#define Max3100TransmitData(_data) max3100_transmit((_data) | MAX3100_WRITE_DATA) -#define Max3100ReadData() max3100_transmit(MAX3100_READ_DATA) - - -static inline void max3100_read_data(void) -{ - Max3100ReadData(); - max3100_status = MAX3100_STATUS_READING; -} - -static inline void max3100_flush(void) -{ - if (max3100_status == MAX3100_STATUS_IDLE - && max3100_tx_extract_idx != max3100_tx_insert_idx - && max3100_transmit_buffer_empty) { - Max3100TransmitData(max3100_tx_buf[max3100_tx_extract_idx]); - max3100_tx_extract_idx++; /* automatic overflow since len=256 */ - max3100_status = MAX3100_STATUS_WRITING; - max3100_transmit_buffer_empty = false; - } -} - -/** Warning: No bufferring; SPI must be available */ -static inline void max3100_putconfchar(char c) -{ - Max3100TransmitConf(c); - max3100_status = MAX3100_STATUS_WRITING; -} - -static inline void max3100_putchar(char c) -{ - max3100_tx_buf[max3100_tx_insert_idx] = c; - max3100_tx_insert_idx++; /* automatic overflow since len=256 */ - /* flushed in the next event */ -} - -extern void max3100_init(void); -extern void max3100_debug(void); - -static inline void max3100_event(void) -{ - if (read_bytes) { - read_bytes = false; - max3100_debug(); - } - if (max3100_status == MAX3100_STATUS_IDLE) { - if (max3100_data_available) { - max3100_data_available = false; - max3100_read_data(); - } else { - max3100_flush(); - } - } -} - - -#endif /* MAX3100_H */ diff --git a/sw/airborne/modules/max7456/max7456.c b/sw/airborne/modules/max7456/max7456.c index 13919ca2cf..05a8d497b8 100644 --- a/sw/airborne/modules/max7456/max7456.c +++ b/sw/airborne/modules/max7456/max7456.c @@ -26,24 +26,25 @@ */ #include "std.h" -#include "stdio.h" +//#include "stdio.h" #include "mcu_periph/sys_time.h" #include "mcu_periph/gpio.h" #include "mcu_periph/spi.h" -#include "led.h" -#include "autopilot.h" -#include "firmwares/fixedwing/nav.h" #include "generated/flight_plan.h" #include "generated/airframe.h" -#include "subsystems/datalink/datalink.h" +#include "autopilot.h" #include "subsystems/electrical.h" - -#include "messages.h" -#include "subsystems/datalink/downlink.h" #include "state.h" +// for GetPosAlt, include correct header until we have unified API +#ifdef AP +#include "subsystems/navigation/nav.h" +#else +#include "firmwares/rotorcraft/navigation.h" +#endif + // Peripherials #include "max7456.h" #include "max7456_regs.h" diff --git a/sw/airborne/modules/sensors/airspeed_otf.c b/sw/airborne/modules/sensors/airspeed_otf.c index 94f7ff147c..5888dc7d65 100644 --- a/sw/airborne/modules/sensors/airspeed_otf.c +++ b/sw/airborne/modules/sensors/airspeed_otf.c @@ -153,8 +153,8 @@ void airspeed_otf_init(void) void airspeed_otf_event(void) { - while (MetLink(ChAvailable())) { - uint8_t ch = MetLink(Getch()); + while (MetBuffer()) { + uint8_t ch = MetGetch(); airspeed_otf_parse(ch); } } diff --git a/sw/airborne/modules/sensors/met_module.h b/sw/airborne/modules/sensors/met_module.h index b6bd3b738c..98dd020787 100644 --- a/sw/airborne/modules/sensors/met_module.h +++ b/sw/airborne/modules/sensors/met_module.h @@ -37,17 +37,16 @@ #ifndef SITL #include "mcu_periph/uart.h" -#define __MetLink(dev, _x) dev##_x -#define _MetLink(dev, _x) __MetLink(dev, _x) -#define MetLink(_x) _MetLink(MET_LINK, _x) +#define MetLinkDevice (&(MET_LINK).device) -#define MetBuffer() MetLink(ChAvailable()) -#define ReadMetBuffer() { while (MetLink(ChAvailable())&&!met_msg_received) parse_met_buffer(MetLink(Getch())); } -#define MetSend1(c) MetLink(Transmit(c)) +#define MetBuffer() MetLinkDevice->char_available(MetLinkDevice->periph) +#define MetGetch() MetLinkDevice->get_byte(MetLinkDevice->periph) +#define ReadMetBuffer() { while (MetBuffer()&&!met_msg_received) parse_met_buffer(MetGetch()); } +#define MetSend1(c) MetLinkDevice->put_byte(MetLinkDevice->periph, c) #define MetUartSend1(c) MetSend1(c) #define MetSend(_dat,_len) { for (uint8_t i = 0; i< (_len); i++) MetSend1(_dat[i]); }; -#define MetUartSetBaudrate(_b) MetLink(SetBaudrate(_b)) -#define MetUartRunning MetLink(TxRunning) +#define MetUartSetBaudrate(_b) uart_periph_set_baudrate(&(MET_LINK), _b) +#define MetUartRunning (MET_LINK).tx_running #endif /** !SITL */ diff --git a/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.c b/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.c index f8bd288963..da43b645ba 100644 --- a/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.c +++ b/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.c @@ -75,6 +75,7 @@ static void sdlog_transmit(struct chibios_sdlog* p __attribute__((unused)), uint static void sdlog_send(struct chibios_sdlog* p __attribute__((unused))) { } +static int null_function(struct chibios_sdlog *p __attribute__((unused))) { return 0; } bool_t chibios_logInit(const bool_t binaryFile) { @@ -84,8 +85,10 @@ bool_t chibios_logInit(const bool_t binaryFile) // 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.put_byte = (put_byte_t) sdlog_transmit; chibios_sdlog.device.send_message = (send_message_t) sdlog_send; + chibios_sdlog.device.char_available = (char_available_t) null_function; // write only + chibios_sdlog.device.get_byte = (get_byte_t) null_function; // write only 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 03526c39fd..c3b106af46 100644 --- a/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.h +++ b/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.h @@ -55,9 +55,4 @@ struct chibios_sdlog { extern struct chibios_sdlog chibios_sdlog; -/** Paparazzi datalink API */ -#define SDLOGCheckFreeSpace(_x) (true) -#define SDLOGTransmit(_x) sdLogWriteByte(&pprzLogFile, _x) -#define SDLOGSendMessage() {} - #endif diff --git a/sw/airborne/subsystems/datalink/downlink.h b/sw/airborne/subsystems/datalink/downlink.h index 97a98d4096..816725db8e 100644 --- a/sw/airborne/subsystems/datalink/downlink.h +++ b/sw/airborne/subsystems/datalink/downlink.h @@ -37,15 +37,8 @@ #include "generated/airframe.h" // AC_ID is required #if defined SITL - -#ifdef SIM_UART -#include "sim_uart.h" -#include "subsystems/datalink/pprz_transport.h" -#include "subsystems/datalink/xbee.h" -#else /* SIM_UART */ /** Software In The Loop simulation uses IVY bus directly as the transport layer */ #include "ivy_transport.h" -#endif #else /** SITL */ diff --git a/sw/airborne/subsystems/datalink/ivy_transport.c b/sw/airborne/subsystems/datalink/ivy_transport.c index 6b98d470ef..b3eab14ccc 100644 --- a/sw/airborne/subsystems/datalink/ivy_transport.c +++ b/sw/airborne/subsystems/datalink/ivy_transport.c @@ -179,6 +179,7 @@ static int check_available_space(struct ivy_transport *trans __attribute__((unus static int check_free_space(struct ivy_transport *p __attribute__((unused)), uint8_t len __attribute__((unused))) { return TRUE; } static void transmit(struct ivy_transport *p __attribute__((unused)), uint8_t byte __attribute__((unused))) {} static void send_message(struct ivy_transport *p __attribute__((unused))) {} +static int null_function(struct ivy_transport *p __attribute__((unused))) { return 0; } void ivy_transport_init(void) { @@ -195,7 +196,9 @@ void ivy_transport_init(void) 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.put_byte = (put_byte_t) transmit; ivy_tp.device.send_message = (send_message_t) send_message; + ivy_tp.device.char_available = (char_available_t) null_function; + ivy_tp.device.get_byte = (get_byte_t) null_function; ivy_tp.device.periph = (void *)(&ivy_tp); } diff --git a/sw/airborne/subsystems/datalink/pprz_transport.c b/sw/airborne/subsystems/datalink/pprz_transport.c index e995397c0b..375cb63cbc 100644 --- a/sw/airborne/subsystems/datalink/pprz_transport.c +++ b/sw/airborne/subsystems/datalink/pprz_transport.c @@ -52,7 +52,7 @@ static void put_1byte(struct pprz_transport *trans, struct link_device *dev, con { trans->ck_a_tx += byte; trans->ck_b_tx += trans->ck_a_tx; - dev->transmit(dev->periph, byte); + dev->put_byte(dev->periph, byte); } static void put_bytes(struct pprz_transport *trans, struct link_device *dev, @@ -82,17 +82,17 @@ static uint8_t size_of(struct pprz_transport *trans __attribute__((unused)), uin static void start_message(struct pprz_transport *trans, struct link_device *dev, uint8_t payload_len) { downlink.nb_msgs++; - dev->transmit(dev->periph, STX); + dev->put_byte(dev->periph, STX); const uint8_t msg_len = size_of(trans, payload_len); - dev->transmit(dev->periph, msg_len); + dev->put_byte(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->put_byte(dev->periph, trans->ck_a_tx); + dev->put_byte(dev->periph, trans->ck_b_tx); dev->send_message(dev->periph); } diff --git a/sw/airborne/subsystems/datalink/pprz_transport.h b/sw/airborne/subsystems/datalink/pprz_transport.h index 04ecc41bd4..1ddb9f7724 100644 --- a/sw/airborne/subsystems/datalink/pprz_transport.h +++ b/sw/airborne/subsystems/datalink/pprz_transport.h @@ -140,24 +140,20 @@ static inline void pprz_parse_payload(struct pprz_transport *t) } -#define PprzBuffer(_dev) TransportLink(_dev,ChAvailable()) +#define PprzCheckAndParse(_dev, _trans) pprz_check_and_parse(&(_dev).device, &(_trans)) -#define ReadPprzBuffer(_dev,_trans) { \ - while (TransportLink(_dev,ChAvailable()) && !(_trans.trans_rx.msg_received)) { \ - parse_pprz(&(_trans),TransportLink(_dev,Getch())); \ - } \ +static inline void pprz_check_and_parse(struct link_device *dev, struct pprz_transport *trans) +{ + if (dev->char_available(dev->periph)) { + while (dev->char_available(dev->periph) && !trans->trans_rx.msg_received) { + parse_pprz(trans, dev->get_byte(dev->periph)); + } + if (trans->trans_rx.msg_received) { + pprz_parse_payload(trans); + trans->trans_rx.msg_received = FALSE; + } } - -#define PprzCheckAndParse(_dev,_trans) { \ - if (PprzBuffer(_dev)) { \ - ReadPprzBuffer(_dev,_trans); \ - if (_trans.trans_rx.msg_received) { \ - pprz_parse_payload(&(_trans)); \ - _trans.trans_rx.msg_received = FALSE; \ - } \ - } \ - } - +} #endif /* PPRZ_TRANSPORT_H */ diff --git a/sw/airborne/subsystems/datalink/pprzlog_transport.c b/sw/airborne/subsystems/datalink/pprzlog_transport.c index 1fa3b170b3..0a47cdff2c 100644 --- a/sw/airborne/subsystems/datalink/pprzlog_transport.c +++ b/sw/airborne/subsystems/datalink/pprzlog_transport.c @@ -51,7 +51,7 @@ struct pprzlog_transport pprzlog_tp; static void put_1byte(struct pprzlog_transport *trans, struct link_device *dev, const uint8_t byte) { trans->ck += byte; - dev->transmit(dev->periph, byte); + dev->put_byte(dev->periph, byte); } static void put_bytes(struct pprzlog_transport *trans, struct link_device *dev, @@ -79,7 +79,7 @@ static uint8_t size_of(struct pprzlog_transport *trans __attribute__((unused)), static void start_message(struct pprzlog_transport *trans, struct link_device *dev, uint8_t payload_len) { - dev->transmit(dev->periph, STX_LOG); + dev->put_byte(dev->periph, STX_LOG); const uint8_t msg_len = size_of(trans, payload_len); trans->ck = 0; put_1byte(trans, dev, msg_len); @@ -90,7 +90,7 @@ static void start_message(struct pprzlog_transport *trans, struct link_device *d static void end_message(struct pprzlog_transport *trans, struct link_device *dev) { - dev->transmit(dev->periph, trans->ck); + dev->put_byte(dev->periph, trans->ck); dev->send_message(dev->periph); } diff --git a/sw/airborne/subsystems/datalink/superbitrf.c b/sw/airborne/subsystems/datalink/superbitrf.c index 5edba4b0fe..eecdbfd177 100644 --- a/sw/airborne/subsystems/datalink/superbitrf.c +++ b/sw/airborne/subsystems/datalink/superbitrf.c @@ -218,6 +218,8 @@ static void superbitrf_transmit(struct SuperbitRF *p, uint8_t byte) static void superbitrf_send(struct SuperbitRF *p __attribute__((unused))) { } +static int null_function(struct SuperbitRF *p __attribute__((unused))) { return 0; } + /** * Initialize the superbitrf */ @@ -241,8 +243,10 @@ void superbitrf_init(void) // 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.put_byte = (put_byte_t) superbitrf_transmit; superbitrf.device.send_message = (send_message_t) superbitrf_send; + superbitrf.device.char_available = (char_available_t) null_function; // not needed + superbitrf.device.get_byte = (get_byte_t) null_function; // not needed // Initialize the binding pin gpio_setup_input(SPEKTRUM_BIND_PIN_PORT, SPEKTRUM_BIND_PIN); diff --git a/sw/airborne/subsystems/datalink/superbitrf.h b/sw/airborne/subsystems/datalink/superbitrf.h index 46ecfdc1a3..fd99e325a4 100644 --- a/sw/airborne/subsystems/datalink/superbitrf.h +++ b/sw/airborne/subsystems/datalink/superbitrf.h @@ -125,14 +125,6 @@ extern void superbitrf_event(void); extern void superbitrf_set_mfg_id(uint32_t id); extern void superbitrf_set_protocol(uint8_t protocol); -/* The datalink defines */ -#define SuperbitRFInit() { }//superbitrf_init(); } -#define SuperbitRFCheckFreeSpace(_x) (((superbitrf.tx_insert_idx+1) %SUPERBITRF_TX_BUFFER_SIZE) != superbitrf.tx_extract_idx) -#define SuperbitRFTransmit(_x) { \ - superbitrf.tx_buffer[superbitrf.tx_insert_idx] = _x; \ - superbitrf.tx_insert_idx = (superbitrf.tx_insert_idx+1) %SUPERBITRF_TX_BUFFER_SIZE; \ - } -#define SuperbitRFSendMessage() { } #define SuperbitRFCheckAndParse() { } #endif /* DATALINK_SUPERBITRF_H */ diff --git a/sw/airborne/subsystems/datalink/transport.h b/sw/airborne/subsystems/datalink/transport.h index 8887fc5e21..a1701954a2 100644 --- a/sw/airborne/subsystems/datalink/transport.h +++ b/sw/airborne/subsystems/datalink/transport.h @@ -43,16 +43,6 @@ struct transport_rx { uint8_t ovrn, error; ///< overrun and error flags }; -/** Transport link macros - * - * make the link between the transport layer - * and the device layer - */ -#define __TransportLink(dev, _x) dev##_x -#define _TransportLink(dev, _x) __TransportLink(dev, _x) -#define TransportLink(_dev, _x) _TransportLink(_dev, _x) - - /** Data type */ enum TransportDataType { diff --git a/sw/airborne/subsystems/datalink/uart_print.h b/sw/airborne/subsystems/datalink/uart_print.h index 20928dcf7b..97f16513dd 100644 --- a/sw/airborne/subsystems/datalink/uart_print.h +++ b/sw/airborne/subsystems/datalink/uart_print.h @@ -1,5 +1,6 @@ /* * Copyright (C) 2005 Pascal Brisset, Antoine Drouin + * Copyright (C) 2015 Gautier Hattenberger * * This file is part of paparazzi. * @@ -24,38 +25,75 @@ #include "mcu_periph/uart.h" #include "mcu_periph/usb_serial.h" +#include "mcu_periph/link_device.h" -#define _PrintString(out_fun, s) { \ +#define _PrintString(out_fun, s) { \ uint8_t i = 0; \ while (s[i]) { \ - out_fun(s[i]); \ + out_fun(s[i]); \ i++; \ } \ } -#define _PrintHex(out_fun, c) { \ +static inline void print_string(struct link_device *dev, char *s) +{ + uint8_t i = 0; + while (s[i]) { + dev->put_byte(dev->periph, s[i]); + i++; + } +} + +#define _PrintHex(out_fun, c) { \ const uint8_t hex[16] = { '0', '1', '2', '3', '4', '5', '6', '7', \ '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' }; \ - uint8_t high = (c & 0xF0)>>4; \ - uint8_t low = c & 0x0F; \ + uint8_t high = (c & 0xF0)>>4; \ + uint8_t low = c & 0x0F; \ out_fun(hex[high]); \ out_fun(hex[low]); \ - } \ - -#define _PrintHex16(out_fun, c ) { \ - uint8_t high16 = (uint8_t)(c>>8); \ - uint8_t low16 = (uint8_t)(c); \ + } + +static inline void print_hex(struct link_device *dev, uint8_t c) +{ + const uint8_t hex[16] = + { '0', '1', '2', '3', '4', '5', '6', '7', + '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' }; + uint8_t high = (c & 0xF0)>>4; + uint8_t low = c & 0x0F; + dev->put_byte(dev->periph, hex[high]); + dev->put_byte(dev->periph, hex[low]); +} + +#define _PrintHex16(out_fun, c ) { \ + uint8_t high16 = (uint8_t)(c>>8); \ + uint8_t low16 = (uint8_t)(c); \ _PrintHex(out_fun, high16); \ _PrintHex(out_fun, low16); \ } -#define _PrintHex32(out_fun, c ) { \ - uint16_t high32 = (uint16_t)(c>>16); \ +static inline void print_hex16(struct link_device *dev, uint16_t c) +{ + uint8_t high16 = (uint8_t)(c>>8); + uint8_t low16 = (uint8_t)(c); + print_hex(dev, high16); + print_hex(dev, low16); +} + +#define _PrintHex32(out_fun, c ) { \ + uint16_t high32 = (uint16_t)(c>>16); \ uint16_t low32 = (uint16_t)(c); \ - _PrintHex16(out_fun, high32); \ - _PrintHex16(out_fun, low32); \ + _PrintHex16(out_fun, high32); \ + _PrintHex16(out_fun, low32); \ } +static inline void print_hex32(struct link_device *dev, uint32_t c) +{ + uint16_t high32 = (uint16_t)(c>>16); + uint16_t low32 = (uint16_t)(c); + print_hex16(dev, high32); + print_hex16(dev, low32); +} + #if USE_UART0 #define UART0PrintHex(c) _PrintHex(UART0Transmit, c) diff --git a/sw/airborne/subsystems/datalink/w5100.c b/sw/airborne/subsystems/datalink/w5100.c index 1b4e38772a..4b040e0488 100644 --- a/sw/airborne/subsystems/datalink/w5100.c +++ b/sw/airborne/subsystems/datalink/w5100.c @@ -172,6 +172,13 @@ static inline uint16_t w5100_sock_get16(uint8_t _sock, uint16_t _reg) 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(); } +static int dev_char_available(struct w5100_periph *p __attribute__((unused))) { return w5100_ch_available; } +static uint8_t dev_getch(struct w5100_periph *p __attribute__((unused))) +{ + uint8_t c = 0; + w5100_receive(&c, 1); + return c; +} void w5100_init(void) { @@ -244,8 +251,10 @@ void w5100_init(void) // 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.put_byte = (put_byte_t) dev_transmit; chip0.device.send_message = (send_message_t) dev_send; + chip0.device.char_available = (char_available_t) dev_char_available; + chip0.device.get_byte = (get_byte_t) dev_getch; } void w5100_transmit(uint8_t data) diff --git a/sw/airborne/subsystems/datalink/w5100.h b/sw/airborne/subsystems/datalink/w5100.h index 2664fa5a33..e5b3cbc87d 100644 --- a/sw/airborne/subsystems/datalink/w5100.h +++ b/sw/airborne/subsystems/datalink/w5100.h @@ -70,17 +70,6 @@ void w5100_send(void); uint16_t w5100_rx_size(uint8_t _s); bool_t w5100_ch_available(void); -// Defines that are done in mcu_periph on behalf of uart. -// We need to do these here... -#define W5100Init() w5100_init() -#define W5100CheckFreeSpace(_x) (TRUE) // w5100_check_free_space(_x) -#define W5100Transmit(_x) w5100_transmit(_x) -#define W5100SendMessage() w5100_send() -#define W5100ChAvailable() w5100_ch_available() -#define W5100Getch() w5100_getch() -#define W5100TxRunning chip0.tx_running -#define W5100SetBaudrate(_b) w5100_set_baudrate(_b) - // W5100 is using pprz_transport // FIXME it should not appear here, this will be fixed with the rx improvements some day... @@ -98,18 +87,17 @@ static inline void w5100_read_buffer(struct pprz_transport *t) } } -#define W5100Buffer(_dev) TransportLink(_dev,ChAvailable()) +#define W5100CheckAndParse(_dev, _trans) w5100_check_and_parse(&(_dev).device, &(_trans)) -#define W5100CheckAndParse(_dev,_trans) { \ - if (W5100Buffer(_dev)) { \ - w5100_read_buffer( &(_trans) ); \ - if (_trans.trans_rx.msg_received) { \ - pprz_parse_payload(&(_trans)); \ - _trans.trans_rx.msg_received = FALSE; \ - } \ - } \ +static inline void w5100_check_and_parse(struct link_device *dev, struct pprz_transport *trans) { + if (dev->char_available(dev->periph)) { + w5100_read_buffer(trans); + if (trans->trans_rx.msg_received) { + pprz_parse_payload(trans); + trans->trans_rx.msg_received = FALSE; + } } - +} #endif /* W5100_H */ diff --git a/sw/airborne/subsystems/datalink/xbee.c b/sw/airborne/subsystems/datalink/xbee.c index 459e415204..c266de1735 100644 --- a/sw/airborne/subsystems/datalink/xbee.c +++ b/sw/airborne/subsystems/datalink/xbee.c @@ -30,10 +30,6 @@ #include "subsystems/datalink/xbee.h" #include "subsystems/datalink/downlink.h" -#ifdef SIM_UART -#include "sim_uart.h" -#endif - /** Ground station address */ #define GROUND_STATION_ADDR 0x100 /** Aircraft address */ @@ -56,7 +52,7 @@ struct xbee_transport xbee_tp; 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); + dev->put_byte(dev->periph, byte); } static void put_bytes(struct xbee_transport *trans, struct link_device *dev, @@ -86,10 +82,10 @@ static uint8_t size_of(struct xbee_transport *trans __attribute__((unused)), uin 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); + dev->put_byte(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)); + dev->put_byte(dev->periph, (len >> 8)); + dev->put_byte(dev->periph, (len & 0xff)); trans->cs_tx = 0; const uint8_t header[] = XBEE_TX_HEADER; put_bytes(trans, dev, DL_TYPE_UINT8, DL_FORMAT_SCALAR, XBEE_TX_OVERHEAD + 1, header); @@ -98,7 +94,7 @@ static void start_message(struct xbee_transport *trans, struct link_device *dev, 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->put_byte(dev->periph, trans->cs_tx); dev->send_message(dev->periph); } @@ -120,13 +116,13 @@ static int check_available_space(struct xbee_transport *trans __attribute__((unu return dev->check_free_space(dev->periph, bytes); } -static uint8_t xbee_text_reply_is_ok(void) +static uint8_t xbee_text_reply_is_ok(struct link_device *dev) { char c[2]; int count = 0; - while (TransportLink(XBEE_UART, ChAvailable())) { - char cc = TransportLink(XBEE_UART, Getch()); + while (dev->char_available(dev->periph)) { + char cc = dev->get_byte(dev->periph); if (count < 2) { c[count] = cc; } @@ -140,20 +136,18 @@ static uint8_t xbee_text_reply_is_ok(void) return FALSE; } -static uint8_t xbee_try_to_enter_api(void) +static uint8_t xbee_try_to_enter_api(struct link_device *dev) { /** Switching to AT mode (FIXME: busy waiting) */ - XBeePrintString(XBEE_UART, AT_COMMAND_SEQUENCE); + print_string(dev, AT_COMMAND_SEQUENCE); /** - busy wait 1.25s */ sys_time_usleep(1250000); - return xbee_text_reply_is_ok(); + return xbee_text_reply_is_ok(dev); } -#define XBeeUartSetBaudrate(_a) TransportLink(XBEE_UART,SetBaudrate(_a)) - #if XBEE_BAUD == B9600 #define XBEE_BAUD_ALTERNATE B57600 @@ -182,30 +176,32 @@ void xbee_init(void) xbee_tp.trans_tx.count_bytes = (count_bytes_t) count_bytes; xbee_tp.trans_tx.impl = (void *)(&xbee_tp); + struct link_device *dev = &((XBEE_UART).device); + // Empty buffer before init process - while (TransportLink(XBEE_UART, ChAvailable())) { - TransportLink(XBEE_UART, Getch()); + while (dev->char_available(dev->periph)) { + dev->get_byte(dev->periph); } #ifndef NO_XBEE_API_INIT /** - busy wait 1.25s */ sys_time_usleep(1250000); - if (! xbee_try_to_enter_api()) { + if (! xbee_try_to_enter_api(dev)) { #ifdef XBEE_BAUD_ALTERNATE // Badly configured... try the alternate baudrate: - XBeeUartSetBaudrate(XBEE_BAUD_ALTERNATE); - if (xbee_try_to_enter_api()) { + uart_periph_set_baudrate(&(XBEE_UART), XBEE_BAUD_ALTERNATE); // FIXME add set_baudrate to generic device, assuming uart for now + if (xbee_try_to_enter_api(dev)) { // The alternate baudrate worked, - XBeePrintString(XBEE_UART, XBEE_ATBD_CODE); + print_string(dev, XBEE_ATBD_CODE); } else { // Complete failure, none of the 2 baudrates result in any reply // TODO: set LED? // Set the default baudrate, just in case everything is right - XBeeUartSetBaudrate(XBEE_BAUD); - XBeePrintString(XBEE_UART, "\r"); + uart_periph_set_baudrate(&(XBEE_UART), XBEE_BAUD); // FIXME add set_baudrate to generic device, assuming uart for now + print_string(dev, "\r"); } #endif @@ -213,21 +209,21 @@ void xbee_init(void) } /** Setting my address */ - XBeePrintString(XBEE_UART, AT_SET_MY); + print_string(dev, AT_SET_MY); uint16_t addr = XBEE_MY_ADDR; - XBeePrintHex16(XBEE_UART, addr); - XBeePrintString(XBEE_UART, "\r"); + print_hex16(dev, addr); + print_string(dev, "\r"); - XBeePrintString(XBEE_UART, AT_AP_MODE); + print_string(dev, AT_AP_MODE); #ifdef XBEE_INIT - XBeePrintString(XBEE_UART, XBEE_INIT); + print_string(dev, XBEE_INIT); #endif /** Switching back to normal mode */ - XBeePrintString(XBEE_UART, AT_EXIT); + print_string(dev, AT_EXIT); - XBeeUartSetBaudrate(XBEE_BAUD); + uart_periph_set_baudrate(&(XBEE_UART), XBEE_BAUD); // FIXME add set_baudrate to generic device, assuming uart for now #endif } diff --git a/sw/airborne/subsystems/datalink/xbee.h b/sw/airborne/subsystems/datalink/xbee.h index 759fdc70e0..7b35568b40 100644 --- a/sw/airborne/subsystems/datalink/xbee.h +++ b/sw/airborne/subsystems/datalink/xbee.h @@ -134,19 +134,19 @@ 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_rx.msg_received)) parse_xbee(&(_trans),TransportLink(_dev,Getch())); } -#define XBeeCheckAndParse(_dev,_trans) { \ - if (XBeeBuffer(_dev)) { \ - ReadXBeeBuffer(_dev,_trans); \ - if (_trans.trans_rx.msg_received) { \ - xbee_parse_payload(&(_trans)); \ - _trans.trans_rx.msg_received = FALSE; \ - } \ - } \ - } +#define XBeeCheckAndParse(_dev, _trans) xbee_check_and_parse(&(_dev).device, &(_trans)) -#define XBeePrintString(_dev, s) TransportLink(_dev,PrintString(s)) -#define XBeePrintHex16(_dev, x) TransportLink(_dev,PrintHex16(x)) +static inline void xbee_check_and_parse(struct link_device *dev, struct xbee_transport *trans) +{ + if (dev->char_available(dev->periph)) { + while (dev->char_available(dev->periph) && !trans->trans_rx.msg_received) { + parse_xbee(trans, dev->get_byte(dev->periph)); + } + if (trans->trans_rx.msg_received) { + xbee_parse_payload(trans); + trans->trans_rx.msg_received = FALSE; + } + } +} #endif /* XBEE_H */ diff --git a/sw/airborne/subsystems/gps/gps_furuno.c b/sw/airborne/subsystems/gps/gps_furuno.c index befb57da65..c86275e69b 100644 --- a/sw/airborne/subsystems/gps/gps_furuno.c +++ b/sw/airborne/subsystems/gps/gps_furuno.c @@ -47,6 +47,8 @@ static const char *gps_furuno_settings[GPS_FURUNO_SETTINGS_NB] = { static void nmea_parse_perdcrv(void); +#define GpsLinkDevice (&(GPS_LINK).device) + void nmea_parse_prop_init(void) { static uint8_t i = 0; @@ -64,9 +66,9 @@ void nmea_parse_prop_init(void) sprintf(buf, "$%s*%02X\r\n", gps_furuno_settings[i], crc); // Check if there is enough space to send the config msg - if (GpsLink(CheckFreeSpace(len + 6))) { + if (GpsLinkDevice->check_free_space(GpsLinkDevice->periph, len + 6)) { for (j = 0; j < len + 6; j++) { - GpsLink(Transmit(buf[j])); + GpsLinkDevice->put_byte(GpsLinkDevice->periph, buf[j]); } } else { break; diff --git a/sw/airborne/subsystems/gps/gps_mtk.c b/sw/airborne/subsystems/gps/gps_mtk.c index 826e1ce127..8591d8fd58 100644 --- a/sw/airborne/subsystems/gps/gps_mtk.c +++ b/sw/airborne/subsystems/gps/gps_mtk.c @@ -115,6 +115,30 @@ void gps_impl_init(void) #endif } +void gps_mtk_msg(void (* _cb)(void)) +{ + gps.last_msg_ticks = sys_time.nb_sec_rem; + gps.last_msg_time = sys_time.nb_sec; + gps_mtk_read_message(); + if (gps_mtk.msg_class == MTK_DIY14_ID && + gps_mtk.msg_id == MTK_DIY14_NAV_ID) { + if (gps.fix == GPS_FIX_3D) { + gps.last_3dfix_ticks = sys_time.nb_sec_rem; + gps.last_3dfix_time = sys_time.nb_sec; + } + _sol_available_callback(); + } + if (gps_mtk.msg_class == MTK_DIY16_ID && + gps_mtk.msg_id == MTK_DIY16_NAV_ID) { + if (gps.fix == GPS_FIX_3D) { + gps.last_3dfix_ticks = sys_time.nb_sec_rem; + gps.last_3dfix_time = sys_time.nb_sec; + } + _cb(); + } + gps_mtk.msg_available = FALSE; +} + static void gps_mtk_time2itow(uint32_t gps_date, uint32_t gps_time, int16_t *gps_week, uint32_t *gps_itow) { @@ -393,9 +417,12 @@ restart: */ #ifdef GPS_CONFIGURE +#include "mcu_periph/link_device.h" + static void MtkSend_CFG(char *dat) { - while (*dat != 0) { GpsLink(Transmit(*dat++)); } + struct link_device *dev = &((GPS_LINK).device); + while (*dat != 0) { dev->put_byte(dev->periph, *dat++); } } void gps_configure_uart(void) diff --git a/sw/airborne/subsystems/gps/gps_mtk.h b/sw/airborne/subsystems/gps/gps_mtk.h index fe6c400cab..3f7799fd8f 100644 --- a/sw/airborne/subsystems/gps/gps_mtk.h +++ b/sw/airborne/subsystems/gps/gps_mtk.h @@ -54,11 +54,7 @@ extern struct GpsMtk gps_mtk; /* * This part is used by the autopilot to read data from a uart */ -#define __GpsLink(dev, _x) dev##_x -#define _GpsLink(dev, _x) __GpsLink(dev, _x) -#define GpsLink(_x) _GpsLink(GPS_LINK, _x) - -#define GpsBuffer() GpsLink(ChAvailable()) +#include "mcu_periph/link_device.h" #ifdef GPS_CONFIGURE extern bool_t gps_configuring; @@ -70,44 +66,24 @@ extern bool_t gps_configuring; #define GpsConfigure() {} #endif -#define GpsEvent(_sol_available_callback) { \ - if (GpsBuffer()) { \ - ReadGpsBuffer(); \ - GpsConfigure(); \ - } \ - if (gps_mtk.msg_available) { \ - gps.last_msg_ticks = sys_time.nb_sec_rem; \ - gps.last_msg_time = sys_time.nb_sec; \ - gps_mtk_read_message(); \ - if (gps_mtk.msg_class == MTK_DIY14_ID && \ - gps_mtk.msg_id == MTK_DIY14_NAV_ID) { \ - if (gps.fix == GPS_FIX_3D) { \ - gps.last_3dfix_ticks = sys_time.nb_sec_rem; \ - gps.last_3dfix_time = sys_time.nb_sec; \ - } \ - _sol_available_callback(); \ - } \ - if (gps_mtk.msg_class == MTK_DIY16_ID && \ - gps_mtk.msg_id == MTK_DIY16_NAV_ID) { \ - if (gps.fix == GPS_FIX_3D) { \ - gps.last_3dfix_ticks = sys_time.nb_sec_rem; \ - gps.last_3dfix_time = sys_time.nb_sec; \ - } \ - _sol_available_callback(); \ - } \ - gps_mtk.msg_available = FALSE; \ - } \ - } - -#define ReadGpsBuffer() { \ - while (GpsLink(ChAvailable())&&!gps_mtk.msg_available) \ - gps_mtk_parse(GpsLink(Getch())); \ - } - - extern void gps_mtk_read_message(void); extern void gps_mtk_parse(uint8_t c); +static inline void GpsEvent(void (* _sol_available_callback)(void)) +{ + struct link_device *dev = &((GPS_LINK).device); + + if (dev->char_available(dev->periph)) { + while (dev->char_available(dev->periph) && !gps_mtk.msg_available) { + gps_mtk_parse(dev->get_byte(dev->periph)); + } + GpsConfigure(); + } + if (gps_mtk.msg_available) { + gps_mtk_msg(_sol_available_callback); + } +} + /* * dynamic GPS configuration */ diff --git a/sw/airborne/subsystems/gps/gps_nmea.c b/sw/airborne/subsystems/gps/gps_nmea.c index 987ebe2a82..fa4445538b 100644 --- a/sw/airborne/subsystems/gps/gps_nmea.c +++ b/sw/airborne/subsystems/gps/gps_nmea.c @@ -63,6 +63,21 @@ void gps_impl_init(void) nmea_parse_prop_init(); } +void gps_nmea_msg(void (* _cb)(void)) +{ + gps.last_msg_ticks = sys_time.nb_sec_rem; + gps.last_msg_time = sys_time.nb_sec; + nmea_parse_msg(); + if (gps_nmea.pos_available) { + if (gps.fix == GPS_FIX_3D) { + gps.last_3dfix_ticks = sys_time.nb_sec_rem; + gps.last_3dfix_time = sys_time.nb_sec; + } + _cb(); + } + gps_nmea.msg_available = FALSE; +} + void WEAK nmea_parse_prop_init(void) { } diff --git a/sw/airborne/subsystems/gps/gps_nmea.h b/sw/airborne/subsystems/gps/gps_nmea.h index d416b34ad7..6353c00fd5 100644 --- a/sw/airborne/subsystems/gps/gps_nmea.h +++ b/sw/airborne/subsystems/gps/gps_nmea.h @@ -52,44 +52,30 @@ extern struct GpsNmea gps_nmea; /* * This part is used by the autopilot to read data from a uart */ -#define __GpsLink(dev, _x) dev##_x -#define _GpsLink(dev, _x) __GpsLink(dev, _x) -#define GpsLink(_x) _GpsLink(GPS_LINK, _x) -#define GpsBuffer() GpsLink(ChAvailable()) +/** The function to be called when a characted from the device is available */ +#include "mcu_periph/link_device.h" -#define GpsEvent(_sol_available_callback) { \ - nmea_parse_prop_init(); \ - if (GpsBuffer()) { \ - ReadGpsBuffer(); \ - } \ - if (gps_nmea.msg_available) { \ - gps.last_msg_ticks = sys_time.nb_sec_rem; \ - gps.last_msg_time = sys_time.nb_sec; \ - nmea_parse_msg(); \ - if (gps_nmea.pos_available) { \ - if (gps.fix == GPS_FIX_3D) { \ - gps.last_3dfix_ticks = sys_time.nb_sec_rem; \ - gps.last_3dfix_time = sys_time.nb_sec; \ - } \ - _sol_available_callback(); \ - } \ - gps_nmea.msg_available = FALSE; \ - } \ - } - -#define ReadGpsBuffer() { \ - while (GpsLink(ChAvailable())&&!gps_nmea.msg_available) \ - nmea_parse_char(GpsLink(Getch())); \ - } - - -/** The function to be called when a characted friom the device is available */ extern void nmea_parse_char(uint8_t c); extern void nmea_parse_msg(void); extern uint8_t nmea_calc_crc(const char *buff, int buff_sz); extern void nmea_parse_prop_init(void); extern void nmea_parse_prop_msg(void); +extern void gps_nmea_msg(void (* _cb)(void)); + +static inline void GpsEvent(void (* _sol_available_callback)(void)) +{ + struct link_device *dev = &((GPS_LINK).device); + + if (dev->char_available(dev->periph)) { + while (dev->char_available(dev->periph)) { + nmea_parse_char(dev->get_byte(dev->periph)); + } + } + if (gps_nmea.msg_available) { + gps_nmea_msg(_sol_available_callback); + } +} /** Read until a certain character, placed here for proprietary includes */ static inline void nmea_read_until(int *i) diff --git a/sw/airborne/subsystems/gps/gps_sirf.c b/sw/airborne/subsystems/gps/gps_sirf.c index d6d6f58f8d..96ed382927 100644 --- a/sw/airborne/subsystems/gps/gps_sirf.c +++ b/sw/airborne/subsystems/gps/gps_sirf.c @@ -48,6 +48,21 @@ void gps_impl_init(void) gps_sirf.read_state = 0; } +void gps_sirf_msg(void (* _cb)(void)) +{ + gps.last_msg_ticks = sys_time.nb_sec_rem; + gps.last_msg_time = sys_time.nb_sec; + sirf_parse_msg(); + if (gps_sirf.pos_available) { + if (gps.fix == GPS_FIX_3D) { + gps.last_3dfix_ticks = sys_time.nb_sec_rem; + gps.last_3dfix_time = sys_time.nb_sec; + } + _cb(); + } + gps_sirf.msg_available = FALSE; +} + void sirf_parse_char(uint8_t c) { switch (gps_sirf.read_state) { diff --git a/sw/airborne/subsystems/gps/gps_sirf.h b/sw/airborne/subsystems/gps/gps_sirf.h index db270a4fe6..0b1b0624e1 100644 --- a/sw/airborne/subsystems/gps/gps_sirf.h +++ b/sw/airborne/subsystems/gps/gps_sirf.h @@ -127,37 +127,24 @@ struct sirf_msg_41 { /* * This part is used by the autopilot to read data from a uart */ -#define __GpsLink(dev, _x) dev##_x -#define _GpsLink(dev, _x) __GpsLink(dev, _x) -#define GpsLink(_x) _GpsLink(GPS_LINK, _x) - -#define GpsBuffer() GpsLink(ChAvailable()) - -#define GpsEvent(_sol_available_callback) { \ - if (GpsBuffer()) { \ - ReadGpsBuffer(); \ - } \ - if (gps_sirf.msg_available) { \ - gps.last_msg_ticks = sys_time.nb_sec_rem; \ - gps.last_msg_time = sys_time.nb_sec; \ - sirf_parse_msg(); \ - if (gps_sirf.pos_available) { \ - if (gps.fix == GPS_FIX_3D) { \ - gps.last_3dfix_ticks = sys_time.nb_sec_rem; \ - gps.last_3dfix_time = sys_time.nb_sec; \ - } \ - _sol_available_callback(); \ - } \ - gps_sirf.msg_available = FALSE; \ - } \ - } - -#define ReadGpsBuffer() { \ - while (GpsLink(ChAvailable())&&!gps_sirf.msg_available) \ - sirf_parse_char(GpsLink(Getch())); \ - } +#include "mcu_periph/link_device.h" extern void sirf_parse_char(uint8_t c); extern void sirf_parse_msg(void); +extern void gps_sirf_msg(void (* _cb)(void)); + +static inline void GpsEvent(void (* _sol_available_callback)(void)) +{ + struct link_device *dev = &((GPS_LINK).device); + + if (dev->char_available(dev->periph)) { + while (dev->char_available(dev->periph)) { + sirf_parse_char(dev->get_byte(dev->periph)); + } + } + if (gps_sirf.msg_available) { + gps_sirf_msg(_sol_available_callback); + } +} #endif /* GPS_SIRF_H */ diff --git a/sw/airborne/subsystems/gps/gps_skytraq.c b/sw/airborne/subsystems/gps/gps_skytraq.c index e46606c697..d2b220f937 100644 --- a/sw/airborne/subsystems/gps/gps_skytraq.c +++ b/sw/airborne/subsystems/gps/gps_skytraq.c @@ -97,6 +97,21 @@ void gps_impl_init(void) } +void gps_skytraq_msg(void (* _cb)(void)) +{ + gps.last_msg_ticks = sys_time.nb_sec_rem; + gps.last_msg_time = sys_time.nb_sec; + gps_skytraq_read_message(); + if (gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) { + if (gps.fix == GPS_FIX_3D) { + gps.last_3dfix_ticks = sys_time.nb_sec_rem; + gps.last_3dfix_time = sys_time.nb_sec; + } + _cb(); + } + gps_skytraq.msg_available = FALSE; +} + void gps_skytraq_read_message(void) { diff --git a/sw/airborne/subsystems/gps/gps_skytraq.h b/sw/airborne/subsystems/gps/gps_skytraq.h index d136dfc9b3..c35f773395 100644 --- a/sw/airborne/subsystems/gps/gps_skytraq.h +++ b/sw/airborne/subsystems/gps/gps_skytraq.h @@ -58,38 +58,24 @@ extern struct GpsSkytraq gps_skytraq; /* * This part is used by the autopilot to read data from a uart */ -#define __GpsLink(dev, _x) dev##_x -#define _GpsLink(dev, _x) __GpsLink(dev, _x) -#define GpsLink(_x) _GpsLink(GPS_LINK, _x) - -#define GpsBuffer() GpsLink(ChAvailable()) - -#define GpsEvent(_sol_available_callback) { \ - if (GpsBuffer()) { \ - ReadGpsBuffer(); \ - } \ - if (gps_skytraq.msg_available) { \ - gps.last_msg_ticks = sys_time.nb_sec_rem; \ - gps.last_msg_time = sys_time.nb_sec; \ - gps_skytraq_read_message(); \ - if (gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) { \ - if (gps.fix == GPS_FIX_3D) { \ - gps.last_3dfix_ticks = sys_time.nb_sec_rem; \ - gps.last_3dfix_time = sys_time.nb_sec; \ - } \ - _sol_available_callback(); \ - } \ - gps_skytraq.msg_available = FALSE; \ - } \ - } - -#define ReadGpsBuffer() { \ - while (GpsLink(ChAvailable())&&!gps_skytraq.msg_available) \ - gps_skytraq_parse(GpsLink(Getch())); \ - } - +#include "mcu_periph/link_device.h" extern void gps_skytraq_read_message(void); extern void gps_skytraq_parse(uint8_t c); +extern void gps_skytraq_msg(void (* _cb)(void)); + +static inline void GpsEvent(void (* _sol_available_callback)(void)) +{ + struct link_device *dev = &((GPS_LINK).device); + + if (dev->char_available(dev->periph)) { + while (dev->char_available(dev->periph) && !gps_mtk.msg_available) { + gps_skytraq_parse(dev->get_byte(dev->periph)); + } + } + if (gps_skytraq.msg_available) { + gps_skytraq_msg(_sol_available_callback); + } +} #endif /* GPS_SKYTRAQ_H */ diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c index c29c1f6b62..0a74c1cd36 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.c +++ b/sw/airborne/subsystems/gps/gps_ubx.c @@ -30,6 +30,9 @@ #include "math/pprz_geodetic_float.h" #endif +/** Includes macros generated from ubx.xml */ +#include "ubx_protocol.h" + /* parser status */ #define UNINIT 0 #define GOT_SYNC1 1 @@ -52,9 +55,6 @@ #define UTM_HEM_NORTH 0 #define UTM_HEM_SOUTH 1 - -#define GpsUartRunning GpsLink(TxRunning) - struct GpsUbx gps_ubx; #if USE_GPS_UBX_RXM_RAW @@ -273,11 +273,69 @@ restart: return; } -void ubxsend_cfg_rst(uint16_t bbr , uint8_t reset_mode) +static void ubx_send_1byte(struct link_device *dev, uint8_t byte) +{ + dev->put_byte(dev->periph, byte); + gps_ubx.send_ck_a += byte; + gps_ubx.send_ck_b += gps_ubx.send_ck_a; +} + +void ubx_header(struct link_device *dev, uint8_t nav_id, uint8_t msg_id, uint16_t len) +{ + dev->put_byte(dev->periph, UBX_SYNC1); + dev->put_byte(dev->periph, UBX_SYNC2); + gps_ubx.send_ck_a = 0; + gps_ubx.send_ck_b = 0; + ubx_send_1byte(dev, nav_id); + ubx_send_1byte(dev, msg_id); + ubx_send_1byte(dev, (uint8_t)(len&0xFF)); + ubx_send_1byte(dev, (uint8_t)(len>>8)); +} + +void ubx_trailer(struct link_device *dev) +{ + dev->put_byte(dev->periph, gps_ubx.send_ck_a); + dev->put_byte(dev->periph, gps_ubx.send_ck_b); + dev->send_message(dev->periph); +} + +void ubx_send_bytes(struct link_device *dev, uint8_t len, uint8_t *bytes) +{ + int i; + for (i = 0; i < len; i++) { + ubx_send_1byte(dev, bytes[i]); + } +} + +void ubx_send_cfg_rst(struct link_device *dev, uint16_t bbr , uint8_t reset_mode) { #ifdef GPS_LINK - UbxSend_CFG_RST(bbr, reset_mode, 0x00); + UbxSend_CFG_RST(dev, bbr, reset_mode, 0x00); #endif /* else less harmful for HITL */ } +#ifndef GPS_UBX_UCENTER +#define gps_ubx_ucenter_event() {} +#else +#include "modules/gps/gps_ubx_ucenter.h" +#endif + +void gps_ubx_msg(void (* _cb)(void)) +{ + gps.last_msg_ticks = sys_time.nb_sec_rem; + gps.last_msg_time = sys_time.nb_sec; + gps_ubx_read_message(); + gps_ubx_ucenter_event(); + if (gps_ubx.msg_class == UBX_NAV_ID && + (gps_ubx.msg_id == UBX_NAV_VELNED_ID || + (gps_ubx.msg_id == UBX_NAV_SOL_ID && + gps_ubx.have_velned == 0))) { + if (gps.fix == GPS_FIX_3D) { + gps.last_3dfix_ticks = sys_time.nb_sec_rem; + gps.last_3dfix_time = sys_time.nb_sec; + } + _cb(); + } + gps_ubx.msg_available = FALSE; +} diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h index 8ff670e005..643f86cec2 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.h +++ b/sw/airborne/subsystems/gps/gps_ubx.h @@ -33,9 +33,6 @@ #include "mcu_periph/uart.h" -/** Includes macros generated from ubx.xml */ -#include "ubx_protocol.h" - #define GPS_NB_CHANNELS 16 #define GPS_UBX_MAX_PAYLOAD 255 @@ -85,53 +82,35 @@ extern struct GpsUbxRaw gps_ubx_raw; /* * This part is used by the autopilot to read data from a uart */ -#define __GpsLink(dev, _x) dev##_x -#define _GpsLink(dev, _x) __GpsLink(dev, _x) -#define GpsLink(_x) _GpsLink(GPS_LINK, _x) +#include "mcu_periph/link_device.h" -#define GpsBuffer() GpsLink(ChAvailable()) +extern void ubx_header(struct link_device *dev, uint8_t nav_id, uint8_t msg_id, uint16_t len); +extern void ubx_trailer(struct link_device *dev); +extern void ubx_send_bytes(struct link_device *dev, uint8_t len, uint8_t *bytes); +extern void ubx_send_cfg_rst(struct link_device *dev, uint16_t bbr, uint8_t reset_mode); -#ifndef GPS_UBX_UCENTER -#define gps_ubx_ucenter_event() {} -#endif +extern void gps_ubx_read_message(void); +extern void gps_ubx_parse(uint8_t c); +extern void gps_ubx_msg(void (* _cb)(void)); /* Gps callback is called when receiving a VELNED or a SOL message * All position/speed messages are sent in one shot and VELNED is the last one on fixedwing * For rotorcraft, only SOL message is needed for pos/speed data */ -#define GpsEvent(_sol_available_callback) { \ - if (GpsBuffer()) { \ - ReadGpsBuffer(); \ - } \ - if (gps_ubx.msg_available) { \ - gps.last_msg_ticks = sys_time.nb_sec_rem; \ - gps.last_msg_time = sys_time.nb_sec; \ - gps_ubx_read_message(); \ - gps_ubx_ucenter_event(); \ - if (gps_ubx.msg_class == UBX_NAV_ID && \ - (gps_ubx.msg_id == UBX_NAV_VELNED_ID || \ - (gps_ubx.msg_id == UBX_NAV_SOL_ID && \ - gps_ubx.have_velned == 0))) { \ - if (gps.fix == GPS_FIX_3D) { \ - gps.last_3dfix_ticks = sys_time.nb_sec_rem; \ - gps.last_3dfix_time = sys_time.nb_sec; \ - } \ - _sol_available_callback(); \ - } \ - gps_ubx.msg_available = FALSE; \ - } \ +static inline void GpsEvent(void (* _sol_available_callback)(void)) +{ + struct link_device *dev = &((GPS_LINK).device); + + if (dev->char_available(dev->periph)) { + while (dev->char_available(dev->periph) && !gps_ubx.msg_available) { + gps_ubx_parse(dev->get_byte(dev->periph)); + } } - -#define ReadGpsBuffer() { \ - while (GpsLink(ChAvailable())&&!gps_ubx.msg_available) \ - gps_ubx_parse(GpsLink(Getch())); \ + if (gps_ubx.msg_available) { + gps_ubx_msg(_sol_available_callback); } - - -extern void gps_ubx_read_message(void); -extern void gps_ubx_parse(uint8_t c); - +} /* @@ -148,36 +127,11 @@ extern void gps_ubx_parse(uint8_t c); #define CFG_RST_BBR_Warmstart 0x0001 #define CFG_RST_BBR_Coldstart 0xffff -extern void ubxsend_cfg_rst(uint16_t, uint8_t); - #define gps_ubx_Reset(_val) { \ gps_ubx.reset = _val; \ if (gps_ubx.reset > CFG_RST_BBR_Warmstart) \ gps_ubx.reset = CFG_RST_BBR_Coldstart; \ - ubxsend_cfg_rst(gps_ubx.reset, CFG_RST_Reset_Controlled); \ + ubx_send_cfg_rst(&(GPS_LINK).device, gps_ubx.reset, CFG_RST_Reset_Controlled); \ } -#define GpsUartSendMessage GpsLink(SendMessage) - -#define GpsUartSend1(c) GpsLink(Transmit(c)) -#define UbxInitCheksum() { gps_ubx.send_ck_a = gps_ubx.send_ck_b = 0; } -#define UbxUpdateChecksum(c) { gps_ubx.send_ck_a += c; gps_ubx.send_ck_b += gps_ubx.send_ck_a; } -#define UbxSend1(c) { uint8_t i8=c; GpsUartSend1(i8); UbxUpdateChecksum(i8); } -#define UbxSend2(c) { uint16_t i16=c; UbxSend1(i16&0xff); UbxSend1(i16 >> 8); } -#define UbxSend1ByAddr(x) { UbxSend1(*x); } -#define UbxSend2ByAddr(x) { UbxSend1(*x); UbxSend1(*(x+1)); } -#define UbxSend4ByAddr(x) { UbxSend1(*x); UbxSend1(*(x+1)); UbxSend1(*(x+2)); UbxSend1(*(x+3)); } -#define GpsUartSetBaudrate(_a) GpsLink(SetBaudrate(_a)) - -#define UbxHeader(nav_id, msg_id, len) { \ - GpsUartSend1(UBX_SYNC1); \ - GpsUartSend1(UBX_SYNC2); \ - UbxInitCheksum(); \ - UbxSend1(nav_id); \ - UbxSend1(msg_id); \ - UbxSend2(len); \ - } - -#define UbxTrailer() { GpsUartSend1(gps_ubx.send_ck_a); GpsUartSend1(gps_ubx.send_ck_b); GpsUartSendMessage(); } - #endif /* GPS_UBX_H */ diff --git a/sw/airborne/subsystems/imu/imu_um6.c b/sw/airborne/subsystems/imu/imu_um6.c index eb4ccf4f88..80d7549d8f 100644 --- a/sw/airborne/subsystems/imu/imu_um6.c +++ b/sw/airborne/subsystems/imu/imu_um6.c @@ -76,7 +76,7 @@ inline uint16_t UM6_calculate_checksum(uint8_t packet_buffer[], uint8_t packet_l inline void UM6_send_packet(uint8_t *packet_buffer, uint8_t packet_length) { for (int i = 0; i < packet_length; i++) { - UM6Link(Transmit(packet_buffer[i])); + uart_transmit(&(UM6_LINK), packet_buffer[i]); } } diff --git a/sw/airborne/subsystems/imu/imu_um6.h b/sw/airborne/subsystems/imu/imu_um6.h index a3bf023525..f6ce48e406 100644 --- a/sw/airborne/subsystems/imu/imu_um6.h +++ b/sw/airborne/subsystems/imu/imu_um6.h @@ -38,12 +38,6 @@ #include "mcu_periph/uart.h" #include "subsystems/ahrs.h" -#define __UM6Link(dev, _x) dev##_x -#define _UM6Link(dev, _x) __UM6Link(dev, _x) -#define UM6Link(_x) _UM6Link(UM6_LINK, _x) - -#define UM6Buffer() UM6Link(ChAvailable()) - #define IMU_UM6_BUFFER_LENGTH 32 #define IMU_UM6_DATA_OFFSET 5 #define IMU_UM6_LONG_DELAY 4000000 @@ -104,23 +98,21 @@ enum UM6Status { UM6Running }; -#define imu_um6_event(_callback1, _callback2, _callback3) { \ - if (UM6Buffer()) { \ - ReadUM6Buffer(); \ - } \ - if (UM6_packet.msg_available) { \ - UM6_packet.msg_available = FALSE; \ - UM6_packet_read_message(); \ - _callback1(); \ - _callback2(); \ - _callback3(); \ - } \ - } - -#define ReadUM6Buffer() { \ - while (UM6Link(ChAvailable())&&!UM6_packet.msg_available) \ - UM6_packet_parse(UM6Link(Getch())); \ +static inline void imu_um6_event(void (* cb1)(void), void (*cb2)(void), void (*cb3)(void)) +{ + if (uart_char_available(&(UM6_LINK))) { + while (uart_char_available(&(UM6_LINK)) && !UM6_packet.msg_available) { + UM6_packet_parse(uart_getch(&(UM6_LINK))); + } + if (UM6_packet.msg_available) { + UM6_packet.msg_available = FALSE; + UM6_packet_read_message(); + cb1(); + cb2(); + cb3(); + } } +} #define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \ imu_um6_event(_gyro_handler, _accel_handler, _mag_handler); \ diff --git a/sw/airborne/subsystems/radio_control/joby.c b/sw/airborne/subsystems/radio_control/joby.c deleted file mode 100644 index dacb0d9a7f..0000000000 --- a/sw/airborne/subsystems/radio_control/joby.c +++ /dev/null @@ -1,96 +0,0 @@ -/* - * Copyright (C) 2008-2009 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -#include "stdio.h" -#include "subsystems/radio_control.h" - -static struct rc_joby_parser_state parser; -static const int16_t rc_joby_signs[RADIO_CONTROL_NB_CHANNEL] = RC_JOBY_SIGNS; - - -static void handle_channel(void (* callback)(void)) -{ - if (parser.parser_normal_buf == RC_JOBY_MAGIC_START) { - // got start channel, look for channel 0 next - parser.current_channel = 0; - } else if (parser.current_channel == -1) { - // looking for start channel byte but didn't get it, reset - parser.current_byte = READING_HIGH_BYTE; - parser.current_inverted = READING_NORMAL; - } else { - // valid channel, store and look for next - radio_control.values[parser.current_channel] = rc_joby_signs[parser.current_channel] * parser.parser_normal_buf; - parser.current_channel++; - if (parser.current_channel == RADIO_CONTROL_NB_CHANNEL) { - // all channels read, reset parser and handle message - parser.current_channel = -1; - radio_control.frame_cpt++; - radio_control.status = RC_OK; - radio_control.time_since_last_frame = 0; - if (callback != NULL) { - callback(); - } - } - } -} - -static void handle_tuple(void (* callback)(void)) -{ - if (parser.current_inverted == READING_NORMAL) { - parser.parser_normal_buf = ((parser.high_byte_buf << 8) | parser.low_byte_buf); - parser.current_inverted = READING_INVERTED; - } else if (parser.current_inverted == READING_INVERTED) { - parser.parser_inverted_buf = ((parser.high_byte_buf << 8) | parser.low_byte_buf); - parser.current_inverted = READING_NORMAL; - if (parser.parser_normal_buf == ~parser.parser_inverted_buf) { - handle_channel(callback); - } else { - // normal didn't match inverted, error, reset - parser.current_inverted = READING_NORMAL; - parser.current_byte = READING_HIGH_BYTE; - parser.current_channel = -1; - parser.error_counter++; - } - } -} - -void rc_joby_parse(int8_t c, void (* callback)(void)) -{ - if (parser.current_byte == READING_HIGH_BYTE) { - parser.high_byte_buf = c; - if (parser.current_channel >= 0 || parser.high_byte_buf == (RC_JOBY_MAGIC_START >> 8) - || parser.current_inverted == READING_INVERTED) { - // only advance parser state to low byte if we're not looking for a sync byte which we didn't find - parser.current_byte = READING_LOW_BYTE; - } - } else { // READING_LOW_BYTE - parser.low_byte_buf = c; - parser.current_byte = READING_HIGH_BYTE; - handle_tuple(callback); - } -} - -void radio_control_impl_init(void) -{ - parser.current_byte = READING_HIGH_BYTE; - parser.current_inverted = READING_NORMAL; - parser.current_channel = -1; -} diff --git a/sw/airborne/subsystems/radio_control/joby.h b/sw/airborne/subsystems/radio_control/joby.h deleted file mode 100644 index 7d39f09d22..0000000000 --- a/sw/airborne/subsystems/radio_control/joby.h +++ /dev/null @@ -1,72 +0,0 @@ -/* - * Copyright (C) 2009 Pascal Brisset , - * Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -#ifndef RADIO_CONTROL_JOBY_H -#define RADIO_CONTROL_JOBY_H - -#include "std.h" -#include "mcu_periph/uart.h" - -#define RC_JOBY_MAGIC_START 13999 - -#include RADIO_CONTROL_JOBY_MODEL_H - -typedef enum { - READING_LOW_BYTE = 0, - READING_HIGH_BYTE -} parser_byte_t; - -typedef enum { - READING_NORMAL = 0, - READING_INVERTED -} parser_inverted_t; - -struct rc_joby_parser_state { - parser_byte_t current_byte; - parser_inverted_t current_inverted; - int current_channel; - - int16_t parser_inverted_buf; - int16_t parser_normal_buf; - uint8_t high_byte_buf; - uint8_t low_byte_buf; - - uint32_t error_counter; -}; - -void rc_joby_parse(int8_t c, void (* callback)(void)); - -#define __RcLink(dev, _x) dev##_x -#define _RcLink(dev, _x) __RcLink(dev, _x) -#define RcLink(_x) _RcLink(RADIO_CONTROL_LINK, _x) - -#define RcLinkChAvailable() RcLink(ChAvailable()) -#define RcLinkGetCh() RcLink(Getch()) - -#define RadioControlEvent(_received_frame_handler) { \ - while (RcLinkChAvailable()) { \ - rc_joby_parse(RcLinkGetCh(), _received_frame_handler); \ - } \ - } - - -#endif /* RADIO_CONTROL_JOBY_H */ diff --git a/sw/tools/generators/gen_mtk.ml b/sw/tools/generators/gen_mtk.ml index fd0885fa72..835d9292ff 100644 --- a/sw/tools/generators/gen_mtk.ml +++ b/sw/tools/generators/gen_mtk.ml @@ -105,33 +105,7 @@ let parse_message = fun class_name m -> if l <> !offset then raise (Length_error (m, l, !offset)) with Xml.No_attribute("length") -> () (** Undefined length authorized *) - end; - - (** Generating send function *) - let param_name = fun f -> String.lowercase (field_name f) in - let rec param_names = fun f r -> - if Xml.tag f = "field" then - param_name f :: r - else - List.fold_right param_names (Xml.children f) r in - let param_type = fun f -> c_type (format f) in - fprintf out "\n#define MtkSend_%s_%s(" class_name msg_name; - fprintf out "%s" (String.concat "," (param_names m [])); - fprintf out ") { \\\n"; - fprintf out " MtkHeader(MTK_%s_ID, %s, %d);\\\n" class_name msg_id !offset; - let rec send_one_field = fun f -> - match Xml.tag f with - "field" -> - let s = sizeof (format f) in - let p = param_name f in - let t = param_type f in - fprintf out " %s _%s = %s; MtkSend%dByAddr((uint8_t*)&_%s);\\\n" t p p s p - | "block" -> - List.iter send_one_field (Xml.children f) - | _ -> assert (false) in - List.iter send_one_field (Xml.children m); - fprintf out " MtkTrailer();\\\n"; - fprintf out "}\n\n#define MTK_%s_%s_LENGTH %d\n" class_name msg_name !offset + end let parse_class = fun c -> diff --git a/sw/tools/generators/gen_ubx.ml b/sw/tools/generators/gen_ubx.ml index ceda079914..4bbce9e933 100644 --- a/sw/tools/generators/gen_ubx.ml +++ b/sw/tools/generators/gen_ubx.ml @@ -2,6 +2,7 @@ * XML preprocessing for UBX protocol * * Copyright (C) 2003 Pascal Brisset, Antoine Drouin + * Copyright (C) 2015 Gautier Hattenberger * * This file is part of paparazzi. * @@ -110,29 +111,31 @@ let parse_message = fun class_name m -> end; (** Generating send function *) - let param_name = fun f -> String.lowercase (field_name f) in + let param_type = fun f -> c_type (format f) in + let param_name = fun f ->String.lowercase (field_name f) in + let param_name_and_type = fun f -> + sprintf "%s %s" (param_type f) (param_name f) in let rec param_names = fun f r -> if Xml.tag f = "field" then - param_name f :: r + param_name_and_type f :: r else List.fold_right param_names (Xml.children f) r in - let param_type = fun f -> c_type (format f) in - fprintf out "\n#define UbxSend_%s_%s(" class_name msg_name; - fprintf out "%s" (String.concat "," (param_names m [])); - fprintf out ") { \\\n"; - fprintf out " UbxHeader(UBX_%s_ID, %s, %d);\\\n" class_name msg_id !offset; + fprintf out "\nstatic inline void UbxSend_%s_%s(" class_name msg_name; + fprintf out "%s" (String.concat ", " (["struct link_device *dev"] @ (param_names m []))); + fprintf out ") {\n"; + fprintf out " ubx_header(dev, UBX_%s_ID, %s, %d);\n" class_name msg_id !offset; let rec send_one_field = fun f -> match Xml.tag f with "field" -> let s = sizeof (format f) in let p = param_name f in let t = param_type f in - fprintf out " %s _%s = %s; UbxSend%dByAddr((uint8_t*)&_%s);\\\n" t p p s p + fprintf out " %s _%s = %s; ubx_send_bytes(dev, %d, (uint8_t*)&_%s);\n" t p p s p | "block" -> List.iter send_one_field (Xml.children f) | _ -> assert (false) in List.iter send_one_field (Xml.children m); - fprintf out " UbxTrailer();\\\n"; + fprintf out " ubx_trailer(dev);\n"; fprintf out "}\n" @@ -156,6 +159,9 @@ let _ = fprintf out "/* Generated by gen_ubx from %s */\n" xml_file; fprintf out "/* Please DO NOT EDIT */\n\n"; + fprintf out "#include \"mcu_periph/link_device.h\"\n\n"; + fprintf out "#include \"subsystems/gps/gps_ubx.h\"\n\n"; + define "UBX_SYNC1" "0xB5"; define "UBX_SYNC2" "0x62";