Add support for gps messages

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

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