diff --git a/conf/boards/lisa_mx_2.0.makefile b/conf/boards/lisa_mx_2.0.makefile
index ee7ca7d8af..ce2ec0fd6d 100644
--- a/conf/boards/lisa_mx_2.0.makefile
+++ b/conf/boards/lisa_mx_2.0.makefile
@@ -51,12 +51,6 @@ MODEM_BAUD ?= B57600
 
 GPS_PORT ?= UART3
 GPS_BAUD ?= B38400
-GPS_PRIMARY_PORT ?= UART3
-GPS_PRIMARY_BAUD ?= B115200
-GPS_SECONDARY_PORT ?= UART1
-GPS_SECONDARY_BAUD ?= B115200
-GPS_PRIMARY_TYPE ?= UBX
-GPS_SECONDARY_TYPE ?= PIKSI
 
 #
 # default PPM input is on PA01 (SERVO6)
diff --git a/conf/firmwares/subsystems/fixedwing/ins_vectornav.makefile b/conf/firmwares/subsystems/fixedwing/ins_vectornav.makefile
index ddeef1b323..45591ff4d0 100644
--- a/conf/firmwares/subsystems/fixedwing/ins_vectornav.makefile
+++ b/conf/firmwares/subsystems/fixedwing/ins_vectornav.makefile
@@ -19,7 +19,7 @@ sim.CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_gps_passthrough_utm.h\"
 sim.srcs   += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough_utm.c
 
 sim.CFLAGS += -DUSE_GPS
-sim.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
+sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
 sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
 sim.srcs += $(SRC_SUBSYSTEMS)/gps.c
 
@@ -32,7 +32,7 @@ nps.srcs   += $(SRC_SUBSYSTEMS)/imu.c $(SRC_SUBSYSTEMS)/imu/imu_nps.c
 
 nps.CFLAGS += -DUSE_GPS
 nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
-nps.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
+nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
 nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
 
 nps.CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_gps_passthrough_utm.h\"
diff --git a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile b/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile
index 5b94b6d618..c243ad9f96 100644
--- a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile
+++ b/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile
@@ -41,7 +41,7 @@ ap.CFLAGS += -DUSE_GPS_XSENS
 ap.CFLAGS += -DUSE_GPS_XSENS_RAW_DATA
 ap.CFLAGS += -DGPS_NB_CHANNELS=16
 ap.CFLAGS += -DUSE_GPS
-ap.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"modules/ins/ins_xsens.h\"
+ap.CFLAGS += -DGPS_TYPE_H=\"modules/ins/ins_xsens.h\"
 ap.srcs += $(SRC_SUBSYSTEMS)/gps.c
 
 
