diff --git a/src/Vehicle/InitialConnectStateMachine.cc b/src/Vehicle/InitialConnectStateMachine.cc index 6f22adc..84d31f1 100644 --- a/src/Vehicle/InitialConnectStateMachine.cc +++ b/src/Vehicle/InitialConnectStateMachine.cc @@ -128,6 +128,8 @@ void InitialConnectStateMachine::_autopilotVersionRequestMessageHandler(void* re mavlink_msg_autopilot_version_decode(&message, &autopilotVersion); vehicle->_uid = (quint64)autopilotVersion.uid; + vehicle->_firmwareBoardVendorId = autopilotVersion.vendor_id; + vehicle->_firmwareBoardProductId = autopilotVersion.product_id; emit vehicle->vehicleUIDChanged(); if (autopilotVersion.flight_sw_version != 0) { diff --git a/src/Vehicle/InitialConnectTest.cc b/src/Vehicle/InitialConnectTest.cc index 6105b51..686b9a8 100644 --- a/src/Vehicle/InitialConnectTest.cc +++ b/src/Vehicle/InitialConnectTest.cc @@ -10,6 +10,7 @@ #include "InitialConnectTest.h" #include "MultiVehicleManager.h" #include "QGCApplication.h" +#include "LinkManager.h" #include "MockLink.h" void InitialConnectTest::_performTestCases(void) @@ -31,3 +32,30 @@ void InitialConnectTest::_performTestCases(void) _disconnectMockLink(); } } + +void InitialConnectTest::_boardVendorProductId(void) +{ + auto *mvm = qgcApp()->toolbox()->multiVehicleManager(); + QSignalSpy activeVehicleSpy{mvm, &MultiVehicleManager::activeVehicleChanged}; + + auto mockConfig = std::make_shared(QString{"MockLink"}); + const uint16_t mockVendor = 1234; + const uint16_t mockProduct = 5678; + mockConfig->setBoardVendorProduct(mockVendor, mockProduct); + + SharedLinkConfigurationPtr linkConfig = mockConfig; + _linkManager->createConnectedLink(linkConfig); + + QVERIFY(activeVehicleSpy.wait()); + auto *vehicle = mvm->activeVehicle(); + QSignalSpy initialConnectCompleteSpy{vehicle, &Vehicle::initialConnectComplete}; + + // Both ends of mocklink (and the initial connect state machine?) operate on + // a different thread. The initial connection may already be complete. + QVERIFY(initialConnectCompleteSpy.wait() || vehicle->isInitialConnectComplete()); + + QCOMPARE(vehicle->firmwareBoardVendorId(), mockVendor); + QCOMPARE(vehicle->firmwareBoardProductId(), mockProduct); + + _linkManager->disconnectAll(); +} diff --git a/src/Vehicle/InitialConnectTest.h b/src/Vehicle/InitialConnectTest.h index 01e7bd0..02f5aa6 100644 --- a/src/Vehicle/InitialConnectTest.h +++ b/src/Vehicle/InitialConnectTest.h @@ -19,4 +19,5 @@ class InitialConnectTest : public UnitTest private slots: void _performTestCases(void); + void _boardVendorProductId(void); }; diff --git a/src/Vehicle/RequestMessageTest.cc b/src/Vehicle/RequestMessageTest.cc index 2ee36fe..12e90c5 100644 --- a/src/Vehicle/RequestMessageTest.cc +++ b/src/Vehicle/RequestMessageTest.cc @@ -80,6 +80,8 @@ void RequestMessageTest::_duplicateCommand(void) _mockLink->clearSendMavCommandCounts(); _mockLink->setRequestMessageFailureMode(testCase.failureMode); + QVERIFY(false == vehicle->isMavCommandPending(MAV_COMP_ID_AUTOPILOT1, MAV_CMD_REQUEST_MESSAGE)); + vehicle->requestMessage(_requestMessageResultHandler, &testCase, MAV_COMP_ID_AUTOPILOT1, MAVLINK_MSG_ID_DEBUG); QVERIFY(QTest::qWaitFor([&]() { return _mockLink->sendMavCommandCount(MAV_CMD_REQUEST_MESSAGE) == 1; }, 10)); QCOMPARE(testCase.resultHandlerCalled, false); @@ -89,6 +91,16 @@ void RequestMessageTest::_duplicateCommand(void) QCOMPARE(testCase.resultHandlerCalled, true); QCOMPARE(_mockLink->sendMavCommandCount(MAV_CMD_REQUEST_MESSAGE), testCase.expectedSendCount); QVERIFY(vehicle->_findMavCommandListEntryIndex(MAV_COMP_ID_AUTOPILOT1, MAV_CMD_REQUEST_MESSAGE) != -1); + QVERIFY(true == vehicle->isMavCommandPending(MAV_COMP_ID_AUTOPILOT1, MAV_CMD_REQUEST_MESSAGE)); + + // MockLink does not ack messages? + // So wait for Vehicle to exhaust retries and then report that failure. + // We should then observe that the command is no longer pending and may send again. + testCase.resultHandlerCalled = false; + testCase.expectedFailureCode = Vehicle::RequestMessageFailureCommandNotAcked; + auto timeout = Vehicle::_mavCommandMaxRetryCount * (Vehicle::_mavCommandResponseCheckTimeoutMSecs + Vehicle::_mavCommandAckTimeoutMSecs); + QVERIFY(QTest::qWaitFor([&]() { return testCase.resultHandlerCalled; }, timeout)); + QVERIFY(false == vehicle->isMavCommandPending(MAV_COMP_ID_AUTOPILOT1, MAV_CMD_REQUEST_MESSAGE)); } void RequestMessageTest::_compIdAllRequestMessageResultHandler(void* resultHandlerData, MAV_RESULT commandResult, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t& /*message*/) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index d24faf7..5fb8e27 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -2764,6 +2764,11 @@ void Vehicle::sendMavCommandInt(int compId, MAV_CMD command, MAV_FRAME frame, bo param1, param2, param3, param4, param5, param6, param7); } +bool Vehicle::isMavCommandPending(int targetCompId, MAV_CMD command) +{ + return ((-1) < _findMavCommandListEntryIndex(targetCompId, command)); +} + int Vehicle::_findMavCommandListEntryIndex(int targetCompId, MAV_CMD command) { for (int i=0; i<_mavCommandList.count(); i++) { @@ -2809,8 +2814,7 @@ bool Vehicle::_sendMavCommandShouldRetry(MAV_CMD command) void Vehicle::_sendMavCommandWorker(bool commandInt, bool showError, MavCmdResultHandler resultHandler, void* resultHandlerData, int targetCompId, MAV_CMD command, MAV_FRAME frame, float param1, float param2, float param3, float param4, float param5, float param6, float param7) { - int entryIndex = _findMavCommandListEntryIndex(targetCompId, command); - if (entryIndex != -1 || targetCompId == MAV_COMP_ID_ALL) { + if ((targetCompId == MAV_COMP_ID_ALL) || isMavCommandPending(targetCompId, command)) { bool compIdAll = targetCompId == MAV_COMP_ID_ALL; QString rawCommandName = _toolbox->missionCommandTree()->rawName(command); @@ -3726,7 +3730,7 @@ void Vehicle::_setMessageInterval(int messageId, int rate) rate); } -bool Vehicle::_initialConnectComplete() const +bool Vehicle::isInitialConnectComplete() const { return !_initialConnectStateMachine->active(); } diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 3858d64..6ded79b 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -252,7 +252,7 @@ public: Q_PROPERTY(bool allSensorsHealthy READ allSensorsHealthy NOTIFY allSensorsHealthyChanged) //< true: all sensors in SYS_STATUS reported as healthy Q_PROPERTY(bool requiresGpsFix READ requiresGpsFix NOTIFY requiresGpsFixChanged) Q_PROPERTY(double loadProgress READ loadProgress NOTIFY loadProgressChanged) - Q_PROPERTY(bool initialConnectComplete READ _initialConnectComplete NOTIFY initialConnectComplete) + Q_PROPERTY(bool initialConnectComplete READ isInitialConnectComplete NOTIFY initialConnectComplete) // The following properties relate to Orbit status Q_PROPERTY(bool orbitActive READ orbitActive NOTIFY orbitActiveChanged) @@ -431,6 +431,7 @@ public: Q_INVOKABLE void flashBootloader(); #endif + bool isInitialConnectComplete() const; bool guidedModeSupported () const; bool pauseVehicleSupported () const; bool orbitModeSupported () const; @@ -675,6 +676,22 @@ public: void sendMavCommand(int compId, MAV_CMD command, bool showError, float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f); void sendMavCommandInt(int compId, MAV_CMD command, MAV_FRAME frame, bool showError, float param1, float param2, float param3, float param4, double param5, double param6, float param7); + /// + /// \brief isMavCommandPending + /// Query whether the specified MAV_CMD is in queue to be sent or has + /// already been sent but whose reply has not yet been received and whose + /// timeout has not yet expired. + /// + /// Or, said another way: if you call `sendMavCommand(compId, command, true, ...)` + /// will an error be shown because you (or another part of QGC) has already + /// sent that command? + /// + /// \param targetCompId + /// \param command + /// \return + /// + bool isMavCommandPending(int targetCompId, MAV_CMD command); + /// Same as sendMavCommand but available from Qml. Q_INVOKABLE void sendCommand(int compId, int command, bool showError, double param1 = 0.0, double param2 = 0.0, double param3 = 0.0, double param4 = 0.0, double param5 = 0.0, double param6 = 0.0, double param7 = 0.0); @@ -722,6 +739,8 @@ public: int firmwareCustomMajorVersion() const { return _firmwareCustomMajorVersion; } int firmwareCustomMinorVersion() const { return _firmwareCustomMinorVersion; } int firmwareCustomPatchVersion() const { return _firmwareCustomPatchVersion; } + int firmwareBoardVendorId() const { return _firmwareBoardVendorId; } + int firmwareBoardProductId() const { return _firmwareBoardProductId; } QString firmwareVersionTypeString() const; void setFirmwareVersion(int majorVersion, int minorVersion, int patchVersion, FIRMWARE_VERSION_TYPE versionType = FIRMWARE_VERSION_TYPE_OFFICIAL); void setFirmwareCustomVersion(int majorVersion, int minorVersion, int patchVersion); @@ -997,7 +1016,6 @@ private: void _chunkedStatusTextTimeout (void); void _chunkedStatusTextCompleted (uint8_t compId); void _setMessageInterval (int messageId, int rate); - bool _initialConnectComplete () const; EventHandler& _eventHandler (uint8_t compid); static void _rebootCommandResultHandler(void* resultHandlerData, int compId, MAV_RESULT commandResult, MavCmdResultFailureCode_t failureCode); @@ -1142,6 +1160,13 @@ private: int _firmwareCustomPatchVersion = versionNotSetValue; FIRMWARE_VERSION_TYPE _firmwareVersionType = FIRMWARE_VERSION_TYPE_OFFICIAL; + // Vendor and Product as reported from the first autopilot version message + // during the initial connect. They may be zero eg ArduPilot SITL reports 0 + // by default. + uint16_t _firmwareBoardVendorId = 0; + uint16_t _firmwareBoardProductId = 0; + + QString _gitHash; quint64 _uid = 0; diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index a1afd34..1659c21 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -93,6 +93,8 @@ MockLink::MockLink(SharedLinkConfigurationPtr& config) _vehicleSystemId = mockConfig->incrementVehicleId() ? _nextVehicleSystemId++ : _nextVehicleSystemId; _vehicleLatitude = _defaultVehicleLatitude + ((_vehicleSystemId - 128) * 0.0001); _vehicleLongitude = _defaultVehicleLongitude + ((_vehicleSystemId - 128) * 0.0001); + _boardVendorId = mockConfig->boardVendorId(); + _boardProductId = mockConfig->boardProductId(); QObject::connect(this, &MockLink::writeBytesQueuedSignal, this, &MockLink::_writeBytesQueued, Qt::QueuedConnection); @@ -1204,8 +1206,8 @@ void MockLink::_respondWithAutopilotVersion(void) (uint8_t *)&customVersion, // flight_custom_version, (uint8_t *)&customVersion, // middleware_custom_version, (uint8_t *)&customVersion, // os_custom_version, - 0, // vendor_id, - 0, // product_id, + _boardVendorId, + _boardProductId, 0, // uid 0); // uid2 respondWithMavlinkMessage(msg); diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index 6897e21..a729fb0 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -46,10 +46,13 @@ public: 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(); } @@ -86,6 +89,8 @@ private: bool _sendStatusText = false; FailureMode_t _failureMode = FailNone; bool _incrementVehicleId = true; + uint16_t _boardVendorId = 0; + uint16_t _boardProductId = 0; static const char* _firmwareTypeKey; static const char* _vehicleTypeKey; @@ -275,6 +280,12 @@ private: 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;