From 199c6f93f8d0a8051a29d0639469dbb8ce2a70f5 Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Fri, 23 Oct 2015 14:33:39 -0700
Subject: [PATCH] Delay HOME_POSITION to better simulate reality

---
 src/comm/MockLink.cc | 49 +++++++++++++++++++++++++++-----------------
 src/comm/MockLink.h  |  2 ++
 2 files changed, 32 insertions(+), 19 deletions(-)

diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc
index dd8e708b2..220d2db0b 100644
--- a/src/comm/MockLink.cc
+++ b/src/comm/MockLink.cc
@@ -88,6 +88,7 @@ MockLink::MockLink(MockConfiguration* config)
     , _fileServer(NULL)
     , _sendStatusText(false)
     , _apmSendHomePositionOnEmptyList(false)
+    , _sendHomePositionDelayCount(2)
 {
     _config = config;
     if (_config) {
@@ -171,7 +172,12 @@ void MockLink::_run1HzTasks(void)
 {
     if (_mavlinkStarted && _connected) {
         _sendHeartBeat();
-        _sendHomePosition();
+        if (_sendHomePositionDelayCount > 0) {
+            // We delay home position a bit to be more realistic
+            _sendHomePositionDelayCount--;
+        } else {
+            _sendHomePosition();
+        }
         if (_sendStatusText) {
             _sendStatusText = false;
             _sendStatusTextMessages();
@@ -715,24 +721,29 @@ void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode
 
 void MockLink::_sendHomePosition(void)
 {
-    mavlink_message_t msg;
-    
-    float bogus[4];
-    bogus[0] = 0.0f;
-    bogus[1] = 0.0f;
-    bogus[2] = 0.0f;
-    bogus[3] = 0.0f;
-   
-    mavlink_msg_home_position_pack(_vehicleSystemId,
-                                   _vehicleComponentId,
-                                   &msg,
-                                   (int32_t)(_vehicleLatitude * 1E7),
-                                   (int32_t)(_vehicleLongitude * 1E7),
-                                   (int32_t)(_vehicleAltitude * 1000),
-                                   0.0f, 0.0f, 0.0f,
-                                   &bogus[0],
-                                   0.0f, 0.0f, 0.0f);
-    respondWithMavlinkMessage(msg);
+    // APM stack does not yet support HOME_POSITION
+
+    if (_firmwareType != MAV_AUTOPILOT_ARDUPILOTMEGA) {
+
+        mavlink_message_t msg;
+
+        float bogus[4];
+        bogus[0] = 0.0f;
+        bogus[1] = 0.0f;
+        bogus[2] = 0.0f;
+        bogus[3] = 0.0f;
+
+        mavlink_msg_home_position_pack(_vehicleSystemId,
+                                       _vehicleComponentId,
+                                       &msg,
+                                       (int32_t)(_vehicleLatitude * 1E7),
+                                       (int32_t)(_vehicleLongitude * 1E7),
+                                       (int32_t)(_vehicleAltitude * 1000),
+                                       0.0f, 0.0f, 0.0f,
+                                       &bogus[0],
+                0.0f, 0.0f, 0.0f);
+        respondWithMavlinkMessage(msg);
+    }
 }
 
 void MockLink::_sendGpsRawInt(void)
diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h
index ae043f92e..a78bbd238 100644
--- a/src/comm/MockLink.h
+++ b/src/comm/MockLink.h
@@ -191,6 +191,8 @@ private:
     bool _sendStatusText;
     bool _apmSendHomePositionOnEmptyList;
 
+    int _sendHomePositionDelayCount;
+
     static float _vehicleLatitude;
     static float _vehicleLongitude;
     static float _vehicleAltitude;
-- 
2.22.0