@@ -62,7 +62,7 @@ $(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_gps_passthrough_utm.h\"
 $(TARGET).srcs   += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough_utm.c
 
 $(TARGET).CFLAGS += -DUSE_GPS
-$(TARGET).CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
+$(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
 
diff --git a/conf/firmwares/subsystems/rotorcraft/gps_ardrone2.makefile b/conf/firmwares/subsystems/rotorcraft/gps_ardrone2.makefile
index 96d287b32d..e95dc52874 100644
--- a/conf/firmwares/subsystems/rotorcraft/gps_ardrone2.makefile
+++ b/conf/firmwares/subsystems/rotorcraft/gps_ardrone2.makefile
@@ -11,11 +11,10 @@ ifneq ($(GPS_LED),none)
 endif
 
 ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ardrone2.h\"
-ap.srcs   += $(SRC_SUBSYSTEMS)/gps/gps_ardrone2.c
-
-$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
+ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_ardrone2.c
+ap.srcs += $(SRC_SUBSYSTEMS)/gps.c
 
 nps.CFLAGS += -DUSE_GPS
-nps.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
+nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
 nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
-
+nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
diff --git a/conf/firmwares/subsystems/rotorcraft/gps_datalink.makefile b/conf/firmwares/subsystems/rotorcraft/gps_datalink.makefile
index 58d37aad10..db20803a65 100644
--- a/conf/firmwares/subsystems/rotorcraft/gps_datalink.makefile
+++ b/conf/firmwares/subsystems/rotorcraft/gps_datalink.makefile
@@ -3,7 +3,7 @@
 GPS_LED ?= none
 
 ap.srcs += $(SRC_SUBSYSTEMS)/gps.c
-ap.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_datalink.h\"
+ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_datalink.h\"
 ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_datalink.c
 
 ap.CFLAGS += -DUSE_GPS -DGPS_DATALINK
@@ -13,6 +13,6 @@ ifneq ($(GPS_LED),none)
 endif
 
 nps.CFLAGS += -DUSE_GPS
-nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
-nps.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
+nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
 nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
+nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
diff --git a/conf/firmwares/subsystems/rotorcraft/gps_piksi.makefile b/conf/firmwares/subsystems/rotorcraft/gps_piksi.makefile
deleted file mode 100644
index 7ce0121335..0000000000
--- a/conf/firmwares/subsystems/rotorcraft/gps_piksi.makefile
+++ /dev/null
@@ -1,28 +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 += -DPIKSI_GPS_LINK=$(GPS_PORT_LOWER)
-
-ifneq ($(GPS_LED),none)
-  ap.CFLAGS += -DGPS_LED=$(GPS_LED)
-endif
-
-$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
-
-ap.CFLAGS += -DPRIMARY_GPS_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 += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
-nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
\ No newline at end of file
diff --git a/conf/firmwares/subsystems/rotorcraft/gps_sim_hitl.makefile b/conf/firmwares/subsystems/rotorcraft/gps_sim_hitl.makefile
index 5bfb1ca832..c49b7239c6 100644
--- a/conf/firmwares/subsystems/rotorcraft/gps_sim_hitl.makefile
+++ b/conf/firmwares/subsystems/rotorcraft/gps_sim_hitl.makefile
@@ -1,7 +1,7 @@
 # Hey Emacs, this is a -*- makefile -*-
 
 ap.CFLAGS += -DUSE_GPS -DHITL
-ap.CFLAGS += -DPRIMARY_GPS_TYPE=\"subsystems/gps/gps_sim_hitl.h\"
+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
@@ -12,5 +12,5 @@ endif
 
 nps.CFLAGS += -DUSE_GPS
 nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
-nps.CFLAGS += -DPRIMARY_GPS_TYPE=\"subsystems/gps/gps_sim_nps.h\"
+nps.CFLAGS += -DGPS_TYPE=\"subsystems/gps/gps_sim_nps.h\"
 nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
diff --git a/conf/firmwares/subsystems/rotorcraft/gps_sirf.makefile b/conf/firmwares/subsystems/rotorcraft/gps_sirf.makefile
index c5d97d02fa..9cbd2990f9 100644
--- a/conf/firmwares/subsystems/rotorcraft/gps_sirf.makefile
+++ b/conf/firmwares/subsystems/rotorcraft/gps_sirf.makefile
@@ -3,23 +3,25 @@
 # 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 += -DSIRF_GPS_LINK=$(SIRF_GPS_PORT_LOWER)
-ap.CFLAGS += -DUSE_$(GPS_PORT)
-ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
+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
 
-ap.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_sirf.h\"
+ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sirf.h\"
 ap.srcs   += $(SRC_SUBSYSTEMS)/gps/gps_sirf.c
-
-$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
+ap.srcs += $(SRC_SUBSYSTEMS)/gps.c
 
 nps.CFLAGS += -DUSE_GPS
-nps.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
+nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
 nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
-
+nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
diff --git a/conf/firmwares/subsystems/rotorcraft/gps_udp.makefile b/conf/firmwares/subsystems/rotorcraft/gps_udp.makefile
index 2e66bd7722..df238a85dc 100644
--- a/conf/firmwares/subsystems/rotorcraft/gps_udp.makefile
+++ b/conf/firmwares/subsystems/rotorcraft/gps_udp.makefile
@@ -3,7 +3,7 @@
 GPS_LED ?= none
 
 ap.srcs += $(SRC_SUBSYSTEMS)/gps.c
-ap.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_udp.h\"
+ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_udp.h\"
 ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_udp.c
 
 ap.CFLAGS += -DUSE_GPS
@@ -14,5 +14,5 @@ endif
 
 nps.CFLAGS += -DUSE_GPS
 nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
-nps.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
+nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
 nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
diff --git a/conf/firmwares/subsystems/rotorcraft/ins_vectornav.makefile b/conf/firmwares/subsystems/rotorcraft/ins_vectornav.makefile
index 0d9e4883a2..a27d5bfe5a 100644
--- a/conf/firmwares/subsystems/rotorcraft/ins_vectornav.makefile
+++ b/conf/firmwares/subsystems/rotorcraft/ins_vectornav.makefile
@@ -14,7 +14,7 @@ nps.srcs   += $(SRC_SUBSYSTEMS)/imu.c $(SRC_SUBSYSTEMS)/imu/imu_nps.c
 
 nps.CFLAGS += -DUSE_GPS
 nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
-nps.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
+nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
 nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
 
 nps.CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_gps_passthrough.h\"
diff --git a/conf/firmwares/subsystems/shared/gps_furuno.makefile b/conf/firmwares/subsystems/shared/gps_furuno.makefile
index ace3306109..26f6e2ca77 100644
--- a/conf/firmwares/subsystems/shared/gps_furuno.makefile
+++ b/conf/firmwares/subsystems/shared/gps_furuno.makefile
@@ -3,28 +3,32 @@
 # 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 += -DNMEA_GPS_LINK=$(FURUNO_GPS_PORT_LOWER)
-ap.CFLAGS += -DUSE_$(GPS_PORT)
-ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
+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
 
-ap.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_nmea.h\"
+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 += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
+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 += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
-nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
\ No newline at end of file
+nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
+nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
+nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
diff --git a/conf/firmwares/subsystems/shared/gps_mediatek_diy.makefile b/conf/firmwares/subsystems/shared/gps_mediatek_diy.makefile
index a37f426188..ab385150e4 100644
--- a/conf/firmwares/subsystems/shared/gps_mediatek_diy.makefile
+++ b/conf/firmwares/subsystems/shared/gps_mediatek_diy.makefile
@@ -3,26 +3,30 @@
 # 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 += -DMTK_GPS_LINK=$(MTK_GPS_PORT_LOWER)
-ap.CFLAGS += -DUSE_$(GPS_PORT)
-ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
+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
 
-ap.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_mtk.h\"
+ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_mtk.h\"
 ap.srcs   += $(SRC_SUBSYSTEMS)/gps/gps_mtk.c
-
-$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
+ap.srcs += $(SRC_SUBSYSTEMS)/gps.c
 
 sim.CFLAGS += -DUSE_GPS
-sim.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
+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 += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
+nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
 nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
+nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
diff --git a/conf/firmwares/subsystems/shared/gps_multi.makefile b/conf/firmwares/subsystems/shared/gps_multi.makefile
deleted file mode 100644
index add8defcdb..0000000000
--- a/conf/firmwares/subsystems/shared/gps_multi.makefile
+++ /dev/null
@@ -1,37 +0,0 @@
-# Hey Emacs, this is a -*- makefile -*-
-
-GPS_LED ?= none
-GPS_PRIMARY_PORT_LOWER=$(shell echo $(GPS_PRIMARY_PORT) | tr A-Z a-z)
-GPS_SECONDARY_PORT_LOWER=$(shell echo $(GPS_SECONDARY_PORT) | tr A-Z a-z)
-
-ap.CFLAGS += -DUSE_GPS
-ap.CFLAGS += -DUSE_MULTI_GPS
-ap.CFLAGS += -DUSE_$(GPS_PRIMARY_PORT) -D$(GPS_PRIMARY_PORT)_BAUD=$(GPS_PRIMARY_BAUD)
-ap.CFLAGS += -DUSE_$(GPS_SECONDARY_PORT) -D$(GPS_SECONDARY_PORT)_BAUD=$(GPS_SECONDARY_BAUD)
-ap.CFLAGS += -DGPS_PRIMARY_PORT=$(GPS_PRIMARY_PORT_LOWER) 
-ap.CFLAGS += -DGPS_SECONDARY_PORT=$(GPS_SECONDARY_PORT_LOWER)
-
-ap.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_ubx.h\"
-ap.CFLAGS += -DSECONDARY_GPS_TYPE_H=\"subsystems/gps/gps_piksi.h\"
-
-ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_piksi.c
-ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_ubx.c
-ap.srcs += $(SRC_SUBSYSTEMS)/gps.c
-
-ap.CFLAGS += -DGPS_PRIMARY_$(GPS_PRIMARY_TYPE)
-ap.CFLAGS += -DGPS_SECONDARY_$(GPS_SECONDARY_TYPE)
-
-ifeq (PIKSI,$(filter PIKSI,$(GPS_PRIMARY_TYPE) $(GPS_SECONDARY_TYPE)))
-  # 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
-endif
-
-ifneq ($(GPS_LED),none)
-  ap.CFLAGS += -DGPS_LED=$(GPS_LED)
-endif
-
-nps.CFLAGS += -DUSE_GPS
-nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
-nps.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
-nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
\ No newline at end of file
diff --git a/conf/firmwares/subsystems/shared/gps_nmea.makefile b/conf/firmwares/subsystems/shared/gps_nmea.makefile
index b0ff377556..4c2e988904 100644
--- a/conf/firmwares/subsystems/shared/gps_nmea.makefile
+++ b/conf/firmwares/subsystems/shared/gps_nmea.makefile
@@ -3,26 +3,30 @@
 # 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 += -DNMEA_GPS_LINK=$(NMEA_GPS_PORT_LOWER)
-ap.CFLAGS += -DUSE_$(GPS_PORT)
-ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
+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
 
-ap.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_nmea.h\"
-ap.srcs   += $(SRC_SUBSYSTEMS)/gps/gps_nmea.c
-
-$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
+ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\"
+ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_nmea.c
+ap.srcs += $(SRC_SUBSYSTEMS)/gps.c
 
 sim.CFLAGS += -DUSE_GPS
-sim.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
+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 += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
+nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
 nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
+nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
diff --git a/conf/firmwares/subsystems/shared/gps_piksi.makefile b/conf/firmwares/subsystems/shared/gps_piksi.makefile
index d910ecbf8f..e6698f1d8f 100644
--- a/conf/firmwares/subsystems/shared/gps_piksi.makefile
+++ b/conf/firmwares/subsystems/shared/gps_piksi.makefile
@@ -3,30 +3,46 @@
 # 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 += -DPIKSI_GPS_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.CFLAGS += -DPRIMARY_GPS=gps_piksi
+endif
+else
+# plain old single GPS usage
+ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_piksi.h\"
+endif
 
-ap.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_piksi.h\"
-ap.srcs   += $(SRC_SUBSYSTEMS)/gps/gps_piksi.c
+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
 ap.srcs		+= $(PAPARAZZI_SRC)/sw/ext/libsbp/c/src/sbp.c $(PAPARAZZI_SRC)/sw/ext/libsbp/c/src/edc.c
 
 sim.CFLAGS += -DUSE_GPS
-sim.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
+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 += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
-nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
\ No newline at end of file
+nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
+nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
+nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
diff --git a/conf/firmwares/subsystems/shared/gps_skytraq.makefile b/conf/firmwares/subsystems/shared/gps_skytraq.makefile
index 94a3f47223..23581fd892 100644
--- a/conf/firmwares/subsystems/shared/gps_skytraq.makefile
+++ b/conf/firmwares/subsystems/shared/gps_skytraq.makefile
@@ -1,27 +1,30 @@
 # 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 += -DSKYTRAQ_GPS_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
 
-ap.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_skytraq.h\"
+ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_skytraq.h\"
 ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_skytraq.c
-
-$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
+ap.srcs += $(SRC_SUBSYSTEMS)/gps.c
 
 sim.CFLAGS += -DUSE_GPS
-sim.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
+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 += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
+nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
 nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
+nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
diff --git a/conf/firmwares/subsystems/shared/gps_ublox.makefile b/conf/firmwares/subsystems/shared/gps_ublox.makefile
index 4c5c6996b3..7685d747f6 100644
--- a/conf/firmwares/subsystems/shared/gps_ublox.makefile
+++ b/conf/firmwares/subsystems/shared/gps_ublox.makefile
@@ -2,26 +2,43 @@
 # UBlox LEA
 
 GPS_LED ?= none
-UBX_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z)
+UBX_GPS_PORT ?= $(GPS_PORT)
+UBX_GPS_BAUD ?= $(GPS_BAUD)
+
+UBX_GPS_PORT_LOWER=$(shell echo $(UBX_GPS_PORT) | tr A-Z a-z)
 
 ap.CFLAGS += -DUSE_GPS -DUBX
 ap.CFLAGS += -DUBX_GPS_LINK=$(UBX_GPS_PORT_LOWER)
-ap.CFLAGS += -DUSE_$(GPS_PORT)
-ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
+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
 
-ap.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_ubx.h\"
-ap.srcs   += $(SRC_SUBSYSTEMS)/gps/gps_ubx.c
+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.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 += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
+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 += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
+nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
 nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
+nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
diff --git a/conf/firmwares/subsystems/shared/ins_xsens700.makefile b/conf/firmwares/subsystems/shared/ins_xsens700.makefile
index 5bc7c31af0..58be4d9d3f 100644
--- a/conf/firmwares/subsystems/shared/ins_xsens700.makefile
+++ b/conf/firmwares/subsystems/shared/ins_xsens700.makefile
@@ -35,7 +35,7 @@ ap.CFLAGS += -DAHRS_TRIGGERED_ATTITUDE_LOOP
 ap.CFLAGS += -DUSE_GPS_XSENS
 ap.CFLAGS += -DGPS_NB_CHANNELS=50
 ap.CFLAGS += -DUSE_GPS
-ap.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"modules/ins/ins_xsens.h\"
+ap.CFLAGS += -DGPS_TYPE_H=\"modules/ins/ins_xsens.h\"
 ap.srcs += $(SRC_SUBSYSTEMS)/gps.c
 
 
@@ -56,7 +56,7 @@ $(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_gps_passthrough_utm.h\"
 $(TARGET).srcs   += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough_utm.c
 
 $(TARGET).CFLAGS += -DUSE_GPS
-$(TARGET).CFLAGS += -DPRIMARY_GPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
+$(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
 
diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c
index c4edafb469..6186d30f16 100644
--- a/sw/airborne/modules/ins/ins_xsens.c
+++ b/sw/airborne/modules/ins/ins_xsens.c
@@ -299,7 +299,7 @@ void ins_xsens_update_gps(struct GpsState *gps_s)
 #endif
 
 #if USE_GPS_XSENS
-void xsens_gps_impl_init(void)
+void gps_xsens_init(void)
 {
   gps.nb_channels = 0;
 }
@@ -316,11 +316,6 @@ static void gps_xsens_publish(void)
   }
   AbiSendMsgGPS(GPS_XSENS_ID, now_ts, &gps);
 }
-
-void xsens_gps_event(void)
-{
-}
-
 #endif
 
 void xsens_periodic(void)
@@ -784,12 +779,8 @@ restart:
 /*
  * register callbacks & structs
  */
-void xsens_gps_register(void)
+void gps_xsens_register(void)
 {
-#ifdef GPS_SECONDARY_XSENS
-  gps_register_impl(xsens_gps_impl_init, xsens_gps_event, GPS_XSENS_ID, 1);
-#else
-  gps_register_impl(xsens_gps_impl_init, xsens_gps_event, GPS_XSENS_ID, 0);
-#endif
+  gps_register_impl(gps_xsens_init, NULL, GPS_XSENS_ID);
 }
 #endif
diff --git a/sw/airborne/modules/ins/ins_xsens.h b/sw/airborne/modules/ins/ins_xsens.h
index 893f2700b4..2b7bf22a9d 100644
--- a/sw/airborne/modules/ins/ins_xsens.h
+++ b/sw/airborne/modules/ins/ins_xsens.h
@@ -79,12 +79,11 @@ extern void ins_xsens_register(void);
 
 
 #if USE_GPS_XSENS
-#ifndef PrimaryGpsImpl
-#define PrimaryGpsImpl xsens
+#ifndef PRIMARY_GPS
+#define PRIMARY_GPS gps_xsens
 #endif
-extern void xsens_gps_event(void);
-extern void xsens_gps_impl_init(void);
-extern void xsens_gps_register(void);
+extern void gps_xsens_init(void);
+extern void gps_xsens_register(void);
 #endif
 
 #endif
diff --git a/sw/airborne/modules/ins/ins_xsens700.c b/sw/airborne/modules/ins/ins_xsens700.c
index 3a29912af5..fa6a787239 100644
--- a/sw/airborne/modules/ins/ins_xsens700.c
+++ b/sw/airborne/modules/ins/ins_xsens700.c
@@ -230,7 +230,7 @@ void ins_xsens_update_gps(struct GpsState *gps_s)
 #endif
 
 #if USE_GPS_XSENS
-void xsens_gps_impl_init(void)
+void gps_xsens_impl_init(void)
 {
   gps.nb_channels = 0;
 }
@@ -247,11 +247,6 @@ static void gps_xsens_publish(void)
   }
   AbiSendMsgGPS(GPS_XSENS_ID, now_ts, &gps);
 }
