diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index c74553ee7afcbecc4768b811c89bf54cb758e33d..1e35f1e6df8a0135b3053771e15c32f5cdebc0c4 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -571,6 +571,7 @@ HEADERS += \ src/qgcunittest/MavlinkLogTest.h \ src/qgcunittest/MessageBoxTest.h \ src/qgcunittest/MultiSignalSpy.h \ + src/qgcunittest/ParameterLoaderTest.h \ src/qgcunittest/RadioConfigTest.h \ src/qgcunittest/TCPLinkTest.h \ src/qgcunittest/TCPLoopBackServer.h \ @@ -596,6 +597,7 @@ SOURCES += \ src/qgcunittest/MavlinkLogTest.cc \ src/qgcunittest/MessageBoxTest.cc \ src/qgcunittest/MultiSignalSpy.cc \ + src/qgcunittest/ParameterLoaderTest.cc \ src/qgcunittest/RadioConfigTest.cc \ src/qgcunittest/TCPLinkTest.cc \ src/qgcunittest/TCPLoopBackServer.cc \ diff --git a/src/FactSystem/FactSystemTest.qml b/src/FactSystem/FactSystemTest.qml index 28e1d0865292dd91ac5e313f7d037a5bfbb94dee..127c2470c5658c029a2caa7b771a9fc2e2c67206 100644 --- a/src/FactSystem/FactSystemTest.qml +++ b/src/FactSystem/FactSystemTest.qml @@ -32,7 +32,7 @@ FactPanel { TextInput { text: fact2.value - property Fact fact2: controller.getParameterFact(51, "COMPONENT_51") + property Fact fact2: controller.getParameterFact(50, "RC_MAP_THROTTLE") onAccepted: fact2.value = text } diff --git a/src/FactSystem/FactSystemTestBase.cc b/src/FactSystem/FactSystemTestBase.cc index c3ab26c86d22c1f4e0ab8784d46715269314335e..fe73eb1bac8084be7f8b9245826d886f00c3c06c 100644 --- a/src/FactSystem/FactSystemTestBase.cc +++ b/src/FactSystem/FactSystemTestBase.cc @@ -62,18 +62,7 @@ void FactSystemTestBase::_parameter_specific_component_id_test(void) QVERIFY(fact != NULL); QVariant factValue = fact->rawValue(); QCOMPARE(factValue.isValid(), true); - - QCOMPARE(factValue.toInt(), 3); - - // Test another component id - QVERIFY(_plugin->factExists(FactSystem::ParameterProvider, 51, "COMPONENT_51")); - fact = _plugin->getFact(FactSystem::ParameterProvider, 51, "COMPONENT_51"); - QVERIFY(fact != NULL); - factValue = fact->rawValue(); - QCOMPARE(factValue.isValid(), true); - - QCOMPARE(factValue.toInt(), 51); } /// Test that QML can reference a Fact diff --git a/src/FactSystem/ParameterLoader.cc b/src/FactSystem/ParameterLoader.cc index d6d0f29f0cfff5b5fe53671776c6b515aa941465..19113fcbf8c5ad0e15d27f190ef119a0fc582beb 100644 --- a/src/FactSystem/ParameterLoader.cc +++ b/src/FactSystem/ParameterLoader.cc @@ -47,6 +47,9 @@ ParameterLoader::ParameterLoader(Vehicle* vehicle) , _defaultComponentId(MAV_COMP_ID_ALL) , _parameterSetMajorVersion(-1) , _parameterMetaData(NULL) + , _prevWaitingReadParamIndexCount(0) + , _prevWaitingReadParamNameCount(0) + , _prevWaitingWriteParamNameCount(0) , _initialRequestRetryCount(0) , _totalParamCount(0) { @@ -90,14 +93,26 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param _initialRequestTimeoutTimer.stop(); - qCDebug(ParameterLoaderLog) << "_parameterUpdate (usaId:" << uasId << - "componentId:" << componentId << - "name:" << parameterName << - "count:" << parameterCount << - "index:" << parameterId << - "mavType:" << mavType << - "value:" << value << - ")"; + if (_initialLoadComplete) { + qCDebug(ParameterLoaderLog) << "_parameterUpdate (id:" << uasId << + "componentId:" << componentId << + "name:" << parameterName << + "count:" << parameterCount << + "index:" << parameterId << + "mavType:" << mavType << + "value:" << value << + ")"; + } else { + // This is too noisy during initial load + qCDebug(ParameterLoaderVerboseLog) << "_parameterUpdate (id:" << uasId << + "componentId:" << componentId << + "name:" << parameterName << + "count:" << parameterCount << + "index:" << parameterId << + "mavType:" << mavType << + "value:" << value << + ")"; + } #if 0 // Handy for testing retry logic @@ -115,7 +130,7 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param } #endif - if (parameterName == "_HASH_CHECK") { + if (_vehicle->px4Firmware() && parameterName == "_HASH_CHECK") { /* we received a cache hash, potentially load from cache */ _tryCacheHashLoad(uasId, componentId, value); return; @@ -201,20 +216,25 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param qCDebug(ParameterLoaderLog) << "waitingWriteParamNameCount:" << waitingWriteParamNameCount; } - int waitingParamCount = waitingReadParamIndexCount + waitingReadParamNameCount + waitingWriteParamNameCount; - if (waitingParamCount) { - qCDebug(ParameterLoaderLog) << "waitingParamCount:" << waitingParamCount; + int readWaitingParamCount = waitingReadParamIndexCount + waitingReadParamNameCount; + int totalWaitingParamCount = readWaitingParamCount + waitingWriteParamNameCount; + if (totalWaitingParamCount) { + qCDebug(ParameterLoaderLog) << "totalWaitingParamCount:" << totalWaitingParamCount; } else if (_defaultComponentId != MAV_COMP_ID_ALL) { // No more parameters to wait for, stop the timeout. Be careful to not stop timer if we don't have the default // component yet. _waitingParamTimeoutTimer.stop(); } - // Update progress bar - if (waitingParamCount == 0) { - emit parameterListProgress(0); + // Update progress bar for waiting reads + if (readWaitingParamCount == 0) { + // We are no longer waiting for any reads to complete + if (_prevWaitingReadParamIndexCount + _prevWaitingReadParamNameCount != 0) { + // Set progress to 0 if not already there + emit parameterListProgress(0); + } } else { - emit parameterListProgress((float)(_totalParamCount - waitingParamCount) / (float)_totalParamCount); + emit parameterListProgress((float)(_totalParamCount - readWaitingParamCount) / (float)_totalParamCount); } // Get parameter set version @@ -286,12 +306,25 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param _setupGroupMap(); } - if (waitingParamCount == 0) { - // Now that we know vehicle is up to date persist + if (_prevWaitingWriteParamNameCount != 0 && waitingWriteParamNameCount == 0) { + // If all the writes just finished the vehicle is up to date, so persist. _saveToEEPROM(); - _writeLocalParamCache(uasId, componentId); } + // Update param cache. The param cache is only used on PX4 Firmware since ArduPilot and Solo have volatile params + // which invalidate the cache. The Solo also streams param updates in flight for things like gimbal values + // which in turn causes a perf problem with all the param cache updates. + if (_vehicle->px4Firmware()) { + if (_prevWaitingReadParamIndexCount + _prevWaitingReadParamNameCount != 0 && readWaitingParamCount == 0) { + // All reads just finished, update the cache + _writeLocalParamCache(uasId, componentId); + } + } + + _prevWaitingReadParamIndexCount = waitingReadParamIndexCount; + _prevWaitingReadParamNameCount = waitingReadParamNameCount; + _prevWaitingWriteParamNameCount = waitingWriteParamNameCount; + // Don't fail initial load complete if default component isn't found yet. That will be handled in wait timeout check. _checkInitialLoadComplete(false /* failIfNoDefaultComponent */); } @@ -955,8 +988,10 @@ void ParameterLoader::_checkInitialLoadComplete(bool failIfNoDefaultComponent) "This will cause QGroundControl to be unable to display its full user interface. " "If you are using modified firmware, you may need to resolve any vehicle startup errors to resolve the issue. " "If you are using standard firmware, you may need to upgrade to a newer version to resolve the issue."); - qCWarning(ParameterLoaderLog) << "The following parameter indices could not be loaded after the maximum number of retries: " << indexList; - emit parametersReady(true); + if (!qgcApp()->runningUnitTests()) { + qCWarning(ParameterLoaderLog) << "The following parameter indices could not be loaded after the maximum number of retries: " << indexList; + } + emit parametersReady(true /* missingParameters */); return; } @@ -966,15 +1001,17 @@ void ParameterLoader::_checkInitialLoadComplete(bool failIfNoDefaultComponent) "This will cause QGroundControl to be unable to display its full user interface. " "If you are using modified firmware, you may need to resolve any vehicle startup errors to resolve the issue. " "If you are using standard firmware, you may need to upgrade to a newer version to resolve the issue."); - qCWarning(ParameterLoaderLog) << "Default component was never found, param:" << _defaultComponentIdParam; - emit parametersReady(true); + if (!qgcApp()->runningUnitTests()) { + qCWarning(ParameterLoaderLog) << "Default component was never found, param:" << _defaultComponentIdParam; + } + emit parametersReady(true /* missingParameters */); return; } // No failures, signal good load _parametersReady = true; _determineDefaultComponentId(); - emit parametersReady(false); + emit parametersReady(false /* no missingParameters */); } void ParameterLoader::_initialRequestTimeout(void) diff --git a/src/FactSystem/ParameterLoader.h b/src/FactSystem/ParameterLoader.h index b57dcb7ec013c20a6358552c5933dedf296c469c..944f5920a5bd16e8876ff8cd76c883baf27e64d2 100644 --- a/src/FactSystem/ParameterLoader.h +++ b/src/FactSystem/ParameterLoader.h @@ -151,7 +151,13 @@ private: int _parameterSetMajorVersion; ///< Version for parameter set, -1 if not known QObject* _parameterMetaData; ///< Opaque data from FirmwarePlugin::loadParameterMetaDataCall - static const int _maxInitialRequestListRetry = 5; ///< Maximum retries for request list + // Wait counts from previous parameter update cycle + int _prevWaitingReadParamIndexCount; + int _prevWaitingReadParamNameCount; + int _prevWaitingWriteParamNameCount; + + + static const int _maxInitialRequestListRetry = 4; ///< Maximum retries for request list int _initialRequestRetryCount; ///< Current retry count for request list static const int _maxInitialLoadRetrySingleParam = 10; ///< Maximum retries for initial index based load of a single param static const int _maxReadWriteRetry = 5; ///< Maximum retries read/write diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index ecf63346c757e35fb5810048f9d6e64c3df91937..f69dc9030bf23334cd885a46a5d4da182228fb6d 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -28,14 +28,16 @@ QGC_LOGGING_CATEGORY(MockLinkVerboseLog, "MockLinkVerboseLog") /// /// @author Don Gagne -float MockLink::_vehicleLatitude = 47.633033f; -float MockLink::_vehicleLongitude = -122.08794f; -float MockLink::_vehicleAltitude = 3.5f; -int MockLink::_nextVehicleSystemId = 128; +float MockLink::_vehicleLatitude = 47.633033f; +float MockLink::_vehicleLongitude = -122.08794f; +float MockLink::_vehicleAltitude = 3.5f; +int MockLink::_nextVehicleSystemId = 128; +const char* MockLink::_failParam = "COM_FLTMODE6"; const char* MockConfiguration::_firmwareTypeKey = "FirmwareType"; const char* MockConfiguration::_vehicleTypeKey = "VehicleType"; const char* MockConfiguration::_sendStatusTextKey = "SendStatusText"; +const char* MockConfiguration::_failureModeKey = "FailureMode"; MockLink::MockLink(MockConfiguration* config) : _missionItemHandler(this, qgcApp()->toolbox()->mavlinkProtocol()) @@ -52,14 +54,18 @@ MockLink::MockLink(MockConfiguration* config) , _fileServer(NULL) , _sendStatusText(false) , _apmSendHomePositionOnEmptyList(false) + , _failureMode(MockConfiguration::FailNone) , _sendHomePositionDelayCount(10) // No home position for 4 seconds , _sendGPSPositionDelayCount(100) // No gps lock for 5 seconds + , _currentParamRequestListComponentIndex(-1) + , _currentParamRequestListParamIndex(-1) { _config = config; if (_config) { _firmwareType = config->firmwareType(); _vehicleType = config->vehicleType(); _sendStatusText = config->sendStatusText(); + _failureMode = config->failureMode(); _config->setLink(this); } @@ -105,23 +111,23 @@ void MockLink::_disconnect(void) void MockLink::run(void) { - QTimer _timer1HzTasks; - QTimer _timer10HzTasks; - QTimer _timer50HzTasks; + QTimer timer1HzTasks; + QTimer timer10HzTasks; + QTimer timer500HzTasks; - QObject::connect(&_timer1HzTasks, &QTimer::timeout, this, &MockLink::_run1HzTasks); - QObject::connect(&_timer10HzTasks, &QTimer::timeout, this, &MockLink::_run10HzTasks); - QObject::connect(&_timer50HzTasks, &QTimer::timeout, this, &MockLink::_run50HzTasks); + QObject::connect(&timer1HzTasks, &QTimer::timeout, this, &MockLink::_run1HzTasks); + QObject::connect(&timer10HzTasks, &QTimer::timeout, this, &MockLink::_run10HzTasks); + QObject::connect(&timer500HzTasks, &QTimer::timeout, this, &MockLink::_run500HzTasks); - _timer1HzTasks.start(1000); - _timer10HzTasks.start(100); - _timer50HzTasks.start(20); + timer1HzTasks.start(1000); + timer10HzTasks.start(100); + timer500HzTasks.start(2); exec(); - QObject::disconnect(&_timer1HzTasks, &QTimer::timeout, this, &MockLink::_run1HzTasks); - QObject::disconnect(&_timer10HzTasks, &QTimer::timeout, this, &MockLink::_run10HzTasks); - QObject::disconnect(&_timer50HzTasks, &QTimer::timeout, this, &MockLink::_run50HzTasks); + QObject::disconnect(&timer1HzTasks, &QTimer::timeout, this, &MockLink::_run1HzTasks); + QObject::disconnect(&timer10HzTasks, &QTimer::timeout, this, &MockLink::_run10HzTasks); + QObject::disconnect(&timer500HzTasks, &QTimer::timeout, this, &MockLink::_run500HzTasks); _missionItemHandler.shutdown(); } @@ -160,9 +166,10 @@ void MockLink::_run10HzTasks(void) } } -void MockLink::_run50HzTasks(void) +void MockLink::_run500HzTasks(void) { if (_mavlinkStarted && _connected) { + _paramRequestListWorker(); } } @@ -532,6 +539,10 @@ float MockLink::_floatUnionForParam(int componentId, const QString& paramName) void MockLink::_handleParamRequestList(const mavlink_message_t& msg) { + if (_failureMode == MockConfiguration::FailParamNoReponseToRequestList) { + return; + } + mavlink_param_request_list_t request; mavlink_msg_param_request_list_decode(&msg, &request); @@ -539,76 +550,59 @@ void MockLink::_handleParamRequestList(const mavlink_message_t& msg) Q_ASSERT(request.target_system == _vehicleSystemId); Q_ASSERT(request.target_component == MAV_COMP_ID_ALL); - // We must send the first parameter for each component first. Otherwise system won't correctly know - // when all parameters are loaded. + // Start the worker routine + _currentParamRequestListComponentIndex = 0; + _currentParamRequestListParamIndex = 0; +} - foreach (int componentId, _mapParamName2Value.keys()) { - uint16_t paramIndex = 0; - int cParameters = _mapParamName2Value[componentId].count(); +/// Sends the next parameter to the vehicle +void MockLink::_paramRequestListWorker(void) +{ + if (_currentParamRequestListComponentIndex == -1) { + // Initial request complete + return; + } - foreach(const QString ¶mName, _mapParamName2Value[componentId].keys()) { - char paramId[MAVLINK_MSG_ID_PARAM_VALUE_LEN]; - mavlink_message_t responseMsg; + int componentId = _mapParamName2Value.keys()[_currentParamRequestListComponentIndex]; + int cParameters = _mapParamName2Value[componentId].count(); + QString paramName = _mapParamName2Value[componentId].keys()[_currentParamRequestListParamIndex]; - Q_ASSERT(_mapParamName2Value[componentId].contains(paramName)); - Q_ASSERT(_mapParamName2MavParamType.contains(paramName)); + if ((_failureMode == MockConfiguration::FailMissingParamOnInitialReqest || _failureMode == MockConfiguration::FailMissingParamOnAllRequests) && paramName == _failParam) { + qCDebug(MockLinkLog) << "Skipping param send:" << paramName; + } else { - MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[paramName]; + char paramId[MAVLINK_MSG_ID_PARAM_VALUE_LEN]; + mavlink_message_t responseMsg; - Q_ASSERT(paramName.length() <= MAVLINK_MSG_ID_PARAM_VALUE_LEN); - strncpy(paramId, paramName.toLocal8Bit().constData(), MAVLINK_MSG_ID_PARAM_VALUE_LEN); + Q_ASSERT(_mapParamName2Value[componentId].contains(paramName)); + Q_ASSERT(_mapParamName2MavParamType.contains(paramName)); - qCDebug(MockLinkLog) << "Sending msg_param_value" << componentId << paramId << paramType << _mapParamName2Value[componentId][paramId]; + MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[paramName]; - mavlink_msg_param_value_pack(_vehicleSystemId, - componentId, // component id - &responseMsg, // Outgoing message - paramId, // Parameter name - _floatUnionForParam(componentId, paramName), // Parameter value - paramType, // MAV_PARAM_TYPE - cParameters, // Total number of parameters - paramIndex++); // Index of this parameter - respondWithMavlinkMessage(responseMsg); + Q_ASSERT(paramName.length() <= MAVLINK_MSG_ID_PARAM_VALUE_LEN); + strncpy(paramId, paramName.toLocal8Bit().constData(), MAVLINK_MSG_ID_PARAM_VALUE_LEN); - // Only first parameter the first time through - break; - } + qCDebug(MockLinkLog) << "Sending msg_param_value" << componentId << paramId << paramType << _mapParamName2Value[componentId][paramId]; + + mavlink_msg_param_value_pack(_vehicleSystemId, + componentId, // component id + &responseMsg, // Outgoing message + paramId, // Parameter name + _floatUnionForParam(componentId, paramName), // Parameter value + paramType, // MAV_PARAM_TYPE + cParameters, // Total number of parameters + _currentParamRequestListParamIndex); // Index of this parameter + respondWithMavlinkMessage(responseMsg); } - foreach (int componentId, _mapParamName2Value.keys()) { - uint16_t paramIndex = 0; - int cParameters = _mapParamName2Value[componentId].count(); - bool skipParam = true; - - foreach(const QString ¶mName, _mapParamName2Value[componentId].keys()) { - if (skipParam) { - // We've already sent the first param - skipParam = false; - paramIndex++; - } else { - char paramId[MAVLINK_MSG_ID_PARAM_VALUE_LEN]; - mavlink_message_t responseMsg; - - Q_ASSERT(_mapParamName2Value[componentId].contains(paramName)); - Q_ASSERT(_mapParamName2MavParamType.contains(paramName)); - - MAV_PARAM_TYPE paramType = _mapParamName2MavParamType[paramName]; - - Q_ASSERT(paramName.length() <= MAVLINK_MSG_ID_PARAM_VALUE_LEN); - strncpy(paramId, paramName.toLocal8Bit().constData(), MAVLINK_MSG_ID_PARAM_VALUE_LEN); - - qCDebug(MockLinkLog) << "Sending msg_param_value" << componentId << paramId << paramType << _mapParamName2Value[componentId][paramId]; - - mavlink_msg_param_value_pack(_vehicleSystemId, - componentId, // component id - &responseMsg, // Outgoing message - paramId, // Parameter name - _floatUnionForParam(componentId, paramName), // Parameter value - paramType, // MAV_PARAM_TYPE - cParameters, // Total number of parameters - paramIndex++); // Index of this parameter - respondWithMavlinkMessage(responseMsg); - } + // Move to next param index + if (++_currentParamRequestListParamIndex >= cParameters) { + // We've sent the last parameter for this component, move to next component + if (++_currentParamRequestListComponentIndex >= _mapParamName2Value.keys().count()) { + // We've finished sending the last parameter for the last component, request is complete + _currentParamRequestListComponentIndex = -1; + } else { + _currentParamRequestListParamIndex = 0; } } } @@ -654,11 +648,11 @@ void MockLink::_handleParamRequestRead(const mavlink_message_t& msg) mavlink_param_request_read_t request; mavlink_msg_param_request_read_decode(&msg, &request); - const QString param_name(QString::fromLocal8Bit(request.param_id, strnlen(request.param_id, MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN))); + const QString paramName(QString::fromLocal8Bit(request.param_id, strnlen(request.param_id, MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN))); int componentId = request.target_component; // special case for magic _HASH_CHECK value - if (request.target_component == MAV_COMP_ID_ALL && param_name == "_HASH_CHECK") { + if (request.target_component == MAV_COMP_ID_ALL && paramName == "_HASH_CHECK") { mavlink_param_union_t valueUnion; valueUnion.type = MAV_PARAM_TYPE_UINT32; valueUnion.param_uint32 = 0; @@ -698,6 +692,12 @@ void MockLink::_handleParamRequestRead(const mavlink_message_t& msg) Q_ASSERT(_mapParamName2Value[componentId].contains(paramId)); Q_ASSERT(_mapParamName2MavParamType.contains(paramId)); + if (_failureMode == MockConfiguration::FailMissingParamOnAllRequests && strcmp(paramId, _failParam) == 0) { + qCDebug(MockLinkLog) << "Ignoring request read for " << _failParam; + // Fail to send this param no matter what + return; + } + mavlink_msg_param_value_pack(_vehicleSystemId, componentId, // component id &responseMsg, // Outgoing message @@ -899,6 +899,7 @@ MockConfiguration::MockConfiguration(const QString& name) , _firmwareType(MAV_AUTOPILOT_PX4) , _vehicleType(MAV_TYPE_QUADROTOR) , _sendStatusText(false) + , _failureMode(FailNone) { } @@ -909,6 +910,7 @@ MockConfiguration::MockConfiguration(MockConfiguration* source) _firmwareType = source->_firmwareType; _vehicleType = source->_vehicleType; _sendStatusText = source->_sendStatusText; + _failureMode = source->_failureMode; } void MockConfiguration::copyFrom(LinkConfiguration *source) @@ -924,6 +926,7 @@ void MockConfiguration::copyFrom(LinkConfiguration *source) _firmwareType = usource->_firmwareType; _vehicleType = usource->_vehicleType; _sendStatusText = usource->_sendStatusText; + _failureMode = usource->_failureMode; } void MockConfiguration::saveSettings(QSettings& settings, const QString& root) @@ -932,6 +935,7 @@ void MockConfiguration::saveSettings(QSettings& settings, const QString& root) settings.setValue(_firmwareTypeKey, (int)_firmwareType); settings.setValue(_vehicleTypeKey, (int)_vehicleType); settings.setValue(_sendStatusTextKey, _sendStatusText); + settings.setValue(_failureModeKey, (int)_failureMode); settings.sync(); settings.endGroup(); } @@ -942,6 +946,7 @@ void MockConfiguration::loadSettings(QSettings& settings, const QString& root) _firmwareType = (MAV_AUTOPILOT)settings.value(_firmwareTypeKey, (int)MAV_AUTOPILOT_PX4).toInt(); _vehicleType = (MAV_TYPE)settings.value(_vehicleTypeKey, (int)MAV_TYPE_QUADROTOR).toInt(); _sendStatusText = settings.value(_sendStatusTextKey, false).toBool(); + _failureMode = (FailureMode_t)settings.value(_failureModeKey, (int)FailNone).toInt(); settings.endGroup(); } @@ -967,46 +972,50 @@ MockLink* MockLink::_startMockLink(MockConfiguration* mockConfig) return qobject_cast(linkManager->createConnectedLink(mockConfig)); } -MockLink* MockLink::startPX4MockLink(bool sendStatusText) +MockLink* MockLink::startPX4MockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) { MockConfiguration* mockConfig = new MockConfiguration("PX4 MockLink"); mockConfig->setFirmwareType(MAV_AUTOPILOT_PX4); mockConfig->setVehicleType(MAV_TYPE_QUADROTOR); mockConfig->setSendStatusText(sendStatusText); + mockConfig->setFailureMode(failureMode); return _startMockLink(mockConfig); } -MockLink* MockLink::startGenericMockLink(bool sendStatusText) +MockLink* MockLink::startGenericMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) { MockConfiguration* mockConfig = new MockConfiguration("Generic MockLink"); mockConfig->setFirmwareType(MAV_AUTOPILOT_GENERIC); mockConfig->setVehicleType(MAV_TYPE_QUADROTOR); mockConfig->setSendStatusText(sendStatusText); + mockConfig->setFailureMode(failureMode); return _startMockLink(mockConfig); } -MockLink* MockLink::startAPMArduCopterMockLink(bool sendStatusText) +MockLink* MockLink::startAPMArduCopterMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) { MockConfiguration* mockConfig = new MockConfiguration("APM ArduCopter MockLink"); mockConfig->setFirmwareType(MAV_AUTOPILOT_ARDUPILOTMEGA); mockConfig->setVehicleType(MAV_TYPE_QUADROTOR); mockConfig->setSendStatusText(sendStatusText); + mockConfig->setFailureMode(failureMode); return _startMockLink(mockConfig); } -MockLink* MockLink::startAPMArduPlaneMockLink(bool sendStatusText) +MockLink* MockLink::startAPMArduPlaneMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode) { MockConfiguration* mockConfig = new MockConfiguration("APM ArduPlane MockLink"); mockConfig->setFirmwareType(MAV_AUTOPILOT_ARDUPILOTMEGA); mockConfig->setVehicleType(MAV_TYPE_FIXED_WING); mockConfig->setSendStatusText(sendStatusText); + mockConfig->setFailureMode(failureMode); return _startMockLink(mockConfig); } diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index 743115a1f6088326cfc6ed9ea083b6cee9046993..fb190cdfcfe426e53c6e19b403dc072add5bc031 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -51,6 +51,15 @@ public: bool sendStatusText(void) { return _sendStatusText; } void setSendStatusText(bool sendStatusText) { _sendStatusText = sendStatusText; emit sendStatusChanged(); } + typedef enum { + FailNone, // No failures + FailParamNoReponseToRequestList, // Do no respond to PARAM_REQUEST_LIST + FailMissingParamOnInitialReqest, // Not all params are sent on initial request, should still succeed since QGC will re-query missing params + FailMissingParamOnAllRequests, // Not all params are sent on initial request, QGC retries will fail as well + } FailureMode_t; + FailureMode_t failureMode(void) { return _failureMode; } + void setFailureMode(FailureMode_t failureMode) { _failureMode = failureMode; } + // Overrides from LinkConfiguration LinkType type (void) { return LinkConfiguration::TypeMock; } void copyFrom (LinkConfiguration* source); @@ -68,10 +77,12 @@ private: MAV_AUTOPILOT _firmwareType; MAV_TYPE _vehicleType; bool _sendStatusText; + FailureMode_t _failureMode; static const char* _firmwareTypeKey; static const char* _vehicleTypeKey; static const char* _sendStatusTextKey; + static const char* _failureModeKey; }; class MockLink : public LinkInterface @@ -88,6 +99,7 @@ public: MAV_AUTOPILOT getFirmwareType(void) { return _firmwareType; } void setFirmwareType(MAV_AUTOPILOT autopilot) { _firmwareType = autopilot; } void setSendStatusText(bool sendStatusText) { _sendStatusText = sendStatusText; } + void setFailureMode(MockConfiguration::FailureMode_t failureMode) { _failureMode = failureMode; } /// 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 @@ -132,10 +144,10 @@ public: /// Reset the state of the MissionItemHandler to no items, no transactions in progress. void resetMissionItemHandler(void) { _missionItemHandler.reset(); } - static MockLink* startPX4MockLink (bool sendStatusText); - static MockLink* startGenericMockLink (bool sendStatusText); - static MockLink* startAPMArduCopterMockLink (bool sendStatusText); - static MockLink* startAPMArduPlaneMockLink (bool sendStatusText); + static MockLink* startPX4MockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); + static MockLink* startGenericMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); + static MockLink* startAPMArduCopterMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); + static MockLink* startAPMArduPlaneMockLink (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone); private slots: virtual void _writeBytes(const QByteArray bytes); @@ -143,7 +155,7 @@ private slots: private slots: void _run1HzTasks(void); void _run10HzTasks(void); - void _run50HzTasks(void); + void _run500HzTasks(void); private: // From LinkInterface @@ -175,6 +187,7 @@ private: void _sendStatusTextMessages(void); void _respondWithAutopilotVersion(void); void _sendRCChannels(void); + void _paramRequestListWorker(void); static MockLink* _startMockLink(MockConfiguration* mockConfig); @@ -204,14 +217,19 @@ private: bool _sendStatusText; bool _apmSendHomePositionOnEmptyList; + MockConfiguration::FailureMode_t _failureMode; int _sendHomePositionDelayCount; int _sendGPSPositionDelayCount; - static float _vehicleLatitude; - static float _vehicleLongitude; - static float _vehicleAltitude; - static int _nextVehicleSystemId; + int _currentParamRequestListComponentIndex; // Current component index for param request list workflow, -1 for no request in progress + int _currentParamRequestListParamIndex; // Current parameter index for param request list workflor + + static float _vehicleLatitude; + static float _vehicleLongitude; + static float _vehicleAltitude; + static int _nextVehicleSystemId; + static const char* _failParam; }; #endif diff --git a/src/comm/PX4MockLink.params b/src/comm/PX4MockLink.params index df7aa97580dbaf19bb192edb4cdfee7131fa0e1a..f975f257b6d1a6600a186cfdfbd236b10bc348fd 100644 --- a/src/comm/PX4MockLink.params +++ b/src/comm/PX4MockLink.params @@ -7,10 +7,10 @@ 1 50 ATT_J33 0.0037 9 1 50 ATT_J_EN 0 6 1 50 ATT_MAG_DECL 0 9 -1 50 BAT_CRIT_THR 0.05 0.09 +1 50 BAT_CRIT_THR 0.05 9 1 50 BAT_CAPACITY -1 9 1 50 BAT_C_SCALING 0.0124 9 -1 50 BAT_LOW_THR 0.15 0.18 +1 50 BAT_LOW_THR 0.15 9 1 50 BAT_N_CELLS 3 6 1 50 BAT_V_CHARGED 4.2 9 1 50 BAT_V_EMPTY 3.4 9 @@ -109,7 +109,7 @@ 1 50 COM_FLTMODE4 -1 6 1 50 COM_FLTMODE5 -1 6 1 50 COM_FLTMODE6 -1 6 -1 50 COM_LOW_BAT_ACT 0 0 +1 50 COM_LOW_BAT_ACT 0 5 1 50 COM_RC_IN_MODE 0 6 1 50 COM_RC_LOSS_T 0.5 9 1 50 EKF_ATT_V3_Q0 0.0001 9 diff --git a/src/qgcunittest/ParameterLoaderTest.cc b/src/qgcunittest/ParameterLoaderTest.cc new file mode 100644 index 0000000000000000000000000000000000000000..0089f410c31c5e66530db0bcc0c1f8f95a0f179f --- /dev/null +++ b/src/qgcunittest/ParameterLoaderTest.cc @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + + +#include "ParameterLoaderTest.h" +#include "MultiVehicleManager.h" +#include "QGCApplication.h" +#include "ParameterLoader.h" + +/// Test failure modes which should still lead to param load success +void ParameterLoaderTest::_noFailureWorker(MockConfiguration::FailureMode_t failureMode) +{ + Q_ASSERT(!_mockLink); + _mockLink = MockLink::startPX4MockLink(false, failureMode); + + MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); + QVERIFY(vehicleMgr); + + // Wait for the Vehicle to get created + QSignalSpy spyVehicle(vehicleMgr, SIGNAL(activeVehicleAvailableChanged(bool))); + QCOMPARE(spyVehicle.wait(5000), true); + QCOMPARE(spyVehicle.count(), 1); + QList arguments = spyVehicle.takeFirst(); + QCOMPARE(arguments.count(), 1); + QCOMPARE(arguments.at(0).toBool(), true); + + Vehicle* vehicle = vehicleMgr->activeVehicle(); + QVERIFY(vehicle); + + // We should get progress bar updates during load + QSignalSpy spyProgress(vehicle->getParameterLoader(), SIGNAL(parameterListProgress(float))); + QCOMPARE(spyProgress.wait(2000), true); + arguments = spyProgress.takeFirst(); + QCOMPARE(arguments.count(), 1); + QVERIFY(arguments.at(0).toFloat() > 0.0f); + + // When param load is complete we get the param ready signal + QSignalSpy spyParamsReady(vehicleMgr, SIGNAL(parameterReadyVehicleAvailableChanged(bool))); + QCOMPARE(spyParamsReady.wait(60000), true); + arguments = spyParamsReady.takeFirst(); + QCOMPARE(arguments.count(), 1); + QCOMPARE(arguments.at(0).toBool(), true); + + // Progress should have been set back to 0 + arguments = spyProgress.takeLast(); + QCOMPARE(arguments.count(), 1); + QCOMPARE(arguments.at(0).toFloat(), 0.0f); +} + + +void ParameterLoaderTest::_noFailure(void) +{ + _noFailureWorker(MockConfiguration::FailNone); +} + +void ParameterLoaderTest::_requestListMissingParamSuccess(void) +{ + _noFailureWorker(MockConfiguration::FailMissingParamOnInitialReqest); +} + +// Test no response to param_request_list +void ParameterLoaderTest::_requestListNoResponse(void) +{ + Q_ASSERT(!_mockLink); + _mockLink = MockLink::startPX4MockLink(false, MockConfiguration::FailParamNoReponseToRequestList); + + MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); + QVERIFY(vehicleMgr); + + // Wait for the Vehicle to get created + QSignalSpy spyVehicle(vehicleMgr, SIGNAL(activeVehicleAvailableChanged(bool))); + QCOMPARE(spyVehicle.wait(5000), true); + QCOMPARE(spyVehicle.count(), 1); + QList arguments = spyVehicle.takeFirst(); + QCOMPARE(arguments.count(), 1); + QCOMPARE(arguments.at(0).toBool(), true); + + Vehicle* vehicle = vehicleMgr->activeVehicle(); + QVERIFY(vehicle); + + QSignalSpy spyParamsReady(vehicleMgr, SIGNAL(parameterReadyVehicleAvailableChanged(bool))); + QSignalSpy spyProgress(vehicle->getParameterLoader(), SIGNAL(parameterListProgress(float))); + + // We should not get any progress bar updates, nor a parameter ready signal + QCOMPARE(spyProgress.wait(500), false); + QCOMPARE(spyParamsReady.wait(40000), false); + + // User should have been notified + checkMultipleExpectedMessageBox(5); +} + +// MockLink will fail to send a param on initial request, it will also fail to send it on subsequent +// param_read requests. +void ParameterLoaderTest::_requestListMissingParamFail(void) +{ + // Will pop error about missing params + setExpectedMessageBox(QMessageBox::Ok); + + Q_ASSERT(!_mockLink); + _mockLink = MockLink::startPX4MockLink(false, MockConfiguration::FailMissingParamOnAllRequests); + + MultiVehicleManager* vehicleMgr = qgcApp()->toolbox()->multiVehicleManager(); + QVERIFY(vehicleMgr); + + // Wait for the Vehicle to get created + QSignalSpy spyVehicle(vehicleMgr, SIGNAL(activeVehicleAvailableChanged(bool))); + QCOMPARE(spyVehicle.wait(5000), true); + QCOMPARE(spyVehicle.count(), 1); + QList arguments = spyVehicle.takeFirst(); + QCOMPARE(arguments.count(), 1); + QCOMPARE(arguments.at(0).toBool(), true); + + Vehicle* vehicle = vehicleMgr->activeVehicle(); + QVERIFY(vehicle); + + QSignalSpy spyParamsReady(vehicleMgr, SIGNAL(parameterReadyVehicleAvailableChanged(bool))); + QSignalSpy spyProgress(vehicle->getParameterLoader(), SIGNAL(parameterListProgress(float))); + + // We will get progress bar updates, since it will fail after getting partially through the request + QCOMPARE(spyProgress.wait(2000), true); + arguments = spyProgress.takeFirst(); + QCOMPARE(arguments.count(), 1); + QVERIFY(arguments.at(0).toFloat() > 0.0f); + + // We should get a parameters ready signal, but Vehicle should indicate missing params + QCOMPARE(spyParamsReady.wait(40000), true); + QCOMPARE(vehicle->missingParameters(), true); + + // User should have been notified + checkExpectedMessageBox(); +} diff --git a/src/qgcunittest/ParameterLoaderTest.h b/src/qgcunittest/ParameterLoaderTest.h new file mode 100644 index 0000000000000000000000000000000000000000..2b251bd430047c5e7107c7d19f5abb614f03a848 --- /dev/null +++ b/src/qgcunittest/ParameterLoaderTest.h @@ -0,0 +1,33 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + + +#ifndef ParameterLoaderTest_H +#define ParameterLoaderTest_H + +#include "UnitTest.h" +#include "MockLink.h" +#include "MultiSignalSpy.h" +#include "MockLink.h" + +class ParameterLoaderTest : public UnitTest +{ + Q_OBJECT + +private slots: + void _noFailure(void); + void _requestListNoResponse(void); + void _requestListMissingParamSuccess(void); + void _requestListMissingParamFail(void); + +private: + void _noFailureWorker(MockConfiguration::FailureMode_t failureMode); +}; + +#endif diff --git a/src/qgcunittest/UnitTest.cc b/src/qgcunittest/UnitTest.cc index fcebc5baa17f9d64d2e71e98d7d0c4edd188f136..d910e26726d32a43810888705c47f657fefb2458 100644 --- a/src/qgcunittest/UnitTest.cc +++ b/src/qgcunittest/UnitTest.cc @@ -198,6 +198,13 @@ void UnitTest::checkExpectedMessageBox(int expectFailFlags) QCOMPARE(messageBoxRespondedTo, true); } +void UnitTest::checkMultipleExpectedMessageBox(int messageCount) +{ + int missedMessageBoxCount = _missedMessageBoxCount; + _missedMessageBoxCount = 0; + QCOMPARE(missedMessageBoxCount, messageCount); +} + void UnitTest::checkExpectedFileDialog(int expectFailFlags) { // Internal testing @@ -377,7 +384,7 @@ void UnitTest::_connectMockLink(MAV_AUTOPILOT autopilot) // Wait for the Vehicle to get created QSignalSpy spyVehicle(qgcApp()->toolbox()->multiVehicleManager(), SIGNAL(parameterReadyVehicleAvailableChanged(bool))); - QCOMPARE(spyVehicle.wait(5000), true); + QCOMPARE(spyVehicle.wait(10000), true); QVERIFY(qgcApp()->toolbox()->multiVehicleManager()->parameterReadyVehicleAvailable()); _vehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle(); QVERIFY(_vehicle); diff --git a/src/qgcunittest/UnitTest.h b/src/qgcunittest/UnitTest.h index a552621bc0fbb2b18536d1a9cf600ac2de43f0bb..849c4f86867ee2a83cdfb400512c93cbba6a69bc 100644 --- a/src/qgcunittest/UnitTest.h +++ b/src/qgcunittest/UnitTest.h @@ -74,6 +74,9 @@ public: // @param Expected failure response flags void checkExpectedMessageBox(int expectFailFlags = expectFailNoFailure); + /// Checks that the specified number of message boxes where shown. Do not call setExpectedMessageBox when using this method. + void checkMultipleExpectedMessageBox(int messageCount); + /// @brief Check whether a message box was displayed and correctly responded to // @param Expected failure response flags void checkExpectedFileDialog(int expectFailFlags = expectFailNoFailure); diff --git a/src/qgcunittest/UnitTestList.cc b/src/qgcunittest/UnitTestList.cc index 7b1a1b461d0d4af33dc92891d126d1a04068b4dc..c8355a40e0a4c5ca64c35d392f62d7f78383db57 100644 --- a/src/qgcunittest/UnitTestList.cc +++ b/src/qgcunittest/UnitTestList.cc @@ -29,6 +29,7 @@ #include "MainWindowTest.h" #include "FileManagerTest.h" #include "TCPLinkTest.h" +#include "ParameterLoaderTest.h" UT_REGISTER_TEST(FactSystemTestGeneric) UT_REGISTER_TEST(FactSystemTestPX4) @@ -46,6 +47,7 @@ UT_REGISTER_TEST(MissionManagerTest) UT_REGISTER_TEST(RadioConfigTest) UT_REGISTER_TEST(TCPLinkTest) UT_REGISTER_TEST(FileManagerTest) +UT_REGISTER_TEST(ParameterLoaderTest) // List of unit test which are currently disabled. // If disabling a new test, include reason in comment.