From d90af7eb6203f0f6dff2c08e41cc15aaa0d3b03f Mon Sep 17 00:00:00 2001
From: masierra <masierra@ucsc.edu>
Date: Thu, 4 Feb 2016 10:59:51 -0800
Subject: [PATCH] [xsens] added new gps support

---
 .../subsystems/fixedwing/ins_xsens.makefile   |  2 +-
 .../subsystems/shared/ins_xsens700.makefile   |  2 +-
 sw/airborne/modules/ins/ins_xsens.c           | 22 +++++++++++++++++--
 sw/airborne/modules/ins/ins_xsens.h           |  7 +++++-
 sw/airborne/modules/ins/ins_xsens700.c        | 21 +++++++++++++++++-
 5 files changed, 48 insertions(+), 6 deletions(-)

diff --git a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile b/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile
index 7c5c390035..5b94b6d618 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 += -DGPS_TYPE_H=\"modules/ins/ins_xsens.h\"
+ap.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"modules/ins/ins_xsens.h\"
 ap.srcs += $(SRC_SUBSYSTEMS)/gps.c
 
 
diff --git a/conf/firmwares/subsystems/shared/ins_xsens700.makefile b/conf/firmwares/subsystems/shared/ins_xsens700.makefile
index a1a886835b..5bc7c31af0 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 += -DGPS_TYPE_H=\"modules/ins/ins_xsens.h\"
+ap.CFLAGS += -DPRIMARY_GPS_TYPE_H=\"modules/ins/ins_xsens.h\"
 ap.srcs += $(SRC_SUBSYSTEMS)/gps.c
 
 
diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c
index 38b0df830a..c4edafb469 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 gps_impl_init(void)
+void xsens_gps_impl_init(void)
 {
   gps.nb_channels = 0;
 }
@@ -316,6 +316,11 @@ static void gps_xsens_publish(void)
   }
   AbiSendMsgGPS(GPS_XSENS_ID, now_ts, &gps);
 }
+
+void xsens_gps_event(void)
+{
+}
+
 #endif
 
 void xsens_periodic(void)
@@ -722,7 +727,6 @@ void parse_ins_msg(void)
 
 }
 
-
 void parse_ins_buffer(uint8_t c)
 {
   ck += c;
@@ -775,3 +779,17 @@ restart:
   xsens_status = UNINIT;
   return;
 }
+
+#ifdef USE_GPS_XSENS
+/*
+ * register callbacks & structs
+ */
+void xsens_gps_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
+}
+#endif
diff --git a/sw/airborne/modules/ins/ins_xsens.h b/sw/airborne/modules/ins/ins_xsens.h
index 43b3b1f05d..893f2700b4 100644
--- a/sw/airborne/modules/ins/ins_xsens.h
+++ b/sw/airborne/modules/ins/ins_xsens.h
@@ -79,7 +79,12 @@ extern void ins_xsens_register(void);
 
 
 #if USE_GPS_XSENS
-#define GpsEvent() {}
+#ifndef PrimaryGpsImpl
+#define PrimaryGpsImpl xsens
+#endif
+extern void xsens_gps_event(void);
+extern void xsens_gps_impl_init(void);
+extern void xsens_gps_register(void);
 #endif
 
 #endif
diff --git a/sw/airborne/modules/ins/ins_xsens700.c b/sw/airborne/modules/ins/ins_xsens700.c
index 44a5b398db..3a29912af5 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 gps_impl_init(void)
+void xsens_gps_impl_init(void)
 {
   gps.nb_channels = 0;
 }
@@ -247,6 +247,11 @@ 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)
@@ -623,3 +628,17 @@ restart:
   xsens_status = UNINIT;
   return;
 }
+
+#ifdef USE_GPS_XSENS
+/*
+ * register callbacks & structs
+ */
+void xsens_gps_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
+}
+#endif