`Vehicle` permits users to query if a command would be duplicated; also record firmware board vendor/product id

* `InitialConnectStateMachine` records the vendor/product reported from the autopilot version message
* `Vehicle::_initialConnectComplete()` has been made public to resolve a timing problem where a mocklink may (rarely) connect to the initial connect complete signal after it has been emitted
* Add `Vehicle::isMavCommandPending()` to query whether trying to send a particular mavlink command would immediately report a duplicate command error

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) {

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<MockConfiguration>(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();
+}

src/Vehicle/InitialConnectTest.h

@@ -19,4 +19,5 @@ class InitialConnectTest : public UnitTest
 
 private slots:
     void _performTestCases(void);
+    void _boardVendorProductId(void);
 };

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*/)

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();
 }

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;
 

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);

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;