From 16c9023cf0df27845ee528105994b1295aabbaf4 Mon Sep 17 00:00:00 2001 From: stefanlynka Date: Thu, 14 Jul 2016 17:12:17 +0900 Subject: [PATCH] Tracker: update how current_loc gets location Get position from EKF but fall back to GPS if that fails --- AntennaTracker/tracking.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/AntennaTracker/tracking.cpp b/AntennaTracker/tracking.cpp index 4121ffa2ce..691423d976 100644 --- a/AntennaTracker/tracking.cpp +++ b/AntennaTracker/tracking.cpp @@ -35,7 +35,7 @@ void Tracker::update_tracker_position() { // update our position if we have at least a 2D fix // REVISIT: what if we lose lock during a mission and the antenna is moving? - if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) { + if (!ahrs.get_position(current_loc) && (gps.status() >= AP_GPS::GPS_OK_FIX_2D)) { current_loc = gps.location(); } }