Tracker: handle Tracker fixed position

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) {}

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;

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[];

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;
     }
 }