diff --git a/ChangeLog.md b/ChangeLog.md index de9e9c84cc44081dc9de41132a42f1edf5ecc74c..4926aee2b08e60ab5e368f90a051e3301ce7cd64 100644 --- a/ChangeLog.md +++ b/ChangeLog.md @@ -6,6 +6,9 @@ Note: This file only contains high level features or important fixes. ### 3.6.0 - Daily Build +* ArduPilot: Support configurable mavlink stream rates. Available from Settings/Mavlink page. +* Major rewrite and bug fix pass through Structure Scan. Previous version had such bad problems that it can no longer be supported. Plans with Structure Scan will need to be recreated. New QGC will not load old Structure Scan plans. + * Major rewrite and bug fix pass through Structure Scan. Previous version had such bad problems that it can no longer be supported. Plans with Structure Scan will need to be recreated. New QGC will not load old Structure Scan plans. ### 3.5.1 - Not yet released diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 0197b2ce9fd8fd17da62f058cbbf91f36dd3b94b..6e79009583e3f314c0f2318eee03d661422e4686 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -596,6 +596,7 @@ HEADERS += \ src/QmlControls/RCChannelMonitorController.h \ src/QmlControls/ScreenToolsController.h \ src/QtLocationPlugin/QMLControl/QGCMapEngineManager.h \ + src/Settings/APMMavlinkStreamRateSettings.h \ src/Settings/AppSettings.h \ src/Settings/AutoConnectSettings.h \ src/Settings/BrandImageSettings.h \ @@ -799,6 +800,7 @@ SOURCES += \ src/QmlControls/RCChannelMonitorController.cc \ src/QmlControls/ScreenToolsController.cc \ src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc \ + src/Settings/APMMavlinkStreamRateSettings.cc \ src/Settings/AppSettings.cc \ src/Settings/AutoConnectSettings.cc \ src/Settings/BrandImageSettings.cc \ diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index e51d4ac77084d4af3bd03de92be6ef60c9696d3d..b811f360d4917c9f150c198e73a2c3c590b6ad3a 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -210,6 +210,7 @@ src/FlightDisplay/VirtualJoystick.qml + src/Settings/APMMavlinkStreamRate.SettingsGroup.json src/MissionManager/CameraCalc.FactMetaData.json src/MissionManager/CameraSpec.FactMetaData.json src/MissionManager/CorridorScan.SettingsGroup.json diff --git a/src/FactSystem/FactGroup.cc b/src/FactSystem/FactGroup.cc index 31913205c3faf63efdba52a4bd722bb636742cbf..c67c02d8f55a2a2ec118db30cf9dd7b46747aa6d 100644 --- a/src/FactSystem/FactGroup.cc +++ b/src/FactSystem/FactGroup.cc @@ -37,7 +37,8 @@ FactGroup::FactGroup(int updateRateMsecs, QObject* parent) void FactGroup::_loadFromJsonArray(const QJsonArray jsonArray) { - _nameToFactMetaDataMap = FactMetaData::createMapFromJsonArray(jsonArray, this); + QMap defineMap; + _nameToFactMetaDataMap = FactMetaData::createMapFromJsonArray(jsonArray, defineMap, this); } void FactGroup::_setupTimer() diff --git a/src/FactSystem/FactMetaData.cc b/src/FactSystem/FactMetaData.cc index 4d07e198b64cba2da5b01b702edf799cfd485e83..570149ab994c7aaec2914c825879bbd484d6aaae 100644 --- a/src/FactSystem/FactMetaData.cc +++ b/src/FactSystem/FactMetaData.cc @@ -35,6 +35,9 @@ const qreal FactMetaData::UnitConsts_s::inchesToCentimeters = 2.54; static const char* kDefaultCategory = QT_TRANSLATE_NOOP("FactMetaData", "Other"); static const char* kDefaultGroup = QT_TRANSLATE_NOOP("FactMetaData", "Misc"); +const char* FactMetaData::_jsonMetaDataDefinesName = "QGC.MetaData.Defines"; +const char* FactMetaData::_jsonMetaDataFactsName = "QGC.MetaData.Facts"; + // Built in translations for all Facts const FactMetaData::BuiltInTranslation_s FactMetaData::_rgBuiltInTranslations[] = { { "centi-degrees", "deg", FactMetaData::_centiDegreesToDegrees, FactMetaData::_degreesToCentiDegrees }, @@ -1057,7 +1060,7 @@ int FactMetaData::decimalPlaces(void) const return actualDecimalPlaces; } -FactMetaData* FactMetaData::createFromJsonObject(const QJsonObject& json, QObject* metaDataParent) +FactMetaData* FactMetaData::createFromJsonObject(const QJsonObject& json, QMap& defineMap, QObject* metaDataParent) { QString errorString; @@ -1098,7 +1101,7 @@ FactMetaData* FactMetaData::createFromJsonObject(const QJsonObject& json, QObjec metaData->_name = json[_nameJsonKey].toString(); QStringList enumValues, enumStrings; - if (JsonHelper::parseEnum(json, enumStrings, enumValues, errorString, metaData->name())) { + if (JsonHelper::parseEnum(json, defineMap, enumStrings, enumValues, errorString, metaData->name())) { for (int i=0; i& defineMap) +{ + for (const QString& defineName: jsonDefinesObject.keys()) { + QString mapKey = _jsonMetaDataDefinesName + QString(".") + defineName; + defineMap[mapKey] = jsonDefinesObject[defineName].toString(); + } +} + QMap FactMetaData::createMapFromJsonFile(const QString& jsonFilename, QObject* metaDataParent) { QMap metaDataMap; @@ -1223,16 +1234,34 @@ QMap FactMetaData::createMapFromJsonFile(const QString& return metaDataMap; } - if (!doc.isArray()) { - qWarning() << "json document is not array"; + QJsonArray factArray; + QMap defineMap; + + if (doc.isObject()) { + // Check for Defines/Facts format + QString errorString; + QList keyInfoList = { + { FactMetaData::_jsonMetaDataDefinesName, QJsonValue::Object, true }, + { FactMetaData::_jsonMetaDataFactsName, QJsonValue::Array, true }, + }; + if (!JsonHelper::validateKeys(doc.object(), keyInfoList, errorString)) { + qWarning() << "Json document incorrect format:" << errorString; + return metaDataMap; + } + + _loadJsonDefines(doc.object()[FactMetaData::_jsonMetaDataDefinesName].toObject(), defineMap); + factArray = doc.object()[FactMetaData::_jsonMetaDataFactsName].toArray(); + } else if (doc.isArray()) { + factArray = doc.array(); + } else { + qWarning() << "Json document is neither array nor object"; return metaDataMap; } - QJsonArray jsonArray = doc.array(); - return createMapFromJsonArray(jsonArray, metaDataParent); + return createMapFromJsonArray(factArray, defineMap, metaDataParent); } -QMap FactMetaData::createMapFromJsonArray(const QJsonArray jsonArray, QObject* metaDataParent) +QMap FactMetaData::createMapFromJsonArray(const QJsonArray jsonArray, QMap& defineMap, QObject* metaDataParent) { QMap metaDataMap; for (int i=0; i FactMetaData::createMapFromJsonArray(const QJsonArr continue; } QJsonObject jsonObject = jsonValue.toObject(); - FactMetaData* metaData = createFromJsonObject(jsonObject, metaDataParent); + FactMetaData* metaData = createFromJsonObject(jsonObject, defineMap, metaDataParent); if (metaDataMap.contains(metaData->name())) { qWarning() << QStringLiteral("Duplicate fact name:") << metaData->name(); delete metaData; diff --git a/src/FactSystem/FactMetaData.h b/src/FactSystem/FactMetaData.h index 8b02c9930b57a6ee229b42544bd9c1b446f69191..04a7837f102b54c911b5435cf67126065b939931 100644 --- a/src/FactSystem/FactMetaData.h +++ b/src/FactSystem/FactMetaData.h @@ -54,9 +54,9 @@ public: FactMetaData(const FactMetaData& other, QObject* parent = nullptr); static QMap createMapFromJsonFile(const QString& jsonFilename, QObject* metaDataParent); - static QMap createMapFromJsonArray(const QJsonArray jsonArray, QObject* metaDataParent); + static QMap createMapFromJsonArray(const QJsonArray jsonArray, QMap& defineMap, QObject* metaDataParent); - static FactMetaData* createFromJsonObject(const QJsonObject& json, QObject* metaDataParent); + static FactMetaData* createFromJsonObject(const QJsonObject& json, QMap& defineMap, QObject* metaDataParent); const FactMetaData& operator=(const FactMetaData& other); @@ -229,6 +229,8 @@ private: static const AppSettingsTranslation_s* _findAppSettingsDistanceUnitsTranslation(const QString& rawUnits); static const AppSettingsTranslation_s* _findAppSettingsAreaUnitsTranslation(const QString& rawUnits); + static void _loadJsonDefines(const QJsonObject& jsonDefinesObject, QMap& defineMap); + ValueType_t _type; // must be first for correct constructor init int _decimalPlaces; QVariant _rawDefaultValue; @@ -292,6 +294,9 @@ private: static const char* _incrementJsonKey; static const char* _hasControlJsonKey; static const char* _qgcRebootRequiredJsonKey; + + static const char* _jsonMetaDataDefinesName; + static const char* _jsonMetaDataFactsName; }; #endif diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 68119e7ffcf0ba468f1dff1e788b441d3f50428e..c9b52dc2ba2f042c44a51fea0907b83d70a5ebd6 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -19,6 +19,7 @@ #include "QGCFileDownload.h" #include "SettingsManager.h" #include "AppSettings.h" +#include "APMMavlinkStreamRateSettings.h" #include @@ -614,13 +615,30 @@ void APMFirmwarePlugin::_adjustCalibrationMessageSeverity(mavlink_message_t* mes void APMFirmwarePlugin::initializeStreamRates(Vehicle* vehicle) { - vehicle->requestDataStream(MAV_DATA_STREAM_RAW_SENSORS, 2); - vehicle->requestDataStream(MAV_DATA_STREAM_EXTENDED_STATUS, 2); - vehicle->requestDataStream(MAV_DATA_STREAM_RC_CHANNELS, 2); - vehicle->requestDataStream(MAV_DATA_STREAM_POSITION, 3); - vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA1, 10); - vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA2, 10); - vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA3, 3); + APMMavlinkStreamRateSettings* streamRates = qgcApp()->toolbox()->settingsManager()->apmMavlinkStreamRateSettings(); + + struct StreamInfo_s { + MAV_DATA_STREAM mavStream; + int streamRate; + }; + + StreamInfo_s rgStreamInfo[] = { + { MAV_DATA_STREAM_RAW_SENSORS, streamRates->streamRateRawSensors()->rawValue().toInt() }, + { MAV_DATA_STREAM_EXTENDED_STATUS, streamRates->streamRateExtendedStatus()->rawValue().toInt() }, + { MAV_DATA_STREAM_RC_CHANNELS, streamRates->streamRateRCChannels()->rawValue().toInt() }, + { MAV_DATA_STREAM_POSITION, streamRates->streamRatePosition()->rawValue().toInt() }, + { MAV_DATA_STREAM_EXTRA1, streamRates->streamRateExtra1()->rawValue().toInt() }, + { MAV_DATA_STREAM_EXTRA2, streamRates->streamRateExtra2()->rawValue().toInt() }, + { MAV_DATA_STREAM_EXTRA3, streamRates->streamRateExtra3()->rawValue().toInt() }, + }; + + for (size_t i=0; i= 0) { + vehicle->requestDataStream(streamInfo.mavStream, static_cast(streamInfo.streamRate)); + } + } } @@ -733,22 +751,16 @@ QString APMFirmwarePlugin::missionCommandOverrides(MAV_TYPE vehicleType) const switch (vehicleType) { case MAV_TYPE_GENERIC: return QStringLiteral(":/json/APM/MavCmdInfoCommon.json"); - break; case MAV_TYPE_FIXED_WING: return QStringLiteral(":/json/APM/MavCmdInfoFixedWing.json"); - break; case MAV_TYPE_QUADROTOR: return QStringLiteral(":/json/APM/MavCmdInfoMultiRotor.json"); - break; case MAV_TYPE_VTOL_QUADROTOR: return QStringLiteral(":/json/APM/MavCmdInfoVTOL.json"); - break; case MAV_TYPE_SUBMARINE: return QStringLiteral(":/json/APM/MavCmdInfoSub.json"); - break; case MAV_TYPE_GROUND_ROVER: return QStringLiteral(":/json/APM/MavCmdInfoRover.json"); - break; default: qWarning() << "APMFirmwarePlugin::missionCommandOverrides called with bad MAV_TYPE:" << vehicleType; return QString(); diff --git a/src/JsonHelper.cc b/src/JsonHelper.cc index 6ef592720734dc5d4bc1a70ccbcc4b0ac1fd11aa..2470251159c8682b0ac818ed58c3da20a670eb03 100644 --- a/src/JsonHelper.cc +++ b/src/JsonHelper.cc @@ -152,7 +152,7 @@ bool JsonHelper::validateKeyTypes(const QJsonObject& jsonObject, const QStringLi return true; } -bool JsonHelper::parseEnum(const QJsonObject& jsonObject, QStringList& enumStrings, QStringList& enumValues, QString& errorString, QString valueName) +bool JsonHelper::_parseEnumWorker(const QJsonObject& jsonObject, QMap& defineMap, QStringList& enumStrings, QStringList& enumValues, QString& errorString, QString valueName) { if(jsonObject.value(_enumStringsJsonKey).isArray()) { // "enumStrings": ["Auto" , "Manual", "Shutter Priority", "Aperture Priority"], @@ -162,7 +162,8 @@ bool JsonHelper::parseEnum(const QJsonObject& jsonObject, QStringList& enumStrin } } else { // "enumStrings": "Auto,Manual,Shutter Priority,Aperture Priority", - enumStrings = jsonObject.value(_enumStringsJsonKey).toString().split(",", QString::SkipEmptyParts); + QString value = jsonObject.value(_enumStringsJsonKey).toString(); + enumStrings = defineMap.value(value, value).split(",", QString::SkipEmptyParts); } if(jsonObject.value(_enumValuesJsonKey).isArray()) { @@ -177,7 +178,8 @@ bool JsonHelper::parseEnum(const QJsonObject& jsonObject, QStringList& enumStrin } } else { // "enumValues": "0,1,2,3,4,5", - enumValues = jsonObject.value(_enumValuesJsonKey).toString().split(",", QString::SkipEmptyParts); + QString value = jsonObject.value(_enumValuesJsonKey).toString(); + enumValues = defineMap.value(value, value).split(",", QString::SkipEmptyParts); } if (enumStrings.count() != enumValues.count()) { @@ -188,6 +190,17 @@ bool JsonHelper::parseEnum(const QJsonObject& jsonObject, QStringList& enumStrin return true; } +bool JsonHelper::parseEnum(const QJsonObject& jsonObject, QMap& defineMap, QStringList& enumStrings, QStringList& enumValues, QString& errorString, QString valueName) +{ + return _parseEnumWorker(jsonObject, defineMap, enumStrings, enumValues, errorString, valueName); +} + +bool JsonHelper::parseEnum(const QJsonObject& jsonObject, QStringList& enumStrings, QStringList& enumValues, QString& errorString, QString valueName) +{ + QMap defineMap; + return _parseEnumWorker(jsonObject, defineMap, enumStrings, enumValues, errorString, valueName); +} + bool JsonHelper::isJsonFile(const QByteArray& bytes, QJsonDocument& jsonDoc, QString& errorString) { QJsonParseError parseError; diff --git a/src/JsonHelper.h b/src/JsonHelper.h index 711c38ace027946e19fbf1fdc6a5e5ba25d6bc24..2398e6f2c5aff5bbed213e66aebbb197d5b1a8a5 100644 --- a/src/JsonHelper.h +++ b/src/JsonHelper.h @@ -122,6 +122,7 @@ public: QJsonArray& polygonArray); ///< Array to save into static bool parseEnum(const QJsonObject& jsonObject, QStringList& enumStrings, QStringList& enumValues, QString& errorString, QString valueName = QString()); + static bool parseEnum(const QJsonObject& jsonObject, QMap& defineMap, QStringList& enumStrings, QStringList& enumValues, QString& errorString, QString valueName = QString()); /// Returns NaN if the value is null, or if not, the double value static double possibleNaNJsonValue(const QJsonValue& value); @@ -142,6 +143,7 @@ private: bool writeAltitude, QJsonValue& jsonValue, bool geoJsonFormat); + static bool _parseEnumWorker(const QJsonObject& jsonObject, QMap& defineMap, QStringList& enumStrings, QStringList& enumValues, QString& errorString, QString valueName); static const char* _enumStringsJsonKey; static const char* _enumValuesJsonKey; diff --git a/src/QmlControls/QGroundControlQmlGlobal.cc b/src/QmlControls/QGroundControlQmlGlobal.cc index 2f892c185d8e2be0fdfb5256c6c6f62a88c0f2f4..b223793fd3501293d02dd2e2289e7656cd8e49f5 100644 --- a/src/QmlControls/QGroundControlQmlGlobal.cc +++ b/src/QmlControls/QGroundControlQmlGlobal.cc @@ -178,6 +178,15 @@ int QGroundControlQmlGlobal::supportedFirmwareCount() return _firmwarePluginManager->supportedFirmwareTypes().count(); } +bool QGroundControlQmlGlobal::px4ProFirmwareSupported() +{ + return _firmwarePluginManager->supportedFirmwareTypes().contains(MAV_AUTOPILOT_PX4); +} + +bool QGroundControlQmlGlobal::apmFirmwareSupported() +{ + return _firmwarePluginManager->supportedFirmwareTypes().contains(MAV_AUTOPILOT_ARDUPILOTMEGA); +} bool QGroundControlQmlGlobal::linesIntersect(QPointF line1A, QPointF line1B, QPointF line2A, QPointF line2B) { diff --git a/src/QmlControls/QGroundControlQmlGlobal.h b/src/QmlControls/QGroundControlQmlGlobal.h index e49ace26cf725b6131f8fabe2def0869a9a3e803..26c3631e564200356b488dd0344c509f0fad6995 100644 --- a/src/QmlControls/QGroundControlQmlGlobal.h +++ b/src/QmlControls/QGroundControlQmlGlobal.h @@ -70,6 +70,8 @@ public: Q_PROPERTY(bool taisyncSupported READ taisyncSupported CONSTANT) Q_PROPERTY(int supportedFirmwareCount READ supportedFirmwareCount CONSTANT) + Q_PROPERTY(bool px4ProFirmwareSupported READ px4ProFirmwareSupported CONSTANT) + Q_PROPERTY(int apmFirmwareSupported READ apmFirmwareSupported CONSTANT) Q_PROPERTY(qreal zOrderTopMost READ zOrderTopMost CONSTANT) ///< z order for top most items, toolbar, main window sub view Q_PROPERTY(qreal zOrderWidgets READ zOrderWidgets CONSTANT) ///< z order value to widgets, for example: zoom controls, hud widgetss @@ -180,6 +182,8 @@ public: int mavlinkSystemID () { return _toolbox->mavlinkProtocol()->getSystemId(); } int supportedFirmwareCount (); + bool px4ProFirmwareSupported (); + bool apmFirmwareSupported (); bool skipSetupPage () { return _skipSetupPage; } void setSkipSetupPage (bool skip); diff --git a/src/Settings/APMMavlinkStreamRate.SettingsGroup.json b/src/Settings/APMMavlinkStreamRate.SettingsGroup.json new file mode 100644 index 0000000000000000000000000000000000000000..954b079dc269972e80b104a1f93782ddabcde55c --- /dev/null +++ b/src/Settings/APMMavlinkStreamRate.SettingsGroup.json @@ -0,0 +1,64 @@ +{ + "QGC.MetaData.Defines": { + "StreamRateEnumStrings": "Controlled By Vehicle,0 hz,1 hz,2 hz,3 hz,4 hz,5 hz,6 hz,7 hz,8 hz,9 hz,10 hz,50 hz,100 hz", + "StreamRateEnumValues": "-1,0,1,2,3,4,5,6,7,8,9,10,50,100" + }, + "QGC.MetaData.Facts": [ + { + "name": "streamRateRawSensors", + "shortDescription": "Stream rate for MAVLink Raw Sensors telemetry stream.", + "type": "int32", + "enumStrings": "QGC.MetaData.Defines.StreamRateEnumStrings", + "enumValues": "QGC.MetaData.Defines.StreamRateEnumValues", + "defaultValue": 2 + }, + { + "name": "streamRateExtendedStatus", + "shortDescription": "Stream rate for MAVLink Extended Status telemetry stream.", + "type": "int32", + "enumStrings": "QGC.MetaData.Defines.StreamRateEnumStrings", + "enumValues": "QGC.MetaData.Defines.StreamRateEnumValues", + "defaultValue": 2 + }, + { + "name": "streamRateRCChannels", + "shortDescription": "Stream rate for MAVLink RC Channels telemetry stream.", + "type": "int32", + "enumStrings": "QGC.MetaData.Defines.StreamRateEnumStrings", + "enumValues": "QGC.MetaData.Defines.StreamRateEnumValues", + "defaultValue": 2 + }, + { + "name": "streamRatePosition", + "shortDescription": "Stream rate for MAVLink Position telemetry stream.", + "type": "int32", + "enumStrings": "QGC.MetaData.Defines.StreamRateEnumStrings", + "enumValues": "QGC.MetaData.Defines.StreamRateEnumValues", + "defaultValue": 3 + }, + { + "name": "streamRateExtra1", + "shortDescription": "Stream rate for MAVLink Extra1 telemetry stream.", + "type": "int32", + "enumStrings": "QGC.MetaData.Defines.StreamRateEnumStrings", + "enumValues": "QGC.MetaData.Defines.StreamRateEnumValues", + "defaultValue": 10 + }, + { + "name": "streamRateExtra2", + "shortDescription": "Stream rate for MAVLink Extra2 telemetry stream.", + "type": "int32", + "enumStrings": "QGC.MetaData.Defines.StreamRateEnumStrings", + "enumValues": "QGC.MetaData.Defines.StreamRateEnumValues", + "defaultValue": 10 + }, + { + "name": "streamRateExtra3", + "shortDescription": "Stream rate for MAVLink Extra3 telemetry stream.", + "type": "int32", + "enumStrings": "QGC.MetaData.Defines.StreamRateEnumStrings", + "enumValues": "QGC.MetaData.Defines.StreamRateEnumValues", + "defaultValue": 3 + } + ] +} diff --git a/src/Settings/APMMavlinkStreamRateSettings.cc b/src/Settings/APMMavlinkStreamRateSettings.cc new file mode 100644 index 0000000000000000000000000000000000000000..7516a60d33684a065005b9bf1a493d28296122b3 --- /dev/null +++ b/src/Settings/APMMavlinkStreamRateSettings.cc @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * (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 "APMMavlinkStreamRateSettings.h" +#include "QGCApplication.h" + +#include +#include + +DECLARE_SETTINGGROUP(APMMavlinkStreamRate, "APMMavlinkStreamRate") +{ + QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); + qmlRegisterUncreatableType("QGroundControl.SettingsManager", 1, 0, "APMMavlinkStreamRateSettings", "Reference only"); + + connect(streamRateRawSensors(), &Fact::rawValueChanged, this, &APMMavlinkStreamRateSettings::_updateStreamRateRawSensors); + connect(streamRateExtendedStatus(), &Fact::rawValueChanged, this, &APMMavlinkStreamRateSettings::_updateStreamRateExtendedStatus); + connect(streamRateRCChannels(), &Fact::rawValueChanged, this, &APMMavlinkStreamRateSettings::_updateStreamRateRCChannels); + connect(streamRatePosition(), &Fact::rawValueChanged, this, &APMMavlinkStreamRateSettings::_updateStreamRatePosition); + connect(streamRateExtra1(), &Fact::rawValueChanged, this, &APMMavlinkStreamRateSettings::_updateStreamRateExtra1); + connect(streamRateExtra2(), &Fact::rawValueChanged, this, &APMMavlinkStreamRateSettings::_updateStreamRateExtra2); + connect(streamRateExtra3(), &Fact::rawValueChanged, this, &APMMavlinkStreamRateSettings::_updateStreamRateExtra3); +} + +DECLARE_SETTINGSFACT(APMMavlinkStreamRateSettings, streamRateRawSensors) +DECLARE_SETTINGSFACT(APMMavlinkStreamRateSettings, streamRateExtendedStatus) +DECLARE_SETTINGSFACT(APMMavlinkStreamRateSettings, streamRateRCChannels) +DECLARE_SETTINGSFACT(APMMavlinkStreamRateSettings, streamRatePosition) +DECLARE_SETTINGSFACT(APMMavlinkStreamRateSettings, streamRateExtra1) +DECLARE_SETTINGSFACT(APMMavlinkStreamRateSettings, streamRateExtra2) +DECLARE_SETTINGSFACT(APMMavlinkStreamRateSettings, streamRateExtra3) + +void APMMavlinkStreamRateSettings::_updateStreamRateWorker(MAV_DATA_STREAM mavStream, QVariant rateVar) +{ + Vehicle* activeVehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle(); + + if (activeVehicle) { + int streamRate = rateVar.toInt(); + if (streamRate >= 0) { + activeVehicle->requestDataStream(mavStream, static_cast(streamRate)); + } + } +} + +void APMMavlinkStreamRateSettings::_updateStreamRateRawSensors(QVariant value) +{ + _updateStreamRateWorker(MAV_DATA_STREAM_RAW_SENSORS, value); +} + +void APMMavlinkStreamRateSettings::_updateStreamRateExtendedStatus(QVariant value) +{ + _updateStreamRateWorker(MAV_DATA_STREAM_EXTENDED_STATUS, value); +} + +void APMMavlinkStreamRateSettings::_updateStreamRateRCChannels(QVariant value) +{ + _updateStreamRateWorker(MAV_DATA_STREAM_RC_CHANNELS, value); +} + +void APMMavlinkStreamRateSettings::_updateStreamRatePosition(QVariant value) +{ + _updateStreamRateWorker(MAV_DATA_STREAM_POSITION, value); +} + +void APMMavlinkStreamRateSettings::_updateStreamRateExtra1(QVariant value) +{ + _updateStreamRateWorker(MAV_DATA_STREAM_EXTRA1, value); +} + +void APMMavlinkStreamRateSettings::_updateStreamRateExtra2(QVariant value) +{ + _updateStreamRateWorker(MAV_DATA_STREAM_EXTRA2, value); +} + +void APMMavlinkStreamRateSettings::_updateStreamRateExtra3(QVariant value) +{ + _updateStreamRateWorker(MAV_DATA_STREAM_EXTRA3, value); +} diff --git a/src/Settings/APMMavlinkStreamRateSettings.h b/src/Settings/APMMavlinkStreamRateSettings.h new file mode 100644 index 0000000000000000000000000000000000000000..bb95ecbde683bc18c5885a15e37b7ad503b185c3 --- /dev/null +++ b/src/Settings/APMMavlinkStreamRateSettings.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * (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. + * + ****************************************************************************/ +#pragma once + +#include "SettingsGroup.h" +#include "QGCMAVLink.h" + +class APMMavlinkStreamRateSettings : public SettingsGroup +{ + Q_OBJECT + +public: + APMMavlinkStreamRateSettings(QObject* parent = nullptr); + + DEFINE_SETTING_NAME_GROUP() + + DEFINE_SETTINGFACT(streamRateRawSensors) + DEFINE_SETTINGFACT(streamRateExtendedStatus) + DEFINE_SETTINGFACT(streamRateRCChannels) + DEFINE_SETTINGFACT(streamRatePosition) + DEFINE_SETTINGFACT(streamRateExtra1) + DEFINE_SETTINGFACT(streamRateExtra2) + DEFINE_SETTINGFACT(streamRateExtra3) + +private slots: + void _updateStreamRateRawSensors (QVariant value); + void _updateStreamRateExtendedStatus(QVariant value); + void _updateStreamRateRCChannels (QVariant value); + void _updateStreamRatePosition (QVariant value); + void _updateStreamRateExtra1 (QVariant value); + void _updateStreamRateExtra2 (QVariant value); + void _updateStreamRateExtra3 (QVariant value); + +private: + void _updateStreamRateWorker(MAV_DATA_STREAM mavStream, QVariant rateVar); +}; diff --git a/src/Settings/App.SettingsGroup.json b/src/Settings/App.SettingsGroup.json index 82917802eefa4c16ec51154684416e7123089391..67ef154dcca8ff523e4b7d9289ebcb46ca263397 100644 --- a/src/Settings/App.SettingsGroup.json +++ b/src/Settings/App.SettingsGroup.json @@ -201,10 +201,11 @@ "defaultValue": 0 }, { - "name": "apmStartMavlinkStreams", - "shortDescription": "Request start of MAVLink telemetry streams (ArduPilot only)", - "type": "bool", - "defaultValue": true + "name": "apmStartMavlinkStreams", + "shortDescription": "Request start of MAVLink telemetry streams (ArduPilot only)", + "type": "bool", + "defaultValue": true, + "qgcRebootRequired": true }, { "name": "enableTaisync", diff --git a/src/Settings/SettingsManager.cc b/src/Settings/SettingsManager.cc index a6393d47c1fcb10f2a3392069d00578785ed68ce..70a44ecc5bf34b08ab999116b71f509146497ef4 100644 --- a/src/Settings/SettingsManager.cc +++ b/src/Settings/SettingsManager.cc @@ -17,15 +17,16 @@ SettingsManager::SettingsManager(QGCApplication* app, QGCToolbox* toolbox) #if defined(QGC_AIRMAP_ENABLED) , _airMapSettings (nullptr) #endif - , _appSettings (nullptr) - , _unitsSettings (nullptr) - , _autoConnectSettings (nullptr) - , _videoSettings (nullptr) - , _flightMapSettings (nullptr) - , _rtkSettings (nullptr) - , _flyViewSettings (nullptr) - , _planViewSettings (nullptr) - , _brandImageSettings (nullptr) + , _appSettings (nullptr) + , _unitsSettings (nullptr) + , _autoConnectSettings (nullptr) + , _videoSettings (nullptr) + , _flightMapSettings (nullptr) + , _rtkSettings (nullptr) + , _flyViewSettings (nullptr) + , _planViewSettings (nullptr) + , _brandImageSettings (nullptr) + , _apmMavlinkStreamRateSettings (nullptr) { } @@ -36,16 +37,17 @@ void SettingsManager::setToolbox(QGCToolbox *toolbox) QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); qmlRegisterUncreatableType("QGroundControl.SettingsManager", 1, 0, "SettingsManager", "Reference only"); - _unitsSettings = new UnitsSettings (this); // Must be first since AppSettings references it - _appSettings = new AppSettings (this); - _autoConnectSettings = new AutoConnectSettings (this); - _videoSettings = new VideoSettings (this); - _flightMapSettings = new FlightMapSettings (this); - _rtkSettings = new RTKSettings (this); - _flyViewSettings = new FlyViewSettings (this); - _planViewSettings = new PlanViewSettings (this); - _brandImageSettings = new BrandImageSettings (this); + _unitsSettings = new UnitsSettings (this); // Must be first since AppSettings references it + _appSettings = new AppSettings (this); + _autoConnectSettings = new AutoConnectSettings (this); + _videoSettings = new VideoSettings (this); + _flightMapSettings = new FlightMapSettings (this); + _rtkSettings = new RTKSettings (this); + _flyViewSettings = new FlyViewSettings (this); + _planViewSettings = new PlanViewSettings (this); + _brandImageSettings = new BrandImageSettings (this); + _apmMavlinkStreamRateSettings = new APMMavlinkStreamRateSettings (this); #if defined(QGC_AIRMAP_ENABLED) - _airMapSettings = new AirMapSettings (this); + _airMapSettings = new AirMapSettings (this); #endif } diff --git a/src/Settings/SettingsManager.h b/src/Settings/SettingsManager.h index 11fdacf48274a41facc9ca0bd800bf863135ea46..47d63b3ef9aa9479d56c14e00c0a7ee04c1de84a 100644 --- a/src/Settings/SettingsManager.h +++ b/src/Settings/SettingsManager.h @@ -23,6 +23,7 @@ #include "FlyViewSettings.h" #include "PlanViewSettings.h" #include "BrandImageSettings.h" +#include "APMMavlinkStreamRateSettings.h" #if defined(QGC_AIRMAP_ENABLED) #include "AirMapSettings.h" #endif @@ -37,17 +38,18 @@ public: SettingsManager(QGCApplication* app, QGCToolbox* toolbox); #if defined(QGC_AIRMAP_ENABLED) - Q_PROPERTY(QObject* airMapSettings READ airMapSettings CONSTANT) + Q_PROPERTY(QObject* airMapSettings READ airMapSettings CONSTANT) #endif - Q_PROPERTY(QObject* appSettings READ appSettings CONSTANT) - Q_PROPERTY(QObject* unitsSettings READ unitsSettings CONSTANT) - Q_PROPERTY(QObject* autoConnectSettings READ autoConnectSettings CONSTANT) - Q_PROPERTY(QObject* videoSettings READ videoSettings CONSTANT) - Q_PROPERTY(QObject* flightMapSettings READ flightMapSettings CONSTANT) - Q_PROPERTY(QObject* rtkSettings READ rtkSettings CONSTANT) - Q_PROPERTY(QObject* flyViewSettings READ flyViewSettings CONSTANT) - Q_PROPERTY(QObject* planViewSettings READ planViewSettings CONSTANT) - Q_PROPERTY(QObject* brandImageSettings READ brandImageSettings CONSTANT) + Q_PROPERTY(QObject* appSettings READ appSettings CONSTANT) + Q_PROPERTY(QObject* unitsSettings READ unitsSettings CONSTANT) + Q_PROPERTY(QObject* autoConnectSettings READ autoConnectSettings CONSTANT) + Q_PROPERTY(QObject* videoSettings READ videoSettings CONSTANT) + Q_PROPERTY(QObject* flightMapSettings READ flightMapSettings CONSTANT) + Q_PROPERTY(QObject* rtkSettings READ rtkSettings CONSTANT) + Q_PROPERTY(QObject* flyViewSettings READ flyViewSettings CONSTANT) + Q_PROPERTY(QObject* planViewSettings READ planViewSettings CONSTANT) + Q_PROPERTY(QObject* brandImageSettings READ brandImageSettings CONSTANT) + Q_PROPERTY(QObject* apmMavlinkStreamRateSettings READ apmMavlinkStreamRateSettings CONSTANT) // Override from QGCTool virtual void setToolbox(QGCToolbox *toolbox); @@ -55,29 +57,31 @@ public: #if defined(QGC_AIRMAP_ENABLED) AirMapSettings* airMapSettings (void) { return _airMapSettings; } #endif - AppSettings* appSettings (void) { return _appSettings; } - UnitsSettings* unitsSettings (void) { return _unitsSettings; } - AutoConnectSettings* autoConnectSettings (void) { return _autoConnectSettings; } - VideoSettings* videoSettings (void) { return _videoSettings; } - FlightMapSettings* flightMapSettings (void) { return _flightMapSettings; } - RTKSettings* rtkSettings (void) { return _rtkSettings; } - FlyViewSettings* flyViewSettings (void) { return _flyViewSettings; } - PlanViewSettings* planViewSettings (void) { return _planViewSettings; } - BrandImageSettings* brandImageSettings (void) { return _brandImageSettings; } + AppSettings* appSettings (void) { return _appSettings; } + UnitsSettings* unitsSettings (void) { return _unitsSettings; } + AutoConnectSettings* autoConnectSettings (void) { return _autoConnectSettings; } + VideoSettings* videoSettings (void) { return _videoSettings; } + FlightMapSettings* flightMapSettings (void) { return _flightMapSettings; } + RTKSettings* rtkSettings (void) { return _rtkSettings; } + FlyViewSettings* flyViewSettings (void) { return _flyViewSettings; } + PlanViewSettings* planViewSettings (void) { return _planViewSettings; } + BrandImageSettings* brandImageSettings (void) { return _brandImageSettings; } + APMMavlinkStreamRateSettings* apmMavlinkStreamRateSettings(void) { return _apmMavlinkStreamRateSettings; } private: #if defined(QGC_AIRMAP_ENABLED) AirMapSettings* _airMapSettings; #endif - AppSettings* _appSettings; - UnitsSettings* _unitsSettings; - AutoConnectSettings* _autoConnectSettings; - VideoSettings* _videoSettings; - FlightMapSettings* _flightMapSettings; - RTKSettings* _rtkSettings; - FlyViewSettings* _flyViewSettings; - PlanViewSettings* _planViewSettings; - BrandImageSettings* _brandImageSettings; + AppSettings* _appSettings; + UnitsSettings* _unitsSettings; + AutoConnectSettings* _autoConnectSettings; + VideoSettings* _videoSettings; + FlightMapSettings* _flightMapSettings; + RTKSettings* _rtkSettings; + FlyViewSettings* _flyViewSettings; + PlanViewSettings* _planViewSettings; + BrandImageSettings* _brandImageSettings; + APMMavlinkStreamRateSettings* _apmMavlinkStreamRateSettings; }; #endif diff --git a/src/ui/preferences/MavlinkSettings.qml b/src/ui/preferences/MavlinkSettings.qml index b154d6ba180e6eb32aee9c182b8af7c782e74586..2db2cc1d958272481712c9e77bb06b7396424a59 100644 --- a/src/ui/preferences/MavlinkSettings.qml +++ b/src/ui/preferences/MavlinkSettings.qml @@ -12,6 +12,7 @@ import QtQuick 2.3 import QtQuick.Controls 1.2 import QtQuick.Controls.Styles 1.4 import QtQuick.Dialogs 1.2 +import QtQuick.Layouts 1.2 import QGroundControl 1.0 import QGroundControl.FactSystem 1.0 @@ -33,6 +34,7 @@ Rectangle { property bool _uploadedSelected: false property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle property var _showMavlinkLog: QGroundControl.corePlugin.options.showMavlinkLogOptions + property bool _showAPMStreamRates: QGroundControl.apmFirmwareSupported && QGroundControl.settingsManager.apmMavlinkStreamRateSettings.visible QGCPalette { id: qgcPal } @@ -143,11 +145,6 @@ Rectangle { } } - FactCheckBox { - text: fact.shortDescription - fact: QGroundControl.settingsManager.appSettings.apmStartMavlinkStreams - } - QGCCheckBox { text: qsTr("Only accept MAVs with same protocol version") checked: QGroundControl.isVersionCheckEnabled @@ -158,6 +155,97 @@ Rectangle { } } //----------------------------------------------------------------- + //-- Stream Rates + Item { + id: apmStreamRatesLabel + width: __mavlinkRoot.width * 0.8 + height: streamRatesLabel.height + anchors.margins: ScreenTools.defaultFontPixelWidth + anchors.horizontalCenter: parent.horizontalCenter + visible: _showAPMStreamRates + QGCLabel { + id: streamRatesLabel + text: qsTr("Telemetry Stream Rates (ArduPilot Only)") + font.family: ScreenTools.demiboldFontFamily + } + } + Rectangle { + height: streamRatesColumn.height + (ScreenTools.defaultFontPixelHeight * 2) + width: __mavlinkRoot.width * 0.8 + color: qgcPal.windowShade + anchors.margins: ScreenTools.defaultFontPixelWidth + anchors.horizontalCenter: parent.horizontalCenter + visible: _showAPMStreamRates + + ColumnLayout { + id: streamRatesColumn + spacing: ScreenTools.defaultFontPixelHeight / 2 + anchors.centerIn: parent + + property bool allStreamsControlledByVehicle: !QGroundControl.settingsManager.appSettings.apmStartMavlinkStreams.rawValue + + QGCCheckBox { + text: qsTr("All Streams Controlled By Vehicle Settings") + checked: streamRatesColumn.allStreamsControlledByVehicle + onClicked: QGroundControl.settingsManager.appSettings.apmStartMavlinkStreams.rawValue = !checked + } + + GridLayout { + columns: 2 + enabled: !streamRatesColumn.allStreamsControlledByVehicle + + QGCLabel { text: qsTr("Raw Sensors") } + FactComboBox { + fact: QGroundControl.settingsManager.apmMavlinkStreamRateSettings.streamRateRawSensors + indexModel: false + Layout.preferredWidth: _valueWidth + } + + QGCLabel { text: qsTr("Extended Status") } + FactComboBox { + fact: QGroundControl.settingsManager.apmMavlinkStreamRateSettings.streamRateExtendedStatus + indexModel: false + Layout.preferredWidth: _valueWidth + } + + QGCLabel { text: qsTr("RC Channel") } + FactComboBox { + fact: QGroundControl.settingsManager.apmMavlinkStreamRateSettings.streamRateRCChannels + indexModel: false + Layout.preferredWidth: _valueWidth + } + + QGCLabel { text: qsTr("Position") } + FactComboBox { + fact: QGroundControl.settingsManager.apmMavlinkStreamRateSettings.streamRatePosition + indexModel: false + Layout.preferredWidth: _valueWidth + } + + QGCLabel { text: qsTr("Extra 1") } + FactComboBox { + fact: QGroundControl.settingsManager.apmMavlinkStreamRateSettings.streamRateExtra1 + indexModel: false + Layout.preferredWidth: _valueWidth + } + + QGCLabel { text: qsTr("Extra 2") } + FactComboBox { + fact: QGroundControl.settingsManager.apmMavlinkStreamRateSettings.streamRateExtra2 + indexModel: false + Layout.preferredWidth: _valueWidth + } + + QGCLabel { text: qsTr("Extra 3") } + FactComboBox { + fact: QGroundControl.settingsManager.apmMavlinkStreamRateSettings.streamRateExtra3 + indexModel: false + Layout.preferredWidth: _valueWidth + } + } + } + } + //----------------------------------------------------------------- //-- Mavlink Status Item { width: __mavlinkRoot.width * 0.8 @@ -253,7 +341,7 @@ Rectangle { visible: _showMavlinkLog QGCLabel { id: mavlogLabel - text: qsTr("MAVLink 2.0 Logging (PX4 Firmware Only)") + text: qsTr("MAVLink 2.0 Logging (PX4 Pro Only)") font.family: ScreenTools.demiboldFontFamily } } @@ -315,7 +403,7 @@ Rectangle { visible: _showMavlinkLog QGCLabel { id: logLabel - text: qsTr("MAVLink 2.0 Log Uploads (PX4 Firmware Only)") + text: qsTr("MAVLink 2.0 Log Uploads (PX4 Pro Only)") font.family: ScreenTools.demiboldFontFamily } }