Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 12:23:23 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

Merge pull request #1532 from flixr/multigps_ms

Browse Source 
support for multiple GPS

To e.g. use piksi as a secondary GPS:
```
<subsystem name="gps" type="ubx">
  <configure name="UBX_GPS_PORT" value="UART2"/>
</subsystem>
<subsystem name="gps" type="piksi">
  <configure name="PIKSI_GPS_PORT" value="UART3"/>
  <configure name="SECONDARY_GPS" value="piksi"/>
</subsystem>
 ```
This commit is contained in:
Felix Ruess
2016-02-12 18:44:09 +01:00
parent 6e75ed84e2 49b215f908
commit 2d2349f421
59 changed files with 1329 additions and 764 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/examples/quadrotor_lisa_mx.xml
+4 -1
View File
@@ -21,7 +21,6 @@
<subsystem name="radio_control" type="spektrum">
<define name="RADIO_MODE" value="RADIO_AUX1"/>
<configure name="USE_SECONDARY_SPEKTRUM_RECEIVER" value="1"/>
</subsystem>
<subsystem name="motor_mixing"/>
@@ -32,6 +31,10 @@
<subsystem name="telemetry" type="transparent"/>
<subsystem name="imu" type="lisa_mx_v2.1"/>
<subsystem name="gps" type="ublox"/>
<subsystem name="gps" type="piksi">
<configure name="PIKSI_GPS_PORT" value="UART5"/>
<configure name="SECONDARY_GPS" value="piksi"/>
</subsystem>
<subsystem name="stabilization" type="int_quat"/>
<subsystem name="stabilization" type="rate"/>
<subsystem name="ahrs" type="float_mlkf">
conf/firmwares/subsystems/rotorcraft/gps_ardrone2.makefile
+4 -5
View File
@@ -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
conf/firmwares/subsystems/rotorcraft/gps_datalink.makefile
+20 -7
View File
@@ -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
conf/firmwares/subsystems/rotorcraft/gps_piksi.makefile
-29
View File
@@ -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
conf/firmwares/subsystems/rotorcraft/gps_sim_hitl.makefile
+5 -4
View File
@@ -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
conf/firmwares/subsystems/rotorcraft/gps_sirf.makefile
+22 -7
View File
@@ -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
conf/firmwares/subsystems/rotorcraft/gps_udp.makefile
+18 -6
View File
@@ -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\"
conf/firmwares/subsystems/shared/gps_furuno.makefile
+24 -6
View File
@@ -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
conf/firmwares/subsystems/shared/gps_mediatek_diy.makefile
+23 -6
View File
@@ -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
conf/firmwares/subsystems/shared/gps_nmea.makefile
+23 -6
View File
@@ -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
conf/firmwares/subsystems/shared/gps_piksi.makefile
+22 -7
View File
@@ -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
conf/firmwares/subsystems/shared/gps_skytraq.makefile
+23 -7
View File
@@ -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
conf/firmwares/subsystems/shared/gps_ublox.makefile
+23 -6
View File
@@ -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
conf/settings/rotorcraft_basic_multi.xml
+22
View File
@@ -0,0 +1,22 @@
<!DOCTYPE settings SYSTEM "settings.dtd">
<settings target="ap|nps">
<dl_settings>
<dl_settings NAME="System">
<dl_setting var="autopilot_mode_auto2" min="0" step="1" max="19" module="autopilot" shortname="auto2" values="KILL|Fail|HOME|Rate|Att|Rate_rcC|Att_rcC|Att_C|Rate_Z|Att_Z|Hover|Hover_C|Hover_Z|Nav|RC_D|CareFree|Forward|Module|Flip|Guided"/>
<dl_setting var="kill_throttle" min="0" step="1" max="1" module="autopilot" values="Resurrect|Kill" handler="KillThrottle"/>
<dl_setting var="autopilot_power_switch" min="0" step="1" max="1" module="autopilot" values="OFF|ON" handler="SetPowerSwitch">
<strip_button name="POWER ON" icon="on.png" value="1" group="power_switch"/>
<strip_button name="POWER OFF" icon="off.png" value="0" group="power_switch"/>
</dl_setting>
<dl_setting var="autopilot_mode" min="0" step="1" max="2" module="autopilot" shortname="mode" values="KILL|Fail|HOME" handler="set_mode"/>
<dl_setting var="multi_gps_mode" min="0" step="1" max="2" module="subsystems/gps" shortname="gpsmode" values="PRIMARY|SECONDARY|AUTO">
<strip_button name="AUTO" icon="gps.png" value="2" group="gps_mode"/>
<strip_button name="PRIMARY" icon="gps1.png" value="0" group="gps_mode_setting"/>
<strip_button name="SECONDARY" icon="gps2.png" value="1" group="gps_mode_setting"/>
</dl_setting>
</dl_settings>
</dl_settings>
</settings>
data/pictures/gcs_icons/gps.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 284 B

