diff --git a/src/Comms/MockLink/CMakeLists.txt b/src/Comms/MockLink/CMakeLists.txt index b5902cde9407..6c845453718f 100644 --- a/src/Comms/MockLink/CMakeLists.txt +++ b/src/Comms/MockLink/CMakeLists.txt @@ -7,6 +7,8 @@ if(CMAKE_BUILD_TYPE STREQUAL "Debug") target_sources(MockLink PRIVATE + MockConfiguration.cc + MockConfiguration.h MockLink.cc MockLink.h MockLinkFTP.cc diff --git a/src/Comms/MockLink/MockConfiguration.cc b/src/Comms/MockLink/MockConfiguration.cc new file mode 100644 index 000000000000..926354719c07 --- /dev/null +++ b/src/Comms/MockLink/MockConfiguration.cc @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * (c) 2009-2024 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "MockConfiguration.h" +#include "QGCLoggingCategory.h" + +QGC_LOGGING_CATEGORY(MockConfigurationLog, "qgc.comms.mocklink.mockconfiguration") + +MockConfiguration::MockConfiguration(const QString &name, QObject *parent) + : LinkConfiguration(name, parent) +{ + // qCDebug(MockConfigurationLog) << Q_FUNC_INFO << this; +} + +MockConfiguration::MockConfiguration(const MockConfiguration *copy, QObject *parent) + : LinkConfiguration(copy, parent) + , _firmwareType(copy->firmwareType()) + , _vehicleType(copy->vehicleType()) + , _sendStatusText(copy->sendStatusText()) + , _incrementVehicleId(copy->incrementVehicleId()) + , _failureMode(copy->failureMode()) +{ + // qCDebug(MockConfigurationLog) << Q_FUNC_INFO << this; +} + +MockConfiguration::~MockConfiguration() +{ + // qCDebug(MockConfigurationLog) << Q_FUNC_INFO << this; +} + +void MockConfiguration::copyFrom(const LinkConfiguration *source) +{ + Q_ASSERT(source); + LinkConfiguration::copyFrom(source); + + const MockConfiguration* const mockLinkSource = qobject_cast(source); + Q_ASSERT(mockLinkSource); + + setFirmwareType(mockLinkSource->firmwareType()); + setVehicleType(mockLinkSource->vehicleType()); + setSendStatusText(mockLinkSource->sendStatusText()); + setIncrementVehicleId(mockLinkSource->incrementVehicleId()); + setFailureMode(mockLinkSource->failureMode()); +} + +void MockConfiguration::loadSettings(QSettings &settings, const QString &root) +{ + settings.beginGroup(root); + + setFirmwareType(static_cast(settings.value(_firmwareTypeKey, static_cast(MAV_AUTOPILOT_PX4)).toInt())); + setVehicleType(static_cast(settings.value(_vehicleTypeKey, static_cast(MAV_TYPE_QUADROTOR)).toInt())); + setSendStatusText(settings.value(_sendStatusTextKey, false).toBool()); + setIncrementVehicleId(settings.value(_incrementVehicleIdKey, true).toBool()); + setFailureMode(static_cast(settings.value(_failureModeKey, static_cast(FailNone)).toInt())); + + settings.endGroup(); +} + +void MockConfiguration::saveSettings(QSettings &settings, const QString &root) +{ + settings.beginGroup(root); + + settings.setValue(_firmwareTypeKey, firmwareType()); + settings.setValue(_vehicleTypeKey, vehicleType()); + settings.setValue(_sendStatusTextKey, sendStatusText()); + settings.setValue(_incrementVehicleIdKey, incrementVehicleId()); + settings.setValue(_failureModeKey, failureMode()); + + settings.endGroup(); +} diff --git a/src/Comms/MockLink/MockConfiguration.h b/src/Comms/MockLink/MockConfiguration.h new file mode 100644 index 000000000000..51df7b74bdeb --- /dev/null +++ b/src/Comms/MockLink/MockConfiguration.h @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * (c) 2009-2024 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "LinkConfiguration.h" +#include "MAVLinkLib.h" + +#include + +Q_DECLARE_LOGGING_CATEGORY(MockConfigurationLog) + +class MockConfiguration : public LinkConfiguration +{ + Q_OBJECT + Q_PROPERTY(int firmware READ firmware WRITE setFirmware NOTIFY firmwareChanged) + Q_PROPERTY(int vehicle READ vehicle WRITE setVehicle NOTIFY vehicleChanged) + Q_PROPERTY(bool sendStatus READ sendStatusText WRITE setSendStatusText NOTIFY sendStatusChanged) + Q_PROPERTY(bool incrementVehicleId READ incrementVehicleId WRITE setIncrementVehicleId NOTIFY incrementVehicleIdChanged) + +public: + explicit MockConfiguration(const QString &name, QObject *parent = nullptr); + explicit MockConfiguration(const MockConfiguration *copy, QObject *parent = nullptr); + ~MockConfiguration(); + + LinkType type() const final { return LinkConfiguration::TypeMock; } + void copyFrom(const LinkConfiguration *source) final; + void loadSettings(QSettings &settings, const QString &root) final; + void saveSettings(QSettings &settings, const QString &root) final; + QString settingsURL() final { return QStringLiteral("MockLinkSettings.qml"); } + QString settingsTitle() final { return tr("Mock Link Settings"); } + + int firmware() const { return static_cast(_firmwareType); } + void setFirmware(int type) { _firmwareType = static_cast(type); emit firmwareChanged(); } + int vehicle() const { return static_cast(_vehicleType); } + void setVehicle(int type) { _vehicleType = static_cast(type); emit vehicleChanged(); } + bool incrementVehicleId() const { return _incrementVehicleId; } + void setIncrementVehicleId(bool incrementVehicleId) { _incrementVehicleId = incrementVehicleId; emit incrementVehicleIdChanged(); } + MAV_AUTOPILOT firmwareType() const { return _firmwareType; } + void setFirmwareType(MAV_AUTOPILOT firmwareType) { _firmwareType = firmwareType; emit firmwareChanged(); } + uint16_t boardVendorId() const { return _boardVendorId; } + uint16_t boardProductId() const { return _boardProductId; } + void setBoardVendorProduct(uint16_t vendorId, uint16_t productId) { _boardVendorId = vendorId; _boardProductId = productId; } + MAV_TYPE vehicleType() const { return _vehicleType; } + void setVehicleType(MAV_TYPE vehicleType) { _vehicleType = vehicleType; emit vehicleChanged(); } + bool sendStatusText() const { return _sendStatusText; } + void setSendStatusText(bool sendStatusText) { _sendStatusText = sendStatusText; emit sendStatusChanged(); } + + enum FailureMode_t { + FailNone, // No failures + FailParamNoReponseToRequestList, // Do no respond to PARAM_REQUEST_LIST + FailMissingParamOnInitialReqest, // Not all params are sent on initial request, should still succeed since QGC will re-query missing params + FailMissingParamOnAllRequests, // Not all params are sent on initial request, QGC retries will fail as well + FailInitialConnectRequestMessageAutopilotVersionFailure, // REQUEST_MESSAGE:AUTOPILOT_VERSION returns failure + FailInitialConnectRequestMessageAutopilotVersionLost, // REQUEST_MESSAGE:AUTOPILOT_VERSION success, AUTOPILOT_VERSION never sent + FailInitialConnectRequestMessageProtocolVersionFailure, // REQUEST_MESSAGE:PROTOCOL_VERSION returns failure + FailInitialConnectRequestMessageProtocolVersionLost, // REQUEST_MESSAGE:PROTOCOL_VERSION success, PROTOCOL_VERSION never sent + }; + FailureMode_t failureMode() const { return _failureMode; } + void setFailureMode(FailureMode_t failureMode) { _failureMode = failureMode; } + +signals: + void firmwareChanged(); + void vehicleChanged(); + void sendStatusChanged(); + void incrementVehicleIdChanged(); + +private: + MAV_AUTOPILOT _firmwareType = MAV_AUTOPILOT_PX4; + MAV_TYPE _vehicleType = MAV_TYPE_QUADROTOR; + bool _sendStatusText = false; + FailureMode_t _failureMode = FailNone; + bool _incrementVehicleId = true; + uint16_t _boardVendorId = 0; + uint16_t _boardProductId = 0; + + static constexpr const char *_firmwareTypeKey = "FirmwareType"; + static constexpr const char *_vehicleTypeKey = "VehicleType"; + static constexpr const char *_sendStatusTextKey = "SendStatusText"; + static constexpr const char *_incrementVehicleIdKey = "IncrementVehicleId"; + static constexpr const char *_failureModeKey = "FailureMode"; +}; diff --git a/src/Comms/MockLink/MockLink.cc b/src/Comms/MockLink/MockLink.cc index 2b7a2822c5d4..d68ed1d81608 100644 --- a/src/Comms/MockLink/MockLink.cc +++ b/src/Comms/MockLink/MockLink.cc @@ -8,10 +8,10 @@ ****************************************************************************/ #include "MockLink.h" -#include "QGCLoggingCategory.h" -#include "QGCApplication.h" + #include "LinkManager.h" #include "MAVLinkProtocol.h" +#include "QGCApplication.h" #include "QGCLoggingCategory.h" #include @@ -20,72 +20,37 @@ #include #include -#include - -// FIXME: Hack to work around clean headers -#include "FirmwarePlugin/PX4/px4_custom_mode.h" - QGC_LOGGING_CATEGORY(MockLinkLog, "MockLinkLog") QGC_LOGGING_CATEGORY(MockLinkVerboseLog, "MockLinkVerboseLog") -// Vehicle position is set close to default Gazebo vehicle location. This allows for multi-vehicle -// testing of a gazebo vehicle and a mocklink vehicle -#if 1 -double MockLink::_defaultVehicleLatitude = 47.397; -double MockLink::_defaultVehicleLongitude = 8.5455; -double MockLink::_defaultVehicleHomeAltitude = 488.056; -#else -double MockLink::_defaultVehicleLatitude = 47.6333022928789; -double MockLink::_defaultVehicleLongitude = -122.08833157994995; -double MockLink::_defaultVehicleHomeAltitude = 19.0; -#endif -int MockLink::_nextVehicleSystemId = 128; +int MockLink::_nextVehicleSystemId = 128; // The LinkManager is only forward declared in the header, so a static_assert is here instead to ensure we update if the value changes. static_assert(LinkManager::invalidMavlinkChannel() == std::numeric_limits::max(), "update MockLink::_mavlinkAuxChannel"); -MockLink::MockLink(SharedLinkConfigurationPtr& config) - : LinkInterface (config) - , _missionItemHandler (this, MAVLinkProtocol::instance()) - , _name ("MockLink") - , _connected (false) - , _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) - , _vehicleAltitudeAMSL (_defaultVehicleHomeAltitude) - , _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) - , _adsbAngles {0} +MockLink::MockLink(SharedLinkConfigurationPtr &config, QObject *parent) + : LinkInterface(config, parent) + , _missionItemHandler(new MockLinkMissionItemHandler(this, MAVLinkProtocol::instance())) + , _vehicleAltitudeAMSL(_defaultVehicleHomeAltitude) { qCDebug(MockLinkLog) << "MockLink" << this; // Initialize 5 ADS-B vehicles with different starting conditions _numberOfVehicles - for (int i = 0; i < 5; ++i) { + for (int i = 0; i < _numberOfVehicles; ++i) { ADSBVehicle vehicle; vehicle.angle = i * 72; // Different starting directions (angles 0, 72, 144, 216, 288) - + // Set initial coordinates slightly offset from the default coordinates double latOffset = 0.001 * i; // Adjust latitude slightly for each vehicle (close proximity) double lonOffset = 0.001 * (i % 2 == 0 ? i : -i); // Adjust longitude with a pattern (close proximity) vehicle.coordinate = QGeoCoordinate(_defaultVehicleLatitude + latOffset, _defaultVehicleLongitude + lonOffset); - + // Set a unique starting altitude for each vehicle near the home altitude vehicle.altitude = _defaultVehicleHomeAltitude + (i * 5); // Altitudes: close to default, with slight variation - + _adsbVehicles.push_back(vehicle); } - + MockConfiguration* mockConfig = qobject_cast(_config.get()); _firmwareType = mockConfig->firmwareType(); _vehicleType = mockConfig->vehicleType(); @@ -99,8 +64,6 @@ MockLink::MockLink(SharedLinkConfigurationPtr& config) QObject::connect(this, &MockLink::writeBytesQueuedSignal, this, &MockLink::_writeBytesQueued, Qt::QueuedConnection); - _mavCustomMode = PX4CustomMode::MANUAL; - _mockLinkFTP = new MockLinkFTP(_vehicleSystemId, _vehicleComponentId, this); moveToThread(this); @@ -109,7 +72,7 @@ MockLink::MockLink(SharedLinkConfigurationPtr& config) _runningTime.start(); } -MockLink::~MockLink(void) +MockLink::~MockLink() { disconnect(); if (!_logDownloadFilename.isEmpty()) { @@ -118,7 +81,7 @@ MockLink::~MockLink(void) qCDebug(MockLinkLog) << "~MockLink" << this; } -bool MockLink::_connect(void) +bool MockLink::_connect() { if (!_connected) { _connected = true; @@ -167,17 +130,12 @@ void MockLink::_freeMavlinkChannel() LinkInterface::_freeMavlinkChannel(); } -uint8_t MockLink::mavlinkAuxChannel(void) const -{ - return _mavlinkAuxChannel; -} - -bool MockLink::mavlinkAuxChannelIsSet(void) const +bool MockLink::mavlinkAuxChannelIsSet() const { return (LinkManager::invalidMavlinkChannel() != _mavlinkAuxChannel); } -void MockLink::disconnect(void) +void MockLink::disconnect() { if (_connected) { _connected = false; @@ -187,12 +145,12 @@ void MockLink::disconnect(void) } } -void MockLink::run(void) +void MockLink::run() { - QTimer timer1HzTasks; - QTimer timer10HzTasks; - QTimer timer500HzTasks; - QTimer timerStatusText; + QTimer timer1HzTasks; + QTimer timer10HzTasks; + QTimer timer500HzTasks; + QTimer timerStatusText; QObject::connect(&timer1HzTasks, &QTimer::timeout, this, &MockLink::_run1HzTasks); QObject::connect(&timer10HzTasks, &QTimer::timeout, this, &MockLink::_run10HzTasks); @@ -220,10 +178,10 @@ void MockLink::run(void) QObject::disconnect(&timer10HzTasks, &QTimer::timeout, this, &MockLink::_run10HzTasks); QObject::disconnect(&timer500HzTasks, &QTimer::timeout, this, &MockLink::_run500HzTasks); - _missionItemHandler.shutdown(); + _missionItemHandler->shutdown(); } -void MockLink::_run1HzTasks(void) +void MockLink::_run1HzTasks() { if (_mavlinkStarted && _connected) { if (linkConfiguration()->isHighLatency() && _highLatencyTransmissionEnabled) { @@ -248,7 +206,7 @@ void MockLink::_run1HzTasks(void) } } -void MockLink::_run10HzTasks(void) +void MockLink::_run10HzTasks() { if (linkConfiguration()->isHighLatency()) { return; @@ -267,7 +225,7 @@ void MockLink::_run10HzTasks(void) } } -void MockLink::_run500HzTasks(void) +void MockLink::_run500HzTasks() { if (linkConfiguration()->isHighLatency()) { return; @@ -279,7 +237,7 @@ void MockLink::_run500HzTasks(void) } } -void MockLink::_loadParams(void) +void MockLink::_loadParams() { QFile paramFile; @@ -355,7 +313,7 @@ void MockLink::_loadParams(void) } } -void MockLink::_sendHeartBeat(void) +void MockLink::_sendHeartBeat() { mavlink_message_t msg; @@ -372,7 +330,7 @@ void MockLink::_sendHeartBeat(void) respondWithMavlinkMessage(msg); } -void MockLink::_sendHighLatency2(void) +void MockLink::_sendHighLatency2() { mavlink_message_t msg; @@ -412,7 +370,7 @@ void MockLink::_sendHighLatency2(void) respondWithMavlinkMessage(msg); } -void MockLink::_sendSysStatus(void) +void MockLink::_sendSysStatus() { mavlink_message_t msg; mavlink_msg_sys_status_pack_chan( @@ -431,7 +389,7 @@ void MockLink::_sendSysStatus(void) respondWithMavlinkMessage(msg); } -void MockLink::_sendBatteryStatus(void) +void MockLink::_sendBatteryStatus() { if(_battery1PctRemaining > 1) { _battery1PctRemaining = static_cast(100 - (_runningTime.elapsed() / 1000)); @@ -516,7 +474,7 @@ void MockLink::_sendBatteryStatus(void) respondWithMavlinkMessage(msg); } -void MockLink::_sendVibration(void) +void MockLink::_sendVibration() { mavlink_message_t msg; @@ -553,7 +511,7 @@ void MockLink::_writeBytes(const QByteArray &bytes) emit writeBytesQueuedSignal(bytes); } -void MockLink::_writeBytesQueued(const QByteArray bytes) +void MockLink::_writeBytesQueued(const QByteArray &bytes) { if (_inNSH) { _handleIncomingNSHBytes(bytes.constData(), bytes.length()); @@ -614,7 +572,7 @@ void MockLink::_handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes) void MockLink::_handleIncomingMavlinkMsg(const mavlink_message_t &msg) { - if (_missionItemHandler.handleMessage(msg)) { + if (_missionItemHandler->handleMessage(msg)) { return; } @@ -847,7 +805,7 @@ void MockLink::_handleParamRequestList(const mavlink_message_t& msg) } /// Sends the next parameter to the vehicle -void MockLink::_paramRequestListWorker(void) +void MockLink::_paramRequestListWorker() { if (_currentParamRequestListComponentIndex == -1) { // Initial request complete @@ -1218,7 +1176,7 @@ void MockLink::sendUnexpectedCommandAck(MAV_CMD command, MAV_RESULT ackResult) respondWithMavlinkMessage(commandAck); } -void MockLink::_respondWithAutopilotVersion(void) +void MockLink::_respondWithAutopilotVersion() { mavlink_message_t msg; @@ -1269,12 +1227,7 @@ void MockLink::_respondWithAutopilotVersion(void) respondWithMavlinkMessage(msg); } -void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode, MAV_MISSION_RESULT failureAckResult) -{ - _missionItemHandler.setFailureMode(failureMode, failureAckResult); -} - -void MockLink::_sendHomePosition(void) +void MockLink::_sendHomePosition() { mavlink_message_t msg; @@ -1298,7 +1251,7 @@ void MockLink::_sendHomePosition(void) respondWithMavlinkMessage(msg); } -void MockLink::_sendGpsRawInt(void) +void MockLink::_sendGpsRawInt() { static uint64_t timeTick = 0; mavlink_message_t msg; @@ -1327,7 +1280,7 @@ void MockLink::_sendGpsRawInt(void) respondWithMavlinkMessage(msg); } -void MockLink::_sendGlobalPositionInt(void) +void MockLink::_sendGlobalPositionInt() { static uint64_t timeTick = 0; mavlink_message_t msg; @@ -1346,7 +1299,7 @@ void MockLink::_sendGlobalPositionInt(void) respondWithMavlinkMessage(msg); } -void MockLink::_sendExtendedSysState(void) +void MockLink::_sendExtendedSysState() { mavlink_message_t msg; @@ -1397,7 +1350,7 @@ void MockLink::_sendChunkedStatusText(uint16_t chunkId, bool missingChunks) } -void MockLink::_sendStatusTextMessages(void) +void MockLink::_sendStatusTextMessages() { struct StatusMessage { MAV_SEVERITY severity; @@ -1438,62 +1391,6 @@ void MockLink::_sendStatusTextMessages(void) _sendChunkedStatusText(4, true /* missingChunks */); // This should cause the timeout to fire } -MockConfiguration::MockConfiguration(const QString& name) - : LinkConfiguration(name) -{ - -} - -MockConfiguration::MockConfiguration(const MockConfiguration* source) - : LinkConfiguration(source) -{ - _firmwareType = source->_firmwareType; - _vehicleType = source->_vehicleType; - _sendStatusText = source->_sendStatusText; - _incrementVehicleId = source->_incrementVehicleId; - _failureMode = source->_failureMode; -} - -void MockConfiguration::copyFrom(const LinkConfiguration *source) -{ - LinkConfiguration::copyFrom(source); - const MockConfiguration* usource = qobject_cast(source); - - if (!usource) { - qCWarning(MockLinkLog) << "dynamic_cast failed" << source << usource; - return; - } - - _firmwareType = usource->_firmwareType; - _vehicleType = usource->_vehicleType; - _sendStatusText = usource->_sendStatusText; - _incrementVehicleId = usource->_incrementVehicleId; - _failureMode = usource->_failureMode; -} - -void MockConfiguration::saveSettings(QSettings& settings, const QString& root) -{ - settings.beginGroup(root); - settings.setValue(_firmwareTypeKey, (int)_firmwareType); - settings.setValue(_vehicleTypeKey, (int)_vehicleType); - settings.setValue(_sendStatusTextKey, _sendStatusText); - settings.setValue(_incrementVehicleIdKey, _incrementVehicleId); - settings.setValue(_failureModeKey, (int)_failureMode); - settings.sync(); - settings.endGroup(); -} - -void MockConfiguration::loadSettings(QSettings& settings, const QString& root) -{ - settings.beginGroup(root); - _firmwareType = (MAV_AUTOPILOT)settings.value(_firmwareTypeKey, (int)MAV_AUTOPILOT_PX4).toInt(); - _vehicleType = (MAV_TYPE)settings.value(_vehicleTypeKey, (int)MAV_TYPE_QUADROTOR).toInt(); - _sendStatusText = settings.value(_sendStatusTextKey, false).toBool(); - _incrementVehicleId = settings.value(_incrementVehicleIdKey, true).toBool(); - _failureMode = (FailureMode_t)settings.value(_failureModeKey, (int)FailNone).toInt(); - settings.endGroup(); -} - MockLink* MockLink::_startMockLink(MockConfiguration* mockConfig) { mockConfig->setDynamic(true); @@ -1506,7 +1403,7 @@ MockLink* MockLink::_startMockLink(MockConfiguration* mockConfig) } } -MockLink* MockLink::_startMockLinkWorker(QString configName, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType, bool sendStatusText, MockConfiguration::FailureMode_t failureMode) +MockLink *MockLink::_startMockLinkWorker(const QString &configName, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType, bool sendStatusText, MockConfiguration::FailureMode_t failureMode) { MockConfiguration* mockConfig = new MockConfiguration(configName); @@ -1520,40 +1417,40 @@ MockLink* MockLink::_startMockLinkWorker(QString configName, MAV_AUTOPILOT firmw MockLink* MockLink::startPX4MockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) { - return _startMockLinkWorker("PX4 MultiRotor MockLink", MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, sendStatusText, failureMode); + return _startMockLinkWorker(QStringLiteral("PX4 MultiRotor MockLink"), MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, sendStatusText, failureMode); } MockLink* MockLink::startGenericMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) { - return _startMockLinkWorker("Generic MockLink", MAV_AUTOPILOT_GENERIC, MAV_TYPE_QUADROTOR, sendStatusText, failureMode); + return _startMockLinkWorker(QStringLiteral("Generic MockLink"), MAV_AUTOPILOT_GENERIC, MAV_TYPE_QUADROTOR, sendStatusText, failureMode); } MockLink* MockLink::startNoInitialConnectMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) { - return _startMockLinkWorker("No Initial Connect MockLink", MAV_AUTOPILOT_PX4, MAV_TYPE_GENERIC, sendStatusText, failureMode); + return _startMockLinkWorker(QStringLiteral("No Initial Connect MockLink"), MAV_AUTOPILOT_PX4, MAV_TYPE_GENERIC, sendStatusText, failureMode); } MockLink* MockLink::startAPMArduCopterMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) { - return _startMockLinkWorker("ArduCopter MockLink",MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_QUADROTOR, sendStatusText, failureMode); + return _startMockLinkWorker(QStringLiteral("ArduCopter MockLink"),MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_QUADROTOR, sendStatusText, failureMode); } MockLink* MockLink::startAPMArduPlaneMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) { - return _startMockLinkWorker("ArduPlane MockLink", MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_FIXED_WING, sendStatusText, failureMode); + return _startMockLinkWorker(QStringLiteral("ArduPlane MockLink"), MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_FIXED_WING, sendStatusText, failureMode); } MockLink* MockLink::startAPMArduSubMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) { - return _startMockLinkWorker("ArduSub MockLink", MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_SUBMARINE, sendStatusText, failureMode); + return _startMockLinkWorker(QStringLiteral("ArduSub MockLink"), MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_SUBMARINE, sendStatusText, failureMode); } MockLink* MockLink::startAPMArduRoverMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) { - return _startMockLinkWorker("ArduRover MockLink", MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_GROUND_ROVER, sendStatusText, failureMode); + return _startMockLinkWorker(QStringLiteral("ArduRover MockLink"), MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_GROUND_ROVER, sendStatusText, failureMode); } -void MockLink::_sendRCChannels(void) +void MockLink::_sendRCChannels() { mavlink_message_t msg; @@ -1681,7 +1578,7 @@ void MockLink::_handleLogRequestData(const mavlink_message_t& msg) _logDownloadBytesRemaining = request.count; } -void MockLink::_logDownloadWorker(void) +void MockLink::_logDownloadWorker() { if (_logDownloadBytesRemaining != 0) { QFile file(_logDownloadFilename); @@ -1715,15 +1612,15 @@ void MockLink::_logDownloadWorker(void) } } -void MockLink::_sendADSBVehicles(void) +void MockLink::_sendADSBVehicles() { for (int i = 0; i < _adsbVehicles.size(); ++i) { // Slightly change the direction to simulate different paths _adsbVehicles[i].angle += (i + 1); // Vary the change to make each path unique - + // Move each vehicle by a smaller distance to simulate slower speed _adsbVehicles[i].coordinate = _adsbVehicles[i].coordinate.atDistanceAndAzimuth(5, _adsbVehicles[i].angle); // 50 meters per update for slower speed - + // Simulate slight variations in altitude _adsbVehicles[i].altitude += (i % 2 == 0 ? 0.5 : -0.5); // Increase or decrease altitude @@ -1754,7 +1651,7 @@ void MockLink::_sendADSBVehicles(void) void MockLink::_moveADSBVehicle(int vehicleIndex) { _adsbAngles[vehicleIndex] += 10; // Increment angle for smoother movement QGeoCoordinate& coord = _adsbVehicleCoordinates[vehicleIndex]; - + // Update the position based on the new angle coord = QGeoCoordinate(coord.latitude(), coord.longitude()) .atDistanceAndAzimuth(500, _adsbAngles[vehicleIndex]); @@ -1844,7 +1741,7 @@ bool MockLink::_handleRequestMessage(const mavlink_command_long_t& request, bool return false; } -void MockLink::_sendGeneralMetaData(void) +void MockLink::_sendGeneralMetaData() { mavlink_message_t responseMsg; #if 1 @@ -1875,7 +1772,7 @@ void MockLink::_sendRemoteIDArmStatus() respondWithMavlinkMessage(msg); } -void MockLink::simulateConnectionRemoved(void) +void MockLink::simulateConnectionRemoved() { _commLost = true; _connectionRemoved(); diff --git a/src/Comms/MockLink/MockLink.h b/src/Comms/MockLink/MockLink.h index bdbc60f09fca..d16c9c964aca 100644 --- a/src/Comms/MockLink/MockLink.h +++ b/src/Comms/MockLink/MockLink.h @@ -9,111 +9,38 @@ #pragma once -#include "MockLinkMissionItemHandler.h" -#include "MockLinkFTP.h" -#include "QGCMAVLink.h" +#include "FirmwarePlugin/PX4/px4_custom_mode.h" #include "LinkInterface.h" -#include "LinkConfiguration.h" +#include "MAVLinkLib.h" +#include "MockConfiguration.h" +#include "MockLinkFTP.h" +#include "MockLinkMissionItemHandler.h" -#include -#include #include +#include #include #include +#include Q_DECLARE_LOGGING_CATEGORY(MockLinkLog) Q_DECLARE_LOGGING_CATEGORY(MockLinkVerboseLog) -class LinkManager; - -class MockConfiguration : public LinkConfiguration -{ - Q_OBJECT - -public: - MockConfiguration(const QString& name); - MockConfiguration(const MockConfiguration* source); - - Q_PROPERTY(int firmware READ firmware WRITE setFirmware NOTIFY firmwareChanged) - Q_PROPERTY(int vehicle READ vehicle WRITE setVehicle NOTIFY vehicleChanged) - Q_PROPERTY(bool sendStatus READ sendStatusText WRITE setSendStatusText NOTIFY sendStatusChanged) - Q_PROPERTY(bool incrementVehicleId READ incrementVehicleId WRITE setIncrementVehicleId NOTIFY incrementVehicleIdChanged) - - int firmware (void) { return (int)_firmwareType; } - void setFirmware (int type) { _firmwareType = (MAV_AUTOPILOT)type; emit firmwareChanged(); } - int vehicle (void) { return (int)_vehicleType; } - bool incrementVehicleId (void) const { return _incrementVehicleId; } - void setVehicle (int type) { _vehicleType = (MAV_TYPE)type; emit vehicleChanged(); } - void setIncrementVehicleId (bool incrementVehicleId) { _incrementVehicleId = incrementVehicleId; emit incrementVehicleIdChanged(); } - - - MAV_AUTOPILOT firmwareType (void) { return _firmwareType; } - uint16_t boardVendorId (void) { return _boardVendorId; } - uint16_t boardProductId (void) { return _boardProductId; } - MAV_TYPE vehicleType (void) { return _vehicleType; } - bool sendStatusText (void) const { return _sendStatusText; } - - void setFirmwareType (MAV_AUTOPILOT firmwareType) { _firmwareType = firmwareType; emit firmwareChanged(); } - void setBoardVendorProduct(uint16_t vendorId, uint16_t productId) { _boardVendorId = vendorId; _boardProductId = productId; } - void setVehicleType (MAV_TYPE vehicleType) { _vehicleType = vehicleType; emit vehicleChanged(); } - void setSendStatusText (bool sendStatusText) { _sendStatusText = sendStatusText; emit sendStatusChanged(); } - - typedef enum { - FailNone, // No failures - FailParamNoReponseToRequestList, // Do no respond to PARAM_REQUEST_LIST - FailMissingParamOnInitialReqest, // Not all params are sent on initial request, should still succeed since QGC will re-query missing params - FailMissingParamOnAllRequests, // Not all params are sent on initial request, QGC retries will fail as well - FailInitialConnectRequestMessageAutopilotVersionFailure, // REQUEST_MESSAGE:AUTOPILOT_VERSION returns failure - FailInitialConnectRequestMessageAutopilotVersionLost, // REQUEST_MESSAGE:AUTOPILOT_VERSION success, AUTOPILOT_VERSION never sent - FailInitialConnectRequestMessageProtocolVersionFailure, // REQUEST_MESSAGE:PROTOCOL_VERSION returns failure - FailInitialConnectRequestMessageProtocolVersionLost, // REQUEST_MESSAGE:PROTOCOL_VERSION success, PROTOCOL_VERSION never sent - } FailureMode_t; - FailureMode_t failureMode(void) { return _failureMode; } - void setFailureMode(FailureMode_t failureMode) { _failureMode = failureMode; } - - // Overrides from LinkConfiguration - LinkType type (void) const override { return LinkConfiguration::TypeMock; } - void copyFrom (const LinkConfiguration* source) override; - void loadSettings (QSettings& settings, const QString& root) override; - void saveSettings (QSettings& settings, const QString& root) override; - QString settingsURL (void) override { return "MockLinkSettings.qml"; } - QString settingsTitle (void) override { return tr("Mock Link Settings"); } - -signals: - void firmwareChanged (void); - void vehicleChanged (void); - void sendStatusChanged (void); - void incrementVehicleIdChanged (void); - -private: - MAV_AUTOPILOT _firmwareType = MAV_AUTOPILOT_PX4; - MAV_TYPE _vehicleType = MAV_TYPE_QUADROTOR; - bool _sendStatusText = false; - FailureMode_t _failureMode = FailNone; - bool _incrementVehicleId = true; - uint16_t _boardVendorId = 0; - uint16_t _boardProductId = 0; - - static constexpr const char* _firmwareTypeKey = "FirmwareType"; - static constexpr const char* _vehicleTypeKey = "VehicleType"; - static constexpr const char* _sendStatusTextKey = "SendStatusText"; - static constexpr const char* _incrementVehicleIdKey = "IncrementVehicleId"; - static constexpr const char* _failureModeKey = "FailureMode"; -}; - class MockLink : public LinkInterface { Q_OBJECT public: - MockLink(SharedLinkConfigurationPtr& config); + explicit MockLink(SharedLinkConfigurationPtr &config, QObject *parent = nullptr); virtual ~MockLink(); - int vehicleId (void) const { return _vehicleSystemId; } - MAV_AUTOPILOT getFirmwareType (void) { return _firmwareType; } - void setFirmwareType (MAV_AUTOPILOT autopilot) { _firmwareType = autopilot; } - void setSendStatusText (bool sendStatusText) { _sendStatusText = sendStatusText; } - void setFailureMode (MockConfiguration::FailureMode_t failureMode) { _failureMode = failureMode; } + bool isConnected() const override { return _connected; } + void disconnect() override; + + int vehicleId() const { return _vehicleSystemId; } + MAV_AUTOPILOT getFirmwareType() const { return _firmwareType; } + void setFirmwareType(MAV_AUTOPILOT autopilot) { _firmwareType = autopilot; } + void setSendStatusText(bool sendStatusText) { _sendStatusText = sendStatusText; } + void setFailureMode(MockConfiguration::FailureMode_t failureMode) { _failureMode = failureMode; } /// APM stack has strange handling of the first item of the mission list. If it has no /// onboard mission items, sometimes it sends back a home position in position 0 and @@ -124,13 +51,9 @@ class MockLink : public LinkInterface void emitRemoteControlChannelRawChanged(int channel, uint16_t raw); /// Sends the specified mavlink message to QGC - void respondWithMavlinkMessage(const mavlink_message_t& msg); - - MockLinkFTP* mockLinkFTP(void) { return _mockLinkFTP; } + void respondWithMavlinkMessage(const mavlink_message_t &msg); - // Overrides from LinkInterface - bool isConnected(void) const override { return _connected; } - void disconnect (void) override; + MockLinkFTP *mockLinkFTP() const { return _mockLinkFTP; } struct ADSBVehicle { QGeoCoordinate coordinate; @@ -138,208 +61,213 @@ class MockLink : public LinkInterface double altitude; // Store unique altitude for each vehicle }; std::vector _adsbVehicles; // Store data for multiple ADS-B vehicles - + /// Sets a failure mode for unit testingqgcm /// @param failureMode Type of failure to simulate /// @param failureAckResult Error to send if one the ack error modes - void setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode, MAV_MISSION_RESULT failureAckResult); + void setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode_t failureMode, MAV_MISSION_RESULT failureAckResult) const { _missionItemHandler->setFailureMode(failureMode, failureAckResult); } /// Called to send a MISSION_ACK message while the MissionManager is in idle state - void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType) { _missionItemHandler.sendUnexpectedMissionAck(ackType); } + void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType) const { _missionItemHandler->sendUnexpectedMissionAck(ackType); } /// Called to send a MISSION_ITEM message while the MissionManager is in idle state - void sendUnexpectedMissionItem(void) { _missionItemHandler.sendUnexpectedMissionItem(); } + void sendUnexpectedMissionItem() const { _missionItemHandler->sendUnexpectedMissionItem(); } /// Called to send a MISSION_REQUEST message while the MissionManager is in idle state - void sendUnexpectedMissionRequest(void) { _missionItemHandler.sendUnexpectedMissionRequest(); } + void sendUnexpectedMissionRequest() const { _missionItemHandler->sendUnexpectedMissionRequest(); } void sendUnexpectedCommandAck(MAV_CMD command, MAV_RESULT ackResult); /// Reset the state of the MissionItemHandler to no items, no transactions in progress. - void resetMissionItemHandler(void) { _missionItemHandler.reset(); } + void resetMissionItemHandler() const { _missionItemHandler->reset(); } /// Returns the filename for the simulated log file. Only available after a download is requested. - QString logDownloadFile(void) { return _logDownloadFilename; } - - Q_INVOKABLE void setCommLost (bool commLost) { _commLost = commLost; } - Q_INVOKABLE void simulateConnectionRemoved (void); - static MockLink* startPX4MockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); - static MockLink* startGenericMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); - static MockLink* startNoInitialConnectMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); - static MockLink* startAPMArduCopterMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); - static MockLink* startAPMArduPlaneMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); - static MockLink* startAPMArduSubMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); - static MockLink* startAPMArduRoverMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); + QString logDownloadFile() const { return _logDownloadFilename; } + + Q_INVOKABLE void setCommLost(bool commLost) { _commLost = commLost; } + Q_INVOKABLE void simulateConnectionRemoved(); + + static MockLink *startPX4MockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); + static MockLink *startGenericMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); + static MockLink *startNoInitialConnectMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); + static MockLink *startAPMArduCopterMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); + static MockLink *startAPMArduPlaneMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); + static MockLink *startAPMArduSubMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); + static MockLink *startAPMArduRoverMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); // Special commands for testing Vehicle::sendMavCommandWithHandler - static constexpr MAV_CMD MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED = MAV_CMD_USER_1; - static constexpr MAV_CMD MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED = MAV_CMD_USER_2; - static constexpr MAV_CMD MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED = MAV_CMD_USER_3; - static constexpr MAV_CMD MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED = MAV_CMD_USER_4; - static constexpr MAV_CMD MAV_CMD_MOCKLINK_NO_RESPONSE = MAV_CMD_USER_5; - static constexpr MAV_CMD MAV_CMD_MOCKLINK_NO_RESPONSE_NO_RETRY = static_cast(MAV_CMD_USER_5 + 1); - static constexpr MAV_CMD MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_ACCEPTED = static_cast(MAV_CMD_USER_5 + 2); - static constexpr MAV_CMD MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_FAILED = static_cast(MAV_CMD_USER_5 + 3); - static constexpr MAV_CMD MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_NO_ACK = static_cast(MAV_CMD_USER_5 + 4); - - void clearReceivedMavCommandCounts(void) { _receivedMavCommandCountMap.clear(); } - int receivedMavCommandCount(MAV_CMD command) { return _receivedMavCommandCountMap[command]; } - - typedef enum { + static constexpr MAV_CMD MAV_CMD_MOCKLINK_ALWAYS_RESULT_ACCEPTED = MAV_CMD_USER_1; + static constexpr MAV_CMD MAV_CMD_MOCKLINK_ALWAYS_RESULT_FAILED = MAV_CMD_USER_2; + static constexpr MAV_CMD MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_ACCEPTED = MAV_CMD_USER_3; + static constexpr MAV_CMD MAV_CMD_MOCKLINK_SECOND_ATTEMPT_RESULT_FAILED = MAV_CMD_USER_4; + static constexpr MAV_CMD MAV_CMD_MOCKLINK_NO_RESPONSE = MAV_CMD_USER_5; + static constexpr MAV_CMD MAV_CMD_MOCKLINK_NO_RESPONSE_NO_RETRY = static_cast(MAV_CMD_USER_5 + 1); + static constexpr MAV_CMD MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_ACCEPTED = static_cast(MAV_CMD_USER_5 + 2); + static constexpr MAV_CMD MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_FAILED = static_cast(MAV_CMD_USER_5 + 3); + static constexpr MAV_CMD MAV_CMD_MOCKLINK_RESULT_IN_PROGRESS_NO_ACK = static_cast(MAV_CMD_USER_5 + 4); + + void clearReceivedMavCommandCounts() { _receivedMavCommandCountMap.clear(); } + int receivedMavCommandCount(MAV_CMD command) const { return _receivedMavCommandCountMap[command]; } + + enum RequestMessageFailureMode_t { FailRequestMessageNone, FailRequestMessageCommandAcceptedMsgNotSent, FailRequestMessageCommandUnsupported, FailRequestMessageCommandNoResponse, - } RequestMessageFailureMode_t; + }; void setRequestMessageFailureMode(RequestMessageFailureMode_t failureMode) { _requestMessageFailureMode = failureMode; } signals: - void writeBytesQueuedSignal (const QByteArray bytes); + void writeBytesQueuedSignal(const QByteArray bytes); void highLatencyTransmissionEnabledChanged (bool highLatencyTransmissionEnabled); private slots: // LinkInterface overrides void _writeBytes(const QByteArray &bytes) final; - void _writeBytesQueued (const QByteArray bytes); - void _run1HzTasks (void); - void _run10HzTasks (void); - void _run500HzTasks (void); - void _sendStatusTextMessages(void); + void _writeBytesQueued(const QByteArray &bytes); + void _run1HzTasks(); + void _run10HzTasks(); + void _run500HzTasks(); + void _sendStatusTextMessages(); private: // LinkInterface overrides - bool _connect (void) override; - bool _allocateMavlinkChannel () override; - void _freeMavlinkChannel () override; - uint8_t mavlinkAuxChannel (void) const; - bool mavlinkAuxChannelIsSet (void) const; + bool _connect() final; + bool _allocateMavlinkChannel() final; + void _freeMavlinkChannel() final; + + uint8_t mavlinkAuxChannel() const { return _mavlinkAuxChannel; } + bool mavlinkAuxChannelIsSet() const; // QThread override - void run(void) final; - - // MockLink methods - void _sendHeartBeat (void); - void _sendHighLatency2 (void); - void _handleIncomingNSHBytes (const char* bytes, int cBytes); - void _handleIncomingMavlinkBytes (const uint8_t* bytes, int cBytes); - void _handleIncomingMavlinkMsg (const mavlink_message_t& msg); - void _loadParams (void); - void _handleHeartBeat (const mavlink_message_t& msg); - void _handleSetMode (const mavlink_message_t& msg); - void _handleParamRequestList (const mavlink_message_t& msg); - void _handleParamSet (const mavlink_message_t& msg); - void _handleParamRequestRead (const mavlink_message_t& msg); - void _handleFTP (const mavlink_message_t& msg); - void _handleCommandLong (const mavlink_message_t& msg); - void _handleInProgressCommandLong (const mavlink_command_long_t& request); - void _handleManualControl (const mavlink_message_t& msg); - void _handlePreFlightCalibration (const mavlink_command_long_t& request); - void _handleTakeoff (const mavlink_command_long_t& request); - void _handleLogRequestList (const mavlink_message_t& msg); - void _handleLogRequestData (const mavlink_message_t& msg); - void _handleParamMapRC (const mavlink_message_t& msg); - bool _handleRequestMessage (const mavlink_command_long_t& request, bool& noAck); - float _floatUnionForParam (int componentId, const QString& paramName); - void _setParamFloatUnionIntoMap (int componentId, const QString& paramName, float paramFloat); - void _sendHomePosition (void); - void _sendGpsRawInt (void); - void _sendGlobalPositionInt (void); - void _sendExtendedSysState (void); - void _sendVibration (void); - void _sendSysStatus (void); - void _sendBatteryStatus (void); - void _sendChunkedStatusText (uint16_t chunkId, bool missingChunks); - void _respondWithAutopilotVersion (void); - void _sendRCChannels (void); - void _paramRequestListWorker (void); - void _logDownloadWorker (void); - void _sendADSBVehicles (void); - void _moveADSBVehicle (int vehicleIndex); - void _sendGeneralMetaData (void); - void _sendRemoteIDArmStatus (void); - - static MockLink* _startMockLinkWorker(QString configName, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType, bool sendStatusText, MockConfiguration::FailureMode_t failureMode); - static MockLink* _startMockLink(MockConfiguration* mockConfig); + void run() final; + + void _sendHeartBeat(); + void _sendHighLatency2(); + void _handleIncomingNSHBytes(const char *bytes, int cBytes); + void _handleIncomingMavlinkBytes(const uint8_t *bytes, int cBytes); + void _handleIncomingMavlinkMsg(const mavlink_message_t &msg); + void _loadParams(); + void _handleHeartBeat(const mavlink_message_t &msg); + void _handleSetMode(const mavlink_message_t &msg); + void _handleParamRequestList(const mavlink_message_t &msg); + void _handleParamSet(const mavlink_message_t &msg); + void _handleParamRequestRead(const mavlink_message_t &msg); + void _handleFTP(const mavlink_message_t &msg); + void _handleCommandLong(const mavlink_message_t &msg); + void _handleInProgressCommandLong(const mavlink_command_long_t &request); + void _handleManualControl(const mavlink_message_t &msg); + void _handlePreFlightCalibration(const mavlink_command_long_t &request); + void _handleTakeoff(const mavlink_command_long_t &request); + void _handleLogRequestList(const mavlink_message_t &msg); + void _handleLogRequestData(const mavlink_message_t &msg); + void _handleParamMapRC(const mavlink_message_t &msg); + bool _handleRequestMessage(const mavlink_command_long_t &request, bool& noAck); + float _floatUnionForParam(int componentId, const QString ¶mName); + void _setParamFloatUnionIntoMap(int componentId, const QString ¶mName, float paramFloat); + void _sendHomePosition(); + void _sendGpsRawInt(); + void _sendGlobalPositionInt(); + void _sendExtendedSysState(); + void _sendVibration(); + void _sendSysStatus(); + void _sendBatteryStatus(); + void _sendChunkedStatusText(uint16_t chunkId, bool missingChunks); + void _respondWithAutopilotVersion(); + void _sendRCChannels(); + void _paramRequestListWorker(); + void _logDownloadWorker(); + void _sendADSBVehicles(); + void _moveADSBVehicle(int vehicleIndex); + void _sendGeneralMetaData(); + void _sendRemoteIDArmStatus(); + + static MockLink *_startMockLinkWorker(const QString &configName, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType, bool sendStatusText, MockConfiguration::FailureMode_t failureMode); + static MockLink *_startMockLink(MockConfiguration *mockConfig); /// Creates a file with random contents of the specified size. /// @return Fully qualified path to created file static QString _createRandomFile(uint32_t byteCount); - uint8_t _mavlinkAuxChannel = std::numeric_limits::max(); - QMutex _mavlinkAuxMutex; + MockLinkMissionItemHandler *_missionItemHandler = nullptr; - MockLinkMissionItemHandler _missionItemHandler; + uint8_t _mavlinkAuxChannel = std::numeric_limits::max(); + QMutex _mavlinkAuxMutex; - QString _name; - bool _connected; + QString _name = QString("MockLink"); + bool _connected = false; - uint8_t _vehicleSystemId; - uint8_t _vehicleComponentId = MAV_COMP_ID_AUTOPILOT1; + uint8_t _vehicleSystemId = 0; + uint8_t _vehicleComponentId = MAV_COMP_ID_AUTOPILOT1; - bool _inNSH; - bool _mavlinkStarted; + bool _inNSH = false; + bool _mavlinkStarted = false; - uint8_t _mavBaseMode; - uint32_t _mavCustomMode; - uint8_t _mavState; + uint8_t _mavBaseMode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + uint32_t _mavCustomMode = PX4CustomMode::MANUAL; + uint8_t _mavState = MAV_STATE_STANDBY; - QElapsedTimer _runningTime; - static const int32_t _batteryMaxTimeRemaining = 15 * 60; - int8_t _battery1PctRemaining = 100; - int32_t _battery1TimeRemaining = _batteryMaxTimeRemaining; - MAV_BATTERY_CHARGE_STATE _battery1ChargeState = MAV_BATTERY_CHARGE_STATE_OK; - int8_t _battery2PctRemaining = 100; - int32_t _battery2TimeRemaining = _batteryMaxTimeRemaining; - MAV_BATTERY_CHARGE_STATE _battery2ChargeState = MAV_BATTERY_CHARGE_STATE_OK; + QElapsedTimer _runningTime; + static constexpr int32_t _batteryMaxTimeRemaining = 15 * 60; + int8_t _battery1PctRemaining = 100; + int32_t _battery1TimeRemaining = _batteryMaxTimeRemaining; + MAV_BATTERY_CHARGE_STATE _battery1ChargeState = MAV_BATTERY_CHARGE_STATE_OK; + int8_t _battery2PctRemaining = 100; + int32_t _battery2TimeRemaining = _batteryMaxTimeRemaining; + MAV_BATTERY_CHARGE_STATE _battery2ChargeState = MAV_BATTERY_CHARGE_STATE_OK; - MAV_AUTOPILOT _firmwareType; - MAV_TYPE _vehicleType; - double _vehicleLatitude; - double _vehicleLongitude; - double _vehicleAltitudeAMSL; - bool _commLost = false; - bool _highLatencyTransmissionEnabled = true; + MAV_AUTOPILOT _firmwareType = MAV_AUTOPILOT_PX4; + MAV_TYPE _vehicleType = MAV_TYPE_QUADROTOR; + double _vehicleLatitude = 0.0; + double _vehicleLongitude = 0.0; + double _vehicleAltitudeAMSL = _defaultVehicleHomeAltitude; + bool _commLost = false; + bool _highLatencyTransmissionEnabled = true; // These are just set for reporting the fields in _respondWithAutopilotVersion() // and ensuring that the Vehicle reports the fields in Vehicle::firmwareBoardVendorId etc. // They do not control any mock simulation (and it is up to the Custom build to do that). - uint16_t _boardVendorId = 0; - uint16_t _boardProductId = 0; + uint16_t _boardVendorId = 0; + uint16_t _boardProductId = 0; - MockLinkFTP* _mockLinkFTP = nullptr; + MockLinkFTP *_mockLinkFTP = nullptr; - bool _sendStatusText; - bool _apmSendHomePositionOnEmptyList; - MockConfiguration::FailureMode_t _failureMode; + bool _sendStatusText = false; + bool _apmSendHomePositionOnEmptyList = false; + MockConfiguration::FailureMode_t _failureMode = MockConfiguration::FailNone; - int _sendHomePositionDelayCount; - int _sendGPSPositionDelayCount; + int _sendHomePositionDelayCount = 10; ///< No home position for 4 seconds + int _sendGPSPositionDelayCount = 100; ///< No gps lock for 5 seconds - int _currentParamRequestListComponentIndex; // Current component index for param request list workflow, -1 for no request in progress - int _currentParamRequestListParamIndex; // Current parameter index for param request list workflow + int _currentParamRequestListComponentIndex = -1; ///< Current component index for param request list workflow, -1 for no request in progress + int _currentParamRequestListParamIndex = -1; ///< Current parameter index for param request list workflow - static const uint16_t _logDownloadLogId = 0; ///< Id of siumulated log file - static const uint32_t _logDownloadFileSize = 1000; ///< Size of simulated log file + static constexpr uint16_t _logDownloadLogId = 0; ///< Id of siumulated log file + static constexpr uint32_t _logDownloadFileSize = 1000; ///< Size of simulated log file - QString _logDownloadFilename; ///< Filename for log download which is in progress - uint32_t _logDownloadCurrentOffset; ///< Current offset we are sending from - uint32_t _logDownloadBytesRemaining; ///< Number of bytes still to send, 0 = send inactive + QString _logDownloadFilename; ///< Filename for log download which is in progress + uint32_t _logDownloadCurrentOffset = 0; ///< Current offset we are sending from + uint32_t _logDownloadBytesRemaining = 0; ///< Number of bytes still to send, 0 = send inactive - QList _adsbVehicleCoordinates; // List for multiple vehicles - double _adsbAngles[5]; // Array for angles of each vehicle - static constexpr int _numberOfVehicles = 5; // Number of ADS-B vehicles + QList _adsbVehicleCoordinates; ///< List for multiple vehicles + static constexpr int _numberOfVehicles = 5; ///< Number of ADS-B vehicles + double _adsbAngles[_numberOfVehicles]{}; ///< Array for angles of each vehicle RequestMessageFailureMode_t _requestMessageFailureMode = FailRequestMessageNone; - QMap _receivedMavCommandCountMap; - QMap> _mapParamName2Value; - QMap> _mapParamName2MavParamType; + QMap _receivedMavCommandCountMap; + QMap> _mapParamName2Value; + QMap> _mapParamName2MavParamType; + + static int _nextVehicleSystemId; + + // Vehicle position is set close to default Gazebo vehicle location. This allows for multi-vehicle + // testing of a gazebo vehicle and a mocklink vehicle + static constexpr double _defaultVehicleLatitude = 47.397; + static constexpr double _defaultVehicleLongitude = 8.5455; + static constexpr double _defaultVehicleHomeAltitude = 488.056; - static double _defaultVehicleLatitude; - static double _defaultVehicleLongitude; - static double _defaultVehicleHomeAltitude; - static int _nextVehicleSystemId; - static constexpr const char* _failParam = "COM_FLTMODE6"; + static constexpr const char *_failParam = "COM_FLTMODE6"; };