diff --git a/conf/airframes/examples/quadrotor_lisa_mx.xml b/conf/airframes/examples/quadrotor_lisa_mx.xml index 6d5bd1f6dd..21450762d6 100644 --- a/conf/airframes/examples/quadrotor_lisa_mx.xml +++ b/conf/airframes/examples/quadrotor_lisa_mx.xml @@ -21,7 +21,6 @@ - @@ -32,6 +31,10 @@ + + + + diff --git a/conf/firmwares/subsystems/rotorcraft/gps_ardrone2.makefile b/conf/firmwares/subsystems/rotorcraft/gps_ardrone2.makefile index 49de927e45..e95dc52874 100644 --- a/conf/firmwares/subsystems/rotorcraft/gps_ardrone2.makefile +++ b/conf/firmwares/subsystems/rotorcraft/gps_ardrone2.makefile @@ -11,11 +11,10 @@ ifneq ($(GPS_LED),none) endif ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ardrone2.h\" -ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_ardrone2.c - -$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c +ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_ardrone2.c +ap.srcs += $(SRC_SUBSYSTEMS)/gps.c nps.CFLAGS += -DUSE_GPS -nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" +nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c - +nps.srcs += $(SRC_SUBSYSTEMS)/gps.c diff --git a/conf/firmwares/subsystems/rotorcraft/gps_datalink.makefile b/conf/firmwares/subsystems/rotorcraft/gps_datalink.makefile index b674b38242..e0255fd978 100644 --- a/conf/firmwares/subsystems/rotorcraft/gps_datalink.makefile +++ b/conf/firmwares/subsystems/rotorcraft/gps_datalink.makefile @@ -2,17 +2,30 @@ GPS_LED ?= none -ap.srcs += $(SRC_SUBSYSTEMS)/gps.c -ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_datalink.h\" -ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_datalink.c - -ap.CFLAGS += -DUSE_GPS -DGPS_DATALINK - ifneq ($(GPS_LED),none) ap.CFLAGS += -DGPS_LED=$(GPS_LED) endif +ifdef SECONDARY_GPS +ifneq (,$(findstring $(SECONDARY_GPS), datalink)) +# this is the secondary GPS +ap.CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_datalink.h\" +ap.CFLAGS += -DSECONDARY_GPS=gps_datalink +else +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_datalink.h\" +ap.CFLAGS += -DPRIMARY_GPS=gps_datalink +endif +else +# plain old single GPS usage +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_datalink.h\" +endif + +ap.srcs += $(SRC_SUBSYSTEMS)/gps.c +ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_datalink.c + +ap.CFLAGS += -DUSE_GPS -DGPS_DATALINK + nps.CFLAGS += -DUSE_GPS -nps.srcs += $(SRC_SUBSYSTEMS)/gps.c nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c +nps.srcs += $(SRC_SUBSYSTEMS)/gps.c diff --git a/conf/firmwares/subsystems/rotorcraft/gps_piksi.makefile b/conf/firmwares/subsystems/rotorcraft/gps_piksi.makefile deleted file mode 100644 index 1cdbe1d32a..0000000000 --- a/conf/firmwares/subsystems/rotorcraft/gps_piksi.makefile +++ /dev/null @@ -1,29 +0,0 @@ -# Hey Emacs, this is a -*- makefile -*- - -# Swift-Nav Piksi RTK module - -GPS_LED ?= none - -GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z) - -ap.CFLAGS += -DUSE_GPS -ap.CFLAGS += -DUSE_$(GPS_PORT) -D$(GPS_PORT)_BAUD=B115200 -ap.CFLAGS += -DGPS_LINK=$(GPS_PORT_LOWER) - -ifneq ($(GPS_LED),none) - ap.CFLAGS += -DGPS_LED=$(GPS_LED) -endif - -$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c - -ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_piksi.h\" -ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_piksi.c - -# libsbp -ap.CFLAGS += -I$(PAPARAZZI_SRC)/sw/ext/libsbp/c/include -ap.srcs += $(PAPARAZZI_SRC)/sw/ext/libsbp/c/src/sbp.c $(PAPARAZZI_SRC)/sw/ext/libsbp/c/src/edc.c - -nps.CFLAGS += -DUSE_GPS -nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" -nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c - diff --git a/conf/firmwares/subsystems/rotorcraft/gps_sim_hitl.makefile b/conf/firmwares/subsystems/rotorcraft/gps_sim_hitl.makefile index c2ea1367a8..c49b7239c6 100644 --- a/conf/firmwares/subsystems/rotorcraft/gps_sim_hitl.makefile +++ b/conf/firmwares/subsystems/rotorcraft/gps_sim_hitl.makefile @@ -1,9 +1,10 @@ # Hey Emacs, this is a -*- makefile -*- -ap.srcs += $(SRC_SUBSYSTEMS)/gps.c -ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_hitl.h\" -ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_hitl.c ap.CFLAGS += -DUSE_GPS -DHITL +ap.CFLAGS += -DGPS_TYPE=\"subsystems/gps/gps_sim_hitl.h\" +ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_hitl.c + +ap.srcs += $(SRC_SUBSYSTEMS)/gps.c ifneq ($(GPS_LED),none) ap.CFLAGS += -DGPS_LED=$(GPS_LED) @@ -11,5 +12,5 @@ endif nps.CFLAGS += -DUSE_GPS nps.srcs += $(SRC_SUBSYSTEMS)/gps.c -nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" +nps.CFLAGS += -DGPS_TYPE=\"subsystems/gps/gps_sim_nps.h\" nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c diff --git a/conf/firmwares/subsystems/rotorcraft/gps_sirf.makefile b/conf/firmwares/subsystems/rotorcraft/gps_sirf.makefile index 3007813055..238c74afca 100644 --- a/conf/firmwares/subsystems/rotorcraft/gps_sirf.makefile +++ b/conf/firmwares/subsystems/rotorcraft/gps_sirf.makefile @@ -3,23 +3,38 @@ # Sirf GPS unit GPS_LED ?= none -SIRF_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z) +SIRF_GPS_PORT ?= $(GPS_PORT) +SIRF_GPS_BAUD ?= $(GPS_BAUD) + +SIRF_GPS_PORT_LOWER=$(shell echo $(SIRF_GPS_PORT) | tr A-Z a-z) ap.CFLAGS += -DUSE_GPS -ap.CFLAGS += -DGPS_LINK=$(SIRF_GPS_PORT_LOWER) -ap.CFLAGS += -DUSE_$(GPS_PORT) -ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD) +ap.CFLAGS += -DSIRF_GPS_LINK=$(SIRF_GPS_PORT_LOWER) +ap.CFLAGS += -DUSE_$(SIRF_GPS_PORT) +ap.CFLAGS += -D$(SIRF_GPS_PORT)_BAUD=$(SIRF_GPS_BAUD) ifneq ($(GPS_LED),none) ap.CFLAGS += -DGPS_LED=$(GPS_LED) endif +ifdef SECONDARY_GPS +ifneq (,$(findstring $(SECONDARY_GPS), sirf)) +# this is the secondary GPS +ap.CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_sirf.h\" +ap.CFLAGS += -DSECONDARY_GPS=gps_sirf +else ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sirf.h\" -ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sirf.c +ap.CFLAGS += -DPRIMARY_GPS=gps_sirf +endif +else +# plain old single GPS usage +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sirf.h\" +endif -$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c +ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sirf.c +ap.srcs += $(SRC_SUBSYSTEMS)/gps.c nps.CFLAGS += -DUSE_GPS nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c - +nps.srcs += $(SRC_SUBSYSTEMS)/gps.c diff --git a/conf/firmwares/subsystems/rotorcraft/gps_udp.makefile b/conf/firmwares/subsystems/rotorcraft/gps_udp.makefile index df238a85dc..0714dce4b1 100644 --- a/conf/firmwares/subsystems/rotorcraft/gps_udp.makefile +++ b/conf/firmwares/subsystems/rotorcraft/gps_udp.makefile @@ -2,16 +2,28 @@ GPS_LED ?= none -ap.srcs += $(SRC_SUBSYSTEMS)/gps.c -ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_udp.h\" -ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_udp.c - -ap.CFLAGS += -DUSE_GPS - ifneq ($(GPS_LED),none) ap.CFLAGS += -DGPS_LED=$(GPS_LED) endif +ifdef SECONDARY_GPS +ifneq (,$(findstring $(SECONDARY_GPS), udp)) +# this is the secondary GPS +ap.CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_udp.h\" +ap.CFLAGS += -DSECONDARY_GPS=gps_udp +else +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_udp.h\" +ap.CFLAGS += -DPRIMARY_GPS=gps_udp +endif +else +# plain old single GPS usage +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_udp.h\" +endif + +ap.CFLAGS += -DUSE_GPS +ap.srcs += $(SRC_SUBSYSTEMS)/gps.c +ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_udp.c + nps.CFLAGS += -DUSE_GPS nps.srcs += $(SRC_SUBSYSTEMS)/gps.c nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" diff --git a/conf/firmwares/subsystems/shared/gps_furuno.makefile b/conf/firmwares/subsystems/shared/gps_furuno.makefile index 3e6407cd07..1a876aee81 100644 --- a/conf/firmwares/subsystems/shared/gps_furuno.makefile +++ b/conf/firmwares/subsystems/shared/gps_furuno.makefile @@ -3,28 +3,46 @@ # Furuno NMEA GPS unit GPS_LED ?= none -FURUNO_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z) +FURUNO_GPS_PORT ?= $(GPS_PORT) +FURUNO_GPS_BAUD ?= $(GPS_BAUD) + +FURUNO_GPS_PORT_LOWER=$(shell echo $(FURUNO_GPS_PORT) | tr A-Z a-z) ap.CFLAGS += -DUSE_GPS -ap.CFLAGS += -DGPS_LINK=$(FURUNO_GPS_PORT_LOWER) -ap.CFLAGS += -DUSE_$(GPS_PORT) -ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD) +ap.CFLAGS += -DNMEA_GPS_LINK=$(FURUNO_GPS_PORT_LOWER) +ap.CFLAGS += -DUSE_$(FURUNO_GPS_PORT) +ap.CFLAGS += -D$(FURUNO_GPS_PORT)_BAUD=$(FURUNO_GPS_BAUD) ap.CFLAGS += -DNMEA_PARSE_PROP ifneq ($(GPS_LED),none) ap.CFLAGS += -DGPS_LED=$(GPS_LED) endif +ifdef SECONDARY_GPS +ifneq (,$(findstring $(SECONDARY_GPS), nmea furono)) +# this is the secondary GPS +ap.CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_nmea.h\" +ap.CFLAGS += -DSECONDARY_GPS=gps_nmea +else +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\" +ap.CFLAGS += -DPRIMARY_GPS=gps_nmea +endif +else +# plain old single GPS usage +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\" +endif + ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\" ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_nmea.c $(SRC_SUBSYSTEMS)/gps/gps_furuno.c - -$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c +ap.srcs += $(SRC_SUBSYSTEMS)/gps.c sim.CFLAGS += -DUSE_GPS sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c +sim.srcs += $(SRC_SUBSYSTEMS)/gps.c nps.CFLAGS += -DUSE_GPS nps.srcs += $(SRC_SUBSYSTEMS)/gps.c nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c +nps.srcs += $(SRC_SUBSYSTEMS)/gps.c diff --git a/conf/firmwares/subsystems/shared/gps_mediatek_diy.makefile b/conf/firmwares/subsystems/shared/gps_mediatek_diy.makefile index fb45804b90..d3df31cd5e 100644 --- a/conf/firmwares/subsystems/shared/gps_mediatek_diy.makefile +++ b/conf/firmwares/subsystems/shared/gps_mediatek_diy.makefile @@ -3,26 +3,43 @@ # Mediatek MT3329, DIYDrones V1.4/1.6 protocol GPS_LED ?= none -MTK_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z) +MTK_GPS_PORT ?= $(GPS_PORT) +MTK_GPS_BAUD ?= $(GPS_BAUD) + +MTK_GPS_PORT_LOWER=$(shell echo $(MTK_GPS_PORT) | tr A-Z a-z) ap.CFLAGS += -DUSE_GPS -DGPS_CONFIGURE -ap.CFLAGS += -DGPS_LINK=$(MTK_GPS_PORT_LOWER) -ap.CFLAGS += -DUSE_$(GPS_PORT) -ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD) +ap.CFLAGS += -DMTK_GPS_LINK=$(MTK_GPS_PORT_LOWER) +ap.CFLAGS += -DUSE_$(MTK_GPS_PORT) +ap.CFLAGS += -D$(MTK_GPS_PORT)_BAUD=$(MTK_GPS_BAUD) ifneq ($(GPS_LED),none) ap.CFLAGS += -DGPS_LED=$(GPS_LED) endif +ifdef SECONDARY_GPS +ifneq (,$(findstring $(SECONDARY_GPS), mtk mediatek)) +# this is the secondary GPS +ap.CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_mtk.h\" +ap.CFLAGS += -DSECONDARY_GPS=gps_mtk +else ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_mtk.h\" -ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_mtk.c +ap.CFLAGS += -DPRIMARY_GPS=gps_mtk +endif +else +# plain old single GPS usage +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_mtk.h\" +endif -$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c +ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_mtk.c +ap.srcs += $(SRC_SUBSYSTEMS)/gps.c sim.CFLAGS += -DUSE_GPS sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c +sim.srcs += $(SRC_SUBSYSTEMS)/gps.c nps.CFLAGS += -DUSE_GPS nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c +nps.srcs += $(SRC_SUBSYSTEMS)/gps.c diff --git a/conf/firmwares/subsystems/shared/gps_nmea.makefile b/conf/firmwares/subsystems/shared/gps_nmea.makefile index ca3934c2f6..235260b62e 100644 --- a/conf/firmwares/subsystems/shared/gps_nmea.makefile +++ b/conf/firmwares/subsystems/shared/gps_nmea.makefile @@ -3,26 +3,43 @@ # NMEA GPS unit GPS_LED ?= none -NMEA_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z) +NMEA_GPS_PORT ?= $(GPS_PORT) +NMEA_GPS_BAUD ?= $(GPS_BAUD) + +NMEA_GPS_PORT_LOWER=$(shell echo $(NMEA_GPS_PORT) | tr A-Z a-z) ap.CFLAGS += -DUSE_GPS -ap.CFLAGS += -DGPS_LINK=$(NMEA_GPS_PORT_LOWER) -ap.CFLAGS += -DUSE_$(GPS_PORT) -ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD) +ap.CFLAGS += -DNMEA_GPS_LINK=$(NMEA_GPS_PORT_LOWER) +ap.CFLAGS += -DUSE_$(NMEA_GPS_PORT) +ap.CFLAGS += -D$(NMEA_GPS_PORT)_BAUD=$(NMEA_GPS_BAUD) ifneq ($(GPS_LED),none) ap.CFLAGS += -DGPS_LED=$(GPS_LED) endif +ifdef SECONDARY_GPS +ifneq (,$(findstring $(SECONDARY_GPS), nmea)) +# this is the secondary GPS +ap.CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_nmea.h\" +ap.CFLAGS += -DSECONDARY_GPS=gps_nmea +else ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\" -ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_nmea.c +ap.CFLAGS += -DPRIMARY_GPS=gps_nmea +endif +else +# plain old single GPS usage +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\" +endif -$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c +ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_nmea.c +ap.srcs += $(SRC_SUBSYSTEMS)/gps.c sim.CFLAGS += -DUSE_GPS sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c +sim.srcs += $(SRC_SUBSYSTEMS)/gps.c nps.CFLAGS += -DUSE_GPS nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c +nps.srcs += $(SRC_SUBSYSTEMS)/gps.c diff --git a/conf/firmwares/subsystems/shared/gps_piksi.makefile b/conf/firmwares/subsystems/shared/gps_piksi.makefile index 50d4879b97..2ec494afb2 100644 --- a/conf/firmwares/subsystems/shared/gps_piksi.makefile +++ b/conf/firmwares/subsystems/shared/gps_piksi.makefile @@ -3,21 +3,35 @@ # Swift-Nav Piksi RTK module GPS_LED ?= none +PIKSI_GPS_PORT ?= $(GPS_PORT) +PIKSI_GPS_BAUD ?= $(GPS_BAUD) -GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z) +PIKSI_GPS_PORT_LOWER=$(shell echo $(PIKSI_GPS_PORT) | tr A-Z a-z) ap.CFLAGS += -DUSE_GPS -ap.CFLAGS += -DUSE_$(GPS_PORT) -D$(GPS_PORT)_BAUD=B115200 -ap.CFLAGS += -DGPS_LINK=$(GPS_PORT_LOWER) +ap.CFLAGS += -DUSE_$(PIKSI_GPS_PORT) -D$(PIKSI_GPS_PORT)_BAUD=B115200 +ap.CFLAGS += -DPIKSI_GPS_LINK=$(PIKSI_GPS_PORT_LOWER) ifneq ($(GPS_LED),none) ap.CFLAGS += -DGPS_LED=$(GPS_LED) endif -$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c - +ifdef SECONDARY_GPS +ifneq (,$(findstring $(SECONDARY_GPS), piksi)) +# this is the secondary GPS +ap.CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_piksi.h\" +ap.CFLAGS += -DSECONDARY_GPS=gps_piksi +else ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_piksi.h\" -ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_piksi.c +ap.CFLAGS += -DPRIMARY_GPS=gps_piksi +endif +else +# plain old single GPS usage +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_piksi.h\" +endif + +ap.srcs += $(SRC_SUBSYSTEMS)/gps.c +ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_piksi.c # libsbp ap.CFLAGS += -I$(PAPARAZZI_SRC)/sw/ext/libsbp/c/include @@ -26,8 +40,9 @@ ap.srcs += $(PAPARAZZI_SRC)/sw/ext/libsbp/c/src/sbp.c $(PAPARAZZI_SRC)/sw/ext/l sim.CFLAGS += -DUSE_GPS sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c +sim.srcs += $(SRC_SUBSYSTEMS)/gps.c nps.CFLAGS += -DUSE_GPS nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c - +nps.srcs += $(SRC_SUBSYSTEMS)/gps.c diff --git a/conf/firmwares/subsystems/shared/gps_skytraq.makefile b/conf/firmwares/subsystems/shared/gps_skytraq.makefile index 3ab9266886..2c5bb00884 100644 --- a/conf/firmwares/subsystems/shared/gps_skytraq.makefile +++ b/conf/firmwares/subsystems/shared/gps_skytraq.makefile @@ -1,27 +1,43 @@ # Hey Emacs, this is a -*- makefile -*- GPS_LED ?= none -SKYTRAQ_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z) +SKYTRAQ_GPS_PORT ?= $(GPS_PORT) +SKYTRAQ_GPS_BAUD ?= $(GPS_BAUD) + +SKYTRAQ_GPS_PORT_LOWER=$(shell echo $(SKYTRAQ_GPS_PORT) | tr A-Z a-z) ap.CFLAGS += -DUSE_GPS -ap.CFLAGS += -DGPS_LINK=$(SKYTRAQ_GPS_PORT_LOWER) -ap.CFLAGS += -DUSE_$(GPS_PORT) -ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD) +ap.CFLAGS += -DSKYTRAQ_GPS_LINK=$(SKYTRAQ_SKYTRAQ_GPS_PORT_LOWER) +ap.CFLAGS += -DUSE_$(SKYTRAQ_GPS_PORT) +ap.CFLAGS += -D$(SKYTRAQ_GPS_PORT)_BAUD=$(SKYTRAQ_GPS_BAUD) ifneq ($(GPS_LED),none) ap.CFLAGS += -DGPS_LED=$(GPS_LED) endif +ifdef SECONDARY_GPS +ifneq (,$(findstring $(SECONDARY_GPS), skytraq)) +# this is the secondary GPS +ap.CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_skytraq.h\" +ap.CFLAGS += -DSECONDARY_GPS=gps_skytraq +else ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_skytraq.h\" -ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_skytraq.c +ap.CFLAGS += -DPRIMARY_GPS=gps_skytraq +endif +else +# plain old single GPS usage +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_skytraq.h\" +endif -$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c +ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_skytraq.c +ap.srcs += $(SRC_SUBSYSTEMS)/gps.c sim.CFLAGS += -DUSE_GPS sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c +sim.srcs += $(SRC_SUBSYSTEMS)/gps.c nps.CFLAGS += -DUSE_GPS -nps.srcs += $(SRC_SUBSYSTEMS)/gps.c nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c +nps.srcs += $(SRC_SUBSYSTEMS)/gps.c diff --git a/conf/firmwares/subsystems/shared/gps_ublox.makefile b/conf/firmwares/subsystems/shared/gps_ublox.makefile index d18fc98df7..7685d747f6 100644 --- a/conf/firmwares/subsystems/shared/gps_ublox.makefile +++ b/conf/firmwares/subsystems/shared/gps_ublox.makefile @@ -2,26 +2,43 @@ # UBlox LEA GPS_LED ?= none -UBX_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z) +UBX_GPS_PORT ?= $(GPS_PORT) +UBX_GPS_BAUD ?= $(GPS_BAUD) + +UBX_GPS_PORT_LOWER=$(shell echo $(UBX_GPS_PORT) | tr A-Z a-z) ap.CFLAGS += -DUSE_GPS -DUBX -ap.CFLAGS += -DGPS_LINK=$(UBX_GPS_PORT_LOWER) -ap.CFLAGS += -DUSE_$(GPS_PORT) -ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD) +ap.CFLAGS += -DUBX_GPS_LINK=$(UBX_GPS_PORT_LOWER) +ap.CFLAGS += -DUSE_$(UBX_GPS_PORT) +ap.CFLAGS += -D$(UBX_GPS_PORT)_BAUD=$(UBX_GPS_BAUD) ifneq ($(GPS_LED),none) ap.CFLAGS += -DGPS_LED=$(GPS_LED) endif +ifdef SECONDARY_GPS +ifneq (,$(findstring $(SECONDARY_GPS), ublox)) +# this is the secondary GPS +ap.CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/gps/gps_ubx.h\" +ap.CFLAGS += -DSECONDARY_GPS=gps_ubx +else ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\" -ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_ubx.c +ap.CFLAGS += -DPRIMARY_GPS=gps_ubx +endif +else +# plain old single GPS usage +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\" +endif -$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c +ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_ubx.c +ap.srcs += $(SRC_SUBSYSTEMS)/gps.c sim.CFLAGS += -DUSE_GPS sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c +sim.srcs += $(SRC_SUBSYSTEMS)/gps.c nps.CFLAGS += -DUSE_GPS nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c +nps.srcs += $(SRC_SUBSYSTEMS)/gps.c diff --git a/conf/settings/rotorcraft_basic_multi.xml b/conf/settings/rotorcraft_basic_multi.xml new file mode 100644 index 0000000000..bc22a69ffd --- /dev/null +++ b/conf/settings/rotorcraft_basic_multi.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/data/pictures/gcs_icons/gps.png b/data/pictures/gcs_icons/gps.png new file mode 100644 index 0000000000..dde583261e Binary files /dev/null and b/data/pictures/gcs_icons/gps.png differ diff --git a/data/pictures/gcs_icons/gps1.png b/data/pictures/gcs_icons/gps1.png new file mode 100644 index 0000000000..6ae346e0dd Binary files /dev/null and b/data/pictures/gcs_icons/gps1.png differ diff --git a/data/pictures/gcs_icons/gps2.png b/data/pictures/gcs_icons/gps2.png new file mode 100644 index 0000000000..354bf81bc7 Binary files /dev/null and b/data/pictures/gcs_icons/gps2.png differ diff --git a/sw/airborne/modules/ahrs/ahrs_infrared.c b/sw/airborne/modules/ahrs/ahrs_infrared.c index 2bb92f73a6..e6a90fb742 100644 --- a/sw/airborne/modules/ahrs/ahrs_infrared.c +++ b/sw/airborne/modules/ahrs/ahrs_infrared.c @@ -53,7 +53,7 @@ static float heading; static abi_event gyro_ev; #ifndef AHRS_INFRARED_GPS_ID -#define AHRS_INFRARED_GPS_ID ABI_BROADCAST +#define AHRS_INFRARED_GPS_ID GPS_MULTI_ID #endif static abi_event gps_ev; void ahrs_infrared_update_gps(struct GpsState *gps_s); diff --git a/sw/airborne/modules/gps/gps_ubx_ucenter.c b/sw/airborne/modules/gps/gps_ubx_ucenter.c index b9c187b9d6..2466744b81 100644 --- a/sw/airborne/modules/gps/gps_ubx_ucenter.c +++ b/sw/airborne/modules/gps/gps_ubx_ucenter.c @@ -58,7 +58,7 @@ static bool_t gps_ubx_ucenter_configure(uint8_t nr); #define GPS_UBX_UCENTER_REPLY_CFG_PRT 4 // Target baudrate for the module -#define UBX_GPS_BAUD (GPS_LINK).baudrate +#define UBX_GPS_BAUD (UBX_GPS_LINK).baudrate // All U-Center data struct gps_ubx_ucenter_struct gps_ubx_ucenter; @@ -87,7 +87,7 @@ void gps_ubx_ucenter_init(void) gps_ubx_ucenter.replies[i] = 0; } - gps_ubx_ucenter.dev = &(GPS_LINK).device; + gps_ubx_ucenter.dev = &(UBX_GPS_LINK).device; } @@ -236,7 +236,7 @@ static bool_t gps_ubx_ucenter_autobaud(uint8_t nr) break; case 2: gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; - uart_periph_set_baudrate(&(GPS_LINK), B38400); // Try the most common first? + uart_periph_set_baudrate(&(UBX_GPS_LINK), B38400); // Try the most common first? gps_ubx_ucenter_config_port_poll(); break; case 3: @@ -245,7 +245,7 @@ static bool_t gps_ubx_ucenter_autobaud(uint8_t nr) return FALSE; } gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; - uart_periph_set_baudrate(&(GPS_LINK), B9600); // Maybe the factory default? + uart_periph_set_baudrate(&(UBX_GPS_LINK), B9600); // Maybe the factory default? gps_ubx_ucenter_config_port_poll(); break; case 4: @@ -254,7 +254,7 @@ static bool_t gps_ubx_ucenter_autobaud(uint8_t nr) return FALSE; } gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; - uart_periph_set_baudrate(&(GPS_LINK), B57600); // The high-rate default? + uart_periph_set_baudrate(&(UBX_GPS_LINK), B57600); // The high-rate default? gps_ubx_ucenter_config_port_poll(); break; case 5: @@ -263,7 +263,7 @@ static bool_t gps_ubx_ucenter_autobaud(uint8_t nr) return FALSE; } gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; - uart_periph_set_baudrate(&(GPS_LINK), B4800); // Default NMEA baudrate? + uart_periph_set_baudrate(&(UBX_GPS_LINK), B4800); // Default NMEA baudrate? gps_ubx_ucenter_config_port_poll(); break; case 6: @@ -272,7 +272,7 @@ static bool_t gps_ubx_ucenter_autobaud(uint8_t nr) return FALSE; } gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; - uart_periph_set_baudrate(&(GPS_LINK), B115200); // Last possible option for ublox + uart_periph_set_baudrate(&(UBX_GPS_LINK), B115200); // Last possible option for ublox gps_ubx_ucenter_config_port_poll(); break; case 7: @@ -281,7 +281,7 @@ static bool_t gps_ubx_ucenter_autobaud(uint8_t nr) return FALSE; } gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; - uart_periph_set_baudrate(&(GPS_LINK), B230400); // Last possible option for ublox + uart_periph_set_baudrate(&(UBX_GPS_LINK), B230400); // Last possible option for ublox gps_ubx_ucenter_config_port_poll(); break; case 8: @@ -293,7 +293,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 - uart_periph_set_baudrate(&(GPS_LINK), B9600); + uart_periph_set_baudrate(&(UBX_GPS_LINK), B9600); return FALSE; default: break; @@ -343,7 +343,7 @@ static bool_t gps_ubx_ucenter_autobaud(uint8_t nr) #define RESERVED 0 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 version message couldn't be fetched, default to NAV5 if (gps_ubx_ucenter.sw_ver_h < 5 && gps_ubx_ucenter.hw_ver_h < 6 && @@ -465,7 +465,7 @@ static bool_t gps_ubx_ucenter_configure(uint8_t nr) } #endif // Now the GPS baudrate should have changed - uart_periph_set_baudrate(&(GPS_LINK), gps_ubx_ucenter.baud_target); + uart_periph_set_baudrate(&(UBX_GPS_LINK), gps_ubx_ucenter.baud_target); gps_ubx_ucenter.baud_run = UART_SPEED(gps_ubx_ucenter.baud_target); UbxSend_MON_GET_VER(gps_ubx_ucenter.dev); break; @@ -559,4 +559,3 @@ 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/ins/ahrs_chimu_spi.c b/sw/airborne/modules/ins/ahrs_chimu_spi.c index 18ed9222d2..a52c2bdadf 100644 --- a/sw/airborne/modules/ins/ahrs_chimu_spi.c +++ b/sw/airborne/modules/ins/ahrs_chimu_spi.c @@ -40,6 +40,13 @@ struct AhrsChimu ahrs_chimu; void ahrs_chimu_update_gps(uint8_t gps_fix, uint16_t gps_speed_3d); #include "subsystems/abi.h" +/** ABI binding for gps data. + * Used for GPS ABI messages. + */ +#ifndef AHRS_CHIMU_GPS_ID +#define AHRS_CHIMU_GPS_ID GPS_MULTI_ID +#endif +PRINT_CONFIG_VAR(AHRS_CHIMU_GPS_ID) static abi_event gps_ev; static void gps_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), @@ -58,7 +65,7 @@ void ahrs_chimu_register(void) { ahrs_chimu_init(); ahrs_register_impl(ahrs_chimu_enable_output); - AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); + AbiBindMsgGPS(AHRS_CHIMU_GPS_ID, &gps_ev, gps_cb); } void ahrs_chimu_init(void) diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index 5c3d3bc9d3..f24b6d5f0c 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -259,6 +259,13 @@ void ins_xsens_init(void) } #include "subsystems/abi.h" +/** ABI binding for gps data. + * Used for GPS ABI messages. + */ +#ifndef INS_XSENS_GPS_ID +#define INS_XSENS_GPS_ID GPS_MULTI_ID +#endif +PRINT_CONFIG_VAR(INS_XSENS_GPS_ID) static abi_event gps_ev; static void gps_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), @@ -270,7 +277,7 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)), void ins_xsens_register(void) { ins_register_impl(ins_xsens_init); - AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); + AbiBindMsgGPS(INS_XSENS_GPS_ID, &gps_ev, gps_cb); } void ins_xsens_update_gps(struct GpsState *gps_s) @@ -292,7 +299,7 @@ void ins_xsens_update_gps(struct GpsState *gps_s) #endif #if USE_GPS_XSENS -void gps_impl_init(void) +void gps_xsens_init(void) { gps.nb_channels = 0; } @@ -715,7 +722,6 @@ void parse_ins_msg(void) } - void parse_ins_buffer(uint8_t c) { ck += c; @@ -768,3 +774,13 @@ restart: xsens_status = UNINIT; return; } + +#ifdef USE_GPS_XSENS +/* + * register callbacks & structs + */ +void gps_xsens_register(void) +{ + gps_register_impl(gps_xsens_init, NULL, GPS_XSENS_ID); +} +#endif diff --git a/sw/airborne/modules/ins/ins_xsens.h b/sw/airborne/modules/ins/ins_xsens.h index 43b3b1f05d..2b7bf22a9d 100644 --- a/sw/airborne/modules/ins/ins_xsens.h +++ b/sw/airborne/modules/ins/ins_xsens.h @@ -79,7 +79,11 @@ extern void ins_xsens_register(void); #if USE_GPS_XSENS -#define GpsEvent() {} +#ifndef PRIMARY_GPS +#define PRIMARY_GPS gps_xsens +#endif +extern void gps_xsens_init(void); +extern void gps_xsens_register(void); #endif #endif diff --git a/sw/airborne/modules/ins/ins_xsens700.c b/sw/airborne/modules/ins/ins_xsens700.c index 44f10dd576..562b5179df 100644 --- a/sw/airborne/modules/ins/ins_xsens700.c +++ b/sw/airborne/modules/ins/ins_xsens700.c @@ -190,6 +190,13 @@ void ins_xsens_init(void) } #include "subsystems/abi.h" +/** ABI binding for gps data. + * Used for GPS ABI messages. + */ +#ifndef INS_XSENS700_GPS_ID +#define INS_XSENS700_GPS_ID GPS_MULTI_ID +#endif +PRINT_CONFIG_VAR(INS_XSENS700_GPS_ID) static abi_event gps_ev; static void gps_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), @@ -201,7 +208,7 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)), void ins_xsens_register(void) { ins_register_impl(ins_xsens_init); - AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); + AbiBindMsgGPS(INS_XSENS700_GPS_ID, &gps_ev, gps_cb); } void ins_xsens_update_gps(struct GpsState *gps_s) @@ -223,7 +230,7 @@ void ins_xsens_update_gps(struct GpsState *gps_s) #endif #if USE_GPS_XSENS -void gps_impl_init(void) +void gps_xsens_impl_init(void) { gps.nb_channels = 0; } @@ -616,3 +623,13 @@ restart: xsens_status = UNINIT; return; } + +#ifdef USE_GPS_XSENS +/* + * register callbacks & structs + */ +void gps_xsens_register(void) +{ + gps_register_impl(gps_xsens_init, NULL, GPS_XSENS_ID); +} +#endif diff --git a/sw/airborne/modules/stereocam/stereocam2state/stereocam2state.c b/sw/airborne/modules/stereocam/stereocam2state/stereocam2state.c index d5b28784eb..c4580d6cc3 100644 --- a/sw/airborne/modules/stereocam/stereocam2state/stereocam2state.c +++ b/sw/airborne/modules/stereocam/stereocam2state/stereocam2state.c @@ -23,7 +23,7 @@ /** ABI binding for gps messages*/ #ifndef STEREOCAM_GPS_ID -#define STEREOCAM_GPS_ID ABI_BROADCAST +#define STEREOCAM_GPS_ID GPS_MULTI_ID #endif static abi_event gps_ev; diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h index 29254371dd..b5a320c979 100644 --- a/sw/airborne/subsystems/abi_sender_ids.h +++ b/sw/airborne/subsystems/abi_sender_ids.h @@ -158,7 +158,10 @@ #define GPS_SIM_ID 11 #endif - +#ifndef GPS_MULTI_ID +#define GPS_MULTI_ID 12 +#endif + /* * IDs of IMU sensors (accel, gyro) */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c index ea202ae141..0607ee9fe7 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c @@ -120,6 +120,13 @@ PRINT_CONFIG_VAR(AHRS_FC_IMU_ID) #define AHRS_FC_MAG_ID AHRS_FC_IMU_ID #endif PRINT_CONFIG_VAR(AHRS_FC_MAG_ID) +/** ABI binding for gps data. + * Used for GPS ABI messages. + */ +#ifndef AHRS_FC_GPS_ID +#define AHRS_FC_GPS_ID GPS_MULTI_ID +#endif +PRINT_CONFIG_VAR(AHRS_FC_GPS_ID) static abi_event gyro_ev; static abi_event accel_ev; static abi_event mag_ev; @@ -288,7 +295,7 @@ void ahrs_fc_register(void) AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb); AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb); AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb); - AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); + AbiBindMsgGPS(AHRS_FC_GPS_ID, &gps_ev, gps_cb); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_EULER, send_euler); diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c index 31b1042d96..c5df92a6ce 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c @@ -71,6 +71,13 @@ PRINT_CONFIG_VAR(AHRS_DCM_IMU_ID) #define AHRS_DCM_MAG_ID AHRS_DCM_IMU_ID #endif PRINT_CONFIG_VAR(AHRS_DCM_MAG_ID) +/** ABI binding for gps data. + * Used for GPS ABI messages. + */ +#ifndef AHRS_DCM_GPS_ID +#define AHRS_DCM_GPS_ID GPS_MULTI_ID +#endif +PRINT_CONFIG_VAR(AHRS_DCM_GPS_ID) static abi_event gyro_ev; static abi_event accel_ev; static abi_event mag_ev; @@ -202,7 +209,7 @@ void ahrs_dcm_register(void) AbiBindMsgIMU_MAG_INT32(AHRS_DCM_MAG_ID, &mag_ev, mag_cb); AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb); AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb); - AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); + AbiBindMsgGPS(AHRS_DCM_GPS_ID, &gps_ev, gps_cb); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status); diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c index e3730539db..accd5a40f8 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c @@ -121,6 +121,13 @@ PRINT_CONFIG_VAR(AHRS_ICQ_IMU_ID) #define AHRS_ICQ_MAG_ID AHRS_ICQ_IMU_ID #endif PRINT_CONFIG_VAR(AHRS_ICQ_MAG_ID) +/** ABI binding for gps data. + * Used for GPS ABI messages. + */ +#ifndef AHRS_ICQ_GPS_ID +#define AHRS_ICQ_GPS_ID GPS_MULTI_ID +#endif +PRINT_CONFIG_VAR(AHRS_ICQ_GPS_ID) static abi_event gyro_ev; static abi_event accel_ev; static abi_event mag_ev; @@ -276,7 +283,7 @@ void ahrs_icq_register(void) AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb); AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb); AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb); - AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); + AbiBindMsgGPS(AHRS_ICQ_GPS_ID, &gps_ev, gps_cb); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_QUAT_INT, send_quat); diff --git a/sw/airborne/subsystems/gps.c b/sw/airborne/subsystems/gps.c index 15a94a50e5..51571751be 100644 --- a/sw/airborne/subsystems/gps.c +++ b/sw/airborne/subsystems/gps.c @@ -24,8 +24,35 @@ * */ +#include "subsystems/abi.h" #include "subsystems/gps.h" #include "led.h" +#include "subsystems/settings.h" +#include "generated/settings.h" + +#ifndef PRIMARY_GPS +#error "PRIMARY_GPS not set!" +#else +PRINT_CONFIG_VAR(PRIMARY_GPS) +#endif + +#ifdef SECONDARY_GPS +PRINT_CONFIG_VAR(SECONDARY_GPS) +#endif + +#define __RegisterGps(_x) _x ## _register() +#define _RegisterGps(_x) __RegisterGps(_x) +#define RegisterGps(_x) _RegisterGps(_x) + +/** maximum number of GPS implementations that can register */ +#ifdef SECONDARY_GPS +#define GPS_NB_IMPL 2 +#else +#define GPS_NB_IMPL 1 +#endif + +#define PRIMARY_GPS_INSTANCE 0 +#define SECONDARY_GPS_INSTANCE 1 #ifdef GPS_POWER_GPIO #include "mcu_periph/gpio.h" @@ -36,11 +63,27 @@ #endif #define MSEC_PER_WEEK (1000*60*60*24*7) +#define TIME_TO_SWITCH 5000 //ten s in ms struct GpsState gps; struct GpsTimeSync gps_time_sync; +#ifdef SECONDARY_GPS +static uint8_t current_gps_id = 0; +#endif + +uint8_t multi_gps_mode; + +/* gps structs */ +struct GpsInstance { + ImplGpsInit init; + ImplGpsEvent event; + uint8_t id; +}; + +struct GpsInstance GpsInstances[GPS_NB_IMPL]; + #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" @@ -109,7 +152,8 @@ static void send_gps_int(struct transport_tx *trans, struct link_device *dev) &gps.tow, &gps.pdop, &gps.num_sv, - &gps.fix); + &gps.fix, + &gps.comp_id); // send SVINFO for available satellites that have new data send_svinfo_available(trans, dev); } @@ -132,8 +176,96 @@ static void send_gps_sol(struct transport_tx *trans, struct link_device *dev) } #endif +void gps_periodic_check(void) +{ + if (sys_time.nb_sec - gps.last_msg_time > GPS_TIMEOUT) { + gps.fix = GPS_FIX_NONE; + } +} + +#ifdef SECONDARY_GPS +static uint8_t gps_multi_switch(struct GpsState *gps_s) { + static uint32_t time_since_last_gps_switch = 0; + + if (multi_gps_mode == GPS_MODE_PRIMARY){ + return GpsInstances[PRIMARY_GPS_INSTANCE].id; + } else if (multi_gps_mode == GPS_MODE_SECONDARY){ + return GpsInstances[SECONDARY_GPS_INSTANCE].id; + } else{ + if (gps_s->fix > gps.fix){ + return gps_s->comp_id; + } else if (gps.fix > gps_s->fix){ + return gps.comp_id; + } else{ + if (get_sys_time_msec() - time_since_last_gps_switch > TIME_TO_SWITCH) { + if (gps_s->num_sv > gps.num_sv) { + current_gps_id = gps_s->comp_id; + time_since_last_gps_switch = get_sys_time_msec(); + } else if (gps.num_sv > gps_s->num_sv) { + current_gps_id = gps.comp_id; + time_since_last_gps_switch = get_sys_time_msec(); + } + } + } + } + return current_gps_id; +} +#endif /*SECONDARY_GPS*/ + +static abi_event gps_ev; +static void gps_cb(uint8_t sender_id, + uint32_t stamp __attribute__((unused)), + struct GpsState *gps_s) +{ + if (sender_id == GPS_MULTI_ID) { + return; + } + uint32_t now_ts = get_sys_time_usec(); +#ifdef SECONDARY_GPS + current_gps_id = gps_multi_switch(gps_s); + if (gps_s->comp_id == current_gps_id) { + gps = *gps_s; + AbiSendMsgGPS(GPS_MULTI_ID, now_ts, gps_s); + } +#else + gps = *gps_s; + AbiSendMsgGPS(GPS_MULTI_ID, now_ts, gps_s); +#endif +} + +/* + * handle gps switching and updating gps instances + */ +void GpsEvent(void) { + // run each gps event + for (int i = 0 ; i < GPS_NB_IMPL ; i++) { + if (GpsInstances[i].event != NULL) { + GpsInstances[i].event(); + } + } +} + +/* + * register gps structs for callback + */ +void gps_register_impl(ImplGpsInit init, ImplGpsEvent event, uint8_t id) +{ + int i; + for (i=0; i < GPS_NB_IMPL; i++) { + if (GpsInstances[i].init == NULL) { + GpsInstances[i].init = init; + GpsInstances[i].event = event; + GpsInstances[i].id = id; + break; + } + } + +} + void gps_init(void) { + multi_gps_mode = MULTI_GPS_MODE; + gps.valid_fields = 0; gps.fix = GPS_FIX_NONE; gps.week = 0; @@ -144,6 +276,7 @@ void gps_init(void) gps.last_3dfix_time = 0; gps.last_msg_ticks = 0; gps.last_msg_time = 0; + #ifdef GPS_POWER_GPIO gpio_setup_output(GPS_POWER_GPIO); GPS_POWER_GPIO_ON(GPS_POWER_GPIO); @@ -151,10 +284,20 @@ void gps_init(void) #ifdef GPS_LED LED_OFF(GPS_LED); #endif -#ifdef GPS_TYPE_H - gps_impl_init(); + + RegisterGps(PRIMARY_GPS); +#ifdef SECONDARY_GPS + RegisterGps(SECONDARY_GPS); #endif + for (int i=0; i < GPS_NB_IMPL; i++) { + if (GpsInstances[i].init != NULL) { + GpsInstances[i].init(); + } + } + + AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); + #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS, send_gps); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_INT, send_gps_int); @@ -164,13 +307,6 @@ void gps_init(void) #endif } -void gps_periodic_check(void) -{ - if (sys_time.nb_sec - gps.last_msg_time > GPS_TIMEOUT) { - gps.fix = GPS_FIX_NONE; - } -} - uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks) { uint32_t clock_delta; diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h index cc7957d927..ce00cfe780 100644 --- a/sw/airborne/subsystems/gps.h +++ b/sw/airborne/subsystems/gps.h @@ -34,11 +34,6 @@ #include "mcu_periph/sys_time.h" -/* GPS model specific implementation or sim */ -#ifdef GPS_TYPE_H -#include GPS_TYPE_H -#endif - #define GPS_FIX_NONE 0x00 ///< No GPS fix #define GPS_FIX_2D 0x02 ///< 2D GPS fix #define GPS_FIX_3D 0x03 ///< 3D GPS fix @@ -50,10 +45,6 @@ #define GpsIsLost() !GpsFixValid() #endif -#ifndef GPS_NB_CHANNELS -#define GPS_NB_CHANNELS 1 -#endif - #define GPS_VALID_POS_ECEF_BIT 0 #define GPS_VALID_POS_LLA_BIT 1 #define GPS_VALID_POS_UTM_BIT 2 @@ -62,6 +53,20 @@ #define GPS_VALID_HMSL_BIT 5 #define GPS_VALID_COURSE_BIT 6 +#ifndef GPS_NB_CHANNELS +#define GPS_NB_CHANNELS 16 +#endif + +#define GPS_MODE_PRIMARY 0 +#define GPS_MODE_SECONDARY 1 +#define GPS_MODE_AUTO 2 + +#ifndef MULTI_GPS_MODE +#define MULTI_GPS_MODE GPS_MODE_AUTO +#endif + +extern uint8_t multi_gps_mode; + /** data structure for Space Vehicle Information of a single satellite */ struct SVinfo { uint8_t svid; ///< Satellite ID @@ -75,6 +80,7 @@ struct SVinfo { /** data structure for GPS information */ struct GpsState { uint8_t valid_fields; ///< bitfield indicating valid fields (GPS_VALID_x_BIT) + uint8_t comp_id; ///< id of current gps struct EcefCoor_i ecef_pos; ///< position in ECEF in cm struct LlaCoor_i lla_pos; ///< position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid) @@ -114,12 +120,27 @@ struct GpsTimeSync { /** global GPS state */ extern struct GpsState gps; +typedef void (*ImplGpsInit)(void); +typedef void (*ImplGpsEvent)(void); + + +extern void GpsEvent(void); + +/** + * register callbacks and state pointers + */ +extern void gps_register_impl(ImplGpsInit init, ImplGpsEvent event, uint8_t id); + +#ifdef GPS_TYPE_H +#include GPS_TYPE_H +#endif +#ifdef GPS_SECONDARY_TYPE_H +#include GPS_SECONDARY_TYPE_H +#endif + /** initialize the global GPS state */ extern void gps_init(void); -/** GPS model specific init implementation */ -extern void gps_impl_init(void); - /** GPS packet injection (default empty) */ extern void gps_inject_data(uint8_t packet_id, uint8_t length, uint8_t *data); diff --git a/sw/airborne/subsystems/gps/gps_datalink.c b/sw/airborne/subsystems/gps/gps_datalink.c index 788a3569c8..ef26d0505c 100644 --- a/sw/airborne/subsystems/gps/gps_datalink.c +++ b/sw/airborne/subsystems/gps/gps_datalink.c @@ -37,15 +37,14 @@ struct LtpDef_i ltp_def; struct EnuCoor_i enu_pos, enu_speed; -bool_t gps_available; ///< Is set to TRUE when a new REMOTE_GPS packet is received and parsed +struct GpsDatalink gps_datalink; /** GPS initialization */ -void gps_impl_init(void) +void gps_datalink_init(void) { - gps.fix = GPS_FIX_NONE; - gps_available = FALSE; - gps.gspeed = 700; // To enable course setting - gps.cacc = 0; // To enable course setting + gps_datalink.fix = GPS_FIX_NONE; + gps_datalink.gspeed = 700; // To enable course setting + gps_datalink.cacc = 0; // To enable course setting struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */ llh_nav0.lat = NAV_LAT0; @@ -73,11 +72,11 @@ void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_x enu_pos.z = (int32_t)(pos_xyz & 0x3FF); // bits 9-0 z position in cm // Convert the ENU coordinates to ECEF - ecef_of_enu_point_i(&gps.ecef_pos, <p_def, &enu_pos); - SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT); + ecef_of_enu_point_i(&gps_datalink.ecef_pos, <p_def, &enu_pos); + SetBit(gps_datalink.valid_fields, GPS_VALID_POS_ECEF_BIT); - lla_of_ecef_i(&gps.lla_pos, &gps.ecef_pos); - SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); + lla_of_ecef_i(&gps_datalink.lla_pos, &gps_datalink.ecef_pos); + SetBit(gps_datalink.valid_fields, GPS_VALID_POS_LLA_BIT); enu_speed.x = (int32_t)((speed_xyz >> 21) & 0x7FF); // bits 31-21 speed x in cm/s if (enu_speed.x & 0x400) { @@ -92,34 +91,33 @@ void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_x enu_speed.z |= 0xFFFFFC00; // sign extend for twos complements } - ecef_of_enu_vect_i(&gps.ecef_vel , <p_def , &enu_speed); - SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT); + ecef_of_enu_vect_i(&gps_datalink.ecef_vel , <p_def , &enu_speed); + SetBit(gps_datalink.valid_fields, GPS_VALID_VEL_ECEF_BIT); - gps.ned_vel.x = enu_speed.y; - gps.ned_vel.y = enu_speed.x; - gps.ned_vel.z = -enu_speed.z; - SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT); + gps_datalink.ned_vel.x = enu_speed.y; + gps_datalink.ned_vel.y = enu_speed.x; + gps_datalink.ned_vel.z = -enu_speed.z; + SetBit(gps_datalink.valid_fields, GPS_VALID_VEL_NED_BIT); - gps.hmsl = ltp_def.hmsl + enu_pos.z * 10; - SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); + gps_datalink.hmsl = ltp_def.hmsl + enu_pos.z * 10; + SetBit(gps_datalink.valid_fields, GPS_VALID_HMSL_BIT); - gps.course = ((int32_t)heading) * 1e3; - SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); + gps_datalink.course = ((int32_t)heading) * 1e3; + SetBit(gps_datalink.valid_fields, GPS_VALID_COURSE_BIT); - gps.num_sv = num_sv; - gps.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); - gps.fix = GPS_FIX_3D; // set 3D fix to true - gps_available = TRUE; // set GPS available to true + gps_datalink.num_sv = num_sv; + gps_datalink.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); + gps_datalink.fix = GPS_FIX_3D; // set 3D fix to true // publish new GPS data uint32_t now_ts = get_sys_time_usec(); - gps.last_msg_ticks = sys_time.nb_sec_rem; - gps.last_msg_time = sys_time.nb_sec; - if (gps.fix == GPS_FIX_3D) { - gps.last_3dfix_ticks = sys_time.nb_sec_rem; - gps.last_3dfix_time = sys_time.nb_sec; + gps_datalink.last_msg_ticks = sys_time.nb_sec_rem; + gps_datalink.last_msg_time = sys_time.nb_sec; + if (gps_datalink.fix == GPS_FIX_3D) { + gps_datalink.last_3dfix_ticks = sys_time.nb_sec_rem; + gps_datalink.last_3dfix_time = sys_time.nb_sec; } - AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps); + AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps_datalink.state); } /** Parse the REMOTE_GPS datalink packet */ @@ -127,49 +125,56 @@ void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t e int32_t alt, int32_t hmsl, int32_t ecef_xd, int32_t ecef_yd, int32_t ecef_zd, uint32_t tow, int32_t course) { - gps.lla_pos.lat = lat; - gps.lla_pos.lon = lon; - gps.lla_pos.alt = alt; - SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); + gps_datalink.lla_pos.lat = lat; + gps_datalink.lla_pos.lon = lon; + gps_datalink.lla_pos.alt = alt; + SetBit(gps_datalink.valid_fields, GPS_VALID_POS_LLA_BIT); - gps.hmsl = hmsl; - SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); + gps_datalink.hmsl = hmsl; + SetBit(gps_datalink.valid_fields, GPS_VALID_HMSL_BIT); - gps.ecef_pos.x = ecef_x; - gps.ecef_pos.y = ecef_y; - gps.ecef_pos.z = ecef_z; - SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT); + gps_datalink.ecef_pos.x = ecef_x; + gps_datalink.ecef_pos.y = ecef_y; + gps_datalink.ecef_pos.z = ecef_z; + SetBit(gps_datalink.valid_fields, GPS_VALID_POS_ECEF_BIT); - gps.ecef_vel.x = ecef_xd; - gps.ecef_vel.y = ecef_yd; - gps.ecef_vel.z = ecef_zd; - SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT); + gps_datalink.ecef_vel.x = ecef_xd; + gps_datalink.ecef_vel.y = ecef_yd; + gps_datalink.ecef_vel.z = ecef_zd; + SetBit(gps_datalink.valid_fields, GPS_VALID_VEL_ECEF_BIT); - gps.ned_vel.x = enu_speed.y; - gps.ned_vel.y = enu_speed.x; - gps.ned_vel.z = -enu_speed.z; - SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT); + gps_datalink.ned_vel.x = enu_speed.y; + gps_datalink.ned_vel.y = enu_speed.x; + gps_datalink.ned_vel.z = -enu_speed.z; + SetBit(gps_datalink.valid_fields, GPS_VALID_VEL_NED_BIT); - gps.course = course; - SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); + gps_datalink.course = course; + SetBit(gps_datalink.valid_fields, GPS_VALID_COURSE_BIT); - gps.num_sv = numsv; + gps_datalink.num_sv = numsv; if (tow == 0) { - gps.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); //tow; + gps_datalink.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); //tow; } else { - gps.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); //tow; + gps_datalink.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); //tow; } - gps.fix = GPS_FIX_3D; - gps_available = TRUE; + gps_datalink.fix = GPS_FIX_3D; // publish new GPS data uint32_t now_ts = get_sys_time_usec(); - gps.last_msg_ticks = sys_time.nb_sec_rem; - gps.last_msg_time = sys_time.nb_sec; - if (gps.fix == GPS_FIX_3D) { - gps.last_3dfix_ticks = sys_time.nb_sec_rem; - gps.last_3dfix_time = sys_time.nb_sec; + gps_datalink.last_msg_ticks = sys_time.nb_sec_rem; + gps_datalink.last_msg_time = sys_time.nb_sec; + if (gps_datalink.fix == GPS_FIX_3D) { + gps_datalink.last_3dfix_ticks = sys_time.nb_sec_rem; + gps_datalink.last_3dfix_time = sys_time.nb_sec; } - AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps); + AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps_datalink.state); } + +/* + * register callbacks & structs + */ +void gps_datalink_register(void) +{ + gps_register_impl(gps_datalink_init, NULL, GPS_DATALINK_ID); +} diff --git a/sw/airborne/subsystems/gps/gps_datalink.h b/sw/airborne/subsystems/gps/gps_datalink.h index 3d82d80277..f71a2ad239 100644 --- a/sw/airborne/subsystems/gps/gps_datalink.h +++ b/sw/airborne/subsystems/gps/gps_datalink.h @@ -32,9 +32,16 @@ #include "std.h" #include "generated/airframe.h" -#define GPS_NB_CHANNELS 0 +#include "subsystems/gps.h" -extern bool_t gps_available; +#ifndef PRIMARY_GPS +#define PRIMARY_GPS gps_datalink +#endif + +extern struct GpsState gps_datalink; + +extern void gps_datalink_init(void); +extern void gps_datalink_register(void); extern void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xyz, int16_t heading); @@ -43,7 +50,5 @@ extern void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, in int32_t ecef_xd, int32_t ecef_yd, int32_t ecef_zd, uint32_t tow, int32_t course); -// dummy -#define GpsEvent() {} #endif /* GPS_DATALINK_H */ diff --git a/sw/airborne/subsystems/gps/gps_furuno.c b/sw/airborne/subsystems/gps/gps_furuno.c index 764cf8a264..7927baa5b8 100644 --- a/sw/airborne/subsystems/gps/gps_furuno.c +++ b/sw/airborne/subsystems/gps/gps_furuno.c @@ -26,8 +26,8 @@ * GPS furuno based NMEA parser */ -#include "gps_nmea.h" #include "subsystems/gps.h" +#include "gps_nmea.h" #include #include @@ -57,7 +57,7 @@ static uint8_t furuno_cfg_cnt = 0; static void nmea_parse_perdcrv(void); -#define GpsLinkDevice (&(GPS_LINK).device) +#define GpsLinkDevice (&(NMEA_GPS_LINK).device) /** * Configure furuno GPS. @@ -111,17 +111,17 @@ void nmea_parse_perdcrv(void) //EAST VEL double east_vel = strtod(&gps_nmea.msg_buf[i], NULL); - gps.ned_vel.y = east_vel * 100; // in cm/s + gps_nmea.state.ned_vel.y = east_vel * 100; // in cm/s // Ignore reserved nmea_read_until(&i); // NORTH VEL double north_vel = strtod(&gps_nmea.msg_buf[i], NULL); - gps.ned_vel.x = north_vel * 100; // in cm/s + gps_nmea.state.ned_vel.x = north_vel * 100; // in cm/s //Convert velocity to ecef struct LtpDef_i ltp; - ltp_def_from_ecef_i(<p, &gps.ecef_pos); - ecef_of_ned_vect_i(&gps.ecef_vel, <p, &gps.ned_vel); + ltp_def_from_ecef_i(<p, &gps_nmea.state.ecef_pos); + ecef_of_ned_vect_i(&gps_nmea.state.ecef_vel, <p, &gps_nmea.state.ned_vel); } diff --git a/sw/airborne/subsystems/gps/gps_mtk.c b/sw/airborne/subsystems/gps/gps_mtk.c index 094ef6b71f..7ff1b4415b 100644 --- a/sw/airborne/subsystems/gps/gps_mtk.c +++ b/sw/airborne/subsystems/gps/gps_mtk.c @@ -31,11 +31,16 @@ * */ -#include "subsystems/gps.h" +#include "gps_mtk.h" #include "subsystems/abi.h" #include "led.h" #include "mcu_periph/sys_time.h" +#include "pprzlink/pprzlink_device.h" + +#ifndef MTK_GPS_LINK +#error "MTK_GPS_LINK not set" +#endif #define MTK_DIY_OUTPUT_RATE MTK_DIY_OUTPUT_4HZ #define OUTPUT_RATE 4 @@ -99,7 +104,11 @@ bool_t gps_configuring; static uint8_t gps_status_config; #endif -void gps_impl_init(void) +void gps_mtk_read_message(void); +void gps_mtk_parse(uint8_t c); +void gps_mtk_msg(void); + +void gps_mtk_init(void) { gps_mtk.status = UNINIT; gps_mtk.msg_available = FALSE; @@ -111,29 +120,42 @@ void gps_impl_init(void) #endif } +void gps_mtk_event(void) +{ + struct link_device *dev = &((MTK_GPS_LINK).device); + + while (dev->char_available(dev->periph)) { + gps_mtk_parse(dev->get_byte(dev->periph)); + if (gps_mtk.msg_available) { + gps_mtk_msg(); + } + GpsConfigure(); + } +} + void gps_mtk_msg(void) { // current timestamp uint32_t now_ts = get_sys_time_usec(); - gps.last_msg_ticks = sys_time.nb_sec_rem; - gps.last_msg_time = sys_time.nb_sec; + gps_mtk.state.last_msg_ticks = sys_time.nb_sec_rem; + gps_mtk.state.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; + if (gps_mtk.state.fix == GPS_FIX_3D) { + gps_mtk.state.last_3dfix_ticks = sys_time.nb_sec_rem; + gps_mtk.state.last_3dfix_time = sys_time.nb_sec; } - AbiSendMsgGPS(GPS_MTK_ID, now_ts, &gps); + AbiSendMsgGPS(GPS_MTK_ID, now_ts, &gps_mtk.state); } 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; + if (gps_mtk.state.fix == GPS_FIX_3D) { + gps_mtk.state.last_3dfix_ticks = sys_time.nb_sec_rem; + gps_mtk.state.last_3dfix_time = sys_time.nb_sec; } - AbiSendMsgGPS(GPS_MTK_ID, now_ts, &gps); + AbiSendMsgGPS(GPS_MTK_ID, now_ts, &gps_mtk.state); } gps_mtk.msg_available = FALSE; } @@ -190,39 +212,39 @@ void gps_mtk_read_message(void) gps_time_sync.t0_ticks = sys_time.nb_tick; gps_time_sync.t0_tow = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf); gps_time_sync.t0_tow_frac = 0; - gps.lla_pos.lat = MTK_DIY14_NAV_LAT(gps_mtk.msg_buf) * 10; - gps.lla_pos.lon = MTK_DIY14_NAV_LON(gps_mtk.msg_buf) * 10; - SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); + gps_mtk.state.lla_pos.lat = MTK_DIY14_NAV_LAT(gps_mtk.msg_buf) * 10; + gps_mtk.state.lla_pos.lon = MTK_DIY14_NAV_LON(gps_mtk.msg_buf) * 10; + SetBit(gps_mtk.state.valid_fields, GPS_VALID_POS_LLA_BIT); // FIXME: with MTK you do not receive vertical speed - if (sys_time.nb_sec - gps.last_3dfix_time < 2) { - gps.ned_vel.z = ((gps.hmsl - + if (sys_time.nb_sec - gps_mtk.state.last_3dfix_time < 2) { + gps_mtk.state.ned_vel.z = ((gps_mtk.state.hmsl - MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf) * 10) * OUTPUT_RATE) / 10; - } else { gps.ned_vel.z = 0; } - gps.hmsl = MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf) * 10; - SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); + } else { gps_mtk.state.ned_vel.z = 0; } + gps_mtk.state.hmsl = MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf) * 10; + SetBit(gps_mtk.state.valid_fields, GPS_VALID_HMSL_BIT); // FIXME: with MTK you do not receive ellipsoid altitude - gps.lla_pos.alt = gps.hmsl; - gps.gspeed = MTK_DIY14_NAV_GSpeed(gps_mtk.msg_buf); + gps_mtk.state.lla_pos.alt = gps_mtk.state.hmsl; + gps_mtk.state.gspeed = MTK_DIY14_NAV_GSpeed(gps_mtk.msg_buf); // FIXME: with MTK you do not receive speed 3D - gps.speed_3d = gps.gspeed; - gps.course = (RadOfDeg(MTK_DIY14_NAV_Heading(gps_mtk.msg_buf))) * 10; - SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); - gps.num_sv = MTK_DIY14_NAV_numSV(gps_mtk.msg_buf); + gps_mtk.state.speed_3d = gps_mtk.state.gspeed; + gps_mtk.state.course = (RadOfDeg(MTK_DIY14_NAV_Heading(gps_mtk.msg_buf))) * 10; + SetBit(gps_mtk.state.valid_fields, GPS_VALID_COURSE_BIT); + gps_mtk.state.num_sv = MTK_DIY14_NAV_numSV(gps_mtk.msg_buf); switch (MTK_DIY14_NAV_GPSfix(gps_mtk.msg_buf)) { case MTK_DIY_FIX_3D: - gps.fix = GPS_FIX_3D; + gps_mtk.state.fix = GPS_FIX_3D; break; case MTK_DIY_FIX_2D: - gps.fix = GPS_FIX_2D; + gps_mtk.state.fix = GPS_FIX_2D; break; default: - gps.fix = GPS_FIX_NONE; + gps_mtk.state.fix = GPS_FIX_NONE; } - gps.tow = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);; + gps_mtk.state.tow = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);; // FIXME: with MTK DIY 1.4 you do not receive GPS week - gps.week = 0; + gps_mtk.state.week = 0; #ifdef GPS_LED - if (gps.fix == GPS_FIX_3D) { + if (gps_mtk.state.fix == GPS_FIX_3D) { LED_ON(GPS_LED); } else { LED_TOGGLE(GPS_LED); @@ -236,43 +258,43 @@ void gps_mtk_read_message(void) uint32_t gps_date, gps_time; gps_date = MTK_DIY16_NAV_UTC_DATE(gps_mtk.msg_buf); gps_time = MTK_DIY16_NAV_UTC_TIME(gps_mtk.msg_buf); - gps_mtk_time2itow(gps_date, gps_time, &gps.week, &gps.tow); + gps_mtk_time2itow(gps_date, gps_time, &gps_mtk.state.week, &gps_mtk.state.tow); #ifdef GPS_TIMESTAMP /* get hardware clock ticks */ - SysTimeTimerStart(gps.t0); - gps.t0_tow = gps.tow; - gps.t0_tow_frac = 0; + SysTimeTimerStart(gps_mtk.state.t0); + gps_mtk.state.t0_tow = gps_mtk.state.tow; + gps_mtk.state.t0_tow_frac = 0; #endif - gps.lla_pos.lat = MTK_DIY16_NAV_LAT(gps_mtk.msg_buf) * 10; - gps.lla_pos.lon = MTK_DIY16_NAV_LON(gps_mtk.msg_buf) * 10; + gps_mtk.state.lla_pos.lat = MTK_DIY16_NAV_LAT(gps_mtk.msg_buf) * 10; + gps_mtk.state.lla_pos.lon = MTK_DIY16_NAV_LON(gps_mtk.msg_buf) * 10; // FIXME: with MTK you do not receive vertical speed - if (sys_time.nb_sec - gps.last_3dfix_time < 2) { - gps.ned_vel.z = ((gps.hmsl - + if (sys_time.nb_sec - gps_mtk.state.last_3dfix_time < 2) { + gps_mtk.state.ned_vel.z = ((gps_mtk.state.hmsl - MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf) * 10) * OUTPUT_RATE) / 10; - } else { gps.ned_vel.z = 0; } - gps.hmsl = MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf) * 10; - SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); + } else { gps_mtk.state.ned_vel.z = 0; } + gps_mtk.state.hmsl = MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf) * 10; + SetBit(gps_mtk.state.valid_fields, GPS_VALID_HMSL_BIT); // FIXME: with MTK you do not receive ellipsoid altitude - gps.lla_pos.alt = gps.hmsl; - gps.gspeed = MTK_DIY16_NAV_GSpeed(gps_mtk.msg_buf); + gps_mtk.state.lla_pos.alt = gps_mtk.state.hmsl; + gps_mtk.state.gspeed = MTK_DIY16_NAV_GSpeed(gps_mtk.msg_buf); // FIXME: with MTK you do not receive speed 3D - gps.speed_3d = gps.gspeed; - gps.course = (RadOfDeg(MTK_DIY16_NAV_Heading(gps_mtk.msg_buf) * 10000)) * 10; - SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); - gps.num_sv = MTK_DIY16_NAV_numSV(gps_mtk.msg_buf); + gps_mtk.state.speed_3d = gps_mtk.state.gspeed; + gps_mtk.state.course = (RadOfDeg(MTK_DIY16_NAV_Heading(gps_mtk.msg_buf) * 10000)) * 10; + SetBit(gps_mtk.state.valid_fields, GPS_VALID_COURSE_BIT); + gps_mtk.state.num_sv = MTK_DIY16_NAV_numSV(gps_mtk.msg_buf); switch (MTK_DIY16_NAV_GPSfix(gps_mtk.msg_buf)) { case MTK_DIY_FIX_3D: - gps.fix = GPS_FIX_3D; + gps_mtk.state.fix = GPS_FIX_3D; break; case MTK_DIY_FIX_2D: - gps.fix = GPS_FIX_2D; + gps_mtk.state.fix = GPS_FIX_2D; break; default: - gps.fix = GPS_FIX_NONE; + gps_mtk.state.fix = GPS_FIX_NONE; } /* HDOP? */ #ifdef GPS_LED - if (gps.fix == GPS_FIX_3D) { + if (gps_mtk.state.fix == GPS_FIX_3D) { LED_ON(GPS_LED); } else { LED_TOGGLE(GPS_LED); @@ -388,6 +410,14 @@ restart: } +/* + * register callbacks & structs + */ +void gps_mtk_register(void) +{ + gps_register_impl(gps_mtk_init, gps_mtk_event, GPS_MTK_ID); +} + /* * * @@ -401,7 +431,7 @@ restart: static void MtkSend_CFG(char *dat) { - struct link_device *dev = &((GPS_LINK).device); + struct link_device *dev = &((MTK_GPS_LINK).device); while (*dat != 0) { dev->put_byte(dev->periph, *dat++); } } diff --git a/sw/airborne/subsystems/gps/gps_mtk.h b/sw/airborne/subsystems/gps/gps_mtk.h index b416a43b4f..26e3673e63 100644 --- a/sw/airborne/subsystems/gps/gps_mtk.h +++ b/sw/airborne/subsystems/gps/gps_mtk.h @@ -34,6 +34,7 @@ #ifndef MTK_H #define MTK_H +#include "subsystems/gps.h" #include "mcu_periph/uart.h" /** Includes macros generated from mtk.xml */ @@ -57,15 +58,15 @@ struct GpsMtk { uint8_t status_flags; uint8_t sol_flags; + + struct GpsState state; }; extern struct GpsMtk gps_mtk; - -/* - * This part is used by the autopilot to read data from a uart - */ -#include "pprzlink/pprzlink_device.h" +extern void gps_mtk_event(void); +extern void gps_mtk_init(void); +extern void gps_mtk_register(void); #ifdef GPS_CONFIGURE extern void gps_configure(void); @@ -79,21 +80,5 @@ extern bool_t gps_configuring; #define GpsConfigure() {} #endif -extern void gps_mtk_read_message(void); -extern void gps_mtk_parse(uint8_t c); -extern void gps_mtk_msg(void); - -static inline void GpsEvent(void) -{ - struct link_device *dev = &((GPS_LINK).device); - - while (dev->char_available(dev->periph)) { - gps_mtk_parse(dev->get_byte(dev->periph)); - if (gps_mtk.msg_available) { - gps_mtk_msg(); - } - GpsConfigure(); - } -} #endif /* MTK_H */ diff --git a/sw/airborne/subsystems/gps/gps_nmea.c b/sw/airborne/subsystems/gps/gps_nmea.c index b2b5a28ab1..105191d277 100644 --- a/sw/airborne/subsystems/gps/gps_nmea.c +++ b/sw/airborne/subsystems/gps/gps_nmea.c @@ -30,6 +30,7 @@ * Parsing GGA, RMC, GSA and GSV. */ +#include "gps_nmea.h" #include "subsystems/gps.h" #include "subsystems/abi.h" #include "led.h" @@ -62,10 +63,9 @@ static void nmea_parse_RMC(void); static void nmea_parse_GGA(void); static void nmea_parse_GSV(void); - -void gps_impl_init(void) +void gps_nmea_init(void) { - gps.nb_channels = GPS_NB_CHANNELS; + gps_nmea.state.nb_channels = GPS_NMEA_NB_CHANNELS; gps_nmea.is_configured = FALSE; gps_nmea.msg_available = FALSE; gps_nmea.pos_available = FALSE; @@ -76,18 +76,34 @@ void gps_impl_init(void) nmea_configure(); } -void gps_nmea_msg(void) +void gps_nmea_event(void) +{ + struct link_device *dev = &((NMEA_GPS_LINK).device); + + if (!gps_nmea.is_configured) { + nmea_configure(); + return; + } + while (dev->char_available(dev->periph)) { + nmea_parse_char(dev->get_byte(dev->periph)); + if (gps_nmea.msg_available) { + nmea_gps_msg(); + } + } +} + +void nmea_gps_msg(void) { // current timestamp uint32_t now_ts = get_sys_time_usec(); - gps.last_msg_ticks = sys_time.nb_sec_rem; - gps.last_msg_time = sys_time.nb_sec; + gps_nmea.state.last_msg_ticks = sys_time.nb_sec_rem; + gps_nmea.state.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; + if (gps_nmea.state.fix == GPS_FIX_3D) { + gps_nmea.state.last_3dfix_ticks = sys_time.nb_sec_rem; + gps_nmea.state.last_3dfix_time = sys_time.nb_sec; } AbiSendMsgGPS(GPS_NMEA_ID, now_ts, &gps); } @@ -251,11 +267,11 @@ static void nmea_parse_GSA(void) // get 2D/3D-fix // set gps_mode=3=3d, 2=2d, 1=no fix or 0 - gps.fix = atoi(&gps_nmea.msg_buf[i]); - if (gps.fix == 1) { - gps.fix = 0; + gps_nmea.state.fix = atoi(&gps_nmea.msg_buf[i]); + if (gps_nmea.state.fix == 1) { + gps_nmea.state.fix = 0; } - NMEA_PRINT("p_GSA() - gps.fix=%i (3=3D)\n\r", gps.fix); + NMEA_PRINT("p_GSA() - gps_nmea.state.fix=%i (3=3D)\n\r", gps_nmea.state.fix); nmea_read_until(&i); // up to 12 PRNs of satellites used for fix @@ -266,13 +282,13 @@ static void nmea_parse_GSA(void) int prn = atoi(&gps_nmea.msg_buf[i]); NMEA_PRINT("p_GSA() - PRN %i=%i\n\r", satcount, prn); if (!gps_nmea.have_gsv) { - gps.svinfos[prn_cnt].svid = prn; + gps_nmea.state.svinfos[prn_cnt].svid = prn; } satcount++; } else { if (!gps_nmea.have_gsv) { - gps.svinfos[prn_cnt].svid = 0; + gps_nmea.state.svinfos[prn_cnt].svid = 0; } } nmea_read_until(&i); @@ -280,7 +296,7 @@ static void nmea_parse_GSA(void) // PDOP float pdop = strtof(&gps_nmea.msg_buf[i], NULL); - gps.pdop = pdop * 100; + gps_nmea.state.pdop = pdop * 100; NMEA_PRINT("p_GSA() - pdop=%f\n\r", pdop); nmea_read_until(&i); @@ -330,15 +346,15 @@ static void nmea_parse_RMC(void) // get speed nmea_read_until(&i); double speed = strtod(&gps_nmea.msg_buf[i], NULL); - gps.gspeed = speed * 1.852 * 100 / (60 * 60); - NMEA_PRINT("p_RMC() - ground-speed=%f knot = %d cm/s\n\r", speed, (gps.gspeed * 1000)); + gps_nmea.state.gspeed = speed * 1.852 * 100 / (60 * 60); + NMEA_PRINT("p_RMC() - ground-speed=%f knot = %d cm/s\n\r", speed, (gps_nmea.state.gspeed * 1000)); // get course nmea_read_until(&i); double course = strtod(&gps_nmea.msg_buf[i], NULL); - gps.course = RadOfDeg(course) * 1e7; + gps_nmea.state.course = RadOfDeg(course) * 1e7; NMEA_PRINT("p_RMC() - course: %f deg\n\r", course); - SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); + SetBit(gps_nmea.state.valid_fields, GPS_VALID_COURSE_BIT); } @@ -363,7 +379,7 @@ static void nmea_parse_GGA(void) // ignored GpsInfo.PosLLA.TimeOfFix.f = strtod(&packet[i], NULL); // FIXME: parse UTC time correctly double utc_time = strtod(&gps_nmea.msg_buf[i], NULL); - gps.tow = (uint32_t)((utc_time + 1) * 1000); + gps_nmea.state.tow = (uint32_t)((utc_time + 1) * 1000); // get latitude [ddmm.mmmmm] nmea_read_until(&i); @@ -380,7 +396,7 @@ static void nmea_parse_GGA(void) // convert to radians lla_f.lat = RadOfDeg(lat); - gps.lla_pos.lat = lat * 1e7; // convert to fixed-point + gps_nmea.state.lla_pos.lat = lat * 1e7; // convert to fixed-point NMEA_PRINT("p_GGA() - lat=%f gps_lat=%f\n\r", (lat * 1000), lla_f.lat); @@ -399,9 +415,9 @@ static void nmea_parse_GGA(void) // convert to radians lla_f.lon = RadOfDeg(lon); - gps.lla_pos.lon = lon * 1e7; // convert to fixed-point - NMEA_PRINT("p_GGA() - lon=%f gps_lon=%f time=%u\n\r", (lon * 1000), lla_f.lon, gps.tow); - SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); + gps_nmea.state.lla_pos.lon = lon * 1e7; // convert to fixed-point + NMEA_PRINT("p_GGA() - lon=%f gps_lon=%f time=%u\n\r", (lon * 1000), lla_f.lon, gps_nmea.state.tow); + SetBit(gps_nmea.state.valid_fields, GPS_VALID_POS_LLA_BIT); // get position fix status nmea_read_until(&i); @@ -417,20 +433,20 @@ static void nmea_parse_GGA(void) // get number of satellites used in GPS solution nmea_read_until(&i); - gps.num_sv = atoi(&gps_nmea.msg_buf[i]); - NMEA_PRINT("p_GGA() - gps_numSatlitesUsed=%i\n\r", gps.num_sv); + gps_nmea.state.num_sv = atoi(&gps_nmea.msg_buf[i]); + NMEA_PRINT("p_GGA() - gps_numSatlitesUsed=%i\n\r", gps_nmea.state.num_sv); // get HDOP, but we use PDOP from GSA message nmea_read_until(&i); //float hdop = strtof(&gps_nmea.msg_buf[i], NULL); - //gps.pdop = hdop * 100; + //gps_nmea.state.pdop = hdop * 100; // get altitude (in meters) above geoid (MSL) nmea_read_until(&i); float hmsl = strtof(&gps_nmea.msg_buf[i], NULL); - gps.hmsl = hmsl * 1000; - NMEA_PRINT("p_GGA() - gps.hmsl=%i\n\r", gps.hmsl); - SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); + gps_nmea.state.hmsl = hmsl * 1000; + NMEA_PRINT("p_GGA() - gps_nmea.state.hmsl=%i\n\r", gps_nmea.state.hmsl); + SetBit(gps_nmea.state.valid_fields, GPS_VALID_HMSL_BIT); // get altitude units (always M) nmea_read_until(&i); @@ -441,8 +457,8 @@ static void nmea_parse_GGA(void) NMEA_PRINT("p_GGA() - geoid alt=%f\n\r", geoid); // height above ellipsoid lla_f.alt = hmsl + geoid; - gps.lla_pos.alt = lla_f.alt * 1000; - NMEA_PRINT("p_GGA() - gps.alt=%i\n\r", gps.lla_pos.alt); + gps_nmea.state.lla_pos.alt = lla_f.alt * 1000; + NMEA_PRINT("p_GGA() - gps_nmea.state.alt=%i\n\r", gps_nmea.state.lla_pos.alt); // get seperations units nmea_read_until(&i); @@ -453,10 +469,10 @@ static void nmea_parse_GGA(void) /* convert to ECEF */ struct EcefCoor_f ecef_f; ecef_of_lla_f(&ecef_f, &lla_f); - gps.ecef_pos.x = ecef_f.x * 100; - gps.ecef_pos.y = ecef_f.y * 100; - gps.ecef_pos.z = ecef_f.z * 100; - SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT); + gps_nmea.state.ecef_pos.x = ecef_f.x * 100; + gps_nmea.state.ecef_pos.y = ecef_f.y * 100; + gps_nmea.state.ecef_pos.z = ecef_f.z * 100; + SetBit(gps_nmea.state.valid_fields, GPS_VALID_POS_ECEF_BIT); } /** @@ -513,10 +529,10 @@ static void nmea_parse_GSV(void) int ch_idx = (cur_sen - 1) * 4 + sat_cnt; // don't populate svinfos with GLONASS sats for now if (!is_glonass && ch_idx > 0 && ch_idx < 12) { - gps.svinfos[ch_idx].svid = prn; - gps.svinfos[ch_idx].cno = snr; - gps.svinfos[ch_idx].elev = elev; - gps.svinfos[ch_idx].azim = azim; + gps_nmea.state.svinfos[ch_idx].svid = prn; + gps_nmea.state.svinfos[ch_idx].cno = snr; + gps_nmea.state.svinfos[ch_idx].elev = elev; + gps_nmea.state.svinfos[ch_idx].azim = azim; } if (is_glonass) { NMEA_PRINT("p_GSV() - GLONASS %i PRN=%i elev=%i azim=%i snr=%i\n\r", ch_idx, prn, elev, azim, snr); @@ -526,3 +542,11 @@ static void nmea_parse_GSV(void) } } } + + /* + * register callbacks & structs + */ +void gps_nmea_register(void) +{ + gps_register_impl(gps_nmea_init, gps_nmea_event, GPS_NMEA_ID); +} diff --git a/sw/airborne/subsystems/gps/gps_nmea.h b/sw/airborne/subsystems/gps/gps_nmea.h index 9e9003a558..43d9abb261 100644 --- a/sw/airborne/subsystems/gps/gps_nmea.h +++ b/sw/airborne/subsystems/gps/gps_nmea.h @@ -31,11 +31,20 @@ #define GPS_NMEA_H #include "mcu_periph/uart.h" +#include "subsystems/gps.h" -#define GPS_NB_CHANNELS 12 +#define GPS_NMEA_NB_CHANNELS 12 #define NMEA_MAXLEN 255 +#ifndef PRIMARY_GPS +#define PRIMARY_GPS gps_nmea +#endif + +extern void gps_nmea_init(void); +extern void gps_nmea_event(void); +extern void gps_nmea_register(void); + struct GpsNmea { bool_t msg_available; bool_t pos_available; @@ -45,6 +54,8 @@ struct GpsNmea { char msg_buf[NMEA_MAXLEN]; ///< buffer for storing one nmea-line int msg_len; uint8_t status; ///< line parser status + + struct GpsState state; }; extern struct GpsNmea gps_nmea; @@ -63,23 +74,7 @@ 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); - -static inline void GpsEvent(void) -{ - struct link_device *dev = &((GPS_LINK).device); - - if (!gps_nmea.is_configured) { - nmea_configure(); - return; - } - while (dev->char_available(dev->periph)) { - nmea_parse_char(dev->get_byte(dev->periph)); - if (gps_nmea.msg_available) { - gps_nmea_msg(); - } - } -} +extern void nmea_gps_msg(void); /** 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_piksi.c b/sw/airborne/subsystems/gps/gps_piksi.c index 140a5240cd..dbb0c0f94c 100644 --- a/sw/airborne/subsystems/gps/gps_piksi.c +++ b/sw/airborne/subsystems/gps/gps_piksi.c @@ -43,6 +43,7 @@ #include #include #include +#include #include #include @@ -52,6 +53,25 @@ #define POS_ECEF_TIMEOUT 1000 +/* + * implementation specific gps state + */ +struct GpsState gps_piksi; +struct GpsTimeSync gps_piksi_time_sync; + +static uint32_t time_since_last_heartbeat; + +#if PERIODIC_TELEMETRY +#include "subsystems/datalink/telemetry.h" + +static void send_piksi_heartbeat(struct transport_tx *trans, struct link_device *dev) +{ + pprz_msg_send_PIKSI_HEARTBEAT(trans, dev, AC_ID, + &time_since_last_heartbeat); +} + +#endif + /* * Set the Piksi GPS antenna (default is Patch, internal) */ @@ -94,6 +114,7 @@ sbp_msg_callbacks_node_t dops_node; sbp_msg_callbacks_node_t gps_time_node; sbp_msg_callbacks_node_t tracking_state_node; sbp_msg_callbacks_node_t tracking_state_dep_a_node; +sbp_msg_callbacks_node_t heartbeat_node; static void gps_piksi_publish(void); @@ -116,20 +137,20 @@ static void sbp_pos_ecef_callback(uint16_t sender_id __attribute__((unused)), msg_pos_ecef_t pos_ecef = *(msg_pos_ecef_t *)msg; // Check if we got RTK fix (FIXME when libsbp has a nicer way of doing this) - gps.fix = get_fix_mode(pos_ecef.flags); + gps_piksi.fix = get_fix_mode(pos_ecef.flags); // get_fix_mode() will still return fix > 3D even if the current flags are spp so ignore when it is spp - if ( ( (gps.fix > GPS_FIX_3D) ) + if ( ( (gps_piksi.fix > GPS_FIX_3D) ) && pos_ecef.flags == SBP_FIX_MODE_SPP) { return; } - gps.ecef_pos.x = (int32_t)(pos_ecef.x * 100.0); - gps.ecef_pos.y = (int32_t)(pos_ecef.y * 100.0); - gps.ecef_pos.z = (int32_t)(pos_ecef.z * 100.0); - SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT); - gps.pacc = (uint32_t)(pos_ecef.accuracy);// FIXME not implemented yet by libswiftnav - gps.num_sv = pos_ecef.n_sats; - gps.tow = pos_ecef.tow; + gps_piksi.ecef_pos.x = (int32_t)(pos_ecef.x * 100.0); + gps_piksi.ecef_pos.y = (int32_t)(pos_ecef.y * 100.0); + gps_piksi.ecef_pos.z = (int32_t)(pos_ecef.z * 100.0); + SetBit(gps_piksi.valid_fields, GPS_VALID_POS_ECEF_BIT); + gps_piksi.pacc = (uint32_t)(pos_ecef.accuracy);// FIXME not implemented yet by libswiftnav + gps_piksi.num_sv = pos_ecef.n_sats; + gps_piksi.tow = pos_ecef.tow; gps_piksi_publish(); // Only if RTK position } @@ -139,11 +160,11 @@ static void sbp_vel_ecef_callback(uint16_t sender_id __attribute__((unused)), void *context __attribute__((unused))) { msg_vel_ecef_t vel_ecef = *(msg_vel_ecef_t *)msg; - gps.ecef_vel.x = (int32_t)(vel_ecef.x / 10); - gps.ecef_vel.y = (int32_t)(vel_ecef.y / 10); - gps.ecef_vel.z = (int32_t)(vel_ecef.z / 10); - SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT); - gps.sacc = (uint32_t)(vel_ecef.accuracy); + gps_piksi.ecef_vel.x = (int32_t)(vel_ecef.x / 10); + gps_piksi.ecef_vel.y = (int32_t)(vel_ecef.y / 10); + gps_piksi.ecef_vel.z = (int32_t)(vel_ecef.z / 10); + SetBit(gps_piksi.valid_fields, GPS_VALID_VEL_ECEF_BIT); + gps_piksi.sacc = (uint32_t)(vel_ecef.accuracy); // Solution available (VEL_ECEF is the last message to be send) gps_piksi_publish(); // TODO: filter out if got RTK position @@ -159,8 +180,8 @@ static void sbp_pos_llh_callback(uint16_t sender_id __attribute__((unused)), // Check if we got RTK fix (FIXME when libsbp has a nicer way of doing this) if (pos_llh.flags > 0 || last_flags == 0) { - gps.lla_pos.lat = (int32_t)(pos_llh.lat * 1e7); - gps.lla_pos.lon = (int32_t)(pos_llh.lon * 1e7); + gps_piksi.lla_pos.lat = (int32_t)(pos_llh.lat * 1e7); + gps_piksi.lla_pos.lon = (int32_t)(pos_llh.lon * 1e7); int32_t alt = (int32_t)(pos_llh.height * 1000.); // height is above ellipsoid or MSL according to bit flag (but not both are available) @@ -168,15 +189,15 @@ static void sbp_pos_llh_callback(uint16_t sender_id __attribute__((unused)), // 1: above MSL // we have to get the HMSL from the flight plan for now if (bit_is_set(pos_llh.flags, 3)) { - gps.hmsl = alt; - gps.lla_pos.alt = alt + NAV_MSL0; + gps_piksi.hmsl = alt; + gps_piksi.lla_pos.alt = alt + NAV_MSL0; } else { - gps.lla_pos.alt = alt; - gps.hmsl = alt - NAV_MSL0; + gps_piksi.lla_pos.alt = alt; + gps_piksi.hmsl = alt - NAV_MSL0; } - SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); - SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); + SetBit(gps_piksi.valid_fields, GPS_VALID_POS_LLA_BIT); + SetBit(gps_piksi.valid_fields, GPS_VALID_HMSL_BIT); } last_flags = pos_llh.flags; } @@ -187,14 +208,14 @@ static void sbp_vel_ned_callback(uint16_t sender_id __attribute__((unused)), void *context __attribute__((unused))) { msg_vel_ned_t vel_ned = *(msg_vel_ned_t *)msg; - gps.ned_vel.x = (int32_t)(vel_ned.n / 10); - gps.ned_vel.y = (int32_t)(vel_ned.e / 10); - gps.ned_vel.z = (int32_t)(vel_ned.d / 10); - SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT); + gps_piksi.ned_vel.x = (int32_t)(vel_ned.n / 10); + gps_piksi.ned_vel.y = (int32_t)(vel_ned.e / 10); + gps_piksi.ned_vel.z = (int32_t)(vel_ned.d / 10); + SetBit(gps_piksi.valid_fields, GPS_VALID_VEL_NED_BIT); - gps.gspeed = int32_sqrt(gps.ned_vel.x * gps.ned_vel.x + gps.ned_vel.y * gps.ned_vel.y); - gps.course = (int32_t)(1e7 * atan2(gps.ned_vel.y, gps.ned_vel.x)); - SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); + gps_piksi.gspeed = int32_sqrt(gps_piksi.ned_vel.x * gps_piksi.ned_vel.x + gps_piksi.ned_vel.y * gps_piksi.ned_vel.y); + gps_piksi.course = (int32_t)(1e7 * atan2(gps_piksi.ned_vel.y, gps_piksi.ned_vel.x)); + SetBit(gps_piksi.valid_fields, GPS_VALID_COURSE_BIT); } static void sbp_dops_callback(uint16_t sender_id __attribute__((unused)), @@ -203,7 +224,7 @@ static void sbp_dops_callback(uint16_t sender_id __attribute__((unused)), void *context __attribute__((unused))) { msg_dops_t dops = *(msg_dops_t *)msg; - gps.pdop = dops.pdop; + gps_piksi.pdop = dops.pdop; } static void sbp_gps_time_callback(uint16_t sender_id __attribute__((unused)), @@ -212,8 +233,8 @@ static void sbp_gps_time_callback(uint16_t sender_id __attribute__((unused)), void *context __attribute__((unused))) { msg_gps_time_t gps_time = *(msg_gps_time_t *)msg; - gps.week = gps_time.wn; - gps.tow = gps_time.tow; + gps_piksi.week = gps_time.wn; + gps_piksi.tow = gps_time.tow; } static void sbp_tracking_state_callback(uint16_t sender_id __attribute__((unused)), @@ -226,8 +247,8 @@ static void sbp_tracking_state_callback(uint16_t sender_id __attribute__((unused for(uint8_t i = 0; i < channels_cnt; i++) { if(tracking_state->states[i].state == 1) { - gps.svinfos[i].svid = tracking_state->states[i].sid + 1; - gps.svinfos[i].cno = tracking_state->states[i].cn0; + gps_piksi.svinfos[i].svid = tracking_state->states[i].sid + 1; + gps_piksi.svinfos[i].cno = tracking_state->states[i].cn0; } } } @@ -242,12 +263,20 @@ static void sbp_tracking_state_dep_a_callback(uint16_t sender_id __attribute__(( for(uint8_t i = 0; i < channels_cnt; i++) { if(tracking_state->states[i].state == 1) { - gps.svinfos[i].svid = tracking_state->states[i].prn + 1; - gps.svinfos[i].cno = tracking_state->states[i].cn0; + gps_piksi.svinfos[i].svid = tracking_state->states[i].prn + 1; + gps_piksi.svinfos[i].cno = tracking_state->states[i].cn0; } } } +static void spb_heartbeat_callback(uint16_t sender_id __attribute__((unused)), + uint8_t len __attribute__((unused)), + uint8_t msg[] __attribute__((unused)), + void *context __attribute__((unused))) +{ + time_since_last_heartbeat = get_sys_time_msec(); +} + /* * Return fix mode based on present and past flags */ @@ -276,7 +305,7 @@ static void sbp_tracking_state_dep_a_callback(uint16_t sender_id __attribute__(( /* * Initialize the Piksi GPS and write the settings */ -void gps_impl_init(void) +void gps_piksi_init(void) { /* Setup SBP nodes */ sbp_state_init(&sbp_state); @@ -290,6 +319,7 @@ void gps_impl_init(void) sbp_register_callback(&sbp_state, SBP_MSG_GPS_TIME, &sbp_gps_time_callback, NULL, &gps_time_node); sbp_register_callback(&sbp_state, SBP_MSG_TRACKING_STATE, &sbp_tracking_state_callback, NULL, &tracking_state_node); sbp_register_callback(&sbp_state, SBP_MSG_TRACKING_STATE_DEP_A, &sbp_tracking_state_dep_a_callback, NULL, &tracking_state_dep_a_node); + sbp_register_callback(&sbp_state, SBP_MSG_HEARTBEAT, &spb_heartbeat_callback, NULL, &heartbeat_node); /* Write settings */ sbp_send_message(&sbp_state, SBP_MSG_SETTINGS_WRITE, SBP_SENDER_ID, sizeof(SBP_ANT_SET), (u8*)(&SBP_ANT_SET), gps_piksi_write); @@ -303,7 +333,14 @@ void gps_impl_init(void) base_pos.height = 50.; sbp_send_message(&sbp_state, SBP_MSG_BASE_POS, SBP_SENDER_ID, sizeof(msg_base_pos_t), (u8*)(&base_pos), gps_piksi_write);*/ - gps.nb_channels = GPS_NB_CHANNELS; + // gps.nb_channels = GPS_NB_CHANNELS; + gps_piksi.comp_id = GPS_PIKSI_ID; + + gps_piksi.nb_channels = 10; + +#if PERIODIC_TELEMETRY + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_PIKSI_HEARTBEAT, send_piksi_heartbeat); +#endif } /* @@ -312,10 +349,10 @@ void gps_impl_init(void) void gps_piksi_event(void) { if ( get_sys_time_msec() - time_since_last_pos_update > POS_ECEF_TIMEOUT ) { - gps.fix = GPS_FIX_NONE; + gps_piksi.fix = GPS_FIX_NONE; } // call sbp event function - if (uart_char_available(&(GPS_LINK))) + if (uart_char_available(&(PIKSI_GPS_LINK))) sbp_process(&sbp_state, &gps_piksi_read); } @@ -327,13 +364,13 @@ static void gps_piksi_publish(void) // current timestamp uint32_t now_ts = get_sys_time_usec(); - gps.last_msg_ticks = sys_time.nb_sec_rem; - gps.last_msg_time = sys_time.nb_sec; - if (gps.fix >= GPS_FIX_3D) { - gps.last_3dfix_ticks = sys_time.nb_sec_rem; - gps.last_3dfix_time = sys_time.nb_sec; + gps_piksi.last_msg_ticks = sys_time.nb_sec_rem; + gps_piksi.last_msg_time = sys_time.nb_sec; + if (gps_piksi.fix >= GPS_FIX_3D) { + gps_piksi.last_3dfix_ticks = sys_time.nb_sec_rem; + gps_piksi.last_3dfix_time = sys_time.nb_sec; } - AbiSendMsgGPS(GPS_PIKSI_ID, now_ts, &gps); + AbiSendMsgGPS(GPS_PIKSI_ID, now_ts, &gps_piksi); } /* @@ -344,10 +381,10 @@ uint32_t gps_piksi_read(uint8_t *buff, uint32_t n, void *context __attribute__(( { uint32_t i; for (i = 0; i < n; i++) { - if (!uart_char_available(&(GPS_LINK))) + if (!uart_char_available(&(PIKSI_GPS_LINK))) break; - buff[i] = uart_getch(&(GPS_LINK)); + buff[i] = uart_getch(&(PIKSI_GPS_LINK)); } return i; } @@ -360,7 +397,7 @@ uint32_t gps_piksi_write(uint8_t *buff, uint32_t n, void *context __attribute__( { uint32_t i = 0; for (i = 0; i < n; i++) { - uart_put_byte(&(GPS_LINK), buff[i]); + uart_put_byte(&(PIKSI_GPS_LINK), buff[i]); } return n; } @@ -372,3 +409,11 @@ void gps_inject_data(uint8_t packet_id, uint8_t length, uint8_t *data) { sbp_send_message(&sbp_state, packet_id, SBP_SENDER_ID, length, data, gps_piksi_write); } + +/* + * register callbacks & structs + */ +void gps_piksi_register(void) +{ + gps_register_impl(gps_piksi_init, gps_piksi_event, GPS_PIKSI_ID); +} diff --git a/sw/airborne/subsystems/gps/gps_piksi.h b/sw/airborne/subsystems/gps/gps_piksi.h index a9024d4965..272db2694b 100644 --- a/sw/airborne/subsystems/gps/gps_piksi.h +++ b/sw/airborne/subsystems/gps/gps_piksi.h @@ -32,18 +32,17 @@ #ifndef GPS_PIKSI_H #define GPS_PIKSI_H -#define GPS_NB_CHANNELS 10 +#ifndef PRIMARY_GPS +#define PRIMARY_GPS gps_piksi +#endif extern void gps_piksi_event(void); +extern void gps_piksi_init(void); +extern void gps_piksi_register(void); /* * Reset base station position */ extern void gps_piksi_set_base_pos(void); -/* - * The GPS event - */ -#define GpsEvent gps_piksi_event - #endif /* GPS_PIKSI_H */ diff --git a/sw/airborne/subsystems/gps/gps_sim.c b/sw/airborne/subsystems/gps/gps_sim.c index 1174c883b6..a4517f1d89 100644 --- a/sw/airborne/subsystems/gps/gps_sim.c +++ b/sw/airborne/subsystems/gps/gps_sim.c @@ -19,10 +19,10 @@ * Boston, MA 02111-1307, USA. */ -#include "subsystems/gps.h" +#include "subsystems/gps/gps_sim.h" #include "subsystems/abi.h" -void gps_impl_init(void) +void gps_sim_init(void) { gps.fix = GPS_FIX_NONE; } @@ -38,3 +38,11 @@ void gps_sim_publish(void) } AbiSendMsgGPS(GPS_SIM_ID, now_ts, &gps); } + +/* + * register callbacks & structs + */ +void gps_sim_register(void) +{ + gps_register_impl(gps_sim_init, NULL, GPS_SIM_ID); +} diff --git a/sw/airborne/subsystems/gps/gps_sim.h b/sw/airborne/subsystems/gps/gps_sim.h index b7012d92d5..d8912678e5 100644 --- a/sw/airborne/subsystems/gps/gps_sim.h +++ b/sw/airborne/subsystems/gps/gps_sim.h @@ -2,13 +2,15 @@ #define GPS_SIM_H #include "std.h" +#include "subsystems/gps.h" -#define GPS_NB_CHANNELS 16 - -extern void gps_impl_init(void); +#ifndef PRIMARY_GPS +#define PRIMARY_GPS gps_sim +#endif extern void gps_sim_publish(void); -#define GpsEvent() {} +extern void gps_sim_init(void); +extern void gps_sim_register(void); #endif /* GPS_SIM_H */ diff --git a/sw/airborne/subsystems/gps/gps_sim_hitl.c b/sw/airborne/subsystems/gps/gps_sim_hitl.c index 96c563538a..451e65fc04 100644 --- a/sw/airborne/subsystems/gps/gps_sim_hitl.c +++ b/sw/airborne/subsystems/gps/gps_sim_hitl.c @@ -24,6 +24,7 @@ * GPS subsystem simulation from rotorcrafts horizontal/vertical reference system */ +#include "subsystems/gps/gps_sim_hitl.h" #include "subsystems/gps.h" #include "subsystems/abi.h" @@ -35,7 +36,7 @@ bool_t gps_available; uint32_t gps_sim_hitl_timer; -void gps_impl_init(void) +void gps_sim_hitl_init(void) { gps.fix = GPS_FIX_NONE; } @@ -93,3 +94,11 @@ void gps_sim_hitl_event(void) AbiSendMsgGPS(GPS_SIM_ID, now_ts, &gps); } } + +/* + * register callbacks & structs + */ +void gps_sim_hitl_register(void) +{ + gps_register_impl(gps_sim_hitl_init, gps_sim_hitl_event, GPS_SIM_ID); +} diff --git a/sw/airborne/subsystems/gps/gps_sim_hitl.h b/sw/airborne/subsystems/gps/gps_sim_hitl.h index 84f4c6e145..cea939aee0 100644 --- a/sw/airborne/subsystems/gps/gps_sim_hitl.h +++ b/sw/airborne/subsystems/gps/gps_sim_hitl.h @@ -27,10 +27,12 @@ #ifndef GPS_SIM_HITL_H #define GPS_SIM_HITL_H -#include "std.h" +#ifndef PRIMARY_GPS +#define PRIMARY_GPS gps_sim_hitl +#endif extern void gps_sim_hitl_event(void); - -#define GpsEvent gps_sim_hitl_event +extern void gps_sim_hitl_impl_init(void); +extern void gps_sim_hitl_register(void); #endif /* GPS_SIM_HITL_H */ diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.c b/sw/airborne/subsystems/gps/gps_sim_nps.c index 852161c619..7ca31f7a3a 100644 --- a/sw/airborne/subsystems/gps/gps_sim_nps.c +++ b/sw/airborne/subsystems/gps/gps_sim_nps.c @@ -25,68 +25,77 @@ #include "nps_sensors.h" #include "nps_fdm.h" +struct GpsState gps_nps; bool_t gps_has_fix; -void gps_feed_value() +void gps_feed_value(void) { // FIXME, set proper time instead of hardcoded to May 2014 - gps.week = 1794; - gps.tow = fdm.time * 1000; + gps_nps.week = 1794; + gps_nps.tow = fdm.time * 1000; - gps.ecef_pos.x = sensors.gps.ecef_pos.x * 100.; - gps.ecef_pos.y = sensors.gps.ecef_pos.y * 100.; - gps.ecef_pos.z = sensors.gps.ecef_pos.z * 100.; - SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT); - gps.ecef_vel.x = sensors.gps.ecef_vel.x * 100.; - gps.ecef_vel.y = sensors.gps.ecef_vel.y * 100.; - gps.ecef_vel.z = sensors.gps.ecef_vel.z * 100.; - SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT); + gps_nps.ecef_pos.x = sensors.gps.ecef_pos.x * 100.; + gps_nps.ecef_pos.y = sensors.gps.ecef_pos.y * 100.; + gps_nps.ecef_pos.z = sensors.gps.ecef_pos.z * 100.; + SetBit(gps_nps.valid_fields, GPS_VALID_POS_ECEF_BIT); + gps_nps.ecef_vel.x = sensors.gps.ecef_vel.x * 100.; + gps_nps.ecef_vel.y = sensors.gps.ecef_vel.y * 100.; + gps_nps.ecef_vel.z = sensors.gps.ecef_vel.z * 100.; + SetBit(gps_nps.valid_fields, GPS_VALID_VEL_ECEF_BIT); //ecef pos seems to be based on geocentric model, hence we get a very high alt when converted to lla - gps.lla_pos.lat = DegOfRad(sensors.gps.lla_pos.lat) * 1e7; - gps.lla_pos.lon = DegOfRad(sensors.gps.lla_pos.lon) * 1e7; - gps.lla_pos.alt = sensors.gps.lla_pos.alt * 1000.; - SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); + gps_nps.lla_pos.lat = DegOfRad(sensors.gps.lla_pos.lat) * 1e7; + gps_nps.lla_pos.lon = DegOfRad(sensors.gps.lla_pos.lon) * 1e7; + gps_nps.lla_pos.alt = sensors.gps.lla_pos.alt * 1000.; + SetBit(gps_nps.valid_fields, GPS_VALID_POS_LLA_BIT); - gps.hmsl = sensors.gps.hmsl * 1000.; - SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); + gps_nps.hmsl = sensors.gps.hmsl * 1000.; + SetBit(gps_nps.valid_fields, GPS_VALID_HMSL_BIT); /* calc NED speed from ECEF */ struct LtpDef_d ref_ltp; ltp_def_from_ecef_d(&ref_ltp, &sensors.gps.ecef_pos); struct NedCoor_d ned_vel_d; ned_of_ecef_vect_d(&ned_vel_d, &ref_ltp, &sensors.gps.ecef_vel); - gps.ned_vel.x = ned_vel_d.x * 100; - gps.ned_vel.y = ned_vel_d.y * 100; - gps.ned_vel.z = ned_vel_d.z * 100; - SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT); + gps_nps.ned_vel.x = ned_vel_d.x * 100; + gps_nps.ned_vel.y = ned_vel_d.y * 100; + gps_nps.ned_vel.z = ned_vel_d.z * 100; + SetBit(gps_nps.valid_fields, GPS_VALID_VEL_NED_BIT); /* horizontal and 3d ground speed in cm/s */ - gps.gspeed = sqrt(ned_vel_d.x * ned_vel_d.x + ned_vel_d.y * ned_vel_d.y) * 100; - gps.speed_3d = sqrt(ned_vel_d.x * ned_vel_d.x + ned_vel_d.y * ned_vel_d.y + ned_vel_d.z * ned_vel_d.z) * 100; + gps_nps.gspeed = sqrt(ned_vel_d.x * ned_vel_d.x + ned_vel_d.y * ned_vel_d.y) * 100; + gps_nps.speed_3d = sqrt(ned_vel_d.x * ned_vel_d.x + ned_vel_d.y * ned_vel_d.y + ned_vel_d.z * ned_vel_d.z) * 100; /* ground course in radians * 1e7 */ - gps.course = atan2(ned_vel_d.y, ned_vel_d.x) * 1e7; - SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); + gps_nps.course = atan2(ned_vel_d.y, ned_vel_d.x) * 1e7; + SetBit(gps_nps.valid_fields, GPS_VALID_COURSE_BIT); if (gps_has_fix) { - gps.fix = GPS_FIX_3D; + gps_nps.fix = GPS_FIX_3D; } else { - gps.fix = GPS_FIX_NONE; + gps_nps.fix = GPS_FIX_NONE; } // publish gps data uint32_t now_ts = get_sys_time_usec(); - gps.last_msg_ticks = sys_time.nb_sec_rem; - gps.last_msg_time = sys_time.nb_sec; - if (gps.fix == GPS_FIX_3D) { - gps.last_3dfix_ticks = sys_time.nb_sec_rem; - gps.last_3dfix_time = sys_time.nb_sec; + gps_nps.last_msg_ticks = sys_time.nb_sec_rem; + gps_nps.last_msg_time = sys_time.nb_sec; + if (gps_nps.fix == GPS_FIX_3D) { + gps_nps.last_3dfix_ticks = sys_time.nb_sec_rem; + gps_nps.last_3dfix_time = sys_time.nb_sec; } - AbiSendMsgGPS(GPS_SIM_ID, now_ts, &gps); + AbiSendMsgGPS(GPS_SIM_ID, now_ts, &gps_nps); } -void gps_impl_init() +void gps_nps_init(void) { gps_has_fix = TRUE; } + +/* + * register callbacks & structs + */ +void gps_nps_register(void) +{ + gps_register_impl(gps_nps_init, NULL, GPS_SIM_ID); +} diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.h b/sw/airborne/subsystems/gps/gps_sim_nps.h index 4eab2552d1..f935277c99 100644 --- a/sw/airborne/subsystems/gps/gps_sim_nps.h +++ b/sw/airborne/subsystems/gps/gps_sim_nps.h @@ -3,14 +3,16 @@ #include "std.h" -#define GPS_NB_CHANNELS 16 +#ifndef PRIMARY_GPS +#define PRIMARY_GPS gps_nps +#endif extern bool_t gps_has_fix; extern void gps_feed_value(); -extern void gps_impl_init(); +extern void gps_nps_impl_init(); +extern void gps_nps_register(void); -#define GpsEvent() {} #endif /* GPS_SIM_NPS_H */ diff --git a/sw/airborne/subsystems/gps/gps_sirf.c b/sw/airborne/subsystems/gps/gps_sirf.c index e6c3950fc4..c3b9da13e0 100644 --- a/sw/airborne/subsystems/gps/gps_sirf.c +++ b/sw/airborne/subsystems/gps/gps_sirf.c @@ -32,11 +32,94 @@ #include #include "gps_sirf.h" +#include "pprzlink/pprzlink_device.h" +#include "mcu_periph/uart.h" + +//Invert bytes +#define Invert2Bytes(x) ((x>>8) | (x<<8)) +#define Invert4Bytes(x) ((x>>24) | ((x<<8) & 0x00FF0000) | ((x>>8) & 0x0000FF00) | (x<<24)) + +/** Message ID 2 from GPS. Total payload length should be 41 bytes. */ +struct sirf_msg_2 { + uint8_t msg_id; ///< hex value 0x02 ( = decimal 2) + int32_t x_pos; ///< x-position in m + int32_t y_pos; ///< y-position in m + int32_t z_pos; ///< z-position in m + int16_t vx; ///< x-velocity * 8 in m/s + int16_t vy; ///< y-velocity * 8 in m/s + int16_t vz; ///< z-velocity * 8 in m/s + uint8_t mode1; + uint8_t hdop; ///< horizontal dilution of precision *5 (0.2 precision) + uint8_t mode2; + uint16_t week; + uint32_t tow; ///< time of week in seconds * 10^2 + uint8_t num_sat; ///< Number of satellites in fix + uint8_t ch1prn; ///< pseudo-random noise, 12 channels + uint8_t ch2prn; + uint8_t ch3prn; + uint8_t ch4prn; + uint8_t ch5prn; + uint8_t ch6prn; + uint8_t ch7prn; + uint8_t ch8prn; + uint8_t ch9prn; + uint8_t ch10prn; + uint8_t ch11prn; + uint8_t ch12prn; +} __attribute__((packed)); + + +/** Message ID 41 from GPS. Total payload length should be 91 bytes. */ +struct sirf_msg_41 { + uint8_t msg_id; ///< hex value 0x29 (= decimal 41) + uint16_t nav_valid; ///< if equal to 0x0000, then navigation solution is valid + uint16_t nav_type; + uint16_t extended_week_number; + uint32_t tow; ///< time of week in seconds *10^3] + uint16_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t minute; + uint16_t second; + uint32_t sat_id; ///< satellites used in solution. Each satellite corresponds with a bit, e.g. bit 1 ON = SV 1 is used in solution + int32_t latitude; ///< in degrees (+= North) *10^7 + int32_t longitude; ///< in degrees (+= East) *10*7 + int32_t alt_ellipsoid; ///< in meters *10^2 + int32_t alt_msl; ///< in meters *10^2 + int8_t map_datum; + uint16_t sog; ///< speed over ground, in m/s * 10^2 + uint16_t cog; ///< course over ground, in degrees clockwise from true north * 10^2 + int16_t mag_var; ///< not implemented + int16_t climb_rate; ///< in m/s * 10^2 + int16_t heading_rate; ///< in deg/s * 10^2 + uint32_t ehpe; ///< estimated horizontal position error, in meters * 10^2 + uint32_t evpe; ///< estimated vertical position error, in meters * 10^2 + uint32_t ete; ///< estimated time error, in seconds * 10^2 + uint16_t ehve; ///< estimated horizontal velocity error in m/s * 10^2 + int32_t clock_bias; ///< in m * 10^2 + uint32_t clock_bias_err; ///< in m * 10^2 + int32_t clock_drift; ///< in m/s * 10^2 + uint32_t clock_drift_err; ///< in m/s * 10^2 + uint32_t distance; ///< Distance traveled since reset in m + uint16_t distance_err; ///< in meters + uint16_t heading_err; ///< in degrees * 10^2 + uint8_t num_sat; ///< Number of satellites used for solution + uint8_t hdop; ///< Horizontal dilution of precision x 5 (0.2 precision) + uint8_t add_info; ///< Additional mode info +} __attribute__((packed)); + + struct GpsSirf gps_sirf; + +void sirf_parse_char(uint8_t c); +void sirf_parse_msg(void); +void gps_sirf_msg(void); + void sirf_parse_2(void); void sirf_parse_41(void); -void gps_impl_init(void) +void gps_sirf_init(void) { gps_sirf.msg_available = FALSE; gps_sirf.pos_available = FALSE; @@ -48,13 +131,13 @@ void gps_sirf_msg(void) { // current timestamp uint32_t now_ts = get_sys_time_usec(); - gps.last_msg_ticks = sys_time.nb_sec_rem; - gps.last_msg_time = sys_time.nb_sec; + gps_sirf.state.last_msg_ticks = sys_time.nb_sec_rem; + gps_sirf.state.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; + if (gps_sirf.state.fix == GPS_FIX_3D) { + gps_sirf.state.last_3dfix_ticks = sys_time.nb_sec_rem; + gps_sirf.state.last_3dfix_time = sys_time.nb_sec; } AbiSendMsgGPS(GPS_SIRF_ID, now_ts, &gps); } @@ -115,32 +198,32 @@ void sirf_parse_41(void) { struct sirf_msg_41 *p = (struct sirf_msg_41 *)&gps_sirf.msg_buf[4]; - gps.tow = Invert4Bytes(p->tow); - gps.hmsl = Invert4Bytes(p->alt_msl) * 10; - SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); - gps.num_sv = p->num_sat; - gps.nb_channels = p ->num_sat; + gps_sirf.state.tow = Invert4Bytes(p->tow); + gps_sirf.state.hmsl = Invert4Bytes(p->alt_msl) * 10; + SetBit(gps_sirf.state.valid_fields, GPS_VALID_HMSL_BIT); + gps_sirf.state.num_sv = p->num_sat; + gps_sirf.state.nb_channels = p ->num_sat; /* read latitude, longitude and altitude from packet */ - gps.lla_pos.lat = Invert4Bytes(p->latitude); - gps.lla_pos.lon = Invert4Bytes(p->longitude); - gps.lla_pos.alt = Invert4Bytes(p->alt_ellipsoid) * 10; - SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); + gps_sirf.state.lla_pos.lat = Invert4Bytes(p->latitude); + gps_sirf.state.lla_pos.lon = Invert4Bytes(p->longitude); + gps_sirf.state.lla_pos.alt = Invert4Bytes(p->alt_ellipsoid) * 10; + SetBit(gps_sirf.state.valid_fields, GPS_VALID_POS_LLA_BIT); - gps.sacc = (Invert2Bytes(p->ehve) >> 16); - gps.course = RadOfDeg(Invert2Bytes(p->cog)) * pow(10, 5); - SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); - gps.gspeed = RadOfDeg(Invert2Bytes(p->sog)) * pow(10, 5); - gps.cacc = RadOfDeg(Invert2Bytes(p->heading_err)) * pow(10, 5); - gps.pacc = Invert4Bytes(p->ehpe); - gps.pdop = p->hdop * 20; + gps_sirf.state.sacc = (Invert2Bytes(p->ehve) >> 16); + gps_sirf.state.course = RadOfDeg(Invert2Bytes(p->cog)) * pow(10, 5); + SetBit(gps_sirf.state.valid_fields, GPS_VALID_COURSE_BIT); + gps_sirf.state.gspeed = RadOfDeg(Invert2Bytes(p->sog)) * pow(10, 5); + gps_sirf.state.cacc = RadOfDeg(Invert2Bytes(p->heading_err)) * pow(10, 5); + gps_sirf.state.pacc = Invert4Bytes(p->ehpe); + gps_sirf.state.pdop = p->hdop * 20; if ((p->nav_type >> 8 & 0x7) >= 0x4) { - gps.fix = GPS_FIX_3D; + gps_sirf.state.fix = GPS_FIX_3D; } else if ((p->nav_type >> 8 & 0x7) >= 0x1) { - gps.fix = GPS_FIX_2D; + gps_sirf.state.fix = GPS_FIX_2D; } else { - gps.fix = GPS_FIX_NONE; + gps_sirf.state.fix = GPS_FIX_NONE; } @@ -152,24 +235,24 @@ void sirf_parse_2(void) { struct sirf_msg_2 *p = (struct sirf_msg_2 *)&gps_sirf.msg_buf[4]; - gps.week = Invert2Bytes(p->week); + gps_sirf.state.week = Invert2Bytes(p->week); - gps.ecef_pos.x = Invert4Bytes(p->x_pos) * 100; - gps.ecef_pos.y = Invert4Bytes(p->y_pos) * 100; - gps.ecef_pos.z = Invert4Bytes(p->z_pos) * 100; - SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT); + gps_sirf.state.ecef_pos.x = Invert4Bytes(p->x_pos) * 100; + gps_sirf.state.ecef_pos.y = Invert4Bytes(p->y_pos) * 100; + gps_sirf.state.ecef_pos.z = Invert4Bytes(p->z_pos) * 100; + SetBit(gps_sirf.state.valid_fields, GPS_VALID_POS_ECEF_BIT); - gps.ecef_vel.x = (Invert2Bytes(p->vx) >> 16) * 100 / 8; - gps.ecef_vel.y = (Invert2Bytes(p->vy) >> 16) * 100 / 8; - gps.ecef_vel.z = (Invert2Bytes(p->vz) >> 16) * 100 / 8; - SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT); + gps_sirf.state.ecef_vel.x = (Invert2Bytes(p->vx) >> 16) * 100 / 8; + gps_sirf.state.ecef_vel.y = (Invert2Bytes(p->vy) >> 16) * 100 / 8; + gps_sirf.state.ecef_vel.z = (Invert2Bytes(p->vz) >> 16) * 100 / 8; + SetBit(gps_sirf.state.valid_fields, GPS_VALID_VEL_ECEF_BIT); - if (gps.fix == GPS_FIX_3D) { + if (gps_sirf.state.fix == GPS_FIX_3D) { ticks++; #if DEBUG_SIRF printf("GPS %i %i %i %i\n", ticks, (sys_time.nb_sec - start_time), ticks2, (sys_time.nb_sec - start_time2)); #endif - } else if (sys_time.nb_sec - gps.last_3dfix_time > 10) { + } else if (sys_time.nb_sec - gps_sirf.state.last_3dfix_time > 10) { start_time = sys_time.nb_sec; ticks = 0; } @@ -201,3 +284,24 @@ void sirf_parse_msg(void) } } + +void gps_sirf_event(void) +{ + struct link_device *dev = &((SIRF_GPS_LINK).device); + + while (dev->char_available(dev->periph)) { + sirf_parse_char(dev->get_byte(dev->periph)); + if (gps_sirf.msg_available) { + gps_sirf_msg(); + } + } +} + + +/* + * register callbacks & structs + */ +void gps_sirf_register(void) +{ + gps_register_impl(gps_sirf_init, gps_sirf_event, GPS_SIRF_ID); +} diff --git a/sw/airborne/subsystems/gps/gps_sirf.h b/sw/airborne/subsystems/gps/gps_sirf.h index d8929120e9..ae3f9d0b1f 100644 --- a/sw/airborne/subsystems/gps/gps_sirf.h +++ b/sw/airborne/subsystems/gps/gps_sirf.h @@ -30,8 +30,13 @@ #define GPS_SIRF_H #include "std.h" +#include "subsystems/gps.h" -#define GPS_NB_CHANNELS 16 +#ifndef PRIMARY_GPS +#define PRIMARY_GPS gps_sirf +#endif + +#define SIRF_GPS_NB_CHANNELS 16 #define SIRF_MAXLEN 255 //Read states @@ -46,104 +51,14 @@ struct GpsSirf { char msg_buf[SIRF_MAXLEN]; ///< buffer for storing one nmea-line int msg_len; int read_state; + struct GpsState state; }; extern struct GpsSirf gps_sirf; -//Invert bytes -#define Invert2Bytes(x) ((x>>8) | (x<<8)) -#define Invert4Bytes(x) ((x>>24) | ((x<<8) & 0x00FF0000) | ((x>>8) & 0x0000FF00) | (x<<24)) +extern void gps_sirf_init(void); +extern void gps_sirf_event(void); +extern void gps_sirf_register(void); -/** Message ID 2 from GPS. Total payload length should be 41 bytes. */ -struct sirf_msg_2 { - uint8_t msg_id; ///< hex value 0x02 ( = decimal 2) - int32_t x_pos; ///< x-position in m - int32_t y_pos; ///< y-position in m - int32_t z_pos; ///< z-position in m - int16_t vx; ///< x-velocity * 8 in m/s - int16_t vy; ///< y-velocity * 8 in m/s - int16_t vz; ///< z-velocity * 8 in m/s - uint8_t mode1; - uint8_t hdop; ///< horizontal dilution of precision *5 (0.2 precision) - uint8_t mode2; - uint16_t week; - uint32_t tow; ///< time of week in seconds * 10^2 - uint8_t num_sat; ///< Number of satellites in fix - uint8_t ch1prn; ///< pseudo-random noise, 12 channels - uint8_t ch2prn; - uint8_t ch3prn; - uint8_t ch4prn; - uint8_t ch5prn; - uint8_t ch6prn; - uint8_t ch7prn; - uint8_t ch8prn; - uint8_t ch9prn; - uint8_t ch10prn; - uint8_t ch11prn; - uint8_t ch12prn; -} __attribute__((packed)); - - -/** Message ID 41 from GPS. Total payload length should be 91 bytes. */ -struct sirf_msg_41 { - uint8_t msg_id; ///< hex value 0x29 (= decimal 41) - uint16_t nav_valid; ///< if equal to 0x0000, then navigation solution is valid - uint16_t nav_type; - uint16_t extended_week_number; - uint32_t tow; ///< time of week in seconds *10^3] - uint16_t year; - uint8_t month; - uint8_t day; - uint8_t hour; - uint8_t minute; - uint16_t second; - uint32_t sat_id; ///< satellites used in solution. Each satellite corresponds with a bit, e.g. bit 1 ON = SV 1 is used in solution - int32_t latitude; ///< in degrees (+= North) *10^7 - int32_t longitude; ///< in degrees (+= East) *10*7 - int32_t alt_ellipsoid; ///< in meters *10^2 - int32_t alt_msl; ///< in meters *10^2 - int8_t map_datum; - uint16_t sog; ///< speed over ground, in m/s * 10^2 - uint16_t cog; ///< course over ground, in degrees clockwise from true north * 10^2 - int16_t mag_var; ///< not implemented - int16_t climb_rate; ///< in m/s * 10^2 - int16_t heading_rate; ///< in deg/s * 10^2 - uint32_t ehpe; ///< estimated horizontal position error, in meters * 10^2 - uint32_t evpe; ///< estimated vertical position error, in meters * 10^2 - uint32_t ete; ///< estimated time error, in seconds * 10^2 - uint16_t ehve; ///< estimated horizontal velocity error in m/s * 10^2 - int32_t clock_bias; ///< in m * 10^2 - uint32_t clock_bias_err; ///< in m * 10^2 - int32_t clock_drift; ///< in m/s * 10^2 - uint32_t clock_drift_err; ///< in m/s * 10^2 - uint32_t distance; ///< Distance traveled since reset in m - uint16_t distance_err; ///< in meters - uint16_t heading_err; ///< in degrees * 10^2 - uint8_t num_sat; ///< Number of satellites used for solution - uint8_t hdop; ///< Horizontal dilution of precision x 5 (0.2 precision) - uint8_t add_info; ///< Additional mode info -} __attribute__((packed)); - -/* - * This part is used by the autopilot to read data from a uart - */ -#include "pprzlink/pprzlink_device.h" -#include "mcu_periph/uart.h" - -extern void sirf_parse_char(uint8_t c); -extern void sirf_parse_msg(void); -extern void gps_sirf_msg(void); - -static inline void GpsEvent(void) -{ - struct link_device *dev = &((GPS_LINK).device); - - while (dev->char_available(dev->periph)) { - sirf_parse_char(dev->get_byte(dev->periph)); - if (gps_sirf.msg_available) { - gps_sirf_msg(); - } - } -} #endif /* GPS_SIRF_H */ diff --git a/sw/airborne/subsystems/gps/gps_skytraq.c b/sw/airborne/subsystems/gps/gps_skytraq.c index c450886421..555de3837e 100644 --- a/sw/airborne/subsystems/gps/gps_skytraq.c +++ b/sw/airborne/subsystems/gps/gps_skytraq.c @@ -20,10 +20,10 @@ */ #include "subsystems/gps.h" +#include "subsystems/gps/gps_skytraq.h" #include "subsystems/abi.h" #include "led.h" - -struct GpsSkytraq gps_skytraq; +#include "pprzlink/pprzlink_device.h" /* parser status */ #define UNINIT 0 @@ -48,6 +48,11 @@ struct GpsSkytraq gps_skytraq; #define SKYTRAQ_SYNC3 0x0D #define SKYTRAQ_SYNC4 0x0A +struct GpsSkytraq gps_skytraq; + +void gps_skytraq_read_message(void); +void gps_skytraq_parse(uint8_t c); +void gps_skytraq_msg(void); static inline uint16_t bswap16(uint16_t a) { @@ -85,96 +90,105 @@ static inline uint16_t bswap16(uint16_t a) static int distance_too_great(struct EcefCoor_i *ecef_ref, struct EcefCoor_i *ecef_pos); -void gps_impl_init(void) +void gps_skytraq_init(void) { - gps_skytraq.status = UNINIT; - } void gps_skytraq_msg(void) { // current timestamp uint32_t now_ts = get_sys_time_usec(); - gps.last_msg_ticks = sys_time.nb_sec_rem; - gps.last_msg_time = sys_time.nb_sec; + gps_skytraq.state.last_msg_ticks = sys_time.nb_sec_rem; + gps_skytraq.state.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; + if (gps_skytraq.state.fix == GPS_FIX_3D) { + gps_skytraq.state.last_3dfix_ticks = sys_time.nb_sec_rem; + gps_skytraq.state.last_3dfix_time = sys_time.nb_sec; } AbiSendMsgGPS(GPS_SKYTRAQ_ID, now_ts, &gps); } gps_skytraq.msg_available = FALSE; } +void gps_skytraq_event(void) +{ + struct link_device *dev = &((SKYTRAQ_GPS_LINK).device); + + while (dev->char_available(dev->periph)) { + gps_skytraq_parse(dev->get_byte(dev->periph)); + if (gps_skytraq.msg_available) { + gps_skytraq_msg(); + } + } +} void gps_skytraq_read_message(void) { if (gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) { - gps.ecef_pos.x = SKYTRAQ_NAVIGATION_DATA_ECEFX(gps_skytraq.msg_buf); - gps.ecef_pos.y = SKYTRAQ_NAVIGATION_DATA_ECEFY(gps_skytraq.msg_buf); - gps.ecef_pos.z = SKYTRAQ_NAVIGATION_DATA_ECEFZ(gps_skytraq.msg_buf); - SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT); + gps_skytraq.state.ecef_pos.x = SKYTRAQ_NAVIGATION_DATA_ECEFX(gps_skytraq.msg_buf); + gps_skytraq.state.ecef_pos.y = SKYTRAQ_NAVIGATION_DATA_ECEFY(gps_skytraq.msg_buf); + gps_skytraq.state.ecef_pos.z = SKYTRAQ_NAVIGATION_DATA_ECEFZ(gps_skytraq.msg_buf); + SetBit(gps_skytraq.state.valid_fields, GPS_VALID_POS_ECEF_BIT); - gps.ecef_vel.x = SKYTRAQ_NAVIGATION_DATA_ECEFVX(gps_skytraq.msg_buf); - gps.ecef_vel.y = SKYTRAQ_NAVIGATION_DATA_ECEFVY(gps_skytraq.msg_buf); - gps.ecef_vel.z = SKYTRAQ_NAVIGATION_DATA_ECEFVZ(gps_skytraq.msg_buf); - SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT); + gps_skytraq.state.ecef_vel.x = SKYTRAQ_NAVIGATION_DATA_ECEFVX(gps_skytraq.msg_buf); + gps_skytraq.state.ecef_vel.y = SKYTRAQ_NAVIGATION_DATA_ECEFVY(gps_skytraq.msg_buf); + gps_skytraq.state.ecef_vel.z = SKYTRAQ_NAVIGATION_DATA_ECEFVZ(gps_skytraq.msg_buf); + SetBit(gps_skytraq.state.valid_fields, GPS_VALID_VEL_ECEF_BIT); - gps.lla_pos.lat = SKYTRAQ_NAVIGATION_DATA_LAT(gps_skytraq.msg_buf); - gps.lla_pos.lon = SKYTRAQ_NAVIGATION_DATA_LON(gps_skytraq.msg_buf); - gps.lla_pos.alt = SKYTRAQ_NAVIGATION_DATA_AEL(gps_skytraq.msg_buf) * 10; - SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); + gps_skytraq.state.lla_pos.lat = SKYTRAQ_NAVIGATION_DATA_LAT(gps_skytraq.msg_buf); + gps_skytraq.state.lla_pos.lon = SKYTRAQ_NAVIGATION_DATA_LON(gps_skytraq.msg_buf); + gps_skytraq.state.lla_pos.alt = SKYTRAQ_NAVIGATION_DATA_AEL(gps_skytraq.msg_buf) * 10; + SetBit(gps_skytraq.state.valid_fields, GPS_VALID_POS_LLA_BIT); - gps.hmsl = SKYTRAQ_NAVIGATION_DATA_ASL(gps_skytraq.msg_buf) * 10; - SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); + gps_skytraq.state.hmsl = SKYTRAQ_NAVIGATION_DATA_ASL(gps_skytraq.msg_buf) * 10; + SetBit(gps_skytraq.state.valid_fields, GPS_VALID_HMSL_BIT); // pacc; // sacc; - gps.pdop = SKYTRAQ_NAVIGATION_DATA_PDOP(gps_skytraq.msg_buf); - gps.num_sv = SKYTRAQ_NAVIGATION_DATA_NumSV(gps_skytraq.msg_buf); - gps.tow = SKYTRAQ_NAVIGATION_DATA_TOW(gps_skytraq.msg_buf) * 10; + gps_skytraq.state.pdop = SKYTRAQ_NAVIGATION_DATA_PDOP(gps_skytraq.msg_buf); + gps_skytraq.state.num_sv = SKYTRAQ_NAVIGATION_DATA_NumSV(gps_skytraq.msg_buf); + gps_skytraq.state.tow = SKYTRAQ_NAVIGATION_DATA_TOW(gps_skytraq.msg_buf) * 10; switch (SKYTRAQ_NAVIGATION_DATA_FixMode(gps_skytraq.msg_buf)) { case SKYTRAQ_FIX_3D_DGPS: case SKYTRAQ_FIX_3D: - gps.fix = GPS_FIX_3D; + gps_skytraq.state.fix = GPS_FIX_3D; break; case SKYTRAQ_FIX_2D: - gps.fix = GPS_FIX_2D; + gps_skytraq.state.fix = GPS_FIX_2D; break; default: - gps.fix = GPS_FIX_NONE; + gps_skytraq.state.fix = GPS_FIX_NONE; } - if (gps.fix == GPS_FIX_3D) { - if (distance_too_great(&gps_skytraq.ref_ltp.ecef, &gps.ecef_pos)) { + if (gps_skytraq.state.fix == GPS_FIX_3D) { + if (distance_too_great(&gps_skytraq.ref_ltp.ecef, &gps_skytraq.state.ecef_pos)) { // just grab current ecef_pos as reference. - ltp_def_from_ecef_i(&gps_skytraq.ref_ltp, &gps.ecef_pos); + ltp_def_from_ecef_i(&gps_skytraq.ref_ltp, &gps_skytraq.state.ecef_pos); } // convert ecef velocity vector to NED vector. - ned_of_ecef_vect_i(&gps.ned_vel, &gps_skytraq.ref_ltp, &gps.ecef_vel); - SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT); + ned_of_ecef_vect_i(&gps_skytraq.state.ned_vel, &gps_skytraq.ref_ltp, &gps_skytraq.state.ecef_vel); + SetBit(gps_skytraq.state.valid_fields, GPS_VALID_VEL_NED_BIT); // ground course in radians - gps.course = (atan2f((float)gps.ned_vel.y, (float)gps.ned_vel.x)) * 1e7; - SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); - // GT: gps.cacc = ... ? what should course accuracy be? + gps_skytraq.state.course = (atan2f((float)gps_skytraq.state.ned_vel.y, (float)gps_skytraq.state.ned_vel.x)) * 1e7; + SetBit(gps_skytraq.state.valid_fields, GPS_VALID_COURSE_BIT); + // GT: gps_skytraq.state.cacc = ... ? what should course accuracy be? // ground speed - gps.gspeed = sqrt(gps.ned_vel.x * gps.ned_vel.x + gps.ned_vel.y * gps.ned_vel.y); - gps.speed_3d = sqrt(gps.ned_vel.x * gps.ned_vel.x + gps.ned_vel.y * gps.ned_vel.y + gps.ned_vel.z * gps.ned_vel.z); + gps_skytraq.state.gspeed = sqrt(gps_skytraq.state.ned_vel.x * gps_skytraq.state.ned_vel.x + gps_skytraq.state.ned_vel.y * gps_skytraq.state.ned_vel.y); + gps_skytraq.state.speed_3d = sqrt(gps_skytraq.state.ned_vel.x * gps_skytraq.state.ned_vel.x + gps_skytraq.state.ned_vel.y * gps_skytraq.state.ned_vel.y + gps_skytraq.state.ned_vel.z * gps_skytraq.state.ned_vel.z); // vertical speed (climb) - // solved by gps.ned.z? + // solved by gps_skytraq.state.ned.z? } #ifdef GPS_LED - if (gps.fix == GPS_FIX_3D) { + if (gps_skytraq.state.fix == GPS_FIX_3D) { LED_ON(GPS_LED); } else { LED_TOGGLE(GPS_LED); @@ -273,3 +287,11 @@ static int distance_too_great(struct EcefCoor_i *ecef_ref, struct EcefCoor_i *ec return FALSE; } + +/* + * register callbacks & structs + */ +void gps_skytraq_register(void) +{ + gps_register_impl(gps_skytraq_init, gps_skytraq_event, GPS_SKYTRAQ_ID); +} diff --git a/sw/airborne/subsystems/gps/gps_skytraq.h b/sw/airborne/subsystems/gps/gps_skytraq.h index 8e51e2da13..f8cc166f6e 100644 --- a/sw/airborne/subsystems/gps/gps_skytraq.h +++ b/sw/airborne/subsystems/gps/gps_skytraq.h @@ -26,6 +26,10 @@ #define SKYTRAQ_ID_NAVIGATION_DATA 0XA8 +#ifndef PRIMARY_GPS +#define PRIMARY_GPS gps_skytraq +#endif + /* last error type */ enum GpsSkytraqError { GPS_SKYTRAQ_ERR_NONE = 0, @@ -50,30 +54,14 @@ struct GpsSkytraq { enum GpsSkytraqError error_last; struct LtpDef_i ref_ltp; + + struct GpsState state; }; extern struct GpsSkytraq gps_skytraq; - -/* - * This part is used by the autopilot to read data from a uart - */ -#include "pprzlink/pprzlink_device.h" - -extern void gps_skytraq_read_message(void); -extern void gps_skytraq_parse(uint8_t c); -extern void gps_skytraq_msg(void); - -static inline void GpsEvent(void) -{ - struct link_device *dev = &((GPS_LINK).device); - - while (dev->char_available(dev->periph)) { - gps_skytraq_parse(dev->get_byte(dev->periph)); - if (gps_skytraq.msg_available) { - gps_skytraq_msg(); - } - } -} +extern void gps_skytraq_init(void); +extern void gps_skytraq_event(void); +extern void gps_skytraq_register(void); #endif /* GPS_SKYTRAQ_H */ diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c index 60de79c337..6914de5b49 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.c +++ b/sw/airborne/subsystems/gps/gps_ubx.c @@ -20,7 +20,7 @@ */ -#include "subsystems/gps.h" +#include "subsystems/gps/gps_ubx.h" #include "subsystems/abi.h" #include "led.h" @@ -55,14 +55,29 @@ struct GpsUbx gps_ubx; struct GpsUbxRaw gps_ubx_raw; #endif -void gps_impl_init(void) +struct GpsTimeSync gps_ubx_time_sync; + +void gps_ubx_init(void) { gps_ubx.status = UNINIT; gps_ubx.msg_available = FALSE; gps_ubx.error_cnt = 0; gps_ubx.error_last = GPS_UBX_ERR_NONE; + + gps_ubx.state.comp_id = GPS_UBX_ID; } +void gps_ubx_event(void) +{ + struct link_device *dev = &((UBX_GPS_LINK).device); + + while (dev->char_available(dev->periph)) { + gps_ubx_parse(dev->get_byte(dev->periph)); + if (gps_ubx.msg_available) { + gps_ubx_msg(); + } + } +} void gps_ubx_read_message(void) { @@ -70,83 +85,83 @@ void gps_ubx_read_message(void) if (gps_ubx.msg_class == UBX_NAV_ID) { if (gps_ubx.msg_id == UBX_NAV_SOL_ID) { /* get hardware clock ticks */ - gps_time_sync.t0_ticks = sys_time.nb_tick; - gps_time_sync.t0_tow = UBX_NAV_SOL_ITOW(gps_ubx.msg_buf); - gps_time_sync.t0_tow_frac = UBX_NAV_SOL_Frac(gps_ubx.msg_buf); - gps.tow = UBX_NAV_SOL_ITOW(gps_ubx.msg_buf); - gps.week = UBX_NAV_SOL_week(gps_ubx.msg_buf); - gps.fix = UBX_NAV_SOL_GPSfix(gps_ubx.msg_buf); - gps.ecef_pos.x = UBX_NAV_SOL_ECEF_X(gps_ubx.msg_buf); - gps.ecef_pos.y = UBX_NAV_SOL_ECEF_Y(gps_ubx.msg_buf); - gps.ecef_pos.z = UBX_NAV_SOL_ECEF_Z(gps_ubx.msg_buf); - SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT); - gps.pacc = UBX_NAV_SOL_Pacc(gps_ubx.msg_buf); - gps.ecef_vel.x = UBX_NAV_SOL_ECEFVX(gps_ubx.msg_buf); - gps.ecef_vel.y = UBX_NAV_SOL_ECEFVY(gps_ubx.msg_buf); - gps.ecef_vel.z = UBX_NAV_SOL_ECEFVZ(gps_ubx.msg_buf); - SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT); - gps.sacc = UBX_NAV_SOL_Sacc(gps_ubx.msg_buf); - gps.pdop = UBX_NAV_SOL_PDOP(gps_ubx.msg_buf); - gps.num_sv = UBX_NAV_SOL_numSV(gps_ubx.msg_buf); + gps_ubx_time_sync.t0_ticks = sys_time.nb_tick; + gps_ubx_time_sync.t0_tow = UBX_NAV_SOL_ITOW(gps_ubx.msg_buf); + gps_ubx_time_sync.t0_tow_frac = UBX_NAV_SOL_Frac(gps_ubx.msg_buf); + gps_ubx.state.tow = UBX_NAV_SOL_ITOW(gps_ubx.msg_buf); + gps_ubx.state.week = UBX_NAV_SOL_week(gps_ubx.msg_buf); + gps_ubx.state.fix = UBX_NAV_SOL_GPSfix(gps_ubx.msg_buf); + gps_ubx.state.ecef_pos.x = UBX_NAV_SOL_ECEF_X(gps_ubx.msg_buf); + gps_ubx.state.ecef_pos.y = UBX_NAV_SOL_ECEF_Y(gps_ubx.msg_buf); + gps_ubx.state.ecef_pos.z = UBX_NAV_SOL_ECEF_Z(gps_ubx.msg_buf); + SetBit(gps_ubx.state.valid_fields, GPS_VALID_POS_ECEF_BIT); + gps_ubx.state.pacc = UBX_NAV_SOL_Pacc(gps_ubx.msg_buf); + gps_ubx.state.ecef_vel.x = UBX_NAV_SOL_ECEFVX(gps_ubx.msg_buf); + gps_ubx.state.ecef_vel.y = UBX_NAV_SOL_ECEFVY(gps_ubx.msg_buf); + gps_ubx.state.ecef_vel.z = UBX_NAV_SOL_ECEFVZ(gps_ubx.msg_buf); + SetBit(gps_ubx.state.valid_fields, GPS_VALID_VEL_ECEF_BIT); + gps_ubx.state.sacc = UBX_NAV_SOL_Sacc(gps_ubx.msg_buf); + gps_ubx.state.pdop = UBX_NAV_SOL_PDOP(gps_ubx.msg_buf); + gps_ubx.state.num_sv = UBX_NAV_SOL_numSV(gps_ubx.msg_buf); #ifdef GPS_LED - if (gps.fix == GPS_FIX_3D) { + if (gps_ubx.state.fix == GPS_FIX_3D) { LED_ON(GPS_LED); } else { LED_TOGGLE(GPS_LED); } #endif } else if (gps_ubx.msg_id == UBX_NAV_POSLLH_ID) { - gps.lla_pos.lat = UBX_NAV_POSLLH_LAT(gps_ubx.msg_buf); - gps.lla_pos.lon = UBX_NAV_POSLLH_LON(gps_ubx.msg_buf); - gps.lla_pos.alt = UBX_NAV_POSLLH_HEIGHT(gps_ubx.msg_buf); - SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); - gps.hmsl = UBX_NAV_POSLLH_HMSL(gps_ubx.msg_buf); - SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); + gps_ubx.state.lla_pos.lat = UBX_NAV_POSLLH_LAT(gps_ubx.msg_buf); + gps_ubx.state.lla_pos.lon = UBX_NAV_POSLLH_LON(gps_ubx.msg_buf); + gps_ubx.state.lla_pos.alt = UBX_NAV_POSLLH_HEIGHT(gps_ubx.msg_buf); + SetBit(gps_ubx.state.valid_fields, GPS_VALID_POS_LLA_BIT); + gps_ubx.state.hmsl = UBX_NAV_POSLLH_HMSL(gps_ubx.msg_buf); + SetBit(gps_ubx.state.valid_fields, GPS_VALID_HMSL_BIT); } else if (gps_ubx.msg_id == UBX_NAV_POSUTM_ID) { - gps.utm_pos.east = UBX_NAV_POSUTM_EAST(gps_ubx.msg_buf); - gps.utm_pos.north = UBX_NAV_POSUTM_NORTH(gps_ubx.msg_buf); + gps_ubx.state.utm_pos.east = UBX_NAV_POSUTM_EAST(gps_ubx.msg_buf); + gps_ubx.state.utm_pos.north = UBX_NAV_POSUTM_NORTH(gps_ubx.msg_buf); uint8_t hem = UBX_NAV_POSUTM_HEM(gps_ubx.msg_buf); if (hem == UTM_HEM_SOUTH) { - gps.utm_pos.north -= 1000000000; /* Subtract false northing: -10000km */ + gps_ubx.state.utm_pos.north -= 1000000000; /* Subtract false northing: -10000km */ } - gps.utm_pos.alt = UBX_NAV_POSUTM_ALT(gps_ubx.msg_buf) * 10; - gps.utm_pos.zone = UBX_NAV_POSUTM_ZONE(gps_ubx.msg_buf); - SetBit(gps.valid_fields, GPS_VALID_POS_UTM_BIT); + gps_ubx.state.utm_pos.alt = UBX_NAV_POSUTM_ALT(gps_ubx.msg_buf) * 10; + gps_ubx.state.utm_pos.zone = UBX_NAV_POSUTM_ZONE(gps_ubx.msg_buf); + SetBit(gps_ubx.state.valid_fields, GPS_VALID_POS_UTM_BIT); - gps.hmsl = gps.utm_pos.alt; - SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); + gps_ubx.state.hmsl = gps_ubx.state.utm_pos.alt; + SetBit(gps_ubx.state.valid_fields, GPS_VALID_HMSL_BIT); /* with UTM only you do not receive ellipsoid altitude, so set only if no valid lla */ - if (!bit_is_set(gps.valid_fields, GPS_VALID_HMSL_BIT)) { - gps.lla_pos.alt = gps.utm_pos.alt; + if (!bit_is_set(gps_ubx.state.valid_fields, GPS_VALID_HMSL_BIT)) { + gps_ubx.state.lla_pos.alt = gps_ubx.state.utm_pos.alt; } } else if (gps_ubx.msg_id == UBX_NAV_VELNED_ID) { - gps.speed_3d = UBX_NAV_VELNED_Speed(gps_ubx.msg_buf); - gps.gspeed = UBX_NAV_VELNED_GSpeed(gps_ubx.msg_buf); - gps.ned_vel.x = UBX_NAV_VELNED_VEL_N(gps_ubx.msg_buf); - gps.ned_vel.y = UBX_NAV_VELNED_VEL_E(gps_ubx.msg_buf); - gps.ned_vel.z = UBX_NAV_VELNED_VEL_D(gps_ubx.msg_buf); - SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT); + gps_ubx.state.speed_3d = UBX_NAV_VELNED_Speed(gps_ubx.msg_buf); + gps_ubx.state.gspeed = UBX_NAV_VELNED_GSpeed(gps_ubx.msg_buf); + gps_ubx.state.ned_vel.x = UBX_NAV_VELNED_VEL_N(gps_ubx.msg_buf); + gps_ubx.state.ned_vel.y = UBX_NAV_VELNED_VEL_E(gps_ubx.msg_buf); + gps_ubx.state.ned_vel.z = UBX_NAV_VELNED_VEL_D(gps_ubx.msg_buf); + SetBit(gps_ubx.state.valid_fields, GPS_VALID_VEL_NED_BIT); // Ublox gives I4 heading in 1e-5 degrees, apparenty from 0 to 360 degrees (not -180 to 180) // I4 max = 2^31 = 214 * 1e5 * 100 < 360 * 1e7: overflow on angles over 214 deg -> casted to -214 deg // solution: First to radians, and then scale to 1e-7 radians // First x 10 for loosing less resolution, then to radians, then multiply x 10 again - gps.course = (RadOfDeg(UBX_NAV_VELNED_Heading(gps_ubx.msg_buf) * 10)) * 10; - SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); - gps.cacc = (RadOfDeg(UBX_NAV_VELNED_CAcc(gps_ubx.msg_buf) * 10)) * 10; - gps.tow = UBX_NAV_VELNED_ITOW(gps_ubx.msg_buf); + gps_ubx.state.course = (RadOfDeg(UBX_NAV_VELNED_Heading(gps_ubx.msg_buf) * 10)) * 10; + SetBit(gps_ubx.state.valid_fields, GPS_VALID_COURSE_BIT); + gps_ubx.state.cacc = (RadOfDeg(UBX_NAV_VELNED_CAcc(gps_ubx.msg_buf) * 10)) * 10; + gps_ubx.state.tow = UBX_NAV_VELNED_ITOW(gps_ubx.msg_buf); } else if (gps_ubx.msg_id == UBX_NAV_SVINFO_ID) { - gps.nb_channels = Min(UBX_NAV_SVINFO_NCH(gps_ubx.msg_buf), GPS_NB_CHANNELS); + gps_ubx.state.nb_channels = Min(UBX_NAV_SVINFO_NCH(gps_ubx.msg_buf), GPS_NB_CHANNELS); uint8_t i; - for (i = 0; i < gps.nb_channels; i++) { - gps.svinfos[i].svid = UBX_NAV_SVINFO_SVID(gps_ubx.msg_buf, i); - gps.svinfos[i].flags = UBX_NAV_SVINFO_Flags(gps_ubx.msg_buf, i); - gps.svinfos[i].qi = UBX_NAV_SVINFO_QI(gps_ubx.msg_buf, i); - gps.svinfos[i].cno = UBX_NAV_SVINFO_CNO(gps_ubx.msg_buf, i); - gps.svinfos[i].elev = UBX_NAV_SVINFO_Elev(gps_ubx.msg_buf, i); - gps.svinfos[i].azim = UBX_NAV_SVINFO_Azim(gps_ubx.msg_buf, i); + for (i = 0; i < gps_ubx.state.nb_channels; i++) { + gps_ubx.state.svinfos[i].svid = UBX_NAV_SVINFO_SVID(gps_ubx.msg_buf, i); + gps_ubx.state.svinfos[i].flags = UBX_NAV_SVINFO_Flags(gps_ubx.msg_buf, i); + gps_ubx.state.svinfos[i].qi = UBX_NAV_SVINFO_QI(gps_ubx.msg_buf, i); + gps_ubx.state.svinfos[i].cno = UBX_NAV_SVINFO_CNO(gps_ubx.msg_buf, i); + gps_ubx.state.svinfos[i].elev = UBX_NAV_SVINFO_Elev(gps_ubx.msg_buf, i); + gps_ubx.state.svinfos[i].azim = UBX_NAV_SVINFO_Azim(gps_ubx.msg_buf, i); } } else if (gps_ubx.msg_id == UBX_NAV_STATUS_ID) { - gps.fix = UBX_NAV_STATUS_GPSfix(gps_ubx.msg_buf); + gps_ubx.state.fix = UBX_NAV_STATUS_GPSfix(gps_ubx.msg_buf); gps_ubx.status_flags = UBX_NAV_STATUS_Flags(gps_ubx.msg_buf); gps_ubx.sol_flags = UBX_NAV_SOL_Flags(gps_ubx.msg_buf); } @@ -158,7 +173,8 @@ void gps_ubx_read_message(void) gps_ubx_raw.week = UBX_RXM_RAW_week(gps_ubx.msg_buf); gps_ubx_raw.numSV = UBX_RXM_RAW_numSV(gps_ubx.msg_buf); uint8_t i; - for (i = 0; i < gps_ubx_raw.numSV; i++) { + uint8_t max_SV = Min(gps_ubx_raw.numSV, GPS_UBX_NB_CHANNELS); + for (i = 0; i < max_SV; i++) { gps_ubx_raw.measures[i].cpMes = UBX_RXM_RAW_cpMes(gps_ubx.msg_buf, i); gps_ubx_raw.measures[i].prMes = UBX_RXM_RAW_prMes(gps_ubx.msg_buf, i); gps_ubx_raw.measures[i].doMes = UBX_RXM_RAW_doMes(gps_ubx.msg_buf, i); @@ -298,7 +314,7 @@ void ubx_send_bytes(struct link_device *dev, uint8_t len, uint8_t *bytes) void ubx_send_cfg_rst(struct link_device *dev, uint16_t bbr , uint8_t reset_mode) { -#ifdef GPS_LINK +#ifdef UBX_GPS_LINK UbxSend_CFG_RST(dev, bbr, reset_mode, 0x00); #endif /* else less harmful for HITL */ } @@ -314,20 +330,24 @@ void gps_ubx_msg(void) // current timestamp uint32_t now_ts = get_sys_time_usec(); - gps.last_msg_ticks = sys_time.nb_sec_rem; - gps.last_msg_time = sys_time.nb_sec; + gps_ubx.state.last_msg_ticks = sys_time.nb_sec_rem; + gps_ubx.state.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 && - !bit_is_set(gps.valid_fields, GPS_VALID_VEL_NED_BIT)))) { - if (gps.fix == GPS_FIX_3D) { - gps.last_3dfix_ticks = sys_time.nb_sec_rem; - gps.last_3dfix_time = sys_time.nb_sec; + !bit_is_set(gps_ubx.state.valid_fields, GPS_VALID_VEL_NED_BIT)))) { + if (gps_ubx.state.fix == GPS_FIX_3D) { + gps_ubx.state.last_3dfix_ticks = sys_time.nb_sec_rem; + gps_ubx.state.last_3dfix_time = sys_time.nb_sec; } - AbiSendMsgGPS(GPS_UBX_ID, now_ts, &gps); + AbiSendMsgGPS(GPS_UBX_ID, now_ts, &gps_ubx.state); } gps_ubx.msg_available = FALSE; } +void gps_ubx_register(void) +{ + gps_register_impl(gps_ubx_init, gps_ubx_event, GPS_UBX_ID); +} diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h index 27cc9a8ea2..f59c377364 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.h +++ b/sw/airborne/subsystems/gps/gps_ubx.h @@ -27,13 +27,23 @@ #ifndef GPS_UBX_H #define GPS_UBX_H +#include "subsystems/gps.h" + #ifdef GPS_CONFIGURE #warning "Please use gps_ubx_ucenter.xml module instead of GPS_CONFIGURE" #endif #include "mcu_periph/uart.h" -#define GPS_NB_CHANNELS 16 +#ifndef PRIMARY_GPS +#define PRIMARY_GPS gps_ubx +#endif + +void gps_ubx_init(void); +void gps_ubx_event(void); +extern void gps_ubx_register(void); + +#define GPS_UBX_NB_CHANNELS 16 #define GPS_UBX_MAX_PAYLOAD 255 struct GpsUbx { @@ -52,6 +62,8 @@ struct GpsUbx { uint8_t status_flags; uint8_t sol_flags; + + struct GpsState state; }; extern struct GpsUbx gps_ubx; @@ -71,7 +83,7 @@ struct GpsUbxRaw { int32_t iTOW; int16_t week; uint8_t numSV; - struct GpsUbxRawMes measures[GPS_NB_CHANNELS]; + struct GpsUbxRawMes measures[GPS_UBX_NB_CHANNELS]; }; extern struct GpsUbxRaw gps_ubx_raw; @@ -91,24 +103,6 @@ extern void gps_ubx_read_message(void); extern void gps_ubx_parse(uint8_t c); extern void gps_ubx_msg(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 - */ -static inline void GpsEvent(void) -{ - struct link_device *dev = &((GPS_LINK).device); - - while (dev->char_available(dev->periph)) { - gps_ubx_parse(dev->get_byte(dev->periph)); - if (gps_ubx.msg_available) { - gps_ubx_msg(); - } - } -} - - /* * GPS Reset */ @@ -127,7 +121,7 @@ static inline void GpsEvent(void) gps_ubx.reset = _val; \ if (gps_ubx.reset > CFG_RST_BBR_Warmstart) \ gps_ubx.reset = CFG_RST_BBR_Coldstart; \ - ubx_send_cfg_rst(&(GPS_LINK).device, gps_ubx.reset, CFG_RST_Reset_Controlled); \ + ubx_send_cfg_rst(&(UBX_GPS_LINK).device, gps_ubx.reset, CFG_RST_Reset_Controlled); \ } #endif /* GPS_UBX_H */ diff --git a/sw/airborne/subsystems/gps/gps_udp.c b/sw/airborne/subsystems/gps/gps_udp.c index 7c54d3e0af..fc8868a075 100644 --- a/sw/airborne/subsystems/gps/gps_udp.c +++ b/sw/airborne/subsystems/gps/gps_udp.c @@ -37,9 +37,9 @@ unsigned char gps_udp_read_buffer[256]; struct FmsNetwork *gps_network = NULL; -void gps_impl_init(void) +void gps_udp_init(void) { - gps.fix = GPS_FIX_NONE; + gps_udp.fix = GPS_FIX_NONE; gps_network = network_new(STRINGIFY(GPS_UDP_HOST), 6000 /*out*/, 7000 /*in*/, TRUE); } @@ -47,7 +47,7 @@ void gps_impl_init(void) #define UDP_GPS_INT(_udp_gps_payload) (int32_t)(*((uint8_t*)_udp_gps_payload)|*((uint8_t*)_udp_gps_payload+1)<<8|((int32_t)*((uint8_t*)_udp_gps_payload+2))<<16|((int32_t)*((uint8_t*)_udp_gps_payload+3))<<24) -void gps_parse(void) +void gps_udp_parse(void) { if (gps_network == NULL) { return; } @@ -57,35 +57,35 @@ void gps_parse(void) if (size > 0) { // Correct MSG if ((size == GPS_UDP_MSG_LEN) && (gps_udp_read_buffer[0] == STX)) { - gps.lla_pos.lat = UDP_GPS_INT(gps_udp_read_buffer + 4); - gps.lla_pos.lon = UDP_GPS_INT(gps_udp_read_buffer + 8); - gps.lla_pos.alt = UDP_GPS_INT(gps_udp_read_buffer + 12); - SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); + gps_udp.lla_pos.lat = UDP_GPS_INT(gps_udp_read_buffer + 4); + gps_udp.lla_pos.lon = UDP_GPS_INT(gps_udp_read_buffer + 8); + gps_udp.lla_pos.alt = UDP_GPS_INT(gps_udp_read_buffer + 12); + SetBit(gps_udp.valid_fields, GPS_VALID_POS_LLA_BIT); - gps.hmsl = UDP_GPS_INT(gps_udp_read_buffer + 16); - SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); + gps_udp.hmsl = UDP_GPS_INT(gps_udp_read_buffer + 16); + SetBit(gps_udp.valid_fields, GPS_VALID_HMSL_BIT); - gps.ecef_pos.x = UDP_GPS_INT(gps_udp_read_buffer + 20); - gps.ecef_pos.y = UDP_GPS_INT(gps_udp_read_buffer + 24); - gps.ecef_pos.z = UDP_GPS_INT(gps_udp_read_buffer + 28); - SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT); + gps_udp.ecef_pos.x = UDP_GPS_INT(gps_udp_read_buffer + 20); + gps_udp.ecef_pos.y = UDP_GPS_INT(gps_udp_read_buffer + 24); + gps_udp.ecef_pos.z = UDP_GPS_INT(gps_udp_read_buffer + 28); + SetBit(gps_udp.valid_fields, GPS_VALID_POS_ECEF_BIT); - gps.ecef_vel.x = UDP_GPS_INT(gps_udp_read_buffer + 32); - gps.ecef_vel.y = UDP_GPS_INT(gps_udp_read_buffer + 36); - gps.ecef_vel.z = UDP_GPS_INT(gps_udp_read_buffer + 40); - SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT); + gps_udp.ecef_vel.x = UDP_GPS_INT(gps_udp_read_buffer + 32); + gps_udp.ecef_vel.y = UDP_GPS_INT(gps_udp_read_buffer + 36); + gps_udp.ecef_vel.z = UDP_GPS_INT(gps_udp_read_buffer + 40); + SetBit(gps_udp.valid_fields, GPS_VALID_VEL_ECEF_BIT); - gps.fix = GPS_FIX_3D; + gps_udp.fix = GPS_FIX_3D; // publish new GPS data uint32_t now_ts = get_sys_time_usec(); - gps.last_msg_ticks = sys_time.nb_sec_rem; - gps.last_msg_time = sys_time.nb_sec; - if (gps.fix == GPS_FIX_3D) { - gps.last_3dfix_ticks = sys_time.nb_sec_rem; - gps.last_3dfix_time = sys_time.nb_sec; + gps_udp.last_msg_ticks = sys_time.nb_sec_rem; + gps_udp.last_msg_time = sys_time.nb_sec; + if (gps_udp.fix == GPS_FIX_3D) { + gps_udp.last_3dfix_ticks = sys_time.nb_sec_rem; + gps_udp.last_3dfix_time = sys_time.nb_sec; } - AbiSendMsgGPS(GPS_UDP_ID, now_ts, &gps); + AbiSendMsgGPS(GPS_UDP_ID, now_ts, &gps_udp); } else { printf("gps_udp error: msg len invalid %d bytes\n", size); @@ -94,3 +94,10 @@ void gps_parse(void) } } +/* + * register callbacks & structs + */ +void gps_udp_register(void) +{ + gps_register_impl(gps_udp_init, gps_udp_parse, GPS_UDP_ID); +} diff --git a/sw/airborne/subsystems/gps/gps_udp.h b/sw/airborne/subsystems/gps/gps_udp.h index cc50687eff..b530687e0b 100644 --- a/sw/airborne/subsystems/gps/gps_udp.h +++ b/sw/airborne/subsystems/gps/gps_udp.h @@ -2,11 +2,16 @@ #define GPS_UDP_H #include "std.h" +#include "subsystems/gps.h" -#define GPS_NB_CHANNELS 16 +#ifndef PRIMARY_GPS +#define PRIMARY_GPS gps_udp +#endif -extern void gps_parse(void); +extern struct GpsState gps_udp; -#define GpsEvent gps_parse +extern void gps_udp_parse(void); +extern void gps_udp_init(void); +extern void gps_udp_register(void); #endif /* GPS_UDP_H */ diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c index 77143cd4ea..0725b7844d 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.c +++ b/sw/airborne/subsystems/ins/ins_alt_float.c @@ -71,10 +71,18 @@ PRINT_CONFIG_MSG("USE_BAROMETER is TRUE: Using baro for altitude estimation.") #endif #endif PRINT_CONFIG_VAR(INS_BARO_ID) + abi_event baro_ev; static void baro_cb(uint8_t sender_id, float pressure); #endif /* USE_BAROMETER */ +/** ABI binding for gps data. + * Used for GPS ABI messages. + */ +#ifndef INS_ALT_GPS_ID +#define INS_ALT_GPS_ID GPS_MULTI_ID +#endif +PRINT_CONFIG_VAR(INS_ALT_GPS_ID) static abi_event gps_ev; static abi_event accel_ev; static abi_event body_to_imu_ev; @@ -383,7 +391,7 @@ void ins_altf_register(void) // Bind to BARO_ABS message AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb); #endif - AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); + AbiBindMsgGPS(INS_ALT_GPS_ID, &gps_ev, gps_cb); AbiBindMsgIMU_ACCEL_INT32(INS_ALT_IMU_ID, &accel_ev, accel_cb); AbiBindMsgBODY_TO_IMU_QUAT(INS_ALT_IMU_ID, &body_to_imu_ev, body_to_imu_cb); } diff --git a/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.c b/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.c index 4c83c57b1c..e5764a2a2c 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.c +++ b/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.c @@ -94,6 +94,14 @@ PRINT_CONFIG_VAR(INS_FINV_IMU_ID) #endif PRINT_CONFIG_VAR(INS_FINV_MAG_ID) +/** ABI binding for gps data. + * Used for GPS ABI messages. + */ +#ifndef INS_FINV_GPS_ID +#define INS_FINV_GPS_ID GPS_MULTI_ID +#endif +PRINT_CONFIG_VAR(INS_FINV_GPS_ID) + static abi_event baro_ev; static abi_event mag_ev; static abi_event gyro_ev; @@ -205,7 +213,7 @@ void ins_float_invariant_register(void) AbiBindMsgIMU_LOWPASSED(INS_FINV_IMU_ID, &aligner_ev, aligner_cb); AbiBindMsgBODY_TO_IMU_QUAT(INS_FINV_IMU_ID, &body_to_imu_ev, body_to_imu_cb); AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb); - AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); + AbiBindMsgGPS(INS_FINV_GPS_ID, &gps_ev, gps_cb); #if PERIODIC_TELEMETRY && !INS_FINV_USE_UTM register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_REF, send_ins_ref); diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough.c b/sw/airborne/subsystems/ins/ins_gps_passthrough.c index fd79437b62..93bf552ffe 100644 --- a/sw/airborne/subsystems/ins/ins_gps_passthrough.c +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough.c @@ -144,6 +144,13 @@ void ins_reset_altitude_ref(void) #include "subsystems/abi.h" +/** ABI binding for gps data. + * Used for GPS ABI messages. + */ +#ifndef INS_PT_GPS_ID +#define INS_PT_GPS_ID GPS_MULTI_ID +#endif +PRINT_CONFIG_VAR(INS_PT_GPS_ID) static abi_event gps_ev; static void gps_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), @@ -173,5 +180,5 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)), void ins_gps_passthrough_register(void) { ins_register_impl(ins_gps_passthrough_init); - AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); + AbiBindMsgGPS(INS_PT_GPS_ID, &gps_ev, gps_cb); } diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c b/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c index 6aa8127229..9439f66a08 100644 --- a/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c @@ -63,6 +63,13 @@ void ins_reset_altitude_ref(void) #include "subsystems/abi.h" +/** ABI binding for gps data. + * Used for GPS ABI messages. + */ +#ifndef INS_PTU_GPS_ID +#define INS_PTU_GPS_ID GPS_MULTI_ID +#endif +PRINT_CONFIG_VAR(INS_PTU_GPS_ID) static abi_event gps_ev; static void gps_cb(uint8_t sender_id __attribute__((unused)), @@ -87,5 +94,5 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)), void ins_gps_utm_register(void) { ins_register_impl(ins_gps_utm_init); - AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); + AbiBindMsgGPS(INS_PTU_GPS_ID, &gps_ev, gps_cb); } diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index 3fbad1560f..1fa6aef4c1 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -137,7 +137,7 @@ static void baro_cb(uint8_t sender_id, float pressure); #define INS_INT_IMU_ID ABI_BROADCAST #endif #ifndef INS_INT_GPS_ID -#define INS_INT_GPS_ID ABI_BROADCAST +#define INS_INT_GPS_ID GPS_MULTI_ID #endif static abi_event accel_ev; static abi_event gps_ev; diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink index 1f3c7cd218..631f5f29f2 160000 --- a/sw/ext/pprzlink +++ b/sw/ext/pprzlink @@ -1 +1 @@ -Subproject commit 1f3c7cd2186ec019d46c73dc2ed73a8714705ba5 +Subproject commit 631f5f29f243503fcd7dcead6c924f2b7f70bc05