data/pictures/gcs_icons/gps1.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 500 B

data/pictures/gcs_icons/gps2.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 732 B

sw/airborne/modules/ahrs/ahrs_infrared.c
+1 -1
View File
@@ -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);
sw/airborne/modules/gps/gps_ubx_ucenter.c
+11 -12
View File
@@ -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
}
sw/airborne/modules/ins/ahrs_chimu_spi.c
+8 -1
View File
@@ -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)
sw/airborne/modules/ins/ins_xsens.c
+19 -3
View File
@@ -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
sw/airborne/modules/ins/ins_xsens.h
+5 -1
View File
@@ -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
sw/airborne/modules/ins/ins_xsens700.c
+19 -2
View File
@@ -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
sw/airborne/modules/stereocam/stereocam2state/stereocam2state.c
+1 -1
View File
@@ -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;
sw/airborne/subsystems/abi_sender_ids.h
+4 -1
View File
@@ -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)
*/
sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c
+8 -1
View File
@@ -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);
sw/airborne/subsystems/ahrs/ahrs_float_dcm_wrapper.c
+8 -1
View File
@@ -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);
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c
+8 -1
View File
@@ -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);
sw/airborne/subsystems/gps.c
+146 -10
View File
@@ -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;
sw/airborne/subsystems/gps.h
+33 -12
View File
@@ -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);
sw/airborne/subsystems/gps/gps_datalink.c
+66 -61
View File
@@ -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, &ltp_def, &enu_pos);
SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT);
ecef_of_enu_point_i(&gps_datalink.ecef_pos, &ltp_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 , &ltp_def , &enu_speed);
SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT);
ecef_of_enu_vect_i(&gps_datalink.ecef_vel , &ltp_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);
}
sw/airborne/subsystems/gps/gps_datalink.h
+9 -4
View File
@@ -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 */
sw/airborne/subsystems/gps/gps_furuno.c
+6 -6
View File
@@ -26,8 +26,8 @@
* GPS furuno based NMEA parser
*/
#include "gps_nmea.h"
#include "subsystems/gps.h"
#include "gps_nmea.h"
#include <stdio.h>
#include <string.h>
@@ -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(&ltp, &gps.ecef_pos);
ecef_of_ned_vect_i(&gps.ecef_vel, &ltp, &gps.ned_vel);
ltp_def_from_ecef_i(&ltp, &gps_nmea.state.ecef_pos);
ecef_of_ned_vect_i(&gps_nmea.state.ecef_vel, &ltp, &gps_nmea.state.ned_vel);
}
sw/airborne/subsystems/gps/gps_mtk.c
+84 -54
View File
@@ -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++); }
}
sw/airborne/subsystems/gps/gps_mtk.h
+6 -21
View File
@@ -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 */
sw/airborne/subsystems/gps/gps_nmea.c
+65 -41
View File
@@ -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);
}
sw/airborne/subsystems/gps/gps_nmea.h
+13 -18
View File
@@ -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)
sw/airborne/subsystems/gps/gps_piksi.c
+94 -49
View File
@@ -43,6 +43,7 @@
#include <libsbp/navigation.h>
#include <libsbp/observation.h>
#include <libsbp/tracking.h>
#include <libsbp/system.h>
#include <libsbp/settings.h>
#include <libsbp/piksi.h>
@@ -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);
}
sw/airborne/subsystems/gps/gps_piksi.h
+5 -6
View File
@@ -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 */
sw/airborne/subsystems/gps/gps_sim.c
+10 -2
View File
@@ -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);
}
sw/airborne/subsystems/gps/gps_sim.h
+6 -4
View File
@@ -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 */
sw/airborne/subsystems/gps/gps_sim_hitl.c
+10 -1
View File
@@ -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);
}
sw/airborne/subsystems/gps/gps_sim_hitl.h
+5 -3
View File
@@ -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 */
sw/airborne/subsystems/gps/gps_sim_nps.c
+43 -34
View File
@@ -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);
}
sw/airborne/subsystems/gps/gps_sim_nps.h
+5 -3
View File
@@ -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 */
sw/airborne/subsystems/gps/gps_sirf.c
+140 -36
View File
@@ -32,11 +32,94 @@
#include <stdio.h>
#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);
}
sw/airborne/subsystems/gps/gps_sirf.h
+10 -95
View File
@@ -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 */
sw/airborne/subsystems/gps/gps_skytraq.c
+64 -42
View File
@@ -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);
}
sw/airborne/subsystems/gps/gps_skytraq.h
+9 -21
View File
@@ -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 */
sw/airborne/subsystems/gps/gps_ubx.c
+85 -65
View File
@@ -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);
}

Some files were not shown because too many files have changed in this diff Show More