From 59526efa02198f16f8aead92d00ddede5e1f37bf Mon Sep 17 00:00:00 2001
From: IamPete1 <33176108+IamPete1@users.noreply.github.com>
Date: Sun, 10 Mar 2019 10:49:28 +0000
Subject: [PATCH] Tracker: restore scan mode functionality

---
 AntennaTracker/control_auto.cpp | 9 ++-------
 AntennaTracker/system.cpp       | 2 ++
 AntennaTracker/tracking.cpp     | 6 +++++-
 3 files changed, 9 insertions(+), 8 deletions(-)

diff --git a/AntennaTracker/control_auto.cpp b/AntennaTracker/control_auto.cpp
index ef7725ce60..a6e8c8c782 100644
--- a/AntennaTracker/control_auto.cpp
+++ b/AntennaTracker/control_auto.cpp
@@ -10,11 +10,6 @@
  */
 void Tracker::update_auto(void)
 {
-    // exit immediately if we do not have a valid vehicle position
-    if (!vehicle.location_valid) {
-        return;
-    }
-
     float yaw = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100); // target yaw in centidegrees
     float pitch = constrain_float(nav_status.pitch+g.pitch_trim, g.pitch_min, g.pitch_max) * 100; // target pitch in centidegrees
 
@@ -26,8 +21,8 @@ void Tracker::update_auto(void)
     float bf_yaw;
     convert_ef_to_bf(pitch, yaw, bf_pitch, bf_yaw);
 
-    // only move servos if target is at least distance_min away
-    if ((g.distance_min <= 0) || (nav_status.distance >= g.distance_min)) {
+    // only move servos if target is at least distance_min away if we  have a target
+    if ((g.distance_min <= 0) || (nav_status.distance >= g.distance_min) || !vehicle.location_valid) {
         update_pitch_servo(bf_pitch);
         update_yaw_servo(bf_yaw);
     }
diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp
index ceefe428cf..8a05b29e9a 100644
--- a/AntennaTracker/system.cpp
+++ b/AntennaTracker/system.cpp
@@ -231,6 +231,8 @@ void Tracker::set_mode(enum ControlMode mode, mode_reason_t reason)
 
 	// log mode change
 	logger.Write_Mode(control_mode, reason);
+
+    nav_status.bearing = ahrs.yaw_sensor * 0.01f;
 }
 
 /*
diff --git a/AntennaTracker/tracking.cpp b/AntennaTracker/tracking.cpp
index cf6bac0c50..ab427f2e27 100644
--- a/AntennaTracker/tracking.cpp
+++ b/AntennaTracker/tracking.cpp
@@ -110,7 +110,11 @@ void Tracker::update_tracking(void)
 
     switch (control_mode) {
     case AUTO:
-        update_auto();
+        if (vehicle.location_valid) {
+            update_auto();
+        } else if (tracker.target_set) {
+            update_scan();
+        }
         break;
 
     case MANUAL: