From 5399b659ea2dd2cdd13ec97255f22df859ce8e1e Mon Sep 17 00:00:00 2001
From: IamPete1 <33176108+IamPete1@users.noreply.github.com>
Date: Thu, 28 Feb 2019 21:38:28 +0000
Subject: [PATCH] Tracker: handle Tracker fixed position

---
 AntennaTracker/GCS_Mavlink.cpp | 22 ++++++++++++++++++++++
 AntennaTracker/GCS_Mavlink.h   |  1 +
 AntennaTracker/Tracker.h       |  1 +
 AntennaTracker/tracking.cpp    |  8 +++++---
 4 files changed, 29 insertions(+), 3 deletions(-)

diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp
index d08b43468e..4f751d3fc7 100644
--- a/AntennaTracker/GCS_Mavlink.cpp
+++ b/AntennaTracker/GCS_Mavlink.cpp
@@ -588,6 +588,28 @@ bool GCS_MAVLINK_Tracker::set_mode(uint8_t mode)
     return false;
 }
 
+// send position tracker is using
+void GCS_MAVLINK_Tracker::send_global_position_int()
+{
+    if (!tracker.stationary) {
+        GCS_MAVLINK::send_global_position_int();
+        return;
+    }
+
+    mavlink_msg_global_position_int_send(
+        chan,
+        AP_HAL::millis(),
+        tracker.current_loc.lat,  // in 1E7 degrees
+        tracker.current_loc.lng,  // in 1E7 degrees
+        tracker.current_loc.alt,  // millimeters above ground/sea level
+        0,                        // millimeters above home
+        0,                        // X speed cm/s (+ve North)
+        0,                        // Y speed cm/s (+ve East)
+        0,                        // Z speed cm/s (+ve Down)
+        tracker.ahrs.yaw_sensor); // compass heading in 1/100 degree
+}
+
+
 /* dummy methods to avoid having to link against AP_Camera */
 void AP_Camera::control_msg(mavlink_message_t const*) {}
 void AP_Camera::configure(float, float, float, float, float, float, float) {}
diff --git a/AntennaTracker/GCS_Mavlink.h b/AntennaTracker/GCS_Mavlink.h
index 1c8e3e8b11..ab3e6f3142 100644
--- a/AntennaTracker/GCS_Mavlink.h
+++ b/AntennaTracker/GCS_Mavlink.h
@@ -41,6 +41,7 @@ private:
     void handleMessage(mavlink_message_t * msg) override;
     bool handle_guided_request(AP_Mission::Mission_Command &cmd) override;
     void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override;
+    void send_global_position_int() override;
 
     MAV_TYPE frame_type() const override;
     MAV_MODE base_mode() const override;
diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h
index a9e07ed1ee..df49fbd945 100644
--- a/AntennaTracker/Tracker.h
+++ b/AntennaTracker/Tracker.h
@@ -188,6 +188,7 @@ private:
 
     uint8_t one_second_counter = 0;
     bool target_set = false;
+    bool stationary = true; // are we using the start lat and log?
 
     static const AP_Scheduler::Task scheduler_tasks[];
     static const AP_Param::Info var_info[];
diff --git a/AntennaTracker/tracking.cpp b/AntennaTracker/tracking.cpp
index 29bf685307..e42eb0fd47 100644
--- a/AntennaTracker/tracking.cpp
+++ b/AntennaTracker/tracking.cpp
@@ -31,10 +31,12 @@ void Tracker::update_vehicle_pos_estimate()
  */
 void Tracker::update_tracker_position()
 {
-    // update our position if we have at least a 2D fix
+    Location temp_loc;
+
     // REVISIT: what if we lose lock during a mission and the antenna is moving?
-    if (!ahrs.get_position(current_loc) && (gps.status() >= AP_GPS::GPS_OK_FIX_2D)) {
-        current_loc = gps.location();
+    if (ahrs.get_position(temp_loc)) {
+        stationary = false;
+        current_loc = temp_loc;
     }
 }