From de2f6344d2e49b0fd8a36de8d3339cece668ecea Mon Sep 17 00:00:00 2001 From: Holden Date: Sat, 18 Jan 2025 08:29:05 -0500 Subject: [PATCH] Cleanup MockLink Helper Classes --- src/Comms/MockLink/MockConfiguration.cc | 2 +- src/Comms/MockLink/MockConfiguration.h | 3 +- src/Comms/MockLink/MockLink.cc | 150 +++--- src/Comms/MockLink/MockLink.h | 141 +++--- src/Comms/MockLink/MockLinkFTP.cc | 312 ++++++------ src/Comms/MockLink/MockLinkFTP.h | 126 ++--- .../MockLink/MockLinkMissionItemHandler.cc | 459 +++++++++--------- .../MockLink/MockLinkMissionItemHandler.h | 98 ++-- test/Comms/CMakeLists.txt | 8 - test/Comms/MockLinkOptionsDlg.qml | 58 --- test/FactSystem/ParameterManagerTest.cc | 1 + test/FactSystem/ParameterManagerTest.h | 2 +- .../MissionCommandTreeEditorTest.h | 1 + test/MissionManager/SimpleMissionItemTest.cc | 2 +- test/MissionManager/SimpleMissionItemTest.h | 5 +- test/UnitTest.qrc | 2 - test/Vehicle/FTPManagerTest.cc | 1 + 17 files changed, 656 insertions(+), 715 deletions(-) delete mode 100644 test/Comms/MockLinkOptionsDlg.qml diff --git a/src/Comms/MockLink/MockConfiguration.cc b/src/Comms/MockLink/MockConfiguration.cc index 926354719c0..76184d43f0b 100644 --- a/src/Comms/MockLink/MockConfiguration.cc +++ b/src/Comms/MockLink/MockConfiguration.cc @@ -39,7 +39,7 @@ void MockConfiguration::copyFrom(const LinkConfiguration *source) Q_ASSERT(source); LinkConfiguration::copyFrom(source); - const MockConfiguration* const mockLinkSource = qobject_cast(source); + const MockConfiguration *const mockLinkSource = qobject_cast(source); Q_ASSERT(mockLinkSource); setFirmwareType(mockLinkSource->firmwareType()); diff --git a/src/Comms/MockLink/MockConfiguration.h b/src/Comms/MockLink/MockConfiguration.h index 51df7b74bde..24c9bd5d55b 100644 --- a/src/Comms/MockLink/MockConfiguration.h +++ b/src/Comms/MockLink/MockConfiguration.h @@ -10,9 +10,10 @@ #pragma once #include "LinkConfiguration.h" -#include "MAVLinkLib.h" #include +#include "MAVLinkLib.h" + Q_DECLARE_LOGGING_CATEGORY(MockConfigurationLog) diff --git a/src/Comms/MockLink/MockLink.cc b/src/Comms/MockLink/MockLink.cc index f3e9596fc1c..4aae43d336f 100644 --- a/src/Comms/MockLink/MockLink.cc +++ b/src/Comms/MockLink/MockLink.cc @@ -10,7 +10,7 @@ #include "MockLink.h" #include "LinkManager.h" -#include "MAVLinkProtocol.h" +#include "MockLinkFTP.h" #include "QGCApplication.h" #include "QGCLoggingCategory.h" @@ -25,17 +25,25 @@ QGC_LOGGING_CATEGORY(MockLinkVerboseLog, "qgc.comms.mocklink.mocklink:verbose") 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, QObject *parent) : LinkInterface(config, parent) - , _missionItemHandler(new MockLinkMissionItemHandler(this, MAVLinkProtocol::instance())) - , _vehicleAltitudeAMSL(_defaultVehicleHomeAltitude) + , _mockConfig(qobject_cast(_config.get())) + , _firmwareType(_mockConfig->firmwareType()) + , _vehicleType(_mockConfig->vehicleType()) + , _sendStatusText(_mockConfig->sendStatusText()) + , _failureMode(_mockConfig->failureMode()) + , _vehicleSystemId(_mockConfig->incrementVehicleId() ? _nextVehicleSystemId++ : _nextVehicleSystemId) + , _vehicleLatitude(_defaultVehicleLatitude + ((_vehicleSystemId - 128) * 0.0001)) + , _vehicleLongitude(_defaultVehicleLongitude + ((_vehicleSystemId - 128) * 0.0001)) + , _boardVendorId(_mockConfig->boardVendorId()) + , _boardProductId(_mockConfig->boardProductId()) + , _missionItemHandler(new MockLinkMissionItemHandler(this)) + , _mockLinkFTP(new MockLinkFTP(_vehicleSystemId, _vehicleComponentId, this)) { // qCDebug(MockLinkLog) << Q_FUNC_INFO << this; // Initialize 5 ADS-B vehicles with different starting conditions _numberOfVehicles + _adsbVehicles.resize(_numberOfVehicles); for (int i = 0; i < _numberOfVehicles; ++i) { ADSBVehicle vehicle{}; vehicle.angle = i * 72; // Different starting directions (angles 0, 72, 144, 216, 288) @@ -48,24 +56,11 @@ MockLink::MockLink(SharedLinkConfigurationPtr &config, QObject *parent) // 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); + _adsbVehicles.append(vehicle); } - MockConfiguration *const mockConfig = qobject_cast(_config.get()); - setFirmwareType(mockConfig->firmwareType()); - _vehicleType = mockConfig->vehicleType(); - setSendStatusText(mockConfig->sendStatusText()); - setFailureMode(mockConfig->failureMode()); - _vehicleSystemId = mockConfig->incrementVehicleId() ? _nextVehicleSystemId++ : _nextVehicleSystemId; - _vehicleLatitude = _defaultVehicleLatitude + ((_vehicleSystemId - 128) * 0.0001); - _vehicleLongitude = _defaultVehicleLongitude + ((_vehicleSystemId - 128) * 0.0001); - _boardVendorId = mockConfig->boardVendorId(); - _boardProductId = mockConfig->boardProductId(); - (void) QObject::connect(this, &MockLink::writeBytesQueuedSignal, this, &MockLink::_writeBytesQueued, Qt::QueuedConnection); - _mockLinkFTP = new MockLinkFTP(_vehicleSystemId, _vehicleComponentId, this); - (void) moveToThread(this); _loadParams(); @@ -89,7 +84,7 @@ bool MockLink::_connect() _connected = true; mavlink_status_t *const mavlinkStatus = mavlink_get_channel_status(mavlinkChannel()); mavlinkStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; - mavlink_status_t *const auxStatus = mavlink_get_channel_status(mavlinkAuxChannel()); + mavlink_status_t *const auxStatus = mavlink_get_channel_status(_getMavlinkAuxChannel()); auxStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; start(); emit connected(); @@ -207,7 +202,7 @@ void MockLink::_run500HzTasks() bool MockLink::_allocateMavlinkChannel() { // should only be called by the LinkManager during setup - Q_ASSERT(!mavlinkAuxChannelIsSet()); + Q_ASSERT(!_mavlinkAuxChannelIsSet()); Q_ASSERT(!mavlinkChannelIsSet()); if (!LinkInterface::_allocateMavlinkChannel()) { @@ -216,11 +211,12 @@ bool MockLink::_allocateMavlinkChannel() } _mavlinkAuxChannel = LinkManager::instance()->allocateMavlinkChannel(); - if (!mavlinkAuxChannelIsSet()) { + if (!_mavlinkAuxChannelIsSet()) { qCWarning(MockLinkLog) << "_allocateMavlinkChannel failed"; LinkInterface::_freeMavlinkChannel(); return false; } + qCDebug(MockLinkLog) << "_allocateMavlinkChannel" << _mavlinkAuxChannel; return true; } @@ -228,7 +224,7 @@ bool MockLink::_allocateMavlinkChannel() void MockLink::_freeMavlinkChannel() { qCDebug(MockLinkLog) << "_freeMavlinkChannel" << _mavlinkAuxChannel; - if (!mavlinkAuxChannelIsSet()) { + if (!_mavlinkAuxChannelIsSet()) { Q_ASSERT(!mavlinkChannelIsSet()); return; } @@ -237,7 +233,7 @@ void MockLink::_freeMavlinkChannel() LinkInterface::_freeMavlinkChannel(); } -bool MockLink::mavlinkAuxChannelIsSet() const +bool MockLink::_mavlinkAuxChannelIsSet() const { return (LinkManager::invalidMavlinkChannel() != _mavlinkAuxChannel); } @@ -522,12 +518,12 @@ void MockLink::_writeBytesQueued(const QByteArray &bytes) return; } - if (bytes.startsWith(QByteArray("\r\r\r"))) { - _inNSH = true; + if (bytes.startsWith(QByteArrayLiteral("\r\r\r"))) { + _inNSH = true; _handleIncomingNSHBytes(&bytes.constData()[3], bytes.length() - 3); } - _handleIncomingMavlinkBytes((uint8_t *)bytes.constData(), bytes.length()); + _handleIncomingMavlinkBytes(reinterpret_cast(bytes.constData()), bytes.length()); } void MockLink::_handleIncomingNSHBytes(const char *bytes, int cBytes) @@ -541,7 +537,7 @@ void MockLink::_handleIncomingNSHBytes(const char *bytes, int cBytes) } if (cBytes > 0) { - qCDebug(MockLinkLog) << "NSH:" << (const char*)bytes; + qCDebug(MockLinkLog) << "NSH:" << bytes; #if 0 // MockLink not quite ready to handle this correctly yet if (strncmp(bytes, "sh /etc/init.d/rc.usb\n", cBytes) == 0) { @@ -559,7 +555,7 @@ void MockLink::_handleIncomingMavlinkBytes(const uint8_t *bytes, int cBytes) QMutexLocker lock(&_mavlinkAuxMutex); for (qint64 i = 0; i < cBytes; i++) { - const int parsed = mavlink_parse_char(mavlinkAuxChannel(), bytes[i], &msg, &comm); + const int parsed = mavlink_parse_char(_getMavlinkAuxChannel(), bytes[i], &msg, &comm); if (!parsed) { continue; } @@ -630,9 +626,9 @@ void MockLink::_handleParamMapRC(const mavlink_message_t &msg) if (paramMapRC.param_index == -1) { qCDebug(MockLinkLog) << QStringLiteral("MockLink - PARAM_MAP_RC: param(%1) tuningID(%2) centerValue(%3) scale(%4) min(%5) max(%6)").arg(paramName).arg(paramMapRC.parameter_rc_channel_index).arg(paramMapRC.param_value0).arg(paramMapRC.scale).arg(paramMapRC.param_value_min).arg(paramMapRC.param_value_max); } else if (paramMapRC.param_index == -2) { - qCDebug(MockLinkLog) << QStringLiteral("MockLink - PARAM_MAP_RC: Clear tuningID(%1)").arg(paramMapRC.parameter_rc_channel_index); + qCDebug(MockLinkLog) << "MockLink - PARAM_MAP_RC: Clear tuningID" << paramMapRC.parameter_rc_channel_index; } else { - qCWarning(MockLinkLog) << QStringLiteral("MockLink - PARAM_MAP_RC: Unsupported param_index(%1)").arg(paramMapRC.param_index); + qCWarning(MockLinkLog) << "MockLink - PARAM_MAP_RC: Unsupported param_index" << paramMapRC.param_index; } } @@ -669,15 +665,12 @@ void MockLink::_setParamFloatUnionIntoMap(int componentId, const QString ¶mN case MAV_PARAM_TYPE_REAL32: paramVariant = QVariant::fromValue(valueUnion.param_float); break; - case MAV_PARAM_TYPE_UINT32: paramVariant = QVariant::fromValue(valueUnion.param_uint32); break; - case MAV_PARAM_TYPE_INT32: paramVariant = QVariant::fromValue(valueUnion.param_int32); break; - case MAV_PARAM_TYPE_UINT16: paramVariant = QVariant::fromValue(valueUnion.param_uint16); break; @@ -796,10 +789,9 @@ void MockLink::_paramRequestListWorker() const int cParameters = _mapParamName2Value[componentId].count(); const QString paramName = _mapParamName2Value[componentId].keys()[_currentParamRequestListParamIndex]; - if ((_failureMode == MockConfiguration::FailMissingParamOnInitialReqest || _failureMode == MockConfiguration::FailMissingParamOnAllRequests) && paramName == _failParam) { + if (((_failureMode == MockConfiguration::FailMissingParamOnInitialReqest) || (_failureMode == MockConfiguration::FailMissingParamOnAllRequests)) && (paramName == _failParam)) { qCDebug(MockLinkLog) << "Skipping param send:" << paramName; } else { - char paramId[MAVLINK_MSG_ID_PARAM_VALUE_LEN]{}; mavlink_message_t responseMsg{}; @@ -845,7 +837,7 @@ void MockLink::_handleParamSet(const mavlink_message_t &msg) mavlink_msg_param_set_decode(&msg, &request); Q_ASSERT(request.target_system == _vehicleSystemId); - int componentId = request.target_component; + const int componentId = request.target_component; // Param may not be null terminated if exactly fits char paramId[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN + 1]{}; @@ -1021,10 +1013,10 @@ void MockLink::_handleInProgressCommandLong(const mavlink_command_long_t &reques &commandAck, request.command, MAV_RESULT_IN_PROGRESS, - 1, // progress - 0, // result_param2 - 0, // target_system - 0 // target_component + 1, // progress + 0, // result_param2 + 0, // target_system + 0 // target_component ); respondWithMavlinkMessage(commandAck); @@ -1036,10 +1028,10 @@ void MockLink::_handleInProgressCommandLong(const mavlink_command_long_t &reques &commandAck, request.command, commandResult, - 0, // progress - 0, // result_param2 - 0, // target_system - 0 // target_component + 0, // progress + 0, // result_param2 + 0, // target_system + 0 // target_component ); respondWithMavlinkMessage(commandAck); } @@ -1050,8 +1042,6 @@ void MockLink::_handleCommandLong(const mavlink_message_t &msg) static bool firstCmdUser3 = true; static bool firstCmdUser4 = true; - bool noAck = false; - mavlink_command_long_t request{}; mavlink_msg_command_long_decode(&msg, &request); @@ -1089,6 +1079,8 @@ void MockLink::_handleCommandLong(const mavlink_message_t &msg) _respondWithAutopilotVersion(); break; case MAV_CMD_REQUEST_MESSAGE: + { + bool noAck = false; if (_handleRequestMessage(request, noAck)) { if (noAck) { return; @@ -1096,6 +1088,7 @@ void MockLink::_handleCommandLong(const mavlink_message_t &msg) commandResult = MAV_RESULT_ACCEPTED; } break; + } case MAV_CMD_NAV_TAKEOFF: _handleTakeoff(request); commandResult = MAV_RESULT_ACCEPTED; @@ -1175,7 +1168,6 @@ void MockLink::sendUnexpectedCommandAck(MAV_CMD command, MAV_RESULT ackResult) void MockLink::_respondWithAutopilotVersion() { - uint8_t customVersion[8]{}; uint32_t flightVersion = 0; #ifndef NO_ARDUPILOT_DIALECT @@ -1201,7 +1193,9 @@ void MockLink::_respondWithAutopilotVersion() #ifndef NO_ARDUPILOT_DIALECT } #endif - const uint64_t capabilities = MAV_PROTOCOL_CAPABILITY_MAVLINK2 | MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY | MAV_PROTOCOL_CAPABILITY_MISSION_INT | (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA ? MAV_PROTOCOL_CAPABILITY_TERRAIN : 0); + + const uint8_t customVersion[8]{}; + const uint64_t capabilities = MAV_PROTOCOL_CAPABILITY_MAVLINK2 | MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY | MAV_PROTOCOL_CAPABILITY_MISSION_INT | ((_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) ? MAV_PROTOCOL_CAPABILITY_TERRAIN : 0); mavlink_message_t msg{}; (void) mavlink_msg_autopilot_version_pack_chan( @@ -1214,9 +1208,9 @@ void MockLink::_respondWithAutopilotVersion() 0, // middleware_sw_version, 0, // os_sw_version, 0, // board_version, - reinterpret_cast(&customVersion), // flight_custom_version, - reinterpret_cast(&customVersion), // middleware_custom_version, - reinterpret_cast(&customVersion), // os_custom_version, + reinterpret_cast(&customVersion), // flight_custom_version, + reinterpret_cast(&customVersion), // middleware_custom_version, + reinterpret_cast(&customVersion), // os_custom_version, _boardVendorId, _boardProductId, 0, // uid @@ -1227,7 +1221,7 @@ void MockLink::_respondWithAutopilotVersion() void MockLink::_sendHomePosition() { - float bogus[4]{}; + const float bogus[4]{}; mavlink_message_t msg{}; (void) mavlink_msg_home_position_pack_chan( @@ -1258,9 +1252,9 @@ void MockLink::_sendGpsRawInt() &msg, timeTick++, // time since boot GPS_FIX_TYPE_3D_FIX, - static_cast(_vehicleLatitude * 1E7), + static_cast(_vehicleLatitude * 1E7), static_cast(_vehicleLongitude * 1E7), - static_cast(_vehicleAltitudeAMSL * 1000), + static_cast(_vehicleAltitudeAMSL * 1000), 3 * 100, // hdop 3 * 100, // vdop UINT16_MAX, // velocity not known @@ -1288,9 +1282,9 @@ void MockLink::_sendGlobalPositionInt() mavlinkChannel(), &msg, timeTick++, // time since boot - static_cast(_vehicleLatitude * 1E7), + static_cast(_vehicleLatitude * 1E7), static_cast(_vehicleLongitude * 1E7), - static_cast(_vehicleAltitudeAMSL * 1000), + static_cast(_vehicleAltitudeAMSL * 1000), static_cast((_vehicleAltitudeAMSL - _defaultVehicleHomeAltitude) * 1000), 0, 0, 0, // no speed sent UINT16_MAX // no heading sent @@ -1314,10 +1308,10 @@ void MockLink::_sendExtendedSysState() void MockLink::_sendChunkedStatusText(uint16_t chunkId, bool missingChunks) { + constexpr int cChunks = 4; - int cChunks = 4; int num = 0; - for (int i=0; igenerate() * 0xFF) / RAND_MAX; - tempFile.write((char *)&byte, 1); - } - tempFile.close(); - return tempFile.fileName(); - } else { + if (!tempFile.open()) { qCWarning(MockLinkLog) << "MockLink::createRandomFile open failed" << tempFile.errorString(); return QString(); } + + for (uint32_t bytesWritten = 0; bytesWritten < byteCount; bytesWritten++) { + const unsigned char byte = (QRandomGenerator::global()->generate() * 0xFF) / RAND_MAX; + (void) tempFile.write(reinterpret_cast(&byte), 1); + } + + tempFile.close(); + return tempFile.fileName(); } void MockLink::_handleLogRequestData(const mavlink_message_t &msg) @@ -1702,7 +1695,7 @@ bool MockLink::_handleRequestMessage(const mavlink_command_long_t &request, bool break; } - uint8_t nullHash[8]{}; + const uint8_t nullHash[8]{}; mavlink_message_t responseMsg{}; (void) mavlink_msg_protocol_version_pack_chan( _vehicleSystemId, @@ -1737,6 +1730,7 @@ bool MockLink::_handleRequestMessage(const mavlink_command_long_t &request, bool noAck = true; return true; } + mavlink_message_t responseMsg{}; (void) mavlink_msg_debug_pack_chan( _vehicleSystemId, @@ -1746,6 +1740,7 @@ bool MockLink::_handleRequestMessage(const mavlink_command_long_t &request, bool 0, 0, 0 ); respondWithMavlinkMessage(responseMsg); + return true; } } @@ -1788,3 +1783,8 @@ void MockLink::simulateConnectionRemoved() _commLost = true; _connectionRemoved(); } + +MockLinkFTP *MockLink::mockLinkFTP() const +{ + return _mockLinkFTP; +} diff --git a/src/Comms/MockLink/MockLink.h b/src/Comms/MockLink/MockLink.h index 95c6a863d66..c8b5d6c8792 100644 --- a/src/Comms/MockLink/MockLink.h +++ b/src/Comms/MockLink/MockLink.h @@ -13,7 +13,6 @@ #include "LinkInterface.h" #include "MAVLinkLib.h" #include "MockConfiguration.h" -#include "MockLinkFTP.h" #include "MockLinkMissionItemHandler.h" #include @@ -22,6 +21,8 @@ #include #include +class MockLinkFTP; + Q_DECLARE_LOGGING_CATEGORY(MockLinkLog) Q_DECLARE_LOGGING_CATEGORY(MockLinkVerboseLog) @@ -36,31 +37,18 @@ class MockLink : public LinkInterface bool isConnected() const final { return _connected; } void disconnect() final; + Q_INVOKABLE void setCommLost(bool commLost) { _commLost = commLost; } + Q_INVOKABLE void simulateConnectionRemoved(); + 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 - /// sometimes it doesn't. Don't ask. This option allows you to configure that behavior - /// for unit testing. - void setAPMMissionResponseMode(bool sendHomePositionOnEmptyList) { _apmSendHomePositionOnEmptyList = sendHomePositionOnEmptyList; } void emitRemoteControlChannelRawChanged(int channel, uint16_t raw); /// Sends the specified mavlink message to QGC void respondWithMavlinkMessage(const mavlink_message_t &msg); - MockLinkFTP *mockLinkFTP() const { return _mockLinkFTP; } - - struct ADSBVehicle { - QGeoCoordinate coordinate; - double angle; - double altitude; // Store unique altitude for each vehicle - }; - std::vector _adsbVehicles; // Store data for multiple ADS-B vehicles + MockLinkFTP *mockLinkFTP() const; /// Sets a failure mode for unit testingqgcm /// @param failureMode Type of failure to simulate @@ -84,8 +72,16 @@ class MockLink : public LinkInterface /// Returns the filename for the simulated log file. Only available after a download is requested. QString logDownloadFile() const { return _logDownloadFilename; } - Q_INVOKABLE void setCommLost(bool commLost) { _commLost = commLost; } - Q_INVOKABLE void simulateConnectionRemoved(); + void clearReceivedMavCommandCounts() { _receivedMavCommandCountMap.clear(); } + int receivedMavCommandCount(MAV_CMD command) const { return _receivedMavCommandCountMap[command]; } + + enum RequestMessageFailureMode_t { + FailRequestMessageNone, + FailRequestMessageCommandAcceptedMsgNotSent, + FailRequestMessageCommandUnsupported, + FailRequestMessageCommandNoResponse, + }; + void setRequestMessageFailureMode(RequestMessageFailureMode_t failureMode) { _requestMessageFailureMode = failureMode; } static MockLink *startPX4MockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); static MockLink *startGenericMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); @@ -106,23 +102,12 @@ class MockLink : public LinkInterface 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, - }; - void setRequestMessageFailureMode(RequestMessageFailureMode_t failureMode) { _requestMessageFailureMode = failureMode; } - signals: void writeBytesQueuedSignal(const QByteArray &bytes); void highLatencyTransmissionEnabledChanged(bool highLatencyTransmissionEnabled); private slots: - /// @brief Called when QGC wants to write bytes to the MAV + /// Called when QGC wants to write bytes to the MAV void _writeBytes(const QByteArray &bytes) final; void _writeBytesQueued(const QByteArray &bytes); @@ -138,17 +123,20 @@ private slots: void run() final; - uint8_t mavlinkAuxChannel() const { return _mavlinkAuxChannel; } - bool mavlinkAuxChannelIsSet() const; + uint8_t _getMavlinkAuxChannel() const { return _mavlinkAuxChannel; } + bool _mavlinkAuxChannelIsSet() const; - void _sendHeartBeat(); - void _sendHighLatency2(); - /// @brief Handle incoming bytes which are meant to be interpreted by the NuttX shell + void _loadParams(); + + /// Convert from a parameter variant to the float value from mavlink_param_union_t + float _floatUnionForParam(int componentId, const QString ¶mName); + void _setParamFloatUnionIntoMap(int componentId, const QString ¶mName, float paramFloat); + + /// Handle incoming bytes which are meant to be interpreted by the NuttX shell void _handleIncomingNSHBytes(const char *bytes, int cBytes); - /// @brief Handle incoming bytes which are meant to be handled by the mavlink protocol + /// Handle incoming bytes which are meant to be handled by the mavlink protocol 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); @@ -164,9 +152,9 @@ private slots: void _handleLogRequestData(const mavlink_message_t &msg); void _handleParamMapRC(const mavlink_message_t &msg); bool _handleRequestMessage(const mavlink_command_long_t &request, bool &noAck); - /// Convert from a parameter variant to the float value from mavlink_param_union_t - float _floatUnionForParam(int componentId, const QString ¶mName); - void _setParamFloatUnionIntoMap(int componentId, const QString ¶mName, float paramFloat); + + void _sendHeartBeat(); + void _sendHighLatency2(); void _sendHomePosition(); void _sendGpsRawInt(); void _sendGlobalPositionInt(); @@ -178,13 +166,14 @@ private slots: void _respondWithAutopilotVersion(); void _sendRCChannels(); /// Sends the next parameter to the vehicle - void _paramRequestListWorker(); - void _logDownloadWorker(); void _sendADSBVehicles(); - void _moveADSBVehicle(int vehicleIndex); void _sendGeneralMetaData(); void _sendRemoteIDArmStatus(); + void _paramRequestListWorker(); + void _logDownloadWorker(); + void _moveADSBVehicle(int vehicleIndex); + static MockLink *_startMockLinkWorker(const QString &configName, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType, bool sendStatusText, MockConfiguration::FailureMode_t failureMode); static MockLink *_startMockLink(MockConfiguration *mockConfig); @@ -192,19 +181,27 @@ private slots: /// @return Fully qualified path to created file static QString _createRandomFile(uint32_t byteCount); - MockLinkMissionItemHandler *_missionItemHandler = nullptr; + const MockConfiguration *_mockConfig = nullptr; + const MAV_AUTOPILOT _firmwareType = MAV_AUTOPILOT_PX4; + const MAV_TYPE _vehicleType = MAV_TYPE_QUADROTOR; + const bool _sendStatusText = false; + const MockConfiguration::FailureMode_t _failureMode = MockConfiguration::FailNone; + const uint8_t _vehicleSystemId = 0; + const double _vehicleLatitude = 0.0; + const double _vehicleLongitude = 0.0; + // 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). + const uint16_t _boardVendorId = 0; + const uint16_t _boardProductId = 0; + MockLinkMissionItemHandler *const _missionItemHandler = nullptr; + MockLinkFTP *const _mockLinkFTP = nullptr; uint8_t _mavlinkAuxChannel = std::numeric_limits::max(); QMutex _mavlinkAuxMutex; - QString _name = QString("MockLink"); bool _connected = false; - - uint8_t _vehicleSystemId = 0; - uint8_t _vehicleComponentId = MAV_COMP_ID_AUTOPILOT1; - bool _inNSH = false; - bool _mavlinkStarted = true; uint8_t _mavBaseMode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; uint32_t _mavCustomMode = PX4CustomMode::MANUAL; @@ -219,49 +216,36 @@ private slots: int32_t _battery2TimeRemaining = _batteryMaxTimeRemaining; MAV_BATTERY_CHARGE_STATE _battery2ChargeState = MAV_BATTERY_CHARGE_STATE_OK; - 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; - - MockLinkFTP *_mockLinkFTP = nullptr; - - bool _sendStatusText = false; - bool _apmSendHomePositionOnEmptyList = false; - MockConfiguration::FailureMode_t _failureMode = MockConfiguration::FailNone; - int _sendHomePositionDelayCount = 10; ///< No home position for 4 seconds int _sendGPSPositionDelayCount = 100; ///< No gps lock for 5 seconds 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 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 = 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 - 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; + struct ADSBVehicle { + QGeoCoordinate coordinate; + double angle = 0.0; + double altitude = 0.0; ///< Store unique altitude for each vehicle + }; + QList _adsbVehicles; ///< Store data for multiple 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 + static int _nextVehicleSystemId; // Vehicle position is set close to default Gazebo vehicle location. This allows for multi-vehicle @@ -271,5 +255,12 @@ private slots: static constexpr double _defaultVehicleHomeAltitude = 488.056; static constexpr const char *_failParam = "COM_FLTMODE6"; + + static constexpr uint8_t _vehicleComponentId = MAV_COMP_ID_AUTOPILOT1; + + static constexpr uint16_t _logDownloadLogId = 0; ///< Id of siumulated log file + static constexpr uint32_t _logDownloadFileSize = 1000; ///< Size of simulated log file + + static constexpr bool _mavlinkStarted = true; }; diff --git a/src/Comms/MockLink/MockLinkFTP.cc b/src/Comms/MockLink/MockLinkFTP.cc index 3dd9c00d83d..68af3ee7c44 100644 --- a/src/Comms/MockLink/MockLinkFTP.cc +++ b/src/Comms/MockLink/MockLinkFTP.cc @@ -7,55 +7,63 @@ * ****************************************************************************/ - #include "MockLinkFTP.h" #include "MockLink.h" +#include "QGCLoggingCategory.h" #include "QGCTemporaryFile.h" -MockLinkFTP::MockLinkFTP(uint8_t systemIdServer, uint8_t componentIdServer, MockLink* mockLink) - : _systemIdServer (systemIdServer) +QGC_LOGGING_CATEGORY(MockLinkFTPLog, "MockLinkMissionItemHandlerLog") + +MockLinkFTP::MockLinkFTP(uint8_t systemIdServer, uint8_t componentIdServer, MockLink *mockLink) + : QObject(mockLink) + , _systemIdServer(systemIdServer) , _componentIdServer(componentIdServer) - , _mockLink (mockLink) + , _mockLink(mockLink) +{ + // qCDebug(MockLinkFTPLog) << Q_FUNC_INFO << this; +} + +MockLinkFTP::~MockLinkFTP() { - srand(0); // make sure unit tests are deterministic + // qCDebug(MockLinkFTPLog) << Q_FUNC_INFO << this; } -void MockLinkFTP::ensureNullTemination(MavlinkFTP::Request* request) +void MockLinkFTP::ensureNullTemination(MavlinkFTP::Request *request) { if (request->hdr.size < sizeof(request->data)) { request->data[request->hdr.size] = '\0'; - } else { - request->data[sizeof(request->data)-1] = '\0'; + request->data[sizeof(request->data) - 1] = '\0'; } } -/// @brief Handles List command requests. Only supports root folder paths. -/// File list returned is set using the setFileList method. -void MockLinkFTP::_listCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request* request, uint16_t seqNumber) +void MockLinkFTP::_listCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request *request, uint16_t seqNumber) { MavlinkFTP::Request ackResponse{}; - QString path; - uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber); - ensureNullTemination(request); + const uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber); + // We only support root path - path = (char *)&request->data[0]; + const QString path = reinterpret_cast(&request->data[0]); if (!path.isEmpty() && path != "/") { _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrFail, outgoingSeqNumber, MavlinkFTP::kCmdListDirectory); return; } - + if (request->hdr.offset > 0) { if (_errMode == errModeNakSecondResponse) { // Nak error all subsequent requests _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrFail, outgoingSeqNumber, MavlinkFTP::kCmdListDirectory); return; - } else if (_errMode == errModeNoSecondResponse) { + } + + if (_errMode == errModeNoSecondResponse) { // No response for all subsequent requests return; - } else if (_errMode == errModeNoSecondResponseAllowRetry) { + } + + if (_errMode == errModeNoSecondResponseAllowRetry) { // No response to this request, subsequent requests will succeed _errMode = errModeNone; return; @@ -70,53 +78,53 @@ void MockLinkFTP::_listCommand(uint8_t senderSystemId, uint8_t senderComponentId // MockLink sends two directory entries per packet for a maximum of 3 packets, 6 total entries if (request->hdr.offset <= 5) { - char *bufPtr = (char *)&ackResponse.data[0]; + char *bufPtr = reinterpret_cast(&ackResponse.data[0]); QString dirEntry = QStringLiteral("Ffile%1.txt").arg(request->hdr.offset); auto cchDirEntry = dirEntry.length(); - strncpy(bufPtr, dirEntry.toStdString().c_str(), cchDirEntry); + (void) strncpy(bufPtr, dirEntry.toStdString().c_str(), cchDirEntry); ackResponse.hdr.size += dirEntry.length() + 1; bufPtr += cchDirEntry + 1; dirEntry = QStringLiteral("Ffile%1.txt").arg(request->hdr.offset + 1); cchDirEntry = dirEntry.length(); - strncpy(bufPtr, dirEntry.toStdString().c_str(), cchDirEntry); + (void) strncpy(bufPtr, dirEntry.toStdString().c_str(), cchDirEntry); ackResponse.hdr.size += dirEntry.length() + 1; } else { ackResponse.hdr.opcode = MavlinkFTP::kRspNak; ackResponse.data[0] = MavlinkFTP::kErrEOF; ackResponse.hdr.size = 1; } + _sendResponse(senderSystemId, senderComponentId, &ackResponse, outgoingSeqNumber); } -void MockLinkFTP::_openCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request* request, uint16_t seqNumber) +void MockLinkFTP::_openCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request *request, uint16_t seqNumber) { MavlinkFTP::Request response{}; - QString path; - uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber); - QString tmpFilename; - ensureNullTemination(request); + const QString path = reinterpret_cast(request->data); + + const uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber); - size_t cchPath = strnlen((char *)request->data, sizeof(request->data)); + const size_t cchPath = strnlen(reinterpret_cast(request->data), sizeof(request->data)); Q_ASSERT(cchPath != sizeof(request->data)); Q_UNUSED(cchPath); // Fix initialized-but-not-referenced warning on release builds - path = (char *)request->data; _currentFile.close(); - QString sizePrefix = sizeFilenamePrefix; + QString tmpFilename; + const QString sizePrefix = sizeFilenamePrefix; if (path.startsWith(sizePrefix)) { - QString sizeString = path.right(path.length() - sizePrefix.length()); + const QString sizeString = path.right(path.length() - sizePrefix.length()); tmpFilename = _createTestTempFile(sizeString.toInt()); } else if (path == "/general.json") { - tmpFilename = ":MockLink/General.MetaData.json"; + tmpFilename = QStringLiteral(":MockLink/General.MetaData.json"); } else if (path == "/general.json.xz") { - tmpFilename = ":MockLink/General.MetaData.json.xz"; + tmpFilename = QStringLiteral(":MockLink/General.MetaData.json.xz"); } else if (path == "/parameter.json") { - tmpFilename = ":MockLink/Parameter.MetaData.json"; + tmpFilename = QStringLiteral(":MockLink/Parameter.MetaData.json"); } else if (path == "/parameter.json.xz") { - tmpFilename = ":MockLink/Parameter.MetaData.json.xz"; - } else if (_BinParamFileEnabled && path == "@PARAM/param.pck") { + tmpFilename = QStringLiteral(":MockLink/Parameter.MetaData.json.xz"); + } else if (_BinParamFileEnabled && (path == "@PARAM/param.pck")) { tmpFilename = ":MockLink/Arduplane.params.ftp.bin"; } @@ -130,99 +138,99 @@ void MockLinkFTP::_openCommand(uint8_t senderSystemId, uint8_t senderComponentId _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrFailFileNotFound, outgoingSeqNumber, MavlinkFTP::kCmdOpenFileRO); return; } - - response.hdr.opcode = MavlinkFTP::kRspAck; + + response.hdr.opcode = MavlinkFTP::kRspAck; response.hdr.req_opcode = MavlinkFTP::kCmdOpenFileRO; - response.hdr.session = _sessionId; - + response.hdr.session = _sessionId; + // Data contains file length response.hdr.size = sizeof(uint32_t); - /* Ardupilot sends constant wrong file size for parameter file due to dynamic on the fly generation */ - response.openFileLength = (path == "@PARAM/param.pck" ? 1024*1024 : _currentFile.size()); - + + // Ardupilot sends constant wrong file size for parameter file due to dynamic on the fly generation + response.openFileLength = ((path == "@PARAM/param.pck") ? qPow(1024, 2) : _currentFile.size()); + _sendResponse(senderSystemId, senderComponentId, &response, outgoingSeqNumber); } -void MockLinkFTP::_readCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request* request, uint16_t seqNumber) +void MockLinkFTP::_readCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request *request, uint16_t seqNumber) { MavlinkFTP::Request response{}; - uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber); + const uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber); if (request->hdr.session != _sessionId) { _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrInvalidSession, outgoingSeqNumber, MavlinkFTP::kCmdReadFile); return; } - - uint32_t readOffset = request->hdr.offset; // offset into file for reading + const uint32_t readOffset = request->hdr.offset; // offset into file for reading if (readOffset != 0) { // If we get here it means the client is requesting additional data past the first request if (_errMode == errModeNakSecondResponse) { // Nak error all subsequent requests _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrFail, outgoingSeqNumber, MavlinkFTP::kCmdReadFile); return; - } else if (_errMode == errModeNoSecondResponse) { + } + + if (_errMode == errModeNoSecondResponse) { // No rsponse for all subsequent requests return; } } - + if (readOffset >= _currentFile.size()) { _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrEOF, outgoingSeqNumber, MavlinkFTP::kCmdReadFile); return; } - - uint8_t cBytesToRead = (uint8_t)qMin((qint64)sizeof(response.data), _currentFile.size() - readOffset); - _currentFile.seek(readOffset); - QByteArray bytes = _currentFile.read(cBytesToRead); - memcpy(response.data, bytes.constData(), cBytesToRead); - + + const uint8_t cBytesToRead = static_cast(qMin(static_cast(sizeof(response.data)), _currentFile.size() - readOffset)); + (void) _currentFile.seek(readOffset); + const QByteArray bytes = _currentFile.read(cBytesToRead); + (void) memcpy(response.data, bytes.constData(), cBytesToRead); + // We should always have written something, otherwise there is something wrong with the code above Q_ASSERT(cBytesToRead); - - response.hdr.session = _sessionId; - response.hdr.size = cBytesToRead; - response.hdr.offset = request->hdr.offset; - response.hdr.opcode = MavlinkFTP::kRspAck; + + response.hdr.session = _sessionId; + response.hdr.size = cBytesToRead; + response.hdr.offset = request->hdr.offset; + response.hdr.opcode = MavlinkFTP::kRspAck; response.hdr.req_opcode = MavlinkFTP::kCmdReadFile; _sendResponse(senderSystemId, senderComponentId, &response, outgoingSeqNumber); } -void MockLinkFTP::_burstReadCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request* request, uint16_t seqNumber) +void MockLinkFTP::_burstReadCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request *request, uint16_t seqNumber) { - uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber); MavlinkFTP::Request response{}; + uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber); if (request->hdr.session != _sessionId) { _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrFail, outgoingSeqNumber, MavlinkFTP::kCmdBurstReadFile); return; } - - int burstMax = 10; - int burstCount = 1; - uint32_t burstOffset = request->hdr.offset; - while (burstOffset < _currentFile.size() && burstCount++ < burstMax) { - _currentFile.seek(burstOffset); + constexpr int burstMax = 10; + int burstCount = 1; + uint32_t burstOffset = request->hdr.offset; - uint8_t cBytes = (uint8_t)qMin((qint64)sizeof(response.data), _currentFile.size() - burstOffset); - QByteArray bytes = _currentFile.read(cBytes); + while ((burstOffset < _currentFile.size()) && (burstCount++ < burstMax)) { + _currentFile.seek(burstOffset); - // We should always have written something, otherwise there is something wrong with the code above - Q_ASSERT(cBytes); + const uint8_t cBytes = static_cast(qMin(static_cast(sizeof(response.data)), _currentFile.size() - burstOffset)); + const QByteArray bytes = _currentFile.read(cBytes); + Q_ASSERT(cBytes); // We should always have written something, otherwise there is something wrong with the code above - memcpy(response.data, bytes.constData(), cBytes); + (void) memcpy(response.data, bytes.constData(), cBytes); - response.hdr.session = _sessionId; - response.hdr.size = cBytes; - response.hdr.offset = burstOffset; - response.hdr.opcode = MavlinkFTP::kRspAck; - response.hdr.req_opcode = MavlinkFTP::kCmdBurstReadFile; - response.hdr.burstComplete = burstCount == burstMax ? 1 : 0; + response.hdr.session = _sessionId; + response.hdr.size = cBytes; + response.hdr.offset = burstOffset; + response.hdr.opcode = MavlinkFTP::kRspAck; + response.hdr.req_opcode = MavlinkFTP::kCmdBurstReadFile; + response.hdr.burstComplete = (burstCount == burstMax) ? 1 : 0; _sendResponse(senderSystemId, senderComponentId, &response, outgoingSeqNumber); - + outgoingSeqNumber = _nextSeqNumber(outgoingSeqNumber); burstOffset += cBytes; } @@ -233,15 +241,15 @@ void MockLinkFTP::_burstReadCommand(uint8_t senderSystemId, uint8_t senderCompon } } -void MockLinkFTP::_terminateCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request* request, uint16_t seqNumber) +void MockLinkFTP::_terminateCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request *request, uint16_t seqNumber) { - uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber); + const uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber); if (request->hdr.session != _sessionId) { _sendNak(senderSystemId, senderComponentId, MavlinkFTP::kErrInvalidSession, outgoingSeqNumber, MavlinkFTP::kCmdTerminateSession); return; } - + _sendAck(senderSystemId, senderComponentId, outgoingSeqNumber, MavlinkFTP::kCmdTerminateSession); emit terminateCommandReceived(); @@ -249,62 +257,63 @@ void MockLinkFTP::_terminateCommand(uint8_t senderSystemId, uint8_t senderCompon void MockLinkFTP::_resetCommand(uint8_t senderSystemId, uint8_t senderComponentId, uint16_t seqNumber) { - uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber); - + const uint16_t outgoingSeqNumber = _nextSeqNumber(seqNumber); + _currentFile.close(); _currentFile.remove(); _sendAck(senderSystemId, senderComponentId, outgoingSeqNumber, MavlinkFTP::kCmdResetSessions); - + emit resetCommandReceived(); } -void MockLinkFTP::mavlinkMessageReceived(const mavlink_message_t& message) +void MockLinkFTP::mavlinkMessageReceived(const mavlink_message_t &message) { if (message.msgid != MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) { return; } - - MavlinkFTP::Request ackResponse{}; - mavlink_file_transfer_protocol_t requestFTP; + mavlink_file_transfer_protocol_t requestFTP{}; mavlink_msg_file_transfer_protocol_decode(&message, &requestFTP); - + if (requestFTP.target_system != _systemIdServer) { return; } - MavlinkFTP::Request* request = (MavlinkFTP::Request*)&requestFTP.payload[0]; + MavlinkFTP::Request *request = reinterpret_cast(&requestFTP.payload[0]); // kCmdOpenFileRO and kCmdResetSessions don't support retry so we can't drop those - if (_randomDropsEnabled && request->hdr.opcode != MavlinkFTP::kCmdOpenFileRO && request->hdr.opcode != MavlinkFTP::kCmdResetSessions) { + if (_randomDropsEnabled && (request->hdr.opcode != MavlinkFTP::kCmdOpenFileRO) && (request->hdr.opcode != MavlinkFTP::kCmdResetSessions)) { if ((rand() % 5) == 0) { - qDebug() << "MockLinkFTP: Random drop of incoming packet"; + qCDebug(MockLinkFTPLog) << "MockLinkFTP: Random drop of incoming packet"; return; } } - if (_lastReplyValid && request->hdr.seqNumber == _lastReplySequence - 1) { + if (_lastReplyValid && (request->hdr.seqNumber == (_lastReplySequence - 1))) { // This is the same request as the one we replied to last. It means the (n)ack got lost, and the GCS // resent the request - qDebug() << "MockLinkFTP: resending response"; + qCDebug(MockLinkFTPLog) << "MockLinkFTP: resending response"; _mockLink->respondWithMavlinkMessage(_lastReply); return; } - uint16_t incomingSeqNumber = request->hdr.seqNumber; - uint16_t outgoingSeqNumber = _nextSeqNumber(incomingSeqNumber); - - if (request->hdr.opcode != MavlinkFTP::kCmdResetSessions && request->hdr.opcode != MavlinkFTP::kCmdTerminateSession) { + const uint16_t incomingSeqNumber = request->hdr.seqNumber; + const uint16_t outgoingSeqNumber = _nextSeqNumber(incomingSeqNumber); + + if ((request->hdr.opcode != MavlinkFTP::kCmdResetSessions) && (request->hdr.opcode != MavlinkFTP::kCmdTerminateSession)) { if (_errMode == errModeNoResponse) { // Don't respond to any requests, this shold cause the client to eventually timeout waiting for the ack return; - } else if (_errMode == errModeNakResponse) { + } + + if (_errMode == errModeNakResponse) { // Nak all requests, the actual error send back doesn't really matter as long as it's an error - _sendNak(message.sysid, message.compid, MavlinkFTP::kErrFail, outgoingSeqNumber, (MavlinkFTP::OpCode_t)request->hdr.opcode); + _sendNak(message.sysid, message.compid, MavlinkFTP::kErrFail, outgoingSeqNumber, static_cast(request->hdr.opcode)); return; } } + MavlinkFTP::Request ackResponse{}; switch (request->hdr.opcode) { case MavlinkFTP::kCmdNone: // ignored, always acked @@ -313,48 +322,40 @@ void MockLinkFTP::mavlinkMessageReceived(const mavlink_message_t& message) ackResponse.hdr.size = 0; _sendResponse(message.sysid, message.compid, &ackResponse, outgoingSeqNumber); break; - case MavlinkFTP::kCmdListDirectory: _listCommand(message.sysid, message.compid, request, incomingSeqNumber); break; - case MavlinkFTP::kCmdOpenFileRO: _openCommand(message.sysid, message.compid, request, incomingSeqNumber); break; - case MavlinkFTP::kCmdReadFile: _readCommand(message.sysid, message.compid, request, incomingSeqNumber); break; - case MavlinkFTP::kCmdBurstReadFile: _burstReadCommand(message.sysid, message.compid, request, incomingSeqNumber); break; - case MavlinkFTP::kCmdTerminateSession: _terminateCommand(message.sysid, message.compid, request, incomingSeqNumber); break; - case MavlinkFTP::kCmdResetSessions: _resetCommand(message.sysid, message.compid, incomingSeqNumber); break; - default: // nack for all NYI opcodes - _sendNak(message.sysid, message.compid, MavlinkFTP::kErrUnknownCommand, outgoingSeqNumber, (MavlinkFTP::OpCode_t)request->hdr.opcode); + _sendNak(message.sysid, message.compid, MavlinkFTP::kErrUnknownCommand, outgoingSeqNumber, static_cast(request->hdr.opcode)); break; } } -/// @brief Sends an Ack void MockLinkFTP::_sendAck(uint8_t targetSystemId, uint8_t targetComponentId, uint16_t seqNumber, MavlinkFTP::OpCode_t reqOpcode) { MavlinkFTP::Request ackResponse{}; - - ackResponse.hdr.opcode = MavlinkFTP::kRspAck; - ackResponse.hdr.req_opcode = reqOpcode; - ackResponse.hdr.session = _sessionId; - ackResponse.hdr.size = 0; - + + ackResponse.hdr.opcode = MavlinkFTP::kRspAck; + ackResponse.hdr.req_opcode = reqOpcode; + ackResponse.hdr.session = _sessionId; + ackResponse.hdr.size = 0; + _sendResponse(targetSystemId, targetComponentId, &ackResponse, seqNumber); } @@ -362,12 +363,12 @@ void MockLinkFTP::_sendNak(uint8_t targetSystemId, uint8_t targetComponentId, Ma { MavlinkFTP::Request nakResponse{}; - nakResponse.hdr.opcode = MavlinkFTP::kRspNak; - nakResponse.hdr.req_opcode = reqOpcode; - nakResponse.hdr.session = _sessionId; - nakResponse.hdr.size = 1; - nakResponse.data[0] = error; - + nakResponse.hdr.opcode = MavlinkFTP::kRspNak; + nakResponse.hdr.req_opcode = reqOpcode; + nakResponse.hdr.session = _sessionId; + nakResponse.hdr.size = 1; + nakResponse.data[0] = error; + _sendResponse(targetSystemId, targetComponentId, &nakResponse, seqNumber); } @@ -375,62 +376,67 @@ void MockLinkFTP::_sendNakErrno(uint8_t targetSystemId, uint8_t targetComponentI { MavlinkFTP::Request nakResponse{}; - nakResponse.hdr.opcode = MavlinkFTP::kRspNak; - nakResponse.hdr.req_opcode = reqOpcode; - nakResponse.hdr.session = _sessionId; - nakResponse.hdr.size = 2; - nakResponse.data[0] = MavlinkFTP::kErrFailErrno; - nakResponse.data[1] = nakErrno; + nakResponse.hdr.opcode = MavlinkFTP::kRspNak; + nakResponse.hdr.req_opcode = reqOpcode; + nakResponse.hdr.session = _sessionId; + nakResponse.hdr.size = 2; + nakResponse.data[0] = MavlinkFTP::kErrFailErrno; + nakResponse.data[1] = nakErrno; _sendResponse(targetSystemId, targetComponentId, &nakResponse, seqNumber); } -/// @brief Emits a Request through the messageReceived signal. -void MockLinkFTP::_sendResponse(uint8_t targetSystemId, uint8_t targetComponentId, MavlinkFTP::Request* request, uint16_t seqNumber) + +void MockLinkFTP::_sendResponse(uint8_t targetSystemId, uint8_t targetComponentId, MavlinkFTP::Request *request, uint16_t seqNumber) { - request->hdr.seqNumber = seqNumber; - _lastReplySequence = seqNumber; - _lastReplyValid = true; - - mavlink_msg_file_transfer_protocol_pack_chan(_systemIdServer, // System ID - _componentIdServer, // Component ID - _mockLink->mavlinkChannel(), - &_lastReply, // Mavlink Message to pack into - 0, // Target network - targetSystemId, - targetComponentId, - (uint8_t*)request); // Payload + request->hdr.seqNumber = seqNumber; + _lastReplySequence = seqNumber; + _lastReplyValid = true; + + (void) mavlink_msg_file_transfer_protocol_pack_chan( + _systemIdServer, // System ID + _componentIdServer, // Component ID + _mockLink->mavlinkChannel(), + &_lastReply, // Mavlink Message to pack into + 0, // Target network + targetSystemId, + targetComponentId, + reinterpret_cast(request) // Payload + ); // kCmdOpenFileRO and kCmdResetSessions don't support retry so we can't drop those - if (_randomDropsEnabled && request->hdr.req_opcode != MavlinkFTP::kCmdOpenFileRO && request->hdr.req_opcode != MavlinkFTP::kCmdResetSessions) { + if (_randomDropsEnabled && (request->hdr.req_opcode != MavlinkFTP::kCmdOpenFileRO) && (request->hdr.req_opcode != MavlinkFTP::kCmdResetSessions)) { if ((rand() % 5) == 0) { - qDebug() << "MockLinkFTP: Random drop of outgoing packet"; + qCDebug(MockLinkFTPLog) << "MockLinkFTP: Random drop of outgoing packet"; return; } } - + _mockLink->respondWithMavlinkMessage(_lastReply); } -/// @brief Generates the next sequence number given an incoming sequence number. Handles generating -/// bad sequence numbers when errModeBadSequence is set. -uint16_t MockLinkFTP::_nextSeqNumber(uint16_t seqNumber) + +uint16_t MockLinkFTP::_nextSeqNumber(uint16_t seqNumber) const { uint16_t outgoingSeqNumber = seqNumber + 1; - + if (_errMode == errModeBadSequence) { outgoingSeqNumber++; } + return outgoingSeqNumber; } QString MockLinkFTP::_createTestTempFile(int size) { QGCTemporaryFile tmpFile("MockLinkFTPTestCase"); - tmpFile.open(QIODevice::WriteOnly | QIODevice::Truncate); - for (int i=0; i +#include +#include +#include + #include "MAVLinkFTP.h" -#include -#include +Q_DECLARE_LOGGING_CATEGORY(MockLinkFTPLog) class MockLink; @@ -21,16 +24,23 @@ class MockLink; class MockLinkFTP : public QObject { Q_OBJECT - + public: - MockLinkFTP(uint8_t systemIdServer, uint8_t componentIdServer, MockLink* mockLink); - - /// @brief Sets the list of files returned by the List command. Prepend names with F or D + MockLinkFTP(uint8_t systemIdServer, uint8_t componentIdServer, MockLink *mockLink); + ~MockLinkFTP(); + + /// Sets the list of files returned by the List command. Prepend names with F or D /// to indicate (F)ile or (D)irectory. - void setFileList(QStringList& fileList) { _fileList = fileList; } - - /// @brief By calling setErrorMode with one of these modes you can cause the server to simulate an error. - typedef enum { + void setFileList(const QStringList &fileList) { _fileList = fileList; } + + /// Called to handle an FTP message + void mavlinkMessageReceived(const mavlink_message_t &message); + + void enableRandromDrops(bool enable) { _randomDropsEnabled = enable; } + void enableBinParamFile(bool enable) { _BinParamFileEnabled = enable; } + + /// By calling setErrorMode with one of these modes you can cause the server to simulate an error. + enum ErrorMode_t { errModeNone, ///< No error, respond correctly errModeNoResponse, ///< No response to any request, client should eventually time out with no Ack errModeNakResponse, ///< Nak all requests @@ -38,12 +48,12 @@ class MockLinkFTP : public QObject errModeNoSecondResponseAllowRetry, ///< No response to subsequent request to initial command, error will be cleared after this so retry will succeed errModeNakSecondResponse, ///< Nak subsequent request to initial command errModeBadSequence ///< Return response with bad sequence number - } ErrorMode_t; - - /// @brief Sets the error mode for command responses. This allows you to simulate various server errors. + }; + + /// Sets the error mode for command responses. This allows you to simulate various server errors. void setErrorMode(ErrorMode_t errMode) { _errMode = errMode; }; - - /// @brief Array of failure modes you can cycle through for testing. By looping through this array you can avoid + + /// Array of failure modes you can cycle through for testing. By looping through this array you can avoid /// hardcoding the specific error modes in your unit test. This way when new error modes are added your unit test /// code may not need to be modified. static constexpr const ErrorMode_t rgFailureModes[] = { @@ -53,55 +63,55 @@ class MockLinkFTP : public QObject errModeNakSecondResponse, errModeBadSequence, }; - - /// @brief The number of ErrorModes in the rgFailureModes array. - static constexpr const size_t cFailureModes = sizeof(MockLinkFTP::rgFailureModes) / sizeof(MockLinkFTP::rgFailureModes[0]); - - /// Called to handle an FTP message - void mavlinkMessageReceived(const mavlink_message_t& message); - void enableRandromDrops(bool enable) { _randomDropsEnabled = enable; } - void enableBinParamFile(bool enable) { _BinParamFileEnabled = enable; } + /// The number of ErrorModes in the rgFailureModes array. + static constexpr const size_t cFailureModes = std::size(MockLinkFTP::rgFailureModes); - static constexpr const char* sizeFilenamePrefix = "mocklink-size-"; + static constexpr const char *sizeFilenamePrefix = "mocklink-size-"; signals: /// You can connect to this signal to be notified when the server receives a Terminate command. - void terminateCommandReceived(void); - + void terminateCommandReceived(); + /// You can connect to this signal to be notified when the server receives a Reset command. - void resetCommandReceived(void); - + void resetCommandReceived(); + private: - void _sendAck (uint8_t targetSystemId, uint8_t targetComponentId, uint16_t seqNumber, MavlinkFTP::OpCode_t reqOpCode); - void _sendNak (uint8_t targetSystemId, uint8_t targetComponentId, MavlinkFTP::ErrorCode_t error, uint16_t seqNumber, MavlinkFTP::OpCode_t reqOpCode); - void _sendNakErrno (uint8_t targetSystemId, uint8_t targetComponentId, uint8_t nakErrno, uint16_t seqNumber, MavlinkFTP::OpCode_t reqOpCode); - void _sendResponse (uint8_t targetSystemId, uint8_t targetComponentId, MavlinkFTP::Request* request, uint16_t seqNumber); - void _listCommand (uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request* request, uint16_t seqNumber); - void _openCommand (uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request* request, uint16_t seqNumber); - void _readCommand (uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request* request, uint16_t seqNumber); - void _burstReadCommand (uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request* request, uint16_t seqNumber); - void _terminateCommand (uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request* request, uint16_t seqNumber); - void _resetCommand (uint8_t senderSystemId, uint8_t senderComponentId, uint16_t seqNumber); - uint16_t _nextSeqNumber (uint16_t seqNumber); - QString _createTestTempFile (int size); - + /// Sends an Ack + void _sendAck(uint8_t targetSystemId, uint8_t targetComponentId, uint16_t seqNumber, MavlinkFTP::OpCode_t reqOpCode); + void _sendNak(uint8_t targetSystemId, uint8_t targetComponentId, MavlinkFTP::ErrorCode_t error, uint16_t seqNumber, MavlinkFTP::OpCode_t reqOpCode); + void _sendNakErrno(uint8_t targetSystemId, uint8_t targetComponentId, uint8_t nakErrno, uint16_t seqNumber, MavlinkFTP::OpCode_t reqOpCode); + /// Emits a Request through the messageReceived signal. + void _sendResponse(uint8_t targetSystemId, uint8_t targetComponentId, MavlinkFTP::Request *request, uint16_t seqNumber); + /// Handles List command requests. Only supports root folder paths. + /// File list returned is set using the setFileList method. + void _listCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request *request, uint16_t seqNumber); + void _openCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request *request, uint16_t seqNumber); + void _readCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request *request, uint16_t seqNumber); + void _burstReadCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request *request, uint16_t seqNumber); + void _terminateCommand(uint8_t senderSystemId, uint8_t senderComponentId, MavlinkFTP::Request *request, uint16_t seqNumber); + void _resetCommand(uint8_t senderSystemId, uint8_t senderComponentId, uint16_t seqNumber); + /// Generates the next sequence number given an incoming sequence number. Handles generating + /// bad sequence numbers when errModeBadSequence is set. + uint16_t _nextSeqNumber(uint16_t seqNumber) const; + static QString _createTestTempFile(int size); + /// if request is a string, this ensures it's null-terminated - static void ensureNullTemination(MavlinkFTP::Request* request); - - QStringList _fileList; ///< List of files returned by List command - - QFile _currentFile; - ErrorMode_t _errMode = errModeNone; ///< Currently set error mode, as specified by setErrorMode - const uint8_t _systemIdServer; ///< System ID for server - const uint8_t _componentIdServer; ///< Component ID for server - MockLink* _mockLink; ///< MockLink to communicate through - bool _lastReplyValid = false; - uint16_t _lastReplySequence = 0; - mavlink_message_t _lastReply; - bool _randomDropsEnabled = false; - bool _BinParamFileEnabled = false; - - static const uint8_t _sessionId = 1; ///< We only support a single fixed session + static void ensureNullTemination(MavlinkFTP::Request *request); + + const uint8_t _systemIdServer; ///< System ID for server + const uint8_t _componentIdServer; ///< Component ID for server + MockLink *_mockLink; ///< MockLink to communicate through + + bool _BinParamFileEnabled = false; + bool _lastReplyValid = false; + bool _randomDropsEnabled = false; + ErrorMode_t _errMode = errModeNone; ///< Currently set error mode, as specified by setErrorMode + mavlink_message_t _lastReply{}; + QFile _currentFile; + QStringList _fileList; ///< List of files returned by List command + uint16_t _lastReplySequence = 0; + + static constexpr uint8_t _sessionId = 1; ///< We only support a single fixed session }; diff --git a/src/Comms/MockLink/MockLinkMissionItemHandler.cc b/src/Comms/MockLink/MockLinkMissionItemHandler.cc index 2cdaeecd5c8..53caa78760d 100644 --- a/src/Comms/MockLink/MockLinkMissionItemHandler.cc +++ b/src/Comms/MockLink/MockLinkMissionItemHandler.cc @@ -7,92 +7,75 @@ * ****************************************************************************/ - #include "MockLinkMissionItemHandler.h" -#include "MockLink.h" + #include "MAVLinkProtocol.h" +#include "MockLink.h" #include "QGCLoggingCategory.h" -#include +QGC_LOGGING_CATEGORY(MockLinkMissionItemHandlerLog, "qgc.comms.mocklink.mocklinkmissionitemhandler") -QGC_LOGGING_CATEGORY(MockLinkMissionItemHandlerLog, "MockLinkMissionItemHandlerLog") - -MockLinkMissionItemHandler::MockLinkMissionItemHandler(MockLink* mockLink, MAVLinkProtocol* mavlinkProtocol) - : _mockLink (mockLink) - , _missionItemResponseTimer (nullptr) - , _failureMode (FailNone) - , _sendHomePositionOnEmptyList (false) - , _mavlinkProtocol (mavlinkProtocol) - , _failReadRequestListFirstResponse (true) - , _failReadRequest1FirstResponse (true) - , _failWriteMissionCountFirstResponse (true) +MockLinkMissionItemHandler::MockLinkMissionItemHandler(MockLink *mockLink) + : QObject(mockLink) + , _mockLink(mockLink) { + // qCDebug(MockLinkMissionItemHandlerLog) << Q_FUNC_INFO << this; + Q_ASSERT(mockLink); + + (void) connect(&_missionItemResponseTimer, &QTimer::timeout, this, &MockLinkMissionItemHandler::_missionItemResponseTimeout); } MockLinkMissionItemHandler::~MockLinkMissionItemHandler() { - + // qCDebug(MockLinkMissionItemHandlerLog) << Q_FUNC_INFO << this; } -void MockLinkMissionItemHandler::_startMissionItemResponseTimer(void) +void MockLinkMissionItemHandler::_startMissionItemResponseTimer() { - if (!_missionItemResponseTimer) { - _missionItemResponseTimer = new QTimer(); - connect(_missionItemResponseTimer, &QTimer::timeout, this, &MockLinkMissionItemHandler::_missionItemResponseTimeout); - } - _missionItemResponseTimer->start(500); + _missionItemResponseTimer.start(500); } -bool MockLinkMissionItemHandler::handleMessage(const mavlink_message_t& msg) +bool MockLinkMissionItemHandler::handleMessage(const mavlink_message_t &msg) { switch (msg.msgid) { case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: _handleMissionRequestList(msg); break; - case MAVLINK_MSG_ID_MISSION_REQUEST_INT: _handleMissionRequest(msg); break; - case MAVLINK_MSG_ID_MISSION_ITEM_INT: _handleMissionItem(msg); break; - case MAVLINK_MSG_ID_MISSION_COUNT: _handleMissionCount(msg); break; - case MAVLINK_MSG_ID_MISSION_ACK: // Acks are received back for each MISSION_ITEM message break; - case MAVLINK_MSG_ID_MISSION_SET_CURRENT: // Sets the currently active mission item break; - case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: _handleMissionClearAll(msg); break; - default: return false; } - + return true; } -void MockLinkMissionItemHandler::_handleMissionClearAll(const mavlink_message_t& msg) +void MockLinkMissionItemHandler::_handleMissionClearAll(const mavlink_message_t &msg) { - - mavlink_mission_clear_all_t clearAll; - + mavlink_mission_clear_all_t clearAll{}; mavlink_msg_mission_clear_all_decode(&msg, &clearAll); Q_ASSERT(clearAll.target_system == _mockLink->vehicleId()); - _requestType = (MAV_MISSION_TYPE)clearAll.mission_type; - qCDebug(MockLinkMissionItemHandlerLog) << QStringLiteral("_handleMissionClearAll %1").arg(_requestType); + _requestType = static_cast(clearAll.mission_type); + qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionClearAll" << _requestType; switch (_requestType) { case MAV_MISSION_TYPE_MISSION: @@ -116,149 +99,162 @@ void MockLinkMissionItemHandler::_handleMissionClearAll(const mavlink_message_t& _sendAck(MAV_MISSION_ACCEPTED); } -void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message_t& msg) +void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message_t &msg) { qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList read sequence"; - + _failReadRequest1FirstResponse = true; if (_failureMode == FailReadRequestListNoResponse) { qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode FailReadRequestListNoResponse"; - } else if (_failureMode == FailReadRequestListFirstResponse && _failReadRequestListFirstResponse) { + return; + } + + if ((_failureMode == FailReadRequestListFirstResponse) && _failReadRequestListFirstResponse) { _failReadRequestListFirstResponse = false; qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode FailReadRequestListFirstResponse"; - } else { - mavlink_mission_request_list_t request; - - _failReadRequestListFirstResponse = true; - mavlink_msg_mission_request_list_decode(&msg, &request); - - Q_ASSERT(request.target_system == _mockLink->vehicleId()); - - _requestType = (MAV_MISSION_TYPE)request.mission_type; - - int itemCount; - switch (_requestType) { - case MAV_MISSION_TYPE_MISSION: - itemCount = _missionItems.count(); - if (itemCount == 0 && _sendHomePositionOnEmptyList) { - itemCount = 1; - } - break; - case MAV_MISSION_TYPE_FENCE: - itemCount = _fenceItems.count(); - break; - case MAV_MISSION_TYPE_RALLY: - itemCount = _rallyItems.count(); - break; - default: - Q_ASSERT(false); + return; + } + + _failReadRequestListFirstResponse = true; + + mavlink_mission_request_list_t request{}; + mavlink_msg_mission_request_list_decode(&msg, &request); + + Q_ASSERT(request.target_system == _mockLink->vehicleId()); + + _requestType = static_cast(request.mission_type); + + int itemCount; + switch (_requestType) { + case MAV_MISSION_TYPE_MISSION: + itemCount = _missionItems.count(); + if (itemCount == 0 && _sendHomePositionOnEmptyList) { + itemCount = 1; } - - mavlink_message_t responseMsg; - - mavlink_msg_mission_count_pack_chan( - _mockLink->vehicleId(), - MAV_COMP_ID_AUTOPILOT1, - _mockLink->mavlinkChannel(), - &responseMsg, // Outgoing message - msg.sysid, // Target is original sender - msg.compid, // Target is original sender - itemCount, // Number of mission items - _requestType, - 0 - ); - _mockLink->respondWithMavlinkMessage(responseMsg); + break; + case MAV_MISSION_TYPE_FENCE: + itemCount = _fenceItems.count(); + break; + case MAV_MISSION_TYPE_RALLY: + itemCount = _rallyItems.count(); + break; + default: + Q_ASSERT(false); } + + mavlink_message_t responseMsg{}; + (void) mavlink_msg_mission_count_pack_chan( + _mockLink->vehicleId(), + MAV_COMP_ID_AUTOPILOT1, + _mockLink->mavlinkChannel(), + &responseMsg, // Outgoing message + msg.sysid, // Target is original sender + msg.compid, // Target is original sender + itemCount, // Number of mission items + _requestType, + 0 + ); + + _mockLink->respondWithMavlinkMessage(responseMsg); } -void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& msg) +void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t &msg) { qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest read sequence"; - - mavlink_mission_request_int_t request; - + + mavlink_mission_request_int_t request{}; mavlink_msg_mission_request_int_decode(&msg, &request); - + Q_ASSERT(request.target_system == _mockLink->vehicleId()); - if (_failureMode == FailReadRequest0NoResponse && request.seq == 0) { + if ((_failureMode == FailReadRequest0NoResponse) && (request.seq == 0)) { qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode FailReadRequest0NoResponse"; - } else if (_failureMode == FailReadRequest1NoResponse && request.seq == 1) { + return; + } + + if ((_failureMode == FailReadRequest1NoResponse) && (request.seq == 1)) { qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode FailReadRequest1NoResponse"; - } else if (_failureMode == FailReadRequest1FirstResponse && request.seq == 1 && _failReadRequest1FirstResponse) { + return; + } + + if ((_failureMode == FailReadRequest1FirstResponse) && (request.seq == 1) && _failReadRequest1FirstResponse) { _failReadRequest1FirstResponse = false; qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest not responding due to failure mode FailReadRequest1FirstResponse"; - } else { - // FIXME: Track whether all items are requested, or requested in sequence - - if ((_failureMode == FailReadRequest0IncorrectSequence && request.seq == 0) || - (_failureMode == FailReadRequest1IncorrectSequence && request.seq == 1)) { - // Send back the incorrect sequence number - qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest sending bad sequence number"; - request.seq++; - } - - if ((_failureMode == FailReadRequest0ErrorAck && request.seq == 0) || - (_failureMode == FailReadRequest1ErrorAck && request.seq == 1)) { - _sendAck(_failureAckResult); + return; + } + + // FIXME: Track whether all items are requested, or requested in sequence + + if (((_failureMode == FailReadRequest0IncorrectSequence) && (request.seq == 0)) || + ((_failureMode == FailReadRequest1IncorrectSequence) && (request.seq == 1))) { + // Send back the incorrect sequence number + qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest sending bad sequence number"; + request.seq++; + } + + if (((_failureMode == FailReadRequest0ErrorAck) && (request.seq == 0)) || + ((_failureMode == FailReadRequest1ErrorAck) && (request.seq == 1))) { + _sendAck(_failureAckResult); + return; + } + + mavlink_mission_item_int_t missionItemInt{}; + + switch (request.mission_type) { + case MAV_MISSION_TYPE_MISSION: + if (_missionItems.isEmpty() && _sendHomePositionOnEmptyList) { + (void) memset(&missionItemInt, 0, sizeof(missionItemInt)); + missionItemInt.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + missionItemInt.command = MAV_CMD_NAV_WAYPOINT; + missionItemInt.autocontinue = true; } else { - mavlink_mission_item_int_t missionItemInt; - - switch (request.mission_type) { - case MAV_MISSION_TYPE_MISSION: - if (_missionItems.count() == 0 && _sendHomePositionOnEmptyList) { - memset(&missionItemInt, 0, sizeof(missionItemInt)); - missionItemInt.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - missionItemInt.command = MAV_CMD_NAV_WAYPOINT; - missionItemInt.autocontinue = true; - } else { - missionItemInt = _missionItems[request.seq]; - } - break; - case MAV_MISSION_TYPE_FENCE: - missionItemInt = _fenceItems[request.seq]; - break; - case MAV_MISSION_TYPE_RALLY: - missionItemInt = _rallyItems[request.seq]; - break; - default: - Q_ASSERT(false); - } - - mavlink_message_t responseMsg; - mavlink_msg_mission_item_int_pack_chan(_mockLink->vehicleId(), - MAV_COMP_ID_AUTOPILOT1, - _mockLink->mavlinkChannel(), - &responseMsg, // Outgoing message - msg.sysid, // Target is original sender - msg.compid, // Target is original sender - request.seq, // Index of mission item being sent - missionItemInt.frame, - missionItemInt.command, - missionItemInt.current, - missionItemInt.autocontinue, - missionItemInt.param1, missionItemInt.param2, missionItemInt.param3, missionItemInt.param4, - missionItemInt.x, missionItemInt.y, missionItemInt.z, - _requestType); - _mockLink->respondWithMavlinkMessage(responseMsg); + missionItemInt = _missionItems[request.seq]; } + break; + case MAV_MISSION_TYPE_FENCE: + missionItemInt = _fenceItems[request.seq]; + break; + case MAV_MISSION_TYPE_RALLY: + missionItemInt = _rallyItems[request.seq]; + break; + default: + Q_ASSERT(false); } + + mavlink_message_t responseMsg{}; + (void) mavlink_msg_mission_item_int_pack_chan( + _mockLink->vehicleId(), + MAV_COMP_ID_AUTOPILOT1, + _mockLink->mavlinkChannel(), + &responseMsg, // Outgoing message + msg.sysid, // Target is original sender + msg.compid, // Target is original sender + request.seq, // Index of mission item being sent + missionItemInt.frame, + missionItemInt.command, + missionItemInt.current, + missionItemInt.autocontinue, + missionItemInt.param1, missionItemInt.param2, missionItemInt.param3, missionItemInt.param4, + missionItemInt.x, missionItemInt.y, missionItemInt.z, + _requestType + ); + + _mockLink->respondWithMavlinkMessage(responseMsg); } -void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& msg) +void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t &msg) { - mavlink_mission_count_t missionCount; - + mavlink_mission_count_t missionCount{}; mavlink_msg_mission_count_decode(&msg, &missionCount); Q_ASSERT(missionCount.target_system == _mockLink->vehicleId()); - + _requestType = (MAV_MISSION_TYPE)missionCount.mission_type; _writeSequenceCount = missionCount.count; Q_ASSERT(_writeSequenceCount >= 0); - + qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount write sequence _writeSequenceCount:" << _writeSequenceCount; - + switch (missionCount.mission_type) { case MAV_MISSION_TYPE_MISSION: _missionItems.clear(); @@ -270,99 +266,103 @@ void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& ms _rallyItems.clear(); break; } - + if (_writeSequenceCount == 0) { _sendAck(MAV_MISSION_ACCEPTED); - } else { - if (_failureMode == FailWriteMissionCountNoResponse) { - qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount not responding due to failure mode FailWriteMissionCountNoResponse"; - return; - } - if (_failureMode == FailWriteMissionCountFirstResponse && _failWriteMissionCountFirstResponse) { - _failWriteMissionCountFirstResponse = false; - qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount not responding due to failure mode FailWriteMissionCountNoResponse"; - return; - } - _failWriteMissionCountFirstResponse = true; - _writeSequenceIndex = 0; - _requestNextMissionItem(_writeSequenceIndex); + return; + } + + if (_failureMode == FailWriteMissionCountNoResponse) { + qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount not responding due to failure mode FailWriteMissionCountNoResponse"; + return; } + + if (_failureMode == FailWriteMissionCountFirstResponse && _failWriteMissionCountFirstResponse) { + _failWriteMissionCountFirstResponse = false; + qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount not responding due to failure mode FailWriteMissionCountNoResponse"; + return; + } + + _failWriteMissionCountFirstResponse = true; + _writeSequenceIndex = 0; + _requestNextMissionItem(_writeSequenceIndex); } void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber) { - qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem write sequence sequenceNumber:" << sequenceNumber << "_failureMode:" << _failureMode; - - if (_failureMode == FailWriteRequest1NoResponse && sequenceNumber == 1) { + qCDebug(MockLinkMissionItemHandlerLog) << "write sequence sequenceNumber:" << sequenceNumber << "_failureMode:" << _failureMode; + + if ((_failureMode == FailWriteRequest1NoResponse) && (sequenceNumber == 1)) { qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem not responding due to failure mode FailWriteRequest1NoResponse"; - } else { - if (sequenceNumber >= _writeSequenceCount) { - qCWarning(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem requested seqeuence number > write count sequenceNumber::_writeSequenceCount" << sequenceNumber << _writeSequenceCount; - return; - } - - if ((_failureMode == FailWriteRequest0IncorrectSequence && sequenceNumber == 0) || - (_failureMode == FailWriteRequest1IncorrectSequence && sequenceNumber == 1)) { - sequenceNumber ++; - } - - if ((_failureMode == FailWriteRequest0ErrorAck && sequenceNumber == 0) || - (_failureMode == FailWriteRequest1ErrorAck && sequenceNumber == 1)) { - qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem sending ack error due to failure mode"; - _sendAck(_failureAckResult); - } else { - mavlink_message_t message; - - mavlink_msg_mission_request_int_pack_chan(_mockLink->vehicleId(), - MAV_COMP_ID_AUTOPILOT1, - _mockLink->mavlinkChannel(), - &message, - _mavlinkProtocol->getSystemId(), - _mavlinkProtocol->getComponentId(), - sequenceNumber, - _requestType); - _mockLink->respondWithMavlinkMessage(message); - - // If response with Mission Item doesn't come before timer fires it's an error - _startMissionItemResponseTimer(); - } + return; + } + + if (sequenceNumber >= _writeSequenceCount) { + qCWarning(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem requested seqeuence number > write count sequenceNumber::_writeSequenceCount" << sequenceNumber << _writeSequenceCount; + return; + } + + if (((_failureMode == FailWriteRequest0IncorrectSequence) && (sequenceNumber == 0)) || + ((_failureMode == FailWriteRequest1IncorrectSequence) && (sequenceNumber == 1))) { + sequenceNumber++; } + + if (((_failureMode == FailWriteRequest0ErrorAck) && (sequenceNumber == 0)) || + ((_failureMode == FailWriteRequest1ErrorAck) && (sequenceNumber == 1))) { + qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem sending ack error due to failure mode"; + _sendAck(_failureAckResult); + return; + } + + mavlink_message_t message{}; + (void) mavlink_msg_mission_request_int_pack_chan( + _mockLink->vehicleId(), + MAV_COMP_ID_AUTOPILOT1, + _mockLink->mavlinkChannel(), + &message, + MAVLinkProtocol::instance()->getSystemId(), + MAVLinkProtocol::getComponentId(), + sequenceNumber, + _requestType + ); + _mockLink->respondWithMavlinkMessage(message); + + // If response with Mission Item doesn't come before timer fires it's an error + _startMissionItemResponseTimer(); } -void MockLinkMissionItemHandler::_sendAck(MAV_MISSION_RESULT ackType) +void MockLinkMissionItemHandler::_sendAck(MAV_MISSION_RESULT ackType) const { qCDebug(MockLinkMissionItemHandlerLog) << "_sendAck write sequence complete ackType:" << ackType; - - mavlink_message_t message; - - mavlink_msg_mission_ack_pack_chan( + + mavlink_message_t message{}; + (void) mavlink_msg_mission_ack_pack_chan( _mockLink->vehicleId(), MAV_COMP_ID_AUTOPILOT1, _mockLink->mavlinkChannel(), &message, - _mavlinkProtocol->getSystemId(), - _mavlinkProtocol->getComponentId(), + MAVLinkProtocol::instance()->getSystemId(), + MAVLinkProtocol::getComponentId(), ackType, _requestType, 0 ); + _mockLink->respondWithMavlinkMessage(message); } -void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg) +void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t &msg) { qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionItem write sequence"; - - _missionItemResponseTimer->stop(); - - MAV_MISSION_TYPE missionType; - uint16_t seq; - mavlink_mission_item_int_t missionItemInt; + _missionItemResponseTimer.stop(); + + mavlink_mission_item_int_t missionItemInt{}; mavlink_msg_mission_item_int_decode(&msg, &missionItemInt); - missionType = static_cast(missionItemInt.mission_type); - seq = missionItemInt.seq; - + + const MAV_MISSION_TYPE missionType = static_cast(missionItemInt.mission_type); + const uint16_t seq = missionItemInt.seq; + switch (missionType) { case MAV_MISSION_TYPE_MISSION: _missionItems[seq] = missionItemInt; @@ -375,33 +375,36 @@ void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg break; case MAV_MISSION_TYPE_ENUM_END: case MAV_MISSION_TYPE_ALL: - qWarning() << "Internal error"; + qCWarning(MockLinkMissionItemHandlerLog) << "Internal error"; break; } _writeSequenceIndex++; if (_writeSequenceIndex < _writeSequenceCount) { - if (_failureMode == FailWriteFinalAckMissingRequests && _writeSequenceIndex == 3) { + if ((_failureMode == FailWriteFinalAckMissingRequests) && (_writeSequenceIndex == 3)) { // Send MAV_MISSION_ACCEPTED ack too early _sendAck(MAV_MISSION_ACCEPTED); } else { _requestNextMissionItem(_writeSequenceIndex); } - } else { - if (_failureMode != FailWriteFinalAckNoResponse) { - MAV_MISSION_RESULT ack = MAV_MISSION_ACCEPTED; - - if (_failureMode == FailWriteFinalAckErrorAck) { - ack = MAV_MISSION_ERROR; - } - _sendAck(ack); + + return; + } + + if (_failureMode != FailWriteFinalAckNoResponse) { + MAV_MISSION_RESULT ack = MAV_MISSION_ACCEPTED; + + if (_failureMode == FailWriteFinalAckErrorAck) { + ack = MAV_MISSION_ERROR; } + + _sendAck(ack); } } -void MockLinkMissionItemHandler::_missionItemResponseTimeout(void) +void MockLinkMissionItemHandler::_missionItemResponseTimeout() { - qWarning() << "Timeout waiting for next MISSION_ITEM_INT"; + qCWarning(MockLinkMissionItemHandlerLog) << "Timeout waiting for next MISSION_ITEM_INT"; Q_ASSERT(false); } @@ -410,13 +413,13 @@ void MockLinkMissionItemHandler::sendUnexpectedMissionAck(MAV_MISSION_RESULT ack _sendAck(ackType); } -void MockLinkMissionItemHandler::sendUnexpectedMissionItem(void) +void MockLinkMissionItemHandler::sendUnexpectedMissionItem() { // FIXME: NYI Q_ASSERT(false); } -void MockLinkMissionItemHandler::sendUnexpectedMissionRequest(void) +void MockLinkMissionItemHandler::sendUnexpectedMissionRequest() { // FIXME: NYI Q_ASSERT(false); @@ -428,9 +431,7 @@ void MockLinkMissionItemHandler::setFailureMode(FailureMode_t failureMode, MAV_M _failureAckResult = failureAckResult; } -void MockLinkMissionItemHandler::shutdown(void) +void MockLinkMissionItemHandler::shutdown() { - if (_missionItemResponseTimer) { - delete _missionItemResponseTimer; - } + _missionItemResponseTimer.stop(); } diff --git a/src/Comms/MockLink/MockLinkMissionItemHandler.h b/src/Comms/MockLink/MockLinkMissionItemHandler.h index ee620d2c9a1..88bcc8be117 100644 --- a/src/Comms/MockLink/MockLinkMissionItemHandler.h +++ b/src/Comms/MockLink/MockLinkMissionItemHandler.h @@ -7,18 +7,16 @@ * ****************************************************************************/ - #pragma once -#include "QGCMAVLink.h" - -#include +#include #include +#include #include -#include + +#include "MAVLinkLib.h" class MockLink; -class MAVLinkProtocol; Q_DECLARE_LOGGING_CATEGORY(MockLinkMissionItemHandlerLog) @@ -27,18 +25,18 @@ class MockLinkMissionItemHandler : public QObject Q_OBJECT public: - MockLinkMissionItemHandler(MockLink* mockLink, MAVLinkProtocol* mavlinkProtocol); + MockLinkMissionItemHandler(MockLink *mockLink); ~MockLinkMissionItemHandler(); - + // Prepares for destruction on correct thread - void shutdown(void); - - /// @brief Called to handle mission item related messages. All messages should be passed to this method. - /// It will handle the appropriate set. - /// @return true: message handled - bool handleMessage(const mavlink_message_t& msg); - - typedef enum { + void shutdown(); + + /// Called to handle mission item related messages. All messages should be passed to this method. + /// It will handle the appropriate set. + /// @return true: message handled + bool handleMessage(const mavlink_message_t &msg); + + enum FailureMode_t { FailNone, // No failures FailReadRequestListNoResponse, // Don't send MISSION_COUNT in response to MISSION_REQUEST_LIST FailReadRequestListFirstResponse, // Don't send MISSION_COUNT in response to first MISSION_REQUEST_LIST, allow subsequent to go through @@ -59,60 +57,58 @@ class MockLinkMissionItemHandler : public QObject FailWriteFinalAckNoResponse, // Don't send the final MISSION_ACK FailWriteFinalAckErrorAck, // Send an error as the final MISSION_ACK FailWriteFinalAckMissingRequests, // Send the MISSION_ACK before all items have been requested - } FailureMode_t; + }; /// Sets a failure mode for unit testing /// @param failureMode Type of failure to simulate /// @param failureAckResult Error to send if one the ack error modes void setFailureMode(FailureMode_t failureMode, MAV_MISSION_RESULT failureAckResult); - + /// Called to send a MISSION_ACK message while the MissionManager is in idle state void sendUnexpectedMissionAck(MAV_MISSION_RESULT ackType); - + /// Called to send a MISSION_ITEM message while the MissionManager is in idle state - void sendUnexpectedMissionItem(void); - + void sendUnexpectedMissionItem(); + /// Called to send a MISSION_REQUEST message while the MissionManager is in idle state - void sendUnexpectedMissionRequest(void); - + void sendUnexpectedMissionRequest(); + /// Reset the state of the MissionItemHandler to no items, no transactions in progress. - void reset(void) { _missionItems.clear(); } + void reset() { _missionItems.clear(); } void setSendHomePositionOnEmptyList(bool sendHomePositionOnEmptyList) { _sendHomePositionOnEmptyList = sendHomePositionOnEmptyList; } private slots: - void _missionItemResponseTimeout(void); + void _missionItemResponseTimeout(); private: - void _handleMissionRequestList (const mavlink_message_t& msg); - void _handleMissionRequest (const mavlink_message_t& msg); - void _handleMissionItem (const mavlink_message_t& msg); - void _handleMissionCount (const mavlink_message_t& msg); - void _handleMissionClearAll (const mavlink_message_t& msg); - void _requestNextMissionItem (int sequenceNumber); - void _sendAck (MAV_MISSION_RESULT ackType); - void _startMissionItemResponseTimer (void); + void _handleMissionRequestList(const mavlink_message_t &msg); + void _handleMissionRequest(const mavlink_message_t &msg); + void _handleMissionItem(const mavlink_message_t &msg); + void _handleMissionCount(const mavlink_message_t &msg); + void _handleMissionClearAll(const mavlink_message_t &msg); + void _requestNextMissionItem(int sequenceNumber); + void _sendAck(MAV_MISSION_RESULT ackType) const; + void _startMissionItemResponseTimer(); -private: - MockLink* _mockLink; - - int _writeSequenceCount; ///< Numbers of items about to be written - int _writeSequenceIndex; ///< Current index being reqested + MockLink *_mockLink = nullptr; + + int _writeSequenceCount = 0; ///< Numbers of items about to be written + int _writeSequenceIndex = 0; ///< Current index being reqested typedef QMap MissionItemList_t; - MAV_MISSION_TYPE _requestType; - MissionItemList_t _missionItems; - MissionItemList_t _fenceItems; - MissionItemList_t _rallyItems; - - QTimer* _missionItemResponseTimer; - FailureMode_t _failureMode; - MAV_MISSION_RESULT _failureAckResult; - bool _sendHomePositionOnEmptyList; - MAVLinkProtocol* _mavlinkProtocol; - bool _failReadRequestListFirstResponse; - bool _failReadRequest1FirstResponse; - bool _failWriteMissionCountFirstResponse; + MAV_MISSION_TYPE _requestType = MAV_MISSION_TYPE_MISSION; + MissionItemList_t _missionItems; + MissionItemList_t _fenceItems; + MissionItemList_t _rallyItems; + + QTimer _missionItemResponseTimer; + FailureMode_t _failureMode = FailNone; + MAV_MISSION_RESULT _failureAckResult; + bool _sendHomePositionOnEmptyList = false; + bool _failReadRequestListFirstResponse = true; + bool _failReadRequest1FirstResponse = true; + bool _failWriteMissionCountFirstResponse = true; }; diff --git a/test/Comms/CMakeLists.txt b/test/Comms/CMakeLists.txt index c296cce6acc..f6395e42754 100644 --- a/test/Comms/CMakeLists.txt +++ b/test/Comms/CMakeLists.txt @@ -14,11 +14,3 @@ target_link_libraries(CommsTest ) target_include_directories(CommsTest PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) - -qt_add_qml_module(CommsTest - URI commstest - VERSION 1.0 - QML_FILES - MockLinkOptionsDlg.qml - IMPORT_PATH ${QT_QML_OUTPUT_DIRECTORY} -) diff --git a/test/Comms/MockLinkOptionsDlg.qml b/test/Comms/MockLinkOptionsDlg.qml deleted file mode 100644 index 7825af2fbbc..00000000000 --- a/test/Comms/MockLinkOptionsDlg.qml +++ /dev/null @@ -1,58 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2020 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - -import QtQuick -import QtQuick.Layouts -import QtQuick.Controls -import QtQuick.Dialogs - -import QGroundControl -import QGroundControl.Controls -import QGroundControl.ScreenTools -import QGroundControl.FactSystem -import QGroundControl.FactControls -import QGroundControl.Controllers - -QGCPopupDialog { - title: qsTr("MockLink Options") - buttons: Dialog.Close - - property var link - - ColumnLayout { - spacing: ScreenTools.defaultFontPixelHeight / 2 - - QGCButton { - Layout.fillWidth: true - text: qsTr("Stop Heartbeats") - onClicked: { - link.setCommLost(true) - close() - } - } - - QGCButton { - Layout.fillWidth: true - text: qsTr("Start Heartbeats") - onClicked: { - link.setCommLost(false) - close() - } - } - - QGCButton { - Layout.fillWidth: true - text: qsTr("Connection Removed") - onClicked: { - link.simulateConnectionRemoved() - close() - } - } - } -} diff --git a/test/FactSystem/ParameterManagerTest.cc b/test/FactSystem/ParameterManagerTest.cc index 4f2c8f931fa..cfa78e9f978 100644 --- a/test/FactSystem/ParameterManagerTest.cc +++ b/test/FactSystem/ParameterManagerTest.cc @@ -12,6 +12,7 @@ #include "MultiVehicleManager.h" #include "Vehicle.h" #include "ParameterManager.h" +#include "MockLinkFTP.h" #include #include diff --git a/test/FactSystem/ParameterManagerTest.h b/test/FactSystem/ParameterManagerTest.h index 43ce641ecbd..d04b407e1b2 100644 --- a/test/FactSystem/ParameterManagerTest.h +++ b/test/FactSystem/ParameterManagerTest.h @@ -12,7 +12,7 @@ #define ParameterManagerTest_H #include "UnitTest.h" -#include "MockLinkMissionItemHandler.h" +#include "MockConfiguration.h" class ParameterManagerTest : public UnitTest { diff --git a/test/MissionManager/MissionCommandTreeEditorTest.h b/test/MissionManager/MissionCommandTreeEditorTest.h index e34f0752796..0f8d80bf1b0 100644 --- a/test/MissionManager/MissionCommandTreeEditorTest.h +++ b/test/MissionManager/MissionCommandTreeEditorTest.h @@ -10,6 +10,7 @@ #pragma once #include "UnitTest.h" +#include "QGCMAVLink.h" /// This unit test is meant to be used stand-alone to generate images for each mission item editor for review class MissionCommandTreeEditorTest : public UnitTest diff --git a/test/MissionManager/SimpleMissionItemTest.cc b/test/MissionManager/SimpleMissionItemTest.cc index bacefb2de67..77f7eadab68 100644 --- a/test/MissionManager/SimpleMissionItemTest.cc +++ b/test/MissionManager/SimpleMissionItemTest.cc @@ -304,7 +304,7 @@ void SimpleMissionItemTest::_testSignals(void) _spyVisualItem->clearAllSignals(); _simpleItem->setAltitudeMode(_simpleItem->altitudeMode() == QGroundControlQmlGlobal::AltitudeModeRelative ? QGroundControlQmlGlobal::AltitudeModeAbsolute : QGroundControlQmlGlobal::AltitudeModeRelative); - QVERIFY(_spySimpleItem->checkOnlySignalByMask(dirtyChangedMask | friendlyEditAllowedChangedMask | altitudeModeChangedMask)); + QVERIFY(_spySimpleItem->checkOnlySignalByMask(dirtyChangedMask | static_cast(friendlyEditAllowedChangedMask | altitudeModeChangedMask))); _spySimpleItem->clearAllSignals(); _spyVisualItem->clearAllSignals(); diff --git a/test/MissionManager/SimpleMissionItemTest.h b/test/MissionManager/SimpleMissionItemTest.h index 0135d0b8a6d..5b476b805e8 100644 --- a/test/MissionManager/SimpleMissionItemTest.h +++ b/test/MissionManager/SimpleMissionItemTest.h @@ -11,6 +11,7 @@ #include "VisualMissionItemTest.h" #include "QGroundControlQmlGlobal.h" +#include "QGCMAVLink.h" class SimpleMissionItem; @@ -84,6 +85,6 @@ private slots: bool _classMatch (QGCMAVLink::VehicleClass_t vehicleClass, QGCMAVLink::VehicleClass_t testClass); SimpleMissionItem* _simpleItem; - MultiSignalSpy* _spySimpleItem; - MultiSignalSpy* _spyVisualItem; + MultiSignalSpy* _spySimpleItem = nullptr; + MultiSignalSpy* _spyVisualItem = nullptr; }; diff --git a/test/UnitTest.qrc b/test/UnitTest.qrc index 6a953229a9d..c8aa3a57706 100644 --- a/test/UnitTest.qrc +++ b/test/UnitTest.qrc @@ -2,7 +2,6 @@ UI/preferences/MockLink.qml UI/preferences/MockLinkSettings.qml - Comms/MockLinkOptionsDlg.qml QmlControls/QGCControlDebug.qml UI/preferences/DebugWindow.qml QmlControls/QmlTest.qml @@ -18,7 +17,6 @@ Vehicle/Components/TranslationTest_de_DE.ts FactSystem/FactSystemTest.qml MissionManager/MissionPlanner.waypoints - Comms/MockLinkOptionsDlg.qml MissionManager/OldFileFormat.mission MissionManager/UT-MavCmdInfoCommon.json MissionManager/UT-MavCmdInfoFixedWing.json diff --git a/test/Vehicle/FTPManagerTest.cc b/test/Vehicle/FTPManagerTest.cc index 977da004485..885ed7496e7 100644 --- a/test/Vehicle/FTPManagerTest.cc +++ b/test/Vehicle/FTPManagerTest.cc @@ -12,6 +12,7 @@ #include "Vehicle.h" #include "MockLink.h" #include "FTPManager.h" +#include "MockLinkFTP.h" #include #include