Separate positioning for multiple vehicles

src/comm/MockLink.cc

@@ -33,11 +33,13 @@ QGC_LOGGING_CATEGORY(MockLinkVerboseLog, "MockLinkVerboseLog")
 ///
 ///     @author Don Gagne <don@thegagnes.com>
 
-float           MockLink::_vehicleLatitude =        47.633033f;
-float           MockLink::_vehicleLongitude =       -122.08794f;
-float           MockLink::_vehicleAltitude =        3.5f;
-int             MockLink::_nextVehicleSystemId =    128;
-const char*     MockLink::_failParam =              "COM_FLTMODE6";
+// Vehicle position is set close to default Gazebo vehicle location. This allows for multi-vehicle
+// testing of a gazebo vehicle and a mocklink vehicle
+double      MockLink::_defaultVehicleLatitude =     47.397f;
+double      MockLink::_defaultVehicleLongitude =    8.5455f;
+double      MockLink::_defaultVehicleAltitude =     488.056f;
+int         MockLink::_nextVehicleSystemId =        128;
+const char* MockLink::_failParam =                  "COM_FLTMODE6";
 
 const char* MockConfiguration::_firmwareTypeKey =   "FirmwareType";
 const char* MockConfiguration::_vehicleTypeKey =    "VehicleType";
@@ -45,29 +47,32 @@ const char* MockConfiguration::_sendStatusTextKey = "SendStatusText";
 const char* MockConfiguration::_failureModeKey =    "FailureMode";
 
 MockLink::MockLink(SharedLinkConfigurationPointer& config)
-    : LinkInterface(config)
-    , _missionItemHandler(this, qgcApp()->toolbox()->mavlinkProtocol())
-    , _name("MockLink")
-    , _connected(false)
-    , _mavlinkChannel(0)
-    , _vehicleSystemId(_nextVehicleSystemId++)
-    , _vehicleComponentId(MAV_COMP_ID_AUTOPILOT1)
-    , _inNSH(false)
-    , _mavlinkStarted(true)
-    , _mavBaseMode(MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)
-    , _mavState(MAV_STATE_STANDBY)
-    , _firmwareType(MAV_AUTOPILOT_PX4)
-    , _vehicleType(MAV_TYPE_QUADROTOR)
-    , _fileServer(NULL)
-    , _sendStatusText(false)
-    , _apmSendHomePositionOnEmptyList(false)
-    , _failureMode(MockConfiguration::FailNone)
-    , _sendHomePositionDelayCount(10)   // No home position for 4 seconds
-    , _sendGPSPositionDelayCount(100)   // No gps lock for 5 seconds
+    : LinkInterface                         (config)
+    , _missionItemHandler                   (this, qgcApp()->toolbox()->mavlinkProtocol())
+    , _name                                 ("MockLink")
+    , _connected                            (false)
+    , _mavlinkChannel                       (0)
+    , _vehicleSystemId                      (_nextVehicleSystemId++)
+    , _vehicleComponentId                   (MAV_COMP_ID_AUTOPILOT1)
+    , _inNSH                                (false)
+    , _mavlinkStarted                       (true)
+    , _mavBaseMode                          (MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)
+    , _mavState                             (MAV_STATE_STANDBY)
+    , _firmwareType                         (MAV_AUTOPILOT_PX4)
+    , _vehicleType                          (MAV_TYPE_QUADROTOR)
+    , _vehicleLatitude                      (_defaultVehicleLatitude + ((_vehicleSystemId - 128) * 0.0001))     // Slight offset for each vehicle
+    , _vehicleLongitude                     (_defaultVehicleLongitude + ((_vehicleSystemId - 128) * 0.0001))
+    , _vehicleAltitude                      (_defaultVehicleAltitude)
+    , _fileServer                           (NULL)
+    , _sendStatusText                       (false)
+    , _apmSendHomePositionOnEmptyList       (false)
+    , _failureMode                          (MockConfiguration::FailNone)
+    , _sendHomePositionDelayCount           (10)    // No home position for 4 seconds
+    , _sendGPSPositionDelayCount            (100)   // No gps lock for 5 seconds
     , _currentParamRequestListComponentIndex(-1)
-    , _currentParamRequestListParamIndex(-1)
-    , _logDownloadCurrentOffset(0)
-    , _logDownloadBytesRemaining(0)
+    , _currentParamRequestListParamIndex    (-1)
+    , _logDownloadCurrentOffset             (0)
+    , _logDownloadBytesRemaining            (0)
 {
     MockConfiguration* mockConfig = qobject_cast<MockConfiguration*>(_config.data());
     _firmwareType = mockConfig->firmwareType();

src/comm/MockLink.h

@@ -216,6 +216,9 @@ private:
 
     MAV_AUTOPILOT       _firmwareType;
     MAV_TYPE            _vehicleType;
+    double              _vehicleLatitude;
+    double              _vehicleLongitude;
+    double              _vehicleAltitude;
 
     MockLinkFileServer* _fileServer;
 
@@ -236,9 +239,9 @@ private:
     uint32_t    _logDownloadCurrentOffset;  ///< Current offset we are sending from
     uint32_t    _logDownloadBytesRemaining; ///< Number of bytes still to send, 0 = send inactive
 
-    static float        _vehicleLatitude;
-    static float        _vehicleLongitude;
-    static float        _vehicleAltitude;
+    static double       _defaultVehicleLatitude;
+    static double       _defaultVehicleLongitude;
+    static double       _defaultVehicleAltitude;
     static int          _nextVehicleSystemId;
     static const char*  _failParam;
 };