From fdb133051cbc3a7476af53cfa0e2fdf3bb01d439 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Mon, 15 Feb 2016 19:45:27 +0100
Subject: [PATCH] [gps] adapt vectornav ins to multi GPS support

---
 .../subsystems/shared/ins_vectornav.makefile        | 13 +++++++++++++
 sw/airborne/subsystems/abi_sender_ids.h             |  6 +++++-
 sw/airborne/subsystems/ins/ins_vectornav_wrapper.c  | 11 +++++++++++
 sw/airborne/subsystems/ins/ins_vectornav_wrapper.h  |  8 ++++++--
 4 files changed, 35 insertions(+), 3 deletions(-)

diff --git a/conf/firmwares/subsystems/shared/ins_vectornav.makefile b/conf/firmwares/subsystems/shared/ins_vectornav.makefile
index f95d231a71..8866e9491e 100644
--- a/conf/firmwares/subsystems/shared/ins_vectornav.makefile
+++ b/conf/firmwares/subsystems/shared/ins_vectornav.makefile
@@ -18,6 +18,19 @@ VN_SRCS += subsystems/ins/ins_vectornav_wrapper.c
 #
 VN_CFLAGS += -DUSE_GPS
 VN_SRCS += $(SRC_SUBSYSTEMS)/gps.c
+ifdef SECONDARY_GPS
+ifneq (,$(findstring $(SECONDARY_GPS), vectornav))
+# this is the secondary GPS
+VN_CFLAGS += -DGPS_SECONDARY_TYPE_H=\"subsystems/ins_vectornav_wrapper.h\"
+VN_CFLAGS += -DSECONDARY_GPS=gps_ubx
+else
+VN_CFLAGS += -DGPS_TYPE_H=\"subsystems/ins/ins_vectornav_wrapper.h\"
+VN_CFLAGS += -DPRIMARY_GPS=gps_ubx
+endif
+else
+# plain old single GPS usage
+VN_CFLAGS += -DGPS_TYPE_H=\"subsystems/ins/ins_vectornav_wrapper.h\"
+endif
 
 
 #
diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h
index b5a320c979..17954941c5 100644
--- a/sw/airborne/subsystems/abi_sender_ids.h
+++ b/sw/airborne/subsystems/abi_sender_ids.h
@@ -161,7 +161,11 @@
 #ifndef GPS_MULTI_ID
 #define GPS_MULTI_ID 12
 #endif
- 
+
+#ifndef GPS_VECTORNAV_ID
+#define GPS_VECTORNAV_ID 13
+#endif
+
 /*
  * IDs of IMU sensors (accel, gyro)
  */
diff --git a/sw/airborne/subsystems/ins/ins_vectornav_wrapper.c b/sw/airborne/subsystems/ins/ins_vectornav_wrapper.c
index b276269649..b98ecec0e2 100644
--- a/sw/airborne/subsystems/ins/ins_vectornav_wrapper.c
+++ b/sw/airborne/subsystems/ins/ins_vectornav_wrapper.c
@@ -33,3 +33,14 @@ void ins_vectornav_register(void)
 {
   ins_register_impl(ins_vectornav_init);
 }
+
+void gps_vectornav_init(void)
+{
+  gps.nb_channels = 0;
+}
+
+void gps_vectornav_register(void)
+{
+  gps_register_impl(gps_vectornav_init, NULL, GPS_VECTORNAV_ID);
+}
+
diff --git a/sw/airborne/subsystems/ins/ins_vectornav_wrapper.h b/sw/airborne/subsystems/ins/ins_vectornav_wrapper.h
index 59a615a21a..45289ca9c2 100644
--- a/sw/airborne/subsystems/ins/ins_vectornav_wrapper.h
+++ b/sw/airborne/subsystems/ins/ins_vectornav_wrapper.h
@@ -37,8 +37,12 @@
 
 #define InsEvent ins_vectornav_event
 
-#define GpsEvent() {}
-
 extern void ins_vectornav_register(void);
 
+#ifndef PRIMARY_GPS
+#define PRIMARY_GPS gps_vectornav
+#endif
+extern void gps_vectornav_init(void);
+extern void gps_vectornav_register(void);
+
 #endif /* INS_VECTORNAV_WRAPPER_H */