diff --git a/src/Vehicle/RequestMessageTest.cc b/src/Vehicle/RequestMessageTest.cc index 3c2e9df99e1..a1070d07705 100644 --- a/src/Vehicle/RequestMessageTest.cc +++ b/src/Vehicle/RequestMessageTest.cc @@ -41,6 +41,9 @@ void RequestMessageTest::_testCaseWorker(TestCase_t& testCase) // Gimbal controller sends message requests when receiving heartbeats, trying to find a gimbal, and it messes with this test so we disable it vehicle->deleteGimbalController(); + // Camera manager also messes with it. + vehicle->deleteCameraManager(); + _mockLink->clearReceivedMavCommandCounts(); _mockLink->setRequestMessageFailureMode(testCase.failureMode); diff --git a/src/Vehicle/SendMavCommandWithSignallingTest.cc b/src/Vehicle/SendMavCommandWithSignallingTest.cc index e1b40a4057c..9de3ca971f1 100644 --- a/src/Vehicle/SendMavCommandWithSignallingTest.cc +++ b/src/Vehicle/SendMavCommandWithSignallingTest.cc @@ -42,7 +42,9 @@ void SendMavCommandWithSignallingTest::_testCaseWorker(TestCase_t& testCase) // Gimbal controler requests MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION on vehicle connection, // and that messes with this test, as it receives response to that command instead. So if we // are taking the response to that MAV_CMD_REQUEST_MESSAGE, we discard it and take the next - while (arguments.at(2).toInt() == MAV_CMD_REQUEST_MESSAGE) { + // Also, the camera manager requests MAVLINK_MSG_ID_CAMERA_INFORMATION on vehicle connection, + // so we need to ignore that as well. + while (arguments.at(2).toInt() == MAV_CMD_REQUEST_MESSAGE || arguments.at(2).toInt() == MAV_CMD_REQUEST_CAMERA_INFORMATION) { qDebug() << "Received response to MAV_CMD_REQUEST_MESSAGE(512), ignoring, waiting for: " << testCase.command; QCOMPARE(spyResult.wait(10000), true); arguments = spyResult.takeFirst(); diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 7b004eb780a..324642ec4e6 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -535,6 +535,14 @@ void Vehicle::prepareDelete() } } +void Vehicle::deleteCameraManager() +{ + if(_cameraManager) { + delete _cameraManager; + _cameraManager = nullptr; + } +} + void Vehicle::deleteGimbalController() { if (_gimbalController) { diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index c54b6bbd81e..a8d12f97417 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -901,6 +901,9 @@ class Vehicle : public FactGroup /// Delete gimbal controller, handy for RequestMessageTest.cc, otherwise gimbal controller message requests will mess with this test void deleteGimbalController(); + /// Delete camera manager, just for testing + void deleteCameraManager(); + quint64 mavlinkSentCount () const{ return _mavlinkSentCount; } /// Calculated total number of messages sent to us quint64 mavlinkReceivedCount () const{ return _mavlinkReceivedCount; } /// Total number of sucessful messages received quint64 mavlinkLossCount () const{ return _mavlinkLossCount; } /// Total number of lost messages