From 5795e64c2bc0cc38b8452e0d0c1747b6e37c2e9e Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Thu, 15 Oct 2015 16:51:16 -0700
Subject: [PATCH] Add support for gps messages

---
 src/comm/MockLink.cc | 33 +++++++++++++++++++++++++++++----
 src/comm/MockLink.h  |  5 +++++
 2 files changed, 34 insertions(+), 4 deletions(-)

diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc
index ee371ff81..a42589137 100644
--- a/src/comm/MockLink.cc
+++ b/src/comm/MockLink.cc
@@ -67,6 +67,10 @@ union px4_custom_mode {
     float data_float;
 };
 
+float MockLink::_vehicleLatitude =  47.633033f;
+float MockLink::_vehicleLongitude = -122.08794f;
+float MockLink::_vehicleAltitude =  2.5f;
+
 MockLink::MockLink(MockConfiguration* config)
     : _missionItemHandler(this)
     , _name("MockLink")
@@ -164,6 +168,7 @@ void MockLink::_run1HzTasks(void)
 void MockLink::_run10HzTasks(void)
 {
     if (_mavlinkStarted && _connected) {
+        _sendGpsRawInt();
     }
 }
 
@@ -696,7 +701,7 @@ void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode
 
 void MockLink::_sendHomePosition(void)
 {
-    mavlink_message_t   msg;
+    mavlink_message_t msg;
     
     float bogus[4];
     bogus[0] = 0.0f;
@@ -707,11 +712,31 @@ void MockLink::_sendHomePosition(void)
     mavlink_msg_home_position_pack(_vehicleSystemId,
                                    _vehicleComponentId,
                                    &msg,
-                                   (int32_t)(47.633033f * 1E7),
-                                   (int32_t)(-122.08794f * 1E7),
-                                   (int32_t)(2.0f * 1000),
+                                   (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)
+{
+    static uint64_t timeTick = 0;
+    mavlink_message_t msg;
+
+    mavlink_msg_gps_raw_int_pack(_vehicleSystemId,
+                                 _vehicleComponentId,
+                                 &msg,
+                                 timeTick++,                            // time since boot
+                                 3,                                     // 3D fix
+                                 (int32_t)(_vehicleLatitude * 1E7),
+                                 (int32_t)(_vehicleLongitude * 1E7),
+                                 (int32_t)(_vehicleAltitude * 1000),
+                                 UINT16_MAX, UINT16_MAX,                // HDOP/VDOP not known
+                                 UINT16_MAX,                            // velocity not known
+                                 UINT16_MAX,                            // course over ground not known
+                                 8);                                    // satellite count
+    respondWithMavlinkMessage(msg);
+}
diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h
index d8692e065..790291833 100644
--- a/src/comm/MockLink.h
+++ b/src/comm/MockLink.h
@@ -144,6 +144,7 @@ private:
     float _floatUnionForParam(int componentId, const QString& paramName);
     void _setParamFloatUnionIntoMap(int componentId, const QString& paramName, float paramFloat);
     void _sendHomePosition(void);
+    void _sendGpsRawInt(void);
 
     MockLinkMissionItemHandler  _missionItemHandler;
 
@@ -167,6 +168,10 @@ private:
     MAV_AUTOPILOT _autopilotType;
     
     MockLinkFileServer* _fileServer;
+
+    static float _vehicleLatitude;
+    static float _vehicleLongitude;
+    static float _vehicleAltitude;
 };
 
 #endif
-- 
2.22.0