diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 1c36b72dc0a661e73ff1c2922bf42f4b9508f8fb..d740c1872307923e848d44432337c59eb76279c7 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -73,6 +73,7 @@ MockLink::MockLink(SharedLinkConfigurationPointer& config) , _currentParamRequestListParamIndex (-1) , _logDownloadCurrentOffset (0) , _logDownloadBytesRemaining (0) + , _adsbAngle (0) { MockConfiguration* mockConfig = qobject_cast(_config.data()); _firmwareType = mockConfig->firmwareType(); @@ -92,6 +93,9 @@ MockLink::MockLink(SharedLinkConfigurationPointer& config) moveToThread(this); _loadParams(); + + _adsbVehicleCoordinate = QGeoCoordinate(_vehicleLatitude, _vehicleLongitude).atDistanceAndAzimuth(1000, _adsbAngle); + _adsbVehicleCoordinate.setAltitude(100); } MockLink::~MockLink(void) @@ -1272,23 +1276,27 @@ void MockLink::_logDownloadWorker(void) void MockLink::_sendADSBVehicles(void) { + _adsbAngle += 2; + _adsbVehicleCoordinate = QGeoCoordinate(_vehicleLatitude, _vehicleLongitude).atDistanceAndAzimuth(500, _adsbAngle); + _adsbVehicleCoordinate.setAltitude(100); + mavlink_message_t responseMsg; mavlink_msg_adsb_vehicle_pack_chan(_vehicleSystemId, _vehicleComponentId, _mavlinkChannel, &responseMsg, - 12345, // ICAO address - (_vehicleLatitude + 0.001) * 1e7, - (_vehicleLongitude + 0.001) * 1e7, + 12345, // ICAO address + _adsbVehicleCoordinate.latitude() * 1e7, + _adsbVehicleCoordinate.longitude() * 1e7, ADSB_ALTITUDE_TYPE_GEOMETRIC, - 100 * 1000, // Altitude in millimeters - 10 * 100, // Heading in centidegress - 0, 0, // Horizontal/Vertical velocity - "N1234500", // Callsign + _adsbVehicleCoordinate.altitude() * 1000, // Altitude in millimeters + 10 * 100, // Heading in centidegress + 0, 0, // Horizontal/Vertical velocity + "N1234500", // Callsign ADSB_EMITTER_TYPE_ROTOCRAFT, - 1, // Seconds since last communication + 1, // Seconds since last communication ADSB_FLAGS_VALID_COORDS | ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_HEADING | ADSB_FLAGS_VALID_CALLSIGN | ADSB_FLAGS_SIMULATED, - 0); // Squawk code + 0); // Squawk code respondWithMavlinkMessage(responseMsg); } diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index 28ec94e4176e996857d332ff9fd4be72f873569c..1b17d6a75eeb0c5c0014d069ff6dda5957cd1826 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -13,6 +13,7 @@ #include #include +#include #include "MockLinkMissionItemHandler.h" #include "MockLinkFileServer.h" @@ -193,6 +194,7 @@ private: void _paramRequestListWorker(void); void _logDownloadWorker(void); void _sendADSBVehicles(void); + void _moveADSBVehicle(void); static MockLink* _startMockLink(MockConfiguration* mockConfig); @@ -240,6 +242,9 @@ private: uint32_t _logDownloadCurrentOffset; ///< Current offset we are sending from uint32_t _logDownloadBytesRemaining; ///< Number of bytes still to send, 0 = send inactive + QGeoCoordinate _adsbVehicleCoordinate; + double _adsbAngle; + static double _defaultVehicleLatitude; static double _defaultVehicleLongitude; static double _defaultVehicleAltitude;