From 6cc4d590f58af5a31b05ca3f0afc64befd6e2635 Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Wed, 18 May 2016 15:23:59 +0200
Subject: [PATCH] [gps] call gps_periodic_check from module

---
 conf/modules/gps_furuno.xml               | 2 +-
 conf/modules/gps_mediatek_diy.xml         | 2 +-
 conf/modules/gps_nmea.xml                 | 2 +-
 conf/modules/gps_nps.xml                  | 2 +-
 conf/modules/gps_piksi.xml                | 3 +--
 conf/modules/gps_sim.xml                  | 2 +-
 conf/modules/gps_sim_hitl.xml             | 1 -
 conf/modules/gps_sirf.xml                 | 2 +-
 conf/modules/gps_skytraq.xml              | 2 +-
 conf/modules/gps_ublox.xml                | 3 +--
 conf/modules/gps_udp.xml                  | 2 +-
 sw/airborne/firmwares/fixedwing/main_ap.c | 4 ----
 sw/airborne/firmwares/rotorcraft/main.c   | 1 -
 sw/airborne/subsystems/gps.c              | 6 +++---
 sw/airborne/subsystems/gps.h              | 2 +-
 sw/airborne/subsystems/gps/gps_piksi.h    | 2 ++
 sw/airborne/subsystems/gps/gps_sim_nps.h  | 1 +
 sw/airborne/subsystems/gps/gps_udp.c      | 2 ++
 18 files changed, 19 insertions(+), 22 deletions(-)

diff --git a/conf/modules/gps_furuno.xml b/conf/modules/gps_furuno.xml
index dc5c3d84d8..fd45e7febc 100644
--- a/conf/modules/gps_furuno.xml
+++ b/conf/modules/gps_furuno.xml
@@ -14,7 +14,7 @@
     <file name="gps.h" dir="subsystems"/>
   </header>
   <init fun="gps_nmea_init()"/>
-  <!--periodic fun="gps_periodic_check()" freq="1." autorun="TRUE"/-->
+  <periodic fun="gps_periodic_check(&gps_nmea.state)" freq="1." autorun="TRUE"/>
   <event fun="gps_nmea_event()"/>
   <makefile target="ap">
     <configure name="FURUNO_GPS_PORT" default="$(GPS_PORT)" case="upper|lower"/>
diff --git a/conf/modules/gps_mediatek_diy.xml b/conf/modules/gps_mediatek_diy.xml
index b79ef51fec..f5cf63fbea 100644
--- a/conf/modules/gps_mediatek_diy.xml
+++ b/conf/modules/gps_mediatek_diy.xml
@@ -14,7 +14,7 @@
     <file name="gps.h" dir="subsystems"/>
   </header>
   <init fun="gps_mtk_init()"/>
-  <!--periodic fun="gps_periodic_check()" freq="1." autorun="TRUE"/-->
+  <periodic fun="gps_periodic_check(&gps_mtk.state)" freq="1." autorun="TRUE"/>
   <event fun="gps_mtk_event()"/>
   <makefile target="ap">
     <configure name="MTK_GPS_PORT" default="$(GPS_PORT)" case="upper|lower"/>
diff --git a/conf/modules/gps_nmea.xml b/conf/modules/gps_nmea.xml
index 367c53d977..1a4767516c 100644
--- a/conf/modules/gps_nmea.xml
+++ b/conf/modules/gps_nmea.xml
@@ -14,7 +14,7 @@
     <file name="gps.h" dir="subsystems"/>
   </header>
   <init fun="gps_nmea_init()"/>
-  <!--periodic fun="gps_periodic_check()" freq="1." autorun="TRUE"/-->
+  <periodic fun="gps_periodic_check(&gps_nmea.state)" freq="1." autorun="TRUE"/>
   <event fun="gps_nmea_event()"/>
   <makefile target="ap">
     <configure name="NMEA_GPS_PORT" default="$(GPS_PORT)" case="upper|lower"/>
diff --git a/conf/modules/gps_nps.xml b/conf/modules/gps_nps.xml
index 5ebd63cd07..eef9e367a5 100644
--- a/conf/modules/gps_nps.xml
+++ b/conf/modules/gps_nps.xml
@@ -12,7 +12,7 @@
     <file name="gps.h" dir="subsystems"/>
   </header>
   <init fun="gps_nps_init()"/>
-  <!--periodic fun="gps_periodic_check()" freq="1." autorun="TRUE"/-->
+  <periodic fun="gps_periodic_check(&gps_nps)" freq="1." autorun="TRUE"/>
   <makefile target="nps">
     <file name="gps.c" dir="subsystems"/>
     <file name="gps_sim_nps.c" dir="subsystems/gps"/>