-
-void xsens_gps_event(void)
-{
-}
-
 #endif
 
 static void xsens_ask_message_rate(uint8_t c1, uint8_t c2, uint8_t freq)
@@ -633,12 +628,8 @@ restart:
 /*
  * register callbacks & structs
  */
-void xsens_gps_register(void)
+void gps_xsens_register(void)
 {
-#ifdef GPS_SECONDARY_XSENS
-  gps_register_impl(xsens_gps_impl_init, xsens_gps_event, GPS_XSENS_ID, 1);
-#else
-  gps_register_impl(xsens_gps_impl_init, xsens_gps_event, GPS_XSENS_ID, 0);
-#endif
+  gps_register_impl(gps_xsens_init, NULL, GPS_XSENS_ID);
 }
 #endif
diff --git a/sw/airborne/subsystems/gps.c b/sw/airborne/subsystems/gps.c
index 489b97be82..fbf7afbe8b 100644
--- a/sw/airborne/subsystems/gps.c
+++ b/sw/airborne/subsystems/gps.c
@@ -30,6 +30,29 @@
 #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 */
+#if USE_MULTI_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"
@@ -40,44 +63,26 @@
 #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;
 
-
-#ifndef PrimaryGpsImpl
-#warning "PrimaryGpsImpl not set!"
-#else
-PRINT_CONFIG_VAR(PrimaryGpsImpl)
-#endif
-#ifdef USE_MULTI_GPS
-#ifndef SecondaryGpsImpl
-#warning "SecondaryGpsImpl not set!"
-#else
-PRINT_CONFIG_VAR(SecondaryGpsImpl)
-#endif
+#if USE_MULTI_GPS
 static uint8_t current_gps_id = 0;
