diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 146b048e000984779f81d44d45ebc648b0928983..0d2db89aa2c20e4a153b3490ba4a0c1e85837326 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -33,11 +33,13 @@ QGC_LOGGING_CATEGORY(MockLinkVerboseLog, "MockLinkVerboseLog") /// /// @author Don Gagne -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(_config.data()); _firmwareType = mockConfig->firmwareType(); diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index 90393507a18eac0d1667e461221b381c8a9ab970..96ff3698e7f3250f17df7a07ffb0973ef2f99ba5 100644 --- a/src/comm/MockLink.h +++ b/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; };