diff --git a/conf/modules/gps_piksi.xml b/conf/modules/gps_piksi.xml
index 3970c27794..5c6299e900 100644
--- a/conf/modules/gps_piksi.xml
+++ b/conf/modules/gps_piksi.xml
@@ -13,9 +13,8 @@
   <header>
     <file name="gps.h" dir="subsystems"/>
   </header>
-  <!-- uncomment these when not called explicitly from main anymore -->
   <init fun="gps_piksi_init()"/>
-  <!--periodic fun="gps_periodic_check()" freq="1." autorun="TRUE"/-->
+  <periodic fun="gps_periodic_check(&gps_piksi)" freq="1." autorun="TRUE"/>
   <event fun="gps_piksi_event()"/>
   <makefile target="ap">
     <configure name="PIKSI_GPS_PORT" default="$(GPS_PORT)" case="upper|lower"/>
diff --git a/conf/modules/gps_sim.xml b/conf/modules/gps_sim.xml
index ff98572b9b..04bf7f7495 100644
--- a/conf/modules/gps_sim.xml
+++ b/conf/modules/gps_sim.xml
@@ -11,7 +11,7 @@
     <file name="gps.h" dir="subsystems"/>
   </header>
   <init fun="gps_sim_init()"/>
-  <!--periodic fun="gps_periodic_check()" freq="1." autorun="TRUE"/-->
+  <periodic fun="gps_periodic_check(&gps)" freq="1." autorun="TRUE"/>
   <makefile target="sim">
     <file name="gps.c" dir="subsystems"/>
     <file name="gps_sim.c" dir="subsystems/gps"/>
diff --git a/conf/modules/gps_sim_hitl.xml b/conf/modules/gps_sim_hitl.xml
index e92f600904..dae6752746 100644
--- a/conf/modules/gps_sim_hitl.xml
+++ b/conf/modules/gps_sim_hitl.xml
@@ -12,7 +12,6 @@
     <file name="gps.h" dir="subsystems"/>
   </header>
   <init fun="gps_sim_hitl_init()"/>
-  <!--periodic fun="gps_periodic_check()" freq="1." autorun="TRUE"/-->
   <event fun="gps_sim_hitl_event()"/>
   <makefile target="ap" firmware="rotorcraft">
     <configure name="GPS_LED" default="none"/>
diff --git a/conf/modules/gps_sirf.xml b/conf/modules/gps_sirf.xml
index dca29eadfc..accd551147 100644
--- a/conf/modules/gps_sirf.xml
+++ b/conf/modules/gps_sirf.xml
@@ -14,7 +14,7 @@
     <file name="gps.h" dir="subsystems"/>
   </header>
   <init fun="gps_sirf_init()"/>
-  <!--periodic fun="gps_periodic_check()" freq="1." autorun="TRUE"/-->
+  <periodic fun="gps_periodic_check(&gps_sirf.state)" freq="1." autorun="TRUE"/>
   <event fun="gps_sirf_event()"/>
   <makefile target="ap">
     <configure name="SIRF_GPS_PORT" default="$(GPS_PORT)" case="upper|lower"/>
diff --git a/conf/modules/gps_skytraq.xml b/conf/modules/gps_skytraq.xml
index cb26f6bec8..18aea9ee74 100644
--- a/conf/modules/gps_skytraq.xml
+++ b/conf/modules/gps_skytraq.xml
@@ -14,7 +14,7 @@
     <file name="gps.h" dir="subsystems"/>
   </header>
   <init fun="gps_skytraq_init()"/>
-  <!--periodic fun="gps_periodic_check()" freq="1." autorun="TRUE"/-->
+  <periodic fun="gps_periodic_check(&gps_skytraq.state)" freq="1." autorun="TRUE"/>
   <event fun="gps_skytraq_event()"/>
   <makefile target="ap">
     <configure name="SKYTRAQ_GPS_PORT" default="$(GPS_PORT)" case="upper|lower"/>
diff --git a/conf/modules/gps_ublox.xml b/conf/modules/gps_ublox.xml
index 988437c20b..70184ef175 100644
--- a/conf/modules/gps_ublox.xml
+++ b/conf/modules/gps_ublox.xml
@@ -13,9 +13,8 @@
   <header>
     <file name="gps.h" dir="subsystems"/>
   </header>
-  <!-- uncomment these when not called explicitly from main anymore -->
   <init fun="gps_ubx_init()"/>
-  <!--periodic fun="gps_periodic_check()" freq="1." autorun="TRUE"/-->
+  <periodic fun="gps_periodic_check(&gps_ubx.state)" freq="1." autorun="TRUE"/>
   <event fun="gps_ubx_event()"/>
   <makefile target="ap">
     <configure name="UBX_GPS_PORT" default="$(GPS_PORT)" case="upper|lower"/>
diff --git a/conf/modules/gps_udp.xml b/conf/modules/gps_udp.xml
index 3bde0c132d..32fca4c882 100644
--- a/conf/modules/gps_udp.xml
+++ b/conf/modules/gps_udp.xml
@@ -13,7 +13,7 @@
     <file name="gps.h" dir="subsystems"/>
   </header>
   <init fun="gps_udp_init()"/>
-  <!--periodic fun="gps_periodic_check()" freq="1." autorun="TRUE"/-->
+  <periodic fun="gps_periodic_check(&gps_udp)" freq="1." autorun="TRUE"/>
   <makefile target="ap">
     <configure name="GPS_LED" default="none"/>
 
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index 11836ad7e3..69cdf9baa3 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -638,10 +638,6 @@ void sensors_task(void)
   update_ahrs_from_sim();
 #endif
 
-#if USE_GPS
-  gps_periodic_check();
-#endif
-
   //FIXME: temporary hack, remove me
 #ifdef InsPeriodic
   InsPeriodic();
diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c
index 607c526bfb..cc5ff88120 100644
--- a/sw/airborne/firmwares/rotorcraft/main.c
+++ b/sw/airborne/firmwares/rotorcraft/main.c
@@ -340,7 +340,6 @@ STATIC_INLINE void failsafe_check(void)
 #endif
 
 #if USE_GPS
-  gps_periodic_check();
   if (autopilot_mode == AP_MODE_NAV &&
       autopilot_motors_on &&
 #if NO_GPS_LOST_WITH_RC_VALID
diff --git a/sw/airborne/subsystems/gps.c b/sw/airborne/subsystems/gps.c
index bc068a9aba..05f977f3e4 100644
--- a/sw/airborne/subsystems/gps.c
+++ b/sw/airborne/subsystems/gps.c
@@ -160,10 +160,10 @@ static void send_gps_sol(struct transport_tx *trans, struct link_device *dev)
 }
 #endif
 
-void gps_periodic_check(void)
+void gps_periodic_check(struct GpsState *gps_s)
 {
-  if (sys_time.nb_sec - gps.last_msg_time > GPS_TIMEOUT) {
-    gps.fix = GPS_FIX_NONE;
+  if (sys_time.nb_sec - gps_s->last_msg_time > GPS_TIMEOUT) {
+    gps_s->fix = GPS_FIX_NONE;
   }
 }
 
diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h
index 56e190d759..d7ef70d6a9 100644
--- a/sw/airborne/subsystems/gps.h
+++ b/sw/airborne/subsystems/gps.h
@@ -151,7 +151,7 @@ static inline bool gps_has_been_good(void)
 /** Periodic GPS check.
  * Marks GPS as lost when no GPS message was received for GPS_TIMEOUT seconds
  */
-extern void gps_periodic_check(void);
+extern void gps_periodic_check(struct GpsState *gps_s);
 
 /**
  * GPS Reset
diff --git a/sw/airborne/subsystems/gps/gps_piksi.h b/sw/airborne/subsystems/gps/gps_piksi.h
index cbe2b0a578..c0ba2356a1 100644
--- a/sw/airborne/subsystems/gps/gps_piksi.h
+++ b/sw/airborne/subsystems/gps/gps_piksi.h
@@ -36,6 +36,8 @@
 #define PRIMARY_GPS GPS_PIKSI
 #endif
 
+extern struct GpsState gps_piksi;
+
 extern void gps_piksi_event(void);
 extern void gps_piksi_init(void);
 
diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.h b/sw/airborne/subsystems/gps/gps_sim_nps.h
index c1de3eee0d..42c6e3c022 100644
--- a/sw/airborne/subsystems/gps/gps_sim_nps.h
+++ b/sw/airborne/subsystems/gps/gps_sim_nps.h
@@ -7,6 +7,7 @@
 #define PRIMARY_GPS GPS_SIM
 #endif
 
+extern struct GpsState gps_nps;
 extern bool gps_has_fix;
 
 extern void gps_feed_value();
diff --git a/sw/airborne/subsystems/gps/gps_udp.c b/sw/airborne/subsystems/gps/gps_udp.c
index 0cd86d7fb0..c30c19069e 100644
--- a/sw/airborne/subsystems/gps/gps_udp.c
+++ b/sw/airborne/subsystems/gps/gps_udp.c
@@ -37,6 +37,8 @@
 unsigned char gps_udp_read_buffer[256];
 struct FmsNetwork *gps_network = NULL;
 
+struct GpsState gps_udp;
+
 void gps_udp_init(void)
 {
   gps_udp.fix = GPS_FIX_NONE;