-#endif /*USE_MULTI_GPS*/
-
-#define __PrimaryGpsRegister(_x) _x ## _gps_register()
-#define _PrimaryGpsRegister(_x) __PrimaryGpsRegister(_x)
-#define PrimaryGpsRegister() _PrimaryGpsRegister(PrimaryGpsImpl)
-
-#define __SecondaryGpsRegister(_x) _x ## _gps_register()
-#define _SecondaryGpsRegister(_x) __SecondaryGpsRegister(_x)
-#define SecondaryGpsRegister() _SecondaryGpsRegister(SecondaryGpsImpl)
-#define TIME_TO_SWITCH 5000 //ten s in ms
+#endif
 
 uint8_t multi_gps_mode;
+
 /* gps structs */
-struct GpsInstance {;
+struct GpsInstance {
   ImplGpsInit init;
   ImplGpsEvent event;
   uint8_t id;
 };
 
-struct GpsInstance GpsInstances[GPS_NUM_INSTANCES];
+struct GpsInstance GpsInstances[GPS_NB_IMPL];
 
 #if PERIODIC_TELEMETRY
 #include "subsystems/datalink/telemetry.h"
@@ -228,35 +233,38 @@ static void gps_cb(uint8_t sender_id,
 #endif
 }
 
-/* 
- * handle gps switching and updating gps instances 
+/*
+ * handle gps switching and updating gps instances
  */
 void GpsEvent(void) {
   // run each gps event
-  uint8_t i;
-  for ( i = 0 ; i < GPS_NUM_INSTANCES ; i++) {
-    GpsInstances[i].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, int8_t instance)
+void gps_register_impl(ImplGpsInit init, ImplGpsEvent event, uint8_t id)
 {
-  GpsInstances[instance].init = init;
-  GpsInstances[instance].event = event;
-  GpsInstances[instance].id = 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;
+    }
+  }
 
-  GpsInstances[instance].init();
 }
 
 void gps_init(void)
 {
-
-// #ifdef USE_MULTI_GPS
   multi_gps_mode = MULTI_GPS_MODE;
-// #endif
 
   gps.valid_fields = 0;
   gps.fix = GPS_FIX_NONE;
@@ -268,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);
@@ -275,13 +284,18 @@ void gps_init(void)
 #ifdef GPS_LED
   LED_OFF(GPS_LED);
 #endif
-#ifdef PrimaryGpsImpl
-  PrimaryGpsRegister();
-#endif
-#ifdef SecondaryGpsImpl
-  SecondaryGpsRegister();
+
+  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
diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h
index 94aba411a2..ce00cfe780 100644
--- a/sw/airborne/subsystems/gps.h
+++ b/sw/airborne/subsystems/gps.h
@@ -34,20 +34,6 @@
 
 #include "mcu_periph/sys_time.h"
 
-#define INS_XSENS700_GPS_ID GPS_MULTI_ID
-#define INS_XSENS_GPS_ID GPS_MULTI_ID
-#define AHRS_CHIMU_GPS_ID GPS_MULTI_ID
-#define STEREOCAM_GPS_ID GPS_MULTI_ID
-#define AHRS_INFRARED_GPS_ID GPS_MULTI_ID
-#define INS_FINV_GPS_ID GPS_MULTI_ID
-#define INS_PTU_GPS_ID GPS_MULTI_ID
-#define INS_PT_GPS_ID GPS_MULTI_ID
-#define INS_INT_GPS_ID GPS_MULTI_ID
-#define INS_ALT_GPS_ID GPS_MULTI_ID
-#define AHRS_DCM_GPS_ID GPS_MULTI_ID
-#define AHRS_FC_GPS_ID GPS_MULTI_ID
-#define AHRS_ICQ_GPS_ID GPS_MULTI_ID
-
 #define GPS_FIX_NONE 0x00     ///< No GPS fix
 #define GPS_FIX_2D   0x02     ///< 2D GPS fix
 #define GPS_FIX_3D   0x03     ///< 3D GPS fix
@@ -67,19 +53,10 @@
 #define GPS_VALID_HMSL_BIT     5
 #define GPS_VALID_COURSE_BIT   6
 
-#define PRIMARY_GPS_INSTANCE 0
-#define SECONDARY_GPS_INSTANCE 1
-
 #ifndef GPS_NB_CHANNELS
 #define GPS_NB_CHANNELS 16
 #endif
 
-#ifdef USE_MULTI_GPS
-#define GPS_NUM_INSTANCES 2
-#else
-#define GPS_NUM_INSTANCES 1
-#endif
-
 #define GPS_MODE_PRIMARY 0
 #define GPS_MODE_SECONDARY 1
 #define GPS_MODE_AUTO 2
@@ -147,25 +124,23 @@ typedef void (*ImplGpsInit)(void);
 typedef void (*ImplGpsEvent)(void);
 
 
-void GpsEvent(void);
-/*
- * register callbacks and state pointers
- */ 
-extern void gps_register_impl(ImplGpsInit init, ImplGpsEvent event, uint8_t id, int8_t instance);
+extern void GpsEvent(void);
 
-#ifdef PRIMARY_GPS_TYPE_H
-#include PRIMARY_GPS_TYPE_H
+/**
+ * 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 SECONDARY_GPS_TYPE_H
-#include SECONDARY_GPS_TYPE_H
+#ifdef GPS_SECONDARY_TYPE_H
+#include GPS_SECONDARY_TYPE_H
 #endif
 
 /** initialize the global GPS state */
 extern void gps_init(void);
 
-/** GPS model specific init implementation */
-extern void gps_impl_init(void);
-
 /** GPS packet injection (default empty) */
 extern void gps_inject_data(uint8_t packet_id, uint8_t length, uint8_t *data);
 
diff --git a/sw/airborne/subsystems/gps/gps_datalink.c b/sw/airborne/subsystems/gps/gps_datalink.c
index fa8d3bcf6a..8e18e3e75a 100644
--- a/sw/airborne/subsystems/gps/gps_datalink.c
+++ b/sw/airborne/subsystems/gps/gps_datalink.c
@@ -40,7 +40,7 @@ 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
 
 /** GPS initialization */
