[gps] update global gps struct from periodic check

thanks to fvantienen
paparazzi · Sep 20, 2016 · 7ce244e · 7ce244e
1 parent 9e560f2
commit 7ce244e
Showing 1 changed file with 18 additions and 6 deletions.
diff --git a/sw/airborne/subsystems/gps.c b/sw/airborne/subsystems/gps.c
@@ -175,12 +175,6 @@ static void send_gps_sol(struct transport_tx *trans, struct link_device *dev)
 }
 #endif
 
-void gps_periodic_check(struct GpsState *gps_s)
-{
-  if (sys_time.nb_sec - gps_s->last_msg_time > GPS_TIMEOUT) {
-    gps_s->fix = GPS_FIX_NONE;
-  }
-}
 
 #ifdef SECONDARY_GPS
 static uint8_t gps_multi_switch(struct GpsState *gps_s) {
@@ -211,6 +205,24 @@ static uint8_t gps_multi_switch(struct GpsState *gps_s) {
 }
 #endif /*SECONDARY_GPS*/
 
+
+void gps_periodic_check(struct GpsState *gps_s)
+{
+  if (sys_time.nb_sec - gps_s->last_msg_time > GPS_TIMEOUT) {
+    gps_s->fix = GPS_FIX_NONE;
+  }
+
+#ifdef SECONDARY_GPS
+  current_gps_id = gps_multi_switch(gps_s);
+  if (gps_s->comp_id == current_gps_id) {
+    gps = *gps_s;
+  }
+#else
+  gps = *gps_s;
+#endif
+}
+
+
 static abi_event gps_ev;
 static void gps_cb(uint8_t sender_id,
                    uint32_t stamp __attribute__((unused)),