-void datalink_gps_impl_init(void)
+void gps_datalink_init(void)
 {
   gps.fix = GPS_FIX_NONE;
   gps_available = FALSE;
@@ -58,10 +58,6 @@ void datalink_gps_impl_init(void)
   ltp_def_from_ecef_i(&ltp_def, &ecef_nav0);
 }
 
-void datalink_gps_event(void)
-{
-}
-
 // Parse the REMOTE_GPS_SMALL datalink packet
 void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xyz, int16_t heading)
 {
@@ -181,7 +177,7 @@ void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t e
 /*
  * register callbacks & structs
  */
-void datalink_gps_register(void)
+void gps_datalink_register(void)
 {
-  gps_register_impl(datalink_gps_impl_init, datalink_gps_event, GPS_DATALINK_ID, 0);
+  gps_register_impl(gps_datalink_init, NULL, GPS_DATALINK_ID);
 }
diff --git a/sw/airborne/subsystems/gps/gps_datalink.h b/sw/airborne/subsystems/gps/gps_datalink.h
index 6bda9a09a0..51630a52ee 100644
--- a/sw/airborne/subsystems/gps/gps_datalink.h
+++ b/sw/airborne/subsystems/gps/gps_datalink.h
@@ -34,15 +34,12 @@
 #include "generated/airframe.h"
 #define DATALINK_GPS_NB_CHANNELS 0
 
-#ifndef PrimaryGpsImpl
-#define PrimaryGpsImpl datalink
+#ifndef PRIMARY_GPS
+#define PRIMARY_GPS gps_datalink
 #endif
 
-extern void datalink_gps_event(void);
-extern void datalink_gps_impl_init(void);
-extern void datalink_gps_register(void);
-
-extern bool_t gps_available;
+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);
 
@@ -51,7 +48,5 @@ extern void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, in
                                int32_t ecef_xd, int32_t ecef_yd, int32_t ecef_zd,
                                uint32_t tow, int32_t course);
 
-// dummy
-// #define GpsEvent() {}
 
 #endif /* GPS_DATALINK_H */
diff --git a/sw/airborne/subsystems/gps/gps_mtk.c b/sw/airborne/subsystems/gps/gps_mtk.c
index e0b277f3bd..7ff1b4415b 100644
--- a/sw/airborne/subsystems/gps/gps_mtk.c
+++ b/sw/airborne/subsystems/gps/gps_mtk.c
@@ -31,10 +31,16 @@
  *
  */
 
+#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
@@ -98,7 +104,11 @@ bool_t gps_configuring;
 static uint8_t gps_status_config;
 #endif
 
-void mtk_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;
@@ -110,7 +120,7 @@ void mtk_gps_impl_init(void)
 #endif
 }
 
-void mtk_gps_event(void)
+void gps_mtk_event(void)
 {
   struct link_device *dev = &((MTK_GPS_LINK).device);
 
@@ -403,13 +413,9 @@ restart:
 /*
  * register callbacks & structs
  */
-void mtk_gps_register(void)
+void gps_mtk_register(void)
 {
-#ifdef GPS_SECONDARY_MTK
-  gps_register_impl(mtk_gps_impl_init, mtk_gps_event, GPS_MTK_ID, 1);
-#else
-  gps_register_impl(mtk_gps_impl_init, mtk_gps_event, GPS_MTK_ID, 0);
-#endif
+  gps_register_impl(gps_mtk_init, gps_mtk_event, GPS_MTK_ID);
 }
 
 /*
diff --git a/sw/airborne/subsystems/gps/gps_mtk.h b/sw/airborne/subsystems/gps/gps_mtk.h
index 7c6e969d58..26e3673e63 100644
--- a/sw/airborne/subsystems/gps/gps_mtk.h
+++ b/sw/airborne/subsystems/gps/gps_mtk.h
@@ -40,22 +40,6 @@
 /** Includes macros generated from mtk.xml */
 #include "mtk_protocol.h"
 
-#if GPS_SECONDARY_MTK
-#ifndef MTK_GPS_LINK
-#define MTK_GPS_LINK GPS_SECONDARY_PORT
-#define SecondaryGpsImpl mtk
-#endif
-#else
-#ifndef PrimaryGpsImpl
-#define PrimaryGpsImpl mtk
-#endif
-#endif
-#if GPS_PRIMARY_MTK
-#ifndef MTK_GPS_LINK
-#define MTK_GPS_LINK GPS_PRIMARY_PORT
-#endif
-#endif
-
 #define GPS_MTK_MAX_PAYLOAD 255
 
 struct GpsMtk {
@@ -80,11 +64,9 @@ struct GpsMtk {
 
 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);
@@ -98,14 +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);
-
-extern void mtk_gps_event(void);
-extern void mtk_gps_impl_init(void);
-extern void mtk_gps_register(void);
-
-
 
 #endif /* MTK_H */
diff --git a/sw/airborne/subsystems/gps/gps_nmea.c b/sw/airborne/subsystems/gps/gps_nmea.c
index ed93644263..1cb33710dc 100644
--- a/sw/airborne/subsystems/gps/gps_nmea.c
+++ b/sw/airborne/subsystems/gps/gps_nmea.c
@@ -63,7 +63,7 @@ static void nmea_parse_RMC(void);
 static void nmea_parse_GGA(void);
 static void nmea_parse_GSV(void);
 
-void nmea_gps_impl_init(void)
+void gps_nmea_init(void)
 {
   gps_nmea.state.nb_channels = GPS_NMEA_NB_CHANNELS;
   gps_nmea.is_configured = FALSE;
@@ -76,7 +76,7 @@ void nmea_gps_impl_init(void)
   nmea_configure();
 }
 
-void nmea_gps_event(void)
+void gps_nmea_event(void)
 {
   struct link_device *dev = &((NMEA_GPS_LINK).device);
 
@@ -546,11 +546,7 @@ static void nmea_parse_GSV(void)
   /*
  * register callbacks & structs
  */
-void nmea_gps_register(void)
+void gps_nmea_register(void)
 {
-#ifdef GPS_SECONDARY_NMEA
-  gps_register_impl(nmea_gps_impl_init, nmea_gps_event, GPS_NMEA_ID, 1);
-#else
-  gps_register_impl(nmea_gps_impl_init, nmea_gps_event, GPS_NMEA_ID, 0);
-#endif
+  gps_register_impl(nmea_gps_init, nmea_gps_event, GPS_NMEA_ID);
 }
diff --git a/sw/airborne/subsystems/gps/gps_nmea.h b/sw/airborne/subsystems/gps/gps_nmea.h
index ff44008afe..43d9abb261 100644
--- a/sw/airborne/subsystems/gps/gps_nmea.h
+++ b/sw/airborne/subsystems/gps/gps_nmea.h
@@ -29,7 +29,7 @@
 
 #ifndef GPS_NMEA_H
 #define GPS_NMEA_H
- 
+
 #include "mcu_periph/uart.h"
 #include "subsystems/gps.h"
 
@@ -37,26 +37,13 @@
 
 #define NMEA_MAXLEN 255
 
-#if GPS_SECONDARY_NMEA
-#ifndef NMEA_GPS_LINK
-#define NMEA_GPS_LINK GPS_SECONDARY_PORT
-#define SecondaryGpsImpl nmea
-#endif
-#else
-#ifndef PrimaryGpsImpl
-#define PrimaryGpsImpl nmea
-#endif
-#endif
-#if GPS_PRIMARY_NMEA
-#ifndef NMEA_GPS_LINK
-#define NMEA_GPS_LINK GPS_PRIMARY_PORT
-#endif
+#ifndef PRIMARY_GPS
+#define PRIMARY_GPS gps_nmea
 #endif
 
-void nmea_gps_impl_init(void);
-void nmea_gps_event(void);
-extern void nmea_gps_register(void);
-void nmea_gps_msg(void);
+extern void gps_nmea_init(void);
+extern void gps_nmea_event(void);
+extern void gps_nmea_register(void);
 
 struct GpsNmea {
   bool_t msg_available;
@@ -87,7 +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);
+extern void nmea_gps_msg(void);
 
 /** Read until a certain character, placed here for proprietary includes */
 static inline void nmea_read_until(int *i)
diff --git a/sw/airborne/subsystems/gps/gps_piksi.c b/sw/airborne/subsystems/gps/gps_piksi.c
index 9af7210c34..6b252b833d 100644
--- a/sw/airborne/subsystems/gps/gps_piksi.c
+++ b/sw/airborne/subsystems/gps/gps_piksi.c
@@ -305,7 +305,7 @@ static void spb_heartbeat_callback(uint16_t sender_id __attribute__((unused)),
 /*
  * Initialize the Piksi GPS and write the settings
  */
-void piksi_gps_impl_init(void)
+void gps_piksi_init(void)
 {
   /* Setup SBP nodes */
   sbp_state_init(&sbp_state);
@@ -346,7 +346,7 @@ void piksi_gps_impl_init(void)
 /*
  * Event handler for reading the GPS UART bytes
  */
-void piksi_gps_event(void)
+void gps_piksi_event(void)
 {
   if ( get_sys_time_msec() - time_since_last_pos_update > POS_ECEF_TIMEOUT ) {
     gps_piksi.fix = GPS_FIX_NONE;
@@ -413,11 +413,7 @@ void gps_inject_data(uint8_t packet_id, uint8_t length, uint8_t *data)
 /*
  * register callbacks & structs
  */
-void piksi_gps_register(void)
+void gps_piksi_register(void)
 {
-#ifdef GPS_SECONDARY_PIKSI
-  gps_register_impl(piksi_gps_impl_init, piksi_gps_event, GPS_PIKSI_ID, 1);
-#else
-  gps_register_impl(piksi_gps_impl_init, piksi_gps_event, GPS_PIKSI_ID, 0);
-#endif
+  gps_register_impl(gps_piksi_init, gps_piksi_event, GPS_PIKSI_ID);
 }
diff --git a/sw/airborne/subsystems/gps/gps_piksi.h b/sw/airborne/subsystems/gps/gps_piksi.h
index 9cba087043..272db2694b 100644
--- a/sw/airborne/subsystems/gps/gps_piksi.h
+++ b/sw/airborne/subsystems/gps/gps_piksi.h
@@ -32,39 +32,17 @@
 #ifndef GPS_PIKSI_H
 #define GPS_PIKSI_H
 
-// #define GPS_NB_CHANNELS 10
-
-#define PIKSI_HEARTBEAT_MSG
-
-#if GPS_SECONDARY_PIKSI
-#ifndef PIKSI_GPS_LINK
-#define PIKSI_GPS_LINK GPS_SECONDARY_PORT
-#define SecondaryGpsImpl piksi
-#endif
-#else
-#ifndef PrimaryGpsImpl
-#define PrimaryGpsImpl piksi
-#endif
-#endif
-#if GPS_PRIMARY_PIKSI
-#ifndef PIKSI_GPS_LINK
-#define PIKSI_GPS_LINK GPS_PRIMARY_PORT
-#endif
+#ifndef PRIMARY_GPS
+#define PRIMARY_GPS gps_piksi
 #endif
 
-
-extern void piksi_gps_event(void);
-extern void piksi_gps_impl_init(void);
-extern void piksi_gps_register(void);
+extern void gps_piksi_event(void);
+extern void gps_piksi_init(void);
+extern void gps_piksi_register(void);
 
 /*
  * Reset base station position
  */
 extern void gps_piksi_set_base_pos(void);
 
-/*
- * The GPS event
- */
-//#define GpsEvent gps_piksi_event
-
 #endif /* GPS_PIKSI_H */
diff --git a/sw/airborne/subsystems/gps/gps_sim.c b/sw/airborne/subsystems/gps/gps_sim.c
index 9887099f08..a4517f1d89 100644
--- a/sw/airborne/subsystems/gps/gps_sim.c
+++ b/sw/airborne/subsystems/gps/gps_sim.c
@@ -22,15 +22,11 @@
 #include "subsystems/gps/gps_sim.h"
 #include "subsystems/abi.h"
 
-void sim_gps_impl_init(void)
+void gps_sim_init(void)
 {
   gps.fix = GPS_FIX_NONE;
 }
 
-void sim_gps_event(void)
-{
-}
-
 void gps_sim_publish(void)
 {
   uint32_t now_ts = get_sys_time_usec();
@@ -46,7 +42,7 @@ void gps_sim_publish(void)
 /*
  * register callbacks & structs
  */
-void sim_gps_register(void)
+void gps_sim_register(void)
 {
-  gps_register_impl(sim_gps_impl_init, sim_gps_event, GPS_SIM_ID, 0);
+  gps_register_impl(gps_sim_init, NULL, GPS_SIM_ID);
 }
diff --git a/sw/airborne/subsystems/gps/gps_sim.h b/sw/airborne/subsystems/gps/gps_sim.h
index 7c8523e145..d8912678e5 100644
--- a/sw/airborne/subsystems/gps/gps_sim.h
+++ b/sw/airborne/subsystems/gps/gps_sim.h
@@ -4,17 +4,13 @@
 #include "std.h"
 #include "subsystems/gps.h"
 
-// #define GPS_NB_CHANNELS 16
-#ifndef PrimaryGpsImpl
-#define PrimaryGpsImpl sim
+#ifndef PRIMARY_GPS
+#define PRIMARY_GPS gps_sim
 #endif
 
 extern void gps_sim_publish(void);
 
-extern void sim_gps_event(void);
-extern void sim_gps_impl_init(void);
-extern void sim_gps_register(void);
-
-// #define GpsEvent() {}
+extern void gps_sim_init(void);
+extern void gps_sim_register(void);
 
 #endif /* GPS_SIM_H */
diff --git a/sw/airborne/subsystems/gps/gps_sim_hitl.c b/sw/airborne/subsystems/gps/gps_sim_hitl.c
index 8f695ac953..451e65fc04 100644
--- a/sw/airborne/subsystems/gps/gps_sim_hitl.c
+++ b/sw/airborne/subsystems/gps/gps_sim_hitl.c
@@ -36,12 +36,12 @@
 bool_t gps_available;
 uint32_t gps_sim_hitl_timer;
 
-void sim_hitl_gps_impl_init(void)
+void gps_sim_hitl_init(void)
 {
   gps.fix = GPS_FIX_NONE;
 }
 
-void sim_hitl_gps_event(void)
+void gps_sim_hitl_event(void)
 {
   if (SysTimeTimer(gps_sim_hitl_timer) > 100000) {
     SysTimeTimerStart(gps_sim_hitl_timer);
@@ -98,7 +98,7 @@ void sim_hitl_gps_event(void)
 /*
  * register callbacks & structs
  */
-void sim_hitl_gps_register(void)
+void gps_sim_hitl_register(void)
 {
-  gps_register_impl(sim_hitl_gps_impl_init, sim_hitl_gps_event, GPS_SIM_ID, 0);
+  gps_register_impl(gps_sim_hitl_init, gps_sim_hitl_event, GPS_SIM_ID);
 }
diff --git a/sw/airborne/subsystems/gps/gps_sim_hitl.h b/sw/airborne/subsystems/gps/gps_sim_hitl.h
index 9e27f78752..cea939aee0 100644
--- a/sw/airborne/subsystems/gps/gps_sim_hitl.h
+++ b/sw/airborne/subsystems/gps/gps_sim_hitl.h
@@ -27,14 +27,12 @@
 #ifndef GPS_SIM_HITL_H
 #define GPS_SIM_HITL_H
 
-#ifndef PrimaryGpsImpl
-#define PrimaryGpsImpl sim_hitl
+#ifndef PRIMARY_GPS
+#define PRIMARY_GPS gps_sim_hitl
 #endif
 
-extern void sim_hitl_gps_event(void);
-extern void sim_hitl_gps_impl_init(void);
-extern void sim_hitl_gps_register(void);
-
-// #define GpsEvent gps_sim_hitl_event
+extern void gps_sim_hitl_event(void);
+extern void gps_sim_hitl_impl_init(void);
+extern void gps_sim_hitl_register(void);
 
 #endif /* GPS_SIM_HITL_H */
diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.c b/sw/airborne/subsystems/gps/gps_sim_nps.c
index 1096023af9..7ca31f7a3a 100644
--- a/sw/airborne/subsystems/gps/gps_sim_nps.c
+++ b/sw/airborne/subsystems/gps/gps_sim_nps.c
@@ -28,7 +28,7 @@
 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_nps.week = 1794;
@@ -87,20 +87,15 @@ void  gps_feed_value()
   AbiSendMsgGPS(GPS_SIM_ID, now_ts, &gps_nps);
 }
 
-void nps_gps_impl_init()
+void gps_nps_init(void)
 {
   gps_has_fix = TRUE;
 }
 
-extern void nps_gps_event(void)
-{
-  return;
-}
-
 /*
  * register callbacks & structs
  */
-void nps_gps_register(void)
+void gps_nps_register(void)
 {
-  gps_register_impl(nps_gps_impl_init, nps_gps_event, GPS_SIM_ID, 0);
+  gps_register_impl(gps_nps_init, NULL, GPS_SIM_ID);
 }
diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.h b/sw/airborne/subsystems/gps/gps_sim_nps.h
index e82fbabfdd..f935277c99 100644
--- a/sw/airborne/subsystems/gps/gps_sim_nps.h
+++ b/sw/airborne/subsystems/gps/gps_sim_nps.h
@@ -3,17 +3,16 @@
 
 #include "std.h"
 
-//#define GPS_NB_CHANNELS 16
-
-#define PrimaryGpsImpl nps
+#ifndef PRIMARY_GPS
+#define PRIMARY_GPS gps_nps
+#endif
 
 extern bool_t gps_has_fix;
 
 extern void gps_feed_value();
 
-extern void nps_gps_impl_init();
-extern void nps_gps_event(void);
-extern void nps_gps_register(void);
+extern void gps_nps_impl_init();
+extern void gps_nps_register(void);
 
 
 #endif /* GPS_SIM_NPS_H */
diff --git a/sw/airborne/subsystems/gps/gps_sirf.c b/sw/airborne/subsystems/gps/gps_sirf.c
index 0fdb079375..c3b9da13e0 100644
--- a/sw/airborne/subsystems/gps/gps_sirf.c
+++ b/sw/airborne/subsystems/gps/gps_sirf.c
@@ -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 sirf_gps_impl_init(void)
+void gps_sirf_init(void)
 {
   gps_sirf.msg_available = FALSE;
   gps_sirf.pos_available = FALSE;
@@ -202,14 +285,23 @@ 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 sirf_gps_register(void)
+void gps_sirf_register(void)
 {
-#ifdef GPS_SECONDARY_SIRF
-  gps_register_impl(sirf_gps_impl_init, sirf_gps_event, GPS_SIRF_ID, 1);
-#else
-  gps_register_impl(sirf_gps_impl_init, sirf_gps_event, GPS_SIRF_ID, 0);
-#endif
+  gps_register_impl(gps_sirf_init, gps_sirf_event, GPS_SIRF_ID);
 }
diff --git a/sw/airborne/subsystems/gps/gps_sirf.h b/sw/airborne/subsystems/gps/gps_sirf.h
index ad2cd0c214..f42623e87e 100644
--- a/sw/airborne/subsystems/gps/gps_sirf.h
+++ b/sw/airborne/subsystems/gps/gps_sirf.h
@@ -31,20 +31,8 @@
 
 #include "std.h"
 
-#if GPS_SECONDARY_SIRF
-#ifndef SIRF_GPS_LINK
-#define SIRF_GPS_LINK GPS_SECONDARY_PORT
-#define SecondaryGpsImpl sirf
-#endif
-#else
-#ifndef PrimaryGpsImpl
-#define PrimaryGpsImpl sirf
-#endif
-#endif
-#if GPS_PRIMARY_SIRF
-#ifndef SIRF_GPS_LINK
-#define SIRF_GPS_LINK GPS_PRIMARY_PORT
-#endif
+#ifndef PRIMARY_GPS
+#define PRIMARY_GPS gps_sirf
 #endif
 
 #define SIRF_GPS_NB_CHANNELS 16
@@ -67,102 +55,9 @@ struct GpsSirf {
 
 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);
-void sirf_gps_impl_init(void);
-void sirf_gps_register(void);
-
-static inline void sirf_gps_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();
-    }
-  }
-}
 
 #endif /* GPS_SIRF_H */
diff --git a/sw/airborne/subsystems/gps/gps_skytraq.c b/sw/airborne/subsystems/gps/gps_skytraq.c
index 1e95f4287e..555de3837e 100644
--- a/sw/airborne/subsystems/gps/gps_skytraq.c
+++ b/sw/airborne/subsystems/gps/gps_skytraq.c
@@ -23,8 +23,7 @@
 #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
@@ -49,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)
 {
@@ -86,11 +90,9 @@ static inline uint16_t bswap16(uint16_t a)
 
 static int distance_too_great(struct EcefCoor_i *ecef_ref, struct EcefCoor_i *ecef_pos);
 
-void skytraq_gps_impl_init(void)
+void gps_skytraq_init(void)
 {
-
   gps_skytraq.status = UNINIT;
-
 }
 
 void gps_skytraq_msg(void)
@@ -110,7 +112,7 @@ void gps_skytraq_msg(void)
   gps_skytraq.msg_available = FALSE;
 }
 
-void skytraq_gps_event(void)
+void gps_skytraq_event(void)
 {
   struct link_device *dev = &((SKYTRAQ_GPS_LINK).device);
 
@@ -289,11 +291,7 @@ static int distance_too_great(struct EcefCoor_i *ecef_ref, struct EcefCoor_i *ec
 /*
  * register callbacks & structs
  */
-void skytraq_gps_register(void)
+void gps_skytraq_register(void)
 {
-#ifdef GPS_SECONDARY_PIKSI
-  gps_register_impl(skytraq_gps_impl_init, skytraq_gps_event, GPS_SKYTRAQ_ID, 1);
-#else
-  gps_register_impl(skytraq_gps_impl_init, skytraq_gps_event, GPS_SKYTRAQ_ID, 0);
-#endif
+  gps_register_impl(gps_skytraq_init, gps_skytraq_event, GPS_SKYTRAQ_ID);
 }
diff --git a/sw/airborne/subsystems/gps/gps_skytraq.h b/sw/airborne/subsystems/gps/gps_skytraq.h
index 3190388ffe..f8cc166f6e 100644
--- a/sw/airborne/subsystems/gps/gps_skytraq.h
+++ b/sw/airborne/subsystems/gps/gps_skytraq.h
@@ -26,20 +26,8 @@
 
 #define SKYTRAQ_ID_NAVIGATION_DATA 0XA8
 
-#if GPS_SECONDARY_SKYTRAQ
-#ifndef SKYTRAQ_GPS_LINK
-#define SKYTRAQ_GPS_LINK GPS_SECONDARY_PORT
-#define SecondaryGpsImpl skytraq
-#endif
-#else
-#ifndef PrimaryGpsImpl
-#define PrimaryGpsImpl skytraq
-#endif
-#endif
-#if GPS_PRIMARY_SKYTRAQ
-#ifndef SKYTRAQ_GPS_LINK
-#define SKYTRAQ_GPS_LINK GPS_PRIMARY_PORT
-#endif
+#ifndef PRIMARY_GPS
+#define PRIMARY_GPS gps_skytraq
 #endif
 
 /* last error type */
@@ -72,17 +60,8 @@ struct GpsSkytraq {
 
 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);
-extern void skytraq_gps_register(void);
-void skytraq_gps_impl_init(void);
-void skytraq_gps_event(void);
+extern void gps_skytraq_init(void);
+extern void gps_skytraq_event(void);
+extern void gps_skytraq_register(void);
 
 #endif /* GPS_SKYTRAQ_H */
diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c
index b9cb7d0ad7..6914de5b49 100644
--- a/sw/airborne/subsystems/gps/gps_ubx.c
+++ b/sw/airborne/subsystems/gps/gps_ubx.c
@@ -57,7 +57,7 @@ struct GpsUbxRaw gps_ubx_raw;
 
 struct GpsTimeSync gps_ubx_time_sync;
 
-void ubx_gps_impl_init(void)
+void gps_ubx_init(void)
 {
   gps_ubx.status = UNINIT;
   gps_ubx.msg_available = FALSE;
@@ -67,7 +67,7 @@ void ubx_gps_impl_init(void)
   gps_ubx.state.comp_id = GPS_UBX_ID;
 }
 
-void ubx_gps_event(void)
+void gps_ubx_event(void)
 {
   struct link_device *dev = &((UBX_GPS_LINK).device);
 
@@ -347,11 +347,7 @@ void gps_ubx_msg(void)
   gps_ubx.msg_available = FALSE;
 }
 
-void ubx_gps_register(void)
+void gps_ubx_register(void)
 {
-#ifdef GPS_SECONDARY_UBX
-  gps_register_impl(ubx_gps_impl_init, ubx_gps_event, GPS_UBX_ID, 1);
-#else
-  gps_register_impl(ubx_gps_impl_init, ubx_gps_event, GPS_UBX_ID, 0);
-#endif
+  gps_register_impl(gps_ubx_init, gps_ubx_event, GPS_UBX_ID);
 }
diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h
index 4958d7bc22..f59c377364 100644
--- a/sw/airborne/subsystems/gps/gps_ubx.h
+++ b/sw/airborne/subsystems/gps/gps_ubx.h
@@ -27,22 +27,6 @@
 #ifndef GPS_UBX_H
 #define GPS_UBX_H
 
-#if GPS_SECONDARY_UBX
-#ifndef UBX_GPS_LINK
-#define UBX_GPS_LINK GPS_SECONDARY_PORT
-#define SecondaryGpsImpl ubx
-#endif
-#else
-#ifndef PrimaryGpsImpl
-#define PrimaryGpsImpl ubx
-#endif
-#endif
-#if GPS_PRIMARY_UBX
-#ifndef UBX_GPS_LINK
-#define UBX_GPS_LINK GPS_PRIMARY_PORT
-#endif
-#endif
- 
 #include "subsystems/gps.h"
 
 #ifdef GPS_CONFIGURE
@@ -51,9 +35,13 @@
 
 #include "mcu_periph/uart.h"
 
-void ubx_gps_impl_init(void);
-void ubx_gps_event(void);
-extern void ubx_gps_register(void);
+#ifndef PRIMARY_GPS
+#define PRIMARY_GPS gps_ubx
+#endif
+
+void gps_ubx_init(void);
+void gps_ubx_event(void);
+extern void gps_ubx_register(void);
 
 #define GPS_UBX_NB_CHANNELS 16
 
diff --git a/sw/airborne/subsystems/gps/gps_udp.c b/sw/airborne/subsystems/gps/gps_udp.c
index 81189f8c88..988d68e5e8 100644
--- a/sw/airborne/subsystems/gps/gps_udp.c
+++ b/sw/airborne/subsystems/gps/gps_udp.c
@@ -37,7 +37,7 @@
 unsigned char gps_udp_read_buffer[256];
 struct FmsNetwork *gps_network = NULL;
 
-void udp_gps_impl_init(void)
+void gps_udp_init(void)
 {
   gps.fix = GPS_FIX_NONE;
   gps_network = network_new(STRINGIFY(GPS_UDP_HOST), 6000 /*out*/, 7000 /*in*/, TRUE);
@@ -47,7 +47,7 @@ void udp_gps_impl_init(void)
 
 #define UDP_GPS_INT(_udp_gps_payload) (int32_t)(*((uint8_t*)_udp_gps_payload)|*((uint8_t*)_udp_gps_payload+1)<<8|((int32_t)*((uint8_t*)_udp_gps_payload+2))<<16|((int32_t)*((uint8_t*)_udp_gps_payload+3))<<24)
 
-void gps_parse(void)
+void gps_udp_parse(void)
 {
   if (gps_network == NULL) { return; }
 
@@ -97,7 +97,7 @@ void gps_parse(void)
 /*
  * register callbacks & structs
  */
-void udp_gps_register(void)
+void gps_udp_register(void)
 {
-  gps_register_impl(udp_gps_impl_init, gps_parse, GPS_UDP_ID, 0);
+  gps_register_impl(gps_udp_init, gps_udp_parse, GPS_UDP_ID);
 }
diff --git a/sw/airborne/subsystems/gps/gps_udp.h b/sw/airborne/subsystems/gps/gps_udp.h
index 2b383fc7f6..892286c05b 100644
--- a/sw/airborne/subsystems/gps/gps_udp.h
+++ b/sw/airborne/subsystems/gps/gps_udp.h
@@ -5,16 +5,12 @@
 
 #define UDP_GPS_NB_CHANNELS 16
 
-#ifndef PrimaryGpsImpl
-#define PrimaryGpsImpl udp
+#ifndef PRIMARY_GPS
+#define PRIMARY_GPS gps_udp
 #endif
 
-
-extern void gps_parse(void);
-extern void udp_gps_impl_init(void);
-extern void udp_gps_register(void);
-
-
-// #define GpsEvent gps_parse
+extern void gps_udp_parse(void);
+extern void gps_udp_init(void);
+extern void gps_udp_register(void);
 
 #endif /* GPS_UDP_H */