From 7bc24b3f9520fdb0cffc4a4fade360c0105f7f11 Mon Sep 17 00:00:00 2001 From: Holden Date: Mon, 7 Oct 2024 16:53:56 -0400 Subject: [PATCH] AnalyzeView: Fix MAVLinkChartInspector --- custom-example/qgroundcontrol.qrc | 4 +- qgroundcontrol.qrc | 4 +- src/AnalyzeView/MAVLinkChart.qml | 140 ++++++ src/AnalyzeView/MAVLinkChartController.cc | 209 ++++---- src/AnalyzeView/MAVLinkChartController.h | 108 ++-- src/AnalyzeView/MAVLinkInspectorController.cc | 354 +++++++------- src/AnalyzeView/MAVLinkInspectorController.h | 121 +++-- src/AnalyzeView/MAVLinkInspectorPage.qml | 304 ++++++------ src/AnalyzeView/MAVLinkMessage.cc | 460 +++++++++--------- src/AnalyzeView/MAVLinkMessage.h | 71 ++- src/AnalyzeView/MAVLinkMessageButton.qml | 67 +++ src/AnalyzeView/MAVLinkMessageField.cc | 189 +++---- src/AnalyzeView/MAVLinkMessageField.h | 89 ++-- src/AnalyzeView/MAVLinkSystem.cc | 153 +++--- src/AnalyzeView/MAVLinkSystem.h | 62 ++- src/QmlControls/MAVLinkChart.qml | 129 ----- src/QmlControls/MAVLinkMessageButton.qml | 63 --- .../QGroundControl/Controls/qmldir | 2 - 18 files changed, 1309 insertions(+), 1220 deletions(-) create mode 100644 src/AnalyzeView/MAVLinkChart.qml create mode 100644 src/AnalyzeView/MAVLinkMessageButton.qml delete mode 100644 src/QmlControls/MAVLinkChart.qml delete mode 100644 src/QmlControls/MAVLinkMessageButton.qml diff --git a/custom-example/qgroundcontrol.qrc b/custom-example/qgroundcontrol.qrc index 0f249c9d16d..3cb44553f6a 100644 --- a/custom-example/qgroundcontrol.qrc +++ b/custom-example/qgroundcontrol.qrc @@ -65,6 +65,8 @@ ../src/UI/preferences/MapSettings.qml ../src/AnalyzeView/MAVLinkConsolePage.qml ../src/AnalyzeView/MAVLinkInspectorPage.qml + ../src/AnalyzeView/MAVLinkChart.qml + ../src/AnalyzeView/MAVLinkMessageButton.qml ../src/UI/preferences/PX4LogTransferSettings.qml ../src/PlanView/MissionSettingsEditor.qml ../src/AutoPilotPlugins/Common/MotorComponent.qml @@ -123,8 +125,6 @@ ../src/UI/toolbar/FlightModeMenuIndicator.qml ../src/UI/toolbar/MainStatusIndicatorOfflinePage.qml ../src/QmlControls/MainWindowSavedState.qml - ../src/QmlControls/MAVLinkChart.qml - ../src/QmlControls/MAVLinkMessageButton.qml ../src/QmlControls/MissionCommandDialog.qml ../src/PlanView/MissionItemEditor.qml ../src/QmlControls/MissionItemIndexLabel.qml diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index 61516930996..96ad11fa35e 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -65,6 +65,8 @@ src/UI/preferences/MapSettings.qml src/AnalyzeView/MAVLinkConsolePage.qml src/AnalyzeView/MAVLinkInspectorPage.qml + src/AnalyzeView/MAVLinkChart.qml + src/AnalyzeView/MAVLinkMessageButton.qml src/UI/preferences/PX4LogTransferSettings.qml src/PlanView/MissionSettingsEditor.qml src/AutoPilotPlugins/Common/MotorComponent.qml @@ -124,8 +126,6 @@ src/UI/toolbar/FlightModeMenuIndicator.qml src/UI/toolbar/MainStatusIndicatorOfflinePage.qml src/QmlControls/MainWindowSavedState.qml - src/QmlControls/MAVLinkChart.qml - src/QmlControls/MAVLinkMessageButton.qml src/QmlControls/MissionCommandDialog.qml src/PlanView/MissionItemEditor.qml src/QmlControls/MissionItemIndexLabel.qml diff --git a/src/AnalyzeView/MAVLinkChart.qml b/src/AnalyzeView/MAVLinkChart.qml new file mode 100644 index 00000000000..03569cbe121 --- /dev/null +++ b/src/AnalyzeView/MAVLinkChart.qml @@ -0,0 +1,140 @@ +import QtQuick +import QtQuick.Controls +import QtQuick.Layouts +import QtCharts + +import QGroundControl +import QGroundControl.Palette +import QGroundControl.Controls +import QGroundControl.Controllers +import QGroundControl.ScreenTools + +ChartView { + id: chartView + theme: ChartView.ChartThemeDark + antialiasing: true + animationOptions: ChartView.NoAnimation + legend.visible: false + backgroundColor: qgcPal.window + backgroundRoundness: 0 + margins.bottom: ScreenTools.defaultFontPixelHeight * 1.5 + margins.top: chartHeader.height + (ScreenTools.defaultFontPixelHeight * 2) + + property var chartController: null + property var seriesColors: ["#00E04B","#DE8500","#F32836","#BFBFBF","#536DFF","#EECC44"] + + function addDimension(field) { + if (!chartController) { + chartController = controller.createChart() + } + + const color = chartView.seriesColors[chartView.count] + const serie = createSeries(ChartView.SeriesTypeLine, field.label) + serie.axisX = axisX + serie.axisY = axisY + serie.useOpenGL = true + serie.color = color + serie.width = 1 + chartController.addSeries(field, serie) + } + + function delDimension(field) { + if (chartController) { + chartView.removeSeries(field.series) + chartController.delSeries(field) + if (chartView.count === 0) { + controller.deleteChart(chartController) + chartController = null + } + } + } + + QGCPalette { id: qgcPal; colorGroupEnabled: enabled } + + DateTimeAxis { + id: axisX + min: chartController ? chartController.rangeXMin : new Date() + max: chartController ? chartController.rangeXMax : new Date() + visible: chartController + format: "
mm:ss.zzz" + tickCount: 5 + gridVisible: true + labelsFont.family: ScreenTools.fixedFontFamily + labelsFont.pointSize: ScreenTools.smallFontPointSize + labelsColor: qgcPal.text + } + + ValueAxis { + id: axisY + min: chartController ? chartController.rangeYMin : 0 + max: chartController ? chartController.rangeYMax : 0 + visible: chartController + lineVisible: false + labelsFont.family: ScreenTools.fixedFontFamily + labelsFont.pointSize: ScreenTools.smallFontPointSize + labelsColor: qgcPal.text + } + + Row { + id: chartHeader + anchors.left: parent.left + anchors.leftMargin: ScreenTools.defaultFontPixelWidth * 4 + anchors.right: parent.right + anchors.rightMargin: ScreenTools.defaultFontPixelWidth * 4 + anchors.top: parent.top + anchors.topMargin: ScreenTools.defaultFontPixelHeight * 1.5 + spacing: ScreenTools.defaultFontPixelWidth * 2 + visible: chartController + + GridLayout { + columns: 2 + columnSpacing: ScreenTools.defaultFontPixelWidth + rowSpacing: ScreenTools.defaultFontPixelHeight * 0.25 + anchors.verticalCenter: parent.verticalCenter + + QGCLabel { + text: qsTr("Scale:"); + Layout.alignment: Qt.AlignVCenter + } + + QGCComboBox { + Layout.minimumWidth: ScreenTools.defaultFontPixelWidth * 10 + Layout.maximumWidth: ScreenTools.defaultFontPixelWidth * 10 + Layout.alignment: Qt.AlignVCenter + height: ScreenTools.defaultFontPixelHeight + model: controller.timeScales + currentIndex: chartController ? chartController.rangeXIndex : 0 + onActivated: (index) => { if(chartController) chartController.rangeXIndex = index; } + } + + QGCLabel { + text: qsTr("Range:"); + Layout.alignment: Qt.AlignVCenter + } + + QGCComboBox { + Layout.minimumWidth: ScreenTools.defaultFontPixelWidth * 10 + Layout.maximumWidth: ScreenTools.defaultFontPixelWidth * 10 + Layout.alignment: Qt.AlignVCenter + height: ScreenTools.defaultFontPixelHeight + model: controller.rangeList + currentIndex: chartController ? chartController.rangeYIndex : 0 + onActivated: (index) => { if(chartController) chartController.rangeYIndex = index; } + } + } + + ColumnLayout { + anchors.verticalCenter: parent.verticalCenter + + Repeater { + model: chartController ? chartController.chartFields : [] + + QGCLabel { + text: modelData.label + color: chartView.series(index).color + font.pointSize: ScreenTools.smallFontPointSize + } + } + } + } +} diff --git a/src/AnalyzeView/MAVLinkChartController.cc b/src/AnalyzeView/MAVLinkChartController.cc index ff9fa29e6ae..2415181e4fc 100644 --- a/src/AnalyzeView/MAVLinkChartController.cc +++ b/src/AnalyzeView/MAVLinkChartController.cc @@ -9,143 +9,170 @@ #include "MAVLinkChartController.h" #include "MAVLinkInspectorController.h" -#include "QGC.h" #include "MAVLinkMessageField.h" +#include "QGC.h" #include "QGCLoggingCategory.h" #include - -QGC_LOGGING_CATEGORY(MAVLinkChartControllerLog, "qgc.analyzeview.mavlinkchartcontroller") +#include Q_DECLARE_METATYPE(QAbstractSeries*) -#define UPDATE_FREQUENCY (1000 / 15) // 15Hz +QGC_LOGGING_CATEGORY(MAVLinkChartControllerLog, "qgc.analyzeview.mavlinkchartcontroller") -//----------------------------------------------------------------------------- -MAVLinkChartController::MAVLinkChartController(MAVLinkInspectorController *parent, int index) - : QObject(parent) +MAVLinkChartController::MAVLinkChartController(MAVLinkInspectorController *controller, int index) + : QObject(controller) , _index(index) - , _controller(parent) + , _controller(controller) + , _updateSeriesTimer(new QTimer(this)) { - connect(&_updateSeriesTimer, &QTimer::timeout, this, &MAVLinkChartController::_refreshSeries); + // qCDebug(MAVLinkChartControllerLog) << Q_FUNC_INFO << this; + + (void) qRegisterMetaType("QAbstractSeries*"); + + (void) connect(_updateSeriesTimer, &QTimer::timeout, this, &MAVLinkChartController::_refreshSeries); + updateXRange(); } -//----------------------------------------------------------------------------- -void -MAVLinkChartController::setRangeYIndex(quint32 r) +MAVLinkChartController::~MAVLinkChartController() { - if(r < static_cast(_controller->rangeSt().count())) { - _rangeYIndex = r; - qreal range = _controller->rangeSt()[static_cast(r)]->range; - emit rangeYIndexChanged(); - //-- If not Auto, use defined range - if(_rangeYIndex > 0) { - _rangeYMin = -range; - emit rangeYMinChanged(); - _rangeYMax = range; - emit rangeYMaxChanged(); - } + // qCDebug(MAVLinkChartControllerLog) << Q_FUNC_INFO << this; +} + +void MAVLinkChartController::setRangeYIndex(quint32 index) +{ + if (index == _rangeYIndex) { + return; + } + + if (index >= static_cast(_controller->rangeSt().count())) { + return; + } + + _rangeYIndex = index; + emit rangeYIndexChanged(); + + // If not Auto, use defined range + const qreal range = _controller->rangeSt()[static_cast(index)]->range; + if (_rangeYIndex > 0) { + _rangeYMin = -range; + emit rangeYMinChanged(); + + _rangeYMax = range; + emit rangeYMaxChanged(); } } -//----------------------------------------------------------------------------- -void -MAVLinkChartController::setRangeXIndex(quint32 t) +void MAVLinkChartController::setRangeXIndex(quint32 index) { - _rangeXIndex = t; + if (index == _rangeXIndex) { + return; + } + + _rangeXIndex = index; emit rangeXIndexChanged(); + updateXRange(); } -//----------------------------------------------------------------------------- -void -MAVLinkChartController::updateXRange() +void MAVLinkChartController::updateXRange() { - if(_rangeXIndex < static_cast(_controller->timeScaleSt().count())) { - qint64 t = static_cast(QGC::bootTimeMilliseconds()); - _rangeXMax = QDateTime::fromMSecsSinceEpoch(t); - _rangeXMin = QDateTime::fromMSecsSinceEpoch(t - _controller->timeScaleSt()[static_cast(_rangeXIndex)]->timeScale); - emit rangeXMinChanged(); - emit rangeXMaxChanged(); + if (_rangeXIndex >= static_cast(_controller->timeScaleSt().count())) { + return; } + + const qint64 bootTime = static_cast(QGC::bootTimeMilliseconds()); + _rangeXMax = QDateTime::fromMSecsSinceEpoch(bootTime); + emit rangeXMaxChanged(); + + _rangeXMin = QDateTime::fromMSecsSinceEpoch(bootTime - _controller->timeScaleSt()[static_cast(_rangeXIndex)]->timeScale); + emit rangeXMinChanged(); } -//----------------------------------------------------------------------------- -void -MAVLinkChartController::updateYRange() +void MAVLinkChartController::updateYRange() { - if(_chartFields.count()) { - qreal vmin = std::numeric_limits::max(); - qreal vmax = std::numeric_limits::min(); - for(int i = 0; i < _chartFields.count(); i++) { - QObject* object = qvariant_cast(_chartFields.at(i)); - QGCMAVLinkMessageField* pField = qobject_cast(object); - if(pField) { - if(vmax < pField->rangeMax()) vmax = pField->rangeMax(); - if(vmin > pField->rangeMin()) vmin = pField->rangeMin(); + if (_chartFields.isEmpty()) { + return; + } + + qreal vmin = std::numeric_limits::max(); + qreal vmax = std::numeric_limits::min(); + for (const QVariant &field : _chartFields) { + QObject *const object = qvariant_cast(field); + QGCMAVLinkMessageField *const pField = qobject_cast(object); + if (pField) { + if (vmax < pField->rangeMax()) { + vmax = pField->rangeMax(); + } + + if (vmin > pField->rangeMin()) { + vmin = pField->rangeMin(); } - } - if(std::abs(_rangeYMin - vmin) > 0.000001) { - _rangeYMin = vmin; - emit rangeYMinChanged(); - } - if(std::abs(_rangeYMax - vmax) > 0.000001) { - _rangeYMax = vmax; - emit rangeYMaxChanged(); } } + + if (qAbs(_rangeYMin - vmin) > 0.000001) { + _rangeYMin = vmin; + emit rangeYMinChanged(); + } + + if (qAbs(_rangeYMax - vmax) > 0.000001) { + _rangeYMax = vmax; + emit rangeYMaxChanged(); + } } -//----------------------------------------------------------------------------- -void -MAVLinkChartController::_refreshSeries() +void MAVLinkChartController::_refreshSeries() { updateXRange(); - for(int i = 0; i < _chartFields.count(); i++) { - QObject* object = qvariant_cast(_chartFields.at(i)); - QGCMAVLinkMessageField* pField = qobject_cast(object); + + for (QVariant &field : _chartFields) { + QObject *const object = qvariant_cast(field); + QGCMAVLinkMessageField *const pField = qobject_cast(object); if(pField) { pField->updateSeries(); } } } -//----------------------------------------------------------------------------- -void -MAVLinkChartController::addSeries(QGCMAVLinkMessageField* field, QAbstractSeries* series) +void MAVLinkChartController::addSeries(QGCMAVLinkMessageField *field, QAbstractSeries *series) { - if(field && series) { - QVariant f = QVariant::fromValue(field); - for(int i = 0; i < _chartFields.count(); i++) { - if(_chartFields.at(i) == f) { - return; - } + if (!field || !series) { + return; + } + + const QVariant f = QVariant::fromValue(field); + for (const QVariant &field : _chartFields) { + if (field == f) { + return; } - _chartFields.append(f); - field->addSeries(this, series); - emit chartFieldsChanged(); - _updateSeriesTimer.start(UPDATE_FREQUENCY); } + + (void) _chartFields.append(f); + field->addSeries(this, series); + emit chartFieldsChanged(); + + _updateSeriesTimer->start(kUpdateFrequency); } -//----------------------------------------------------------------------------- -void -MAVLinkChartController::delSeries(QGCMAVLinkMessageField* field) +void MAVLinkChartController::delSeries(QGCMAVLinkMessageField *field) { - if(field) { - field->delSeries(); - QVariant f = QVariant::fromValue(field); - for(int i = 0; i < _chartFields.count(); i++) { - if(_chartFields.at(i) == f) { - _chartFields.removeAt(i); - emit chartFieldsChanged(); - if(_chartFields.count() == 0) { - updateXRange(); - _updateSeriesTimer.stop(); - } - return; - } + if (!field) { + return; + } + + field->delSeries(); + + const QVariant f = QVariant::fromValue(field); + const QList::const_iterator it = std::find(_chartFields.constBegin(), _chartFields.constEnd(), f); + if (it != _chartFields.end()) { + _chartFields.erase(it); + emit chartFieldsChanged(); + + if (_chartFields.isEmpty()) { + updateXRange(); + _updateSeriesTimer->stop(); } } } diff --git a/src/AnalyzeView/MAVLinkChartController.h b/src/AnalyzeView/MAVLinkChartController.h index 38425fab4c9..e0373048e4f 100644 --- a/src/AnalyzeView/MAVLinkChartController.h +++ b/src/AnalyzeView/MAVLinkChartController.h @@ -13,82 +13,80 @@ #pragma once -#include -#include #include -#include #include +#include +#include #include Q_DECLARE_LOGGING_CATEGORY(MAVLinkChartControllerLog) -class QGCMAVLinkMessageField; class MAVLinkInspectorController; class QAbstractSeries; +class QGCMAVLinkMessageField; +class QTimer; -//----------------------------------------------------------------------------- -/// MAVLink message charting controller class MAVLinkChartController : public QObject { Q_OBJECT QML_ELEMENT Q_MOC_INCLUDE("MAVLinkInspectorController.h") Q_MOC_INCLUDE("MAVLinkMessageField.h") - Q_MOC_INCLUDE() + Q_MOC_INCLUDE("QtCharts/qabstractseries.h") + Q_PROPERTY(QVariantList chartFields READ chartFields NOTIFY chartFieldsChanged) + Q_PROPERTY(QDateTime rangeXMin READ rangeXMin NOTIFY rangeXMinChanged) + Q_PROPERTY(QDateTime rangeXMax READ rangeXMax NOTIFY rangeXMaxChanged) + Q_PROPERTY(qreal rangeYMin READ rangeYMin NOTIFY rangeYMinChanged) + Q_PROPERTY(qreal rangeYMax READ rangeYMax NOTIFY rangeYMaxChanged) + Q_PROPERTY(int chartIndex READ chartIndex CONSTANT) + Q_PROPERTY(quint32 rangeYIndex READ rangeYIndex WRITE setRangeYIndex NOTIFY rangeYIndexChanged) + Q_PROPERTY(quint32 rangeXIndex READ rangeXIndex WRITE setRangeXIndex NOTIFY rangeXIndexChanged) public: - MAVLinkChartController(MAVLinkInspectorController* parent, int index); - - Q_PROPERTY(QVariantList chartFields READ chartFields NOTIFY chartFieldsChanged) - Q_PROPERTY(QDateTime rangeXMin READ rangeXMin NOTIFY rangeXMinChanged) - Q_PROPERTY(QDateTime rangeXMax READ rangeXMax NOTIFY rangeXMaxChanged) - Q_PROPERTY(qreal rangeYMin READ rangeYMin NOTIFY rangeYMinChanged) - Q_PROPERTY(qreal rangeYMax READ rangeYMax NOTIFY rangeYMaxChanged) - Q_PROPERTY(int chartIndex READ chartIndex CONSTANT) - - Q_PROPERTY(quint32 rangeYIndex READ rangeYIndex WRITE setRangeYIndex NOTIFY rangeYIndexChanged) - Q_PROPERTY(quint32 rangeXIndex READ rangeXIndex WRITE setRangeXIndex NOTIFY rangeXIndexChanged) - - Q_INVOKABLE void addSeries (QGCMAVLinkMessageField* field, QAbstractSeries* series); - Q_INVOKABLE void delSeries (QGCMAVLinkMessageField* field); - - Q_INVOKABLE MAVLinkInspectorController* controller() { return _controller; } - - QVariantList chartFields () { return _chartFields; } - QDateTime rangeXMin () { return _rangeXMin; } - QDateTime rangeXMax () { return _rangeXMax; } - qreal rangeYMin () const{ return _rangeYMin; } - qreal rangeYMax () const{ return _rangeYMax; } - quint32 rangeXIndex () const{ return _rangeXIndex; } - quint32 rangeYIndex () const{ return _rangeYIndex; } - int chartIndex () const{ return _index; } - - void setRangeXIndex (quint32 t); - void setRangeYIndex (quint32 r); - void updateXRange (); - void updateYRange (); + MAVLinkChartController(MAVLinkInspectorController *controller, int index); + ~MAVLinkChartController(); + + Q_INVOKABLE void addSeries(QGCMAVLinkMessageField *field, QAbstractSeries *series); + Q_INVOKABLE void delSeries(QGCMAVLinkMessageField *field); + Q_INVOKABLE MAVLinkInspectorController *controller() { return _controller; } + + QVariantList chartFields() const { return _chartFields; } + QDateTime rangeXMin() const { return _rangeXMin; } + QDateTime rangeXMax() const { return _rangeXMax; } + qreal rangeYMin() const { return _rangeYMin; } + qreal rangeYMax() const { return _rangeYMax; } + quint32 rangeXIndex() const { return _rangeXIndex; } + quint32 rangeYIndex() const { return _rangeYIndex; } + int chartIndex() const { return _index; } + + void setRangeXIndex(quint32 index); + void setRangeYIndex(quint32 index); + void updateXRange(); + void updateYRange(); signals: - void chartFieldsChanged (); - void rangeXMinChanged (); - void rangeXMaxChanged (); - void rangeYMinChanged (); - void rangeYMaxChanged (); - void rangeYIndexChanged (); - void rangeXIndexChanged (); + void chartFieldsChanged(); + void rangeXMinChanged(); + void rangeXMaxChanged(); + void rangeYMinChanged(); + void rangeYMaxChanged(); + void rangeYIndexChanged(); + void rangeXIndexChanged(); private slots: - void _refreshSeries (); + void _refreshSeries(); private: - QTimer _updateSeriesTimer; - QDateTime _rangeXMin; - QDateTime _rangeXMax; - int _index = 0; - qreal _rangeYMin = 0; - qreal _rangeYMax = 1; - quint32 _rangeXIndex = 0; ///< 5 Seconds - quint32 _rangeYIndex = 0; ///< Auto Range - QVariantList _chartFields; - MAVLinkInspectorController* _controller = nullptr; + QDateTime _rangeXMin; + QDateTime _rangeXMax; + qreal _rangeYMin = 0; + qreal _rangeYMax = 1; + quint32 _rangeXIndex = 0; ///< 5 Seconds + quint32 _rangeYIndex = 0; ///< Auto Range + QVariantList _chartFields; + int _index = 0; + MAVLinkInspectorController *_controller = nullptr; + QTimer *_updateSeriesTimer = nullptr; + + static constexpr int kUpdateFrequency = 1000 / 15; ///< 15Hz }; diff --git a/src/AnalyzeView/MAVLinkInspectorController.cc b/src/AnalyzeView/MAVLinkInspectorController.cc index 290da65c7a6..ec5db203491 100644 --- a/src/AnalyzeView/MAVLinkInspectorController.cc +++ b/src/AnalyzeView/MAVLinkInspectorController.cc @@ -8,269 +8,297 @@ ****************************************************************************/ #include "MAVLinkInspectorController.h" -#include "QGCApplication.h" -#include "MultiVehicleManager.h" -#include "Vehicle.h" -#include "MAVLinkProtocol.h" #include "MAVLinkChartController.h" -#include "MAVLinkSystem.h" #include "MAVLinkMessage.h" +#include "MAVLinkProtocol.h" +#include "MAVLinkSystem.h" +#include "MultiVehicleManager.h" +#include "QGCApplication.h" #include "QGCLoggingCategory.h" +#include "QmlObjectListModel.h" +#include "Vehicle.h" #include QGC_LOGGING_CATEGORY(MAVLinkInspectorControllerLog, "qgc.analyzeview.mavlinkinspectorcontroller") -//----------------------------------------------------------------------------- -MAVLinkInspectorController::MAVLinkInspectorController() +MAVLinkInspectorController::TimeScale_st::TimeScale_st(const QString &label_, uint32_t timeScale_) + : label(label_) + , timeScale(timeScale_) { - MultiVehicleManager* multiVehicleManager = qgcApp()->toolbox()->multiVehicleManager(); - connect(multiVehicleManager, &MultiVehicleManager::vehicleAdded, this, &MAVLinkInspectorController::_vehicleAdded); - connect(multiVehicleManager, &MultiVehicleManager::vehicleRemoved, this, &MAVLinkInspectorController::_vehicleRemoved); - connect(multiVehicleManager, &MultiVehicleManager::activeVehicleChanged, this, &MAVLinkInspectorController::_setActiveVehicle); - MAVLinkProtocol* mavlinkProtocol = qgcApp()->toolbox()->mavlinkProtocol(); - connect(mavlinkProtocol, &MAVLinkProtocol::messageReceived, this, &MAVLinkInspectorController::_receiveMessage); - connect(&_updateFrequencyTimer, &QTimer::timeout, this, &MAVLinkInspectorController::_refreshFrequency); - _updateFrequencyTimer.start(1000); - _timeScaleSt.append(new TimeScale_st(this, tr("5 Sec"), 5 * 1000)); - _timeScaleSt.append(new TimeScale_st(this, tr("10 Sec"), 10 * 1000)); - _timeScaleSt.append(new TimeScale_st(this, tr("30 Sec"), 30 * 1000)); - _timeScaleSt.append(new TimeScale_st(this, tr("60 Sec"), 60 * 1000)); + +} + +/*===========================================================================*/ + +MAVLinkInspectorController::Range_st::Range_st(const QString &label_, qreal range_) + : label(label_) + , range(range_) +{ + +} + +/*===========================================================================*/ + +MAVLinkInspectorController::MAVLinkInspectorController(QObject *parent) + : QObject(parent) + , _updateFrequencyTimer(new QTimer(this)) + , _systems(new QmlObjectListModel(this)) + , _charts(new QmlObjectListModel(this)) +{ + // qCDebug(MAVLinkInspectorControllerLog) << Q_FUNC_INFO << this; + + MultiVehicleManager *const multiVehicleManager = qgcApp()->toolbox()->multiVehicleManager(); + (void) connect(multiVehicleManager, &MultiVehicleManager::vehicleAdded, this, &MAVLinkInspectorController::_vehicleAdded); + (void) connect(multiVehicleManager, &MultiVehicleManager::vehicleRemoved, this, &MAVLinkInspectorController::_vehicleRemoved); + (void) connect(multiVehicleManager, &MultiVehicleManager::activeVehicleChanged, this, &MAVLinkInspectorController::_setActiveVehicle); + + MAVLinkProtocol *const mavlinkProtocol = qgcApp()->toolbox()->mavlinkProtocol(); + (void) connect(mavlinkProtocol, &MAVLinkProtocol::messageReceived, this, &MAVLinkInspectorController::_receiveMessage); + (void) connect(_updateFrequencyTimer, &QTimer::timeout, this, &MAVLinkInspectorController::_refreshFrequency); + + _updateFrequencyTimer->setInterval(1000); + _updateFrequencyTimer->setSingleShot(false); + _updateFrequencyTimer->start(); + + (void) _timeScaleSt.append(new TimeScale_st(tr("5 Sec"), 5 * 1000)); + (void) _timeScaleSt.append(new TimeScale_st(tr("10 Sec"), 10 * 1000)); + (void) _timeScaleSt.append(new TimeScale_st(tr("30 Sec"), 30 * 1000)); + (void) _timeScaleSt.append(new TimeScale_st(tr("60 Sec"), 60 * 1000)); emit timeScalesChanged(); - _rangeSt.append(new Range_st(this, tr("Auto"), 0)); - _rangeSt.append(new Range_st(this, tr("10,000"), 10000)); - _rangeSt.append(new Range_st(this, tr("1,000"), 1000)); - _rangeSt.append(new Range_st(this, tr("100"), 100)); - _rangeSt.append(new Range_st(this, tr("10"), 10)); - _rangeSt.append(new Range_st(this, tr("1"), 1)); - _rangeSt.append(new Range_st(this, tr("0.1"), 0.1)); - _rangeSt.append(new Range_st(this, tr("0.01"), 0.01)); - _rangeSt.append(new Range_st(this, tr("0.001"), 0.001)); - _rangeSt.append(new Range_st(this, tr("0.0001"), 0.0001)); + + (void) _rangeSt.append(new Range_st(tr("Auto"), 0)); + (void) _rangeSt.append(new Range_st(tr("10,000"), 10000)); + (void) _rangeSt.append(new Range_st(tr("1,000"), 1000)); + (void) _rangeSt.append(new Range_st(tr("100"), 100)); + (void) _rangeSt.append(new Range_st(tr("10"), 10)); + (void) _rangeSt.append(new Range_st(tr("1"), 1)); + (void) _rangeSt.append(new Range_st(tr("0.1"), 0.1)); + (void) _rangeSt.append(new Range_st(tr("0.01"), 0.01)); + (void) _rangeSt.append(new Range_st(tr("0.001"), 0.001)); + (void) _rangeSt.append(new Range_st(tr("0.0001"), 0.0001)); emit rangeListChanged(); } -//----------------------------------------------------------------------------- MAVLinkInspectorController::~MAVLinkInspectorController() { - _charts.clearAndDeleteContents(); - _systems.clearAndDeleteContents(); + qDeleteAll(_timeScaleSt); + qDeleteAll(_rangeSt); + _charts->clearAndDeleteContents(); + _systems->clearAndDeleteContents(); + + // qCDebug(MAVLinkInspectorControllerLog) << Q_FUNC_INFO << this; } -//---------------------------------------------------------------------------------------- -QStringList -MAVLinkInspectorController::timeScales() +QStringList MAVLinkInspectorController::timeScales() { - if(!_timeScales.count()) { - for(int i = 0; i < _timeScaleSt.count(); i++) { - _timeScales << _timeScaleSt[i]->label; + if (_timeScales.isEmpty()) { + for (const TimeScale_st *timeScale : _timeScaleSt) { + _timeScales << timeScale->label; } } + return _timeScales; } -//---------------------------------------------------------------------------------------- -QStringList -MAVLinkInspectorController::rangeList() +QStringList MAVLinkInspectorController::rangeList() { - if(!_rangeList.count()) { - for(int i = 0; i < _rangeSt.count(); i++) { - _rangeList << _rangeSt[i]->label; + if (_rangeList.isEmpty()) { + for (const Range_st *range : _rangeSt) { + _rangeList << range->label; } } + return _rangeList; } -//---------------------------------------------------------------------------------------- -void -MAVLinkInspectorController::_setActiveVehicle(Vehicle* vehicle) +void MAVLinkInspectorController::_setActiveVehicle(Vehicle *vehicle) { - if(vehicle) { - QGCMAVLinkSystem* v = _findVehicle(static_cast(vehicle->id())); - if(v) { - _activeSystem = v; + if (vehicle) { + QGCMAVLinkSystem *const system = _findVehicle(static_cast(vehicle->id())); + if (system) { + _activeSystem = system; } else { _activeSystem = nullptr; } } else { _activeSystem = nullptr; } + emit activeSystemChanged(); } -//----------------------------------------------------------------------------- -QGCMAVLinkSystem* -MAVLinkInspectorController::_findVehicle(uint8_t id) +QGCMAVLinkSystem *MAVLinkInspectorController::_findVehicle(uint8_t id) { - for(int i = 0; i < _systems.count(); i++) { - QGCMAVLinkSystem* v = qobject_cast(_systems.get(i)); - if(v) { - if(v->id() == id) { - return v; - } + for (int i = 0; i < _systems->count(); i++) { + QGCMAVLinkSystem *const system = qobject_cast(_systems->get(i)); + if (system && (system->id() == id)) { + return system; } } + return nullptr; } -//----------------------------------------------------------------------------- -void -MAVLinkInspectorController::_refreshFrequency() +void MAVLinkInspectorController::_refreshFrequency() { - for(int i = 0; i < _systems.count(); i++) { - QGCMAVLinkSystem* v = qobject_cast(_systems.get(i)); - if(v) { - for(int i = 0; i < v->messages()->count(); i++) { - QGCMAVLinkMessage* m = qobject_cast(v->messages()->get(i)); - if(m) { - m->updateFreq(); - } + for (int i = 0; i < _systems->count(); i++) { + QGCMAVLinkSystem *const system = qobject_cast(_systems->get(i)); + if (!system) { + continue; + } + + for (int i = 0; i < system->messages()->count(); i++) { + QGCMAVLinkMessage *const msg = qobject_cast(system->messages()->get(i)); + if (msg) { + msg->updateFreq(); } } } } -//----------------------------------------------------------------------------- -void -MAVLinkInspectorController::_vehicleAdded(Vehicle* vehicle) +void MAVLinkInspectorController::_vehicleAdded(Vehicle *vehicle) { - QGCMAVLinkSystem* sys = _findVehicle(static_cast(vehicle->id())); - if(sys) - { + QGCMAVLinkSystem *sys = _findVehicle(static_cast(vehicle->id())); + + if (sys) { sys->messages()->clearAndDeleteContents(); - } - else - { - sys = new QGCMAVLinkSystem(this, static_cast(vehicle->id())); - _systems.append(sys); - _systemNames.append(tr("System %1").arg(vehicle->id())); - connect(vehicle, &Vehicle::mavlinkMsgIntervalsChanged, sys, [sys](uint8_t compid, uint16_t msgId, int32_t rate) - { - for(int i = 0; i < sys->messages()->count(); i++) - { - QGCMAVLinkMessage* msg = qobject_cast(sys->messages()->get(i)); - if((msg->compId() == compid) && (msg->id() == msgId)) - { + } else { + sys = new QGCMAVLinkSystem(static_cast(vehicle->id()), this); + (void) _systems->append(sys); + (void) _systemNames.append(tr("System %1").arg(vehicle->id())); + + (void) connect(vehicle, &Vehicle::mavlinkMsgIntervalsChanged, sys, [sys](uint8_t compid, uint16_t msgId, int32_t rate) { + for (int i = 0; i < sys->messages()->count(); i++) { + QGCMAVLinkMessage *const msg = qobject_cast(sys->messages()->get(i)); + if ((msg->compId() == compid) && (msg->id() == msgId)) { msg->setTargetRateHz(rate); break; } } }); } + emit systemsChanged(); } -//----------------------------------------------------------------------------- -void -MAVLinkInspectorController::_vehicleRemoved(Vehicle* vehicle) +void MAVLinkInspectorController::_vehicleRemoved(Vehicle *vehicle) { - QGCMAVLinkSystem* v = _findVehicle(static_cast(vehicle->id())); - if(v) { - v->deleteLater(); - _systems.removeOne(v); - QString vs = tr("System %1").arg(vehicle->id()); - _systemNames.removeOne(vs); - emit systemsChanged(); + QGCMAVLinkSystem *const system = _findVehicle(static_cast(vehicle->id())); + if (!system) { + return; } + + system->deleteLater(); + _systems->removeOne(system); + + const QString systemName = tr("System %1").arg(vehicle->id()); + _systemNames.removeOne(systemName); + + emit systemsChanged(); } -//----------------------------------------------------------------------------- -void -MAVLinkInspectorController::_receiveMessage(LinkInterface*, mavlink_message_t message) +void MAVLinkInspectorController::_receiveMessage(LinkInterface *link, const mavlink_message_t &message) { - QGCMAVLinkMessage* m = nullptr; - QGCMAVLinkSystem* v = _findVehicle(message.sysid); - if(!v) { - v = new QGCMAVLinkSystem(this, message.sysid); - _systems.append(v); - _systemNames.append(tr("System %1").arg(message.sysid)); + Q_UNUSED(link); + + QGCMAVLinkMessage *msg = nullptr; + QGCMAVLinkSystem *system = _findVehicle(message.sysid); + + if (!system) { + system = new QGCMAVLinkSystem(message.sysid, this); + (void) _systems->append(system); + (void) _systemNames.append(tr("System %1").arg(message.sysid)); emit systemsChanged(); - if(!_activeSystem) { - _activeSystem = v; + + if (!_activeSystem) { + _activeSystem = system; emit activeSystemChanged(); } } else { - m = v->findMessage(message.msgid, message.compid); + msg = system->findMessage(message.msgid, message.compid); } - if(!m) { - m = new QGCMAVLinkMessage(this, &message); - v->append(m); + + if (!msg) { + msg = new QGCMAVLinkMessage(message, this); + (void) system->append(msg); } else { - m->update(&message); + msg->update(message); } } -//----------------------------------------------------------------------------- -MAVLinkChartController* -MAVLinkInspectorController::createChart() +MAVLinkChartController *MAVLinkInspectorController::createChart() { - MAVLinkChartController* pChart = new MAVLinkChartController(this, _charts.count()); + MAVLinkChartController *const pChart = new MAVLinkChartController(this, _charts->count()); QQmlEngine::setObjectOwnership(pChart, QQmlEngine::CppOwnership); - _charts.append(pChart); + + (void) _charts->append(pChart); emit chartsChanged(); + return pChart; } -//----------------------------------------------------------------------------- -void -MAVLinkInspectorController::deleteChart(MAVLinkChartController* chart) +void MAVLinkInspectorController::deleteChart(MAVLinkChartController *chart) { - if(chart) { - for(int i = 0; i < _charts.count(); i++) { - MAVLinkChartController* c = qobject_cast(_charts.get(i)); - if(c && c == chart) { - _charts.removeOne(c); - delete c; - break; - } - } - emit chartsChanged(); + if (!chart) { + return; } -} -//----------------------------------------------------------------------------- -MAVLinkInspectorController::TimeScale_st::TimeScale_st(QObject* parent, const QString& l, uint32_t t) - : QObject(parent) - , label(l) - , timeScale(t) -{ -} + bool found = false; + for (int i = 0; i < _charts->count(); i++) { + MAVLinkChartController *const controller = qobject_cast(_charts->get(i)); + if (controller && (controller == chart)) { + found = true; + _charts->removeOne(controller); + delete controller; + break; + } + } -//----------------------------------------------------------------------------- -MAVLinkInspectorController::Range_st::Range_st(QObject* parent, const QString& l, qreal r) - : QObject(parent) - , label(l) - , range(r) -{ + if (found) { + emit chartsChanged(); + } } void MAVLinkInspectorController::setActiveSystem(int systemId) { - QGCMAVLinkSystem* v = _findVehicle(systemId); - if (v != _activeSystem) { - _activeSystem = v; + QGCMAVLinkSystem *const system = _findVehicle(systemId); + if (system != _activeSystem) { + _activeSystem = system; emit activeSystemChanged(); } } -void MAVLinkInspectorController::setMessageInterval(int32_t rate) +void MAVLinkInspectorController::setMessageInterval(int32_t rate) const { - if(!_activeSystem) return; + if (!_activeSystem) { + return; + } - MultiVehicleManager* multiVehicleManager = qgcApp()->toolbox()->multiVehicleManager(); - if(!multiVehicleManager) return; + MultiVehicleManager *const multiVehicleManager = qgcApp()->toolbox()->multiVehicleManager(); + if (!multiVehicleManager) { + return; + } const uint8_t sysId = _selectedSystemID(); - if(sysId == 0) return; + if (sysId == 0) { + return; + } - Vehicle* vehicle = multiVehicleManager->getVehicleById(sysId); - if(!vehicle) return; + Vehicle *const vehicle = multiVehicleManager->getVehicleById(sysId); + if (!vehicle) { + return; + } - QGCMAVLinkMessage* msg = _activeSystem->selectedMsg(); - if(!msg) return; + const QGCMAVLinkMessage *const msg = _activeSystem->selectedMsg(); + if (!msg) { + return; + } const uint8_t compId = _selectedComponentID(); - if(compId == 0) return; + if (compId == 0) { + return; + } // TODO: Make QGCMAVLinkMessage a part of comm and use signals/slots for msg rate changes vehicle->setMessageRate(compId, msg->id(), rate); @@ -278,11 +306,11 @@ void MAVLinkInspectorController::setMessageInterval(int32_t rate) uint8_t MAVLinkInspectorController::_selectedSystemID() const { - return _activeSystem ? _activeSystem->id() : 0; + return (_activeSystem ? _activeSystem->id() : 0); } uint8_t MAVLinkInspectorController::_selectedComponentID() const { - QGCMAVLinkMessage* msg = _activeSystem->selectedMsg(); + const QGCMAVLinkMessage *const msg = _activeSystem ? _activeSystem->selectedMsg() : nullptr; return (msg ? msg->compId() : 0); } diff --git a/src/AnalyzeView/MAVLinkInspectorController.h b/src/AnalyzeView/MAVLinkInspectorController.h index 61dbd060211..163eedb0683 100644 --- a/src/AnalyzeView/MAVLinkInspectorController.h +++ b/src/AnalyzeView/MAVLinkInspectorController.h @@ -13,23 +13,22 @@ #pragma once +#include #include #include -#include -#include #include #include "MAVLinkLib.h" -#include "QmlObjectListModel.h" Q_DECLARE_LOGGING_CATEGORY(MAVLinkInspectorControllerLog) -class MAVLinkChartController; -class Vehicle; class LinkInterface; +class MAVLinkChartController; class QGCMAVLinkSystem; +class QmlObjectListModel; +class QTimer; +class Vehicle; -//----------------------------------------------------------------------------- /// MAVLink message inspector controller (provides the logic for UI display) class MAVLinkInspectorController : public QObject { @@ -39,77 +38,77 @@ class MAVLinkInspectorController : public QObject Q_MOC_INCLUDE("Vehicle.h") Q_MOC_INCLUDE("MAVLinkSystem.h") Q_MOC_INCLUDE("MAVLinkChartController.h") - -public: - MAVLinkInspectorController(); - ~MAVLinkInspectorController(); - - Q_PROPERTY(QStringList systemNames READ systemNames NOTIFY systemsChanged) - Q_PROPERTY(QmlObjectListModel* systems READ systems NOTIFY systemsChanged) - Q_PROPERTY(QmlObjectListModel* charts READ charts NOTIFY chartsChanged) - Q_PROPERTY(QGCMAVLinkSystem* activeSystem READ activeSystem NOTIFY activeSystemChanged) + Q_MOC_INCLUDE("QmlObjectListModel.h") + Q_PROPERTY(QmlObjectListModel *systems READ systems NOTIFY systemsChanged) + Q_PROPERTY(QmlObjectListModel *charts READ charts NOTIFY chartsChanged) + Q_PROPERTY(QGCMAVLinkSystem *activeSystem READ activeSystem NOTIFY activeSystemChanged) Q_PROPERTY(QStringList timeScales READ timeScales NOTIFY timeScalesChanged) Q_PROPERTY(QStringList rangeList READ rangeList NOTIFY rangeListChanged) + Q_PROPERTY(QStringList systemNames READ systemNames NOTIFY systemsChanged) - Q_INVOKABLE MAVLinkChartController* createChart (); - Q_INVOKABLE void deleteChart (MAVLinkChartController* chart); - Q_INVOKABLE void setActiveSystem (int systemId); - Q_INVOKABLE void setMessageInterval(int32_t rate); - - QmlObjectListModel* systems () { return &_systems; } - QmlObjectListModel* charts () { return &_charts; } - QGCMAVLinkSystem* activeSystem() { return _activeSystem; } - QStringList systemNames () { return _systemNames; } - QStringList timeScales (); - QStringList rangeList (); - - class TimeScale_st : public QObject { + class TimeScale_st + { public: - TimeScale_st(QObject* parent, const QString& l, uint32_t t); - QString label; - uint32_t timeScale; + TimeScale_st(const QString &label_, uint32_t timeScale_); + + QString label; + uint32_t timeScale; }; - class Range_st : public QObject { + class Range_st + { public: - Range_st(QObject* parent, const QString& l, qreal r); - QString label; - qreal range; + Range_st(const QString &label_, qreal range_); + + QString label; + qreal range; }; - const QList& timeScaleSt () { return _timeScaleSt; } - const QList& rangeSt () { return _rangeSt; } +public: + MAVLinkInspectorController(QObject *parent = nullptr); + ~MAVLinkInspectorController(); + + Q_INVOKABLE MAVLinkChartController *createChart(); + Q_INVOKABLE void deleteChart(MAVLinkChartController *chart); + Q_INVOKABLE void setActiveSystem(int systemId); + Q_INVOKABLE void setMessageInterval(int32_t rate) const; + + QmlObjectListModel *systems() { return _systems; } + QmlObjectListModel *charts() { return _charts; } + QGCMAVLinkSystem *activeSystem() { return _activeSystem; } + QStringList systemNames() const { return _systemNames; } + QStringList timeScales(); + QStringList rangeList(); + + const QList &timeScaleSt() const { return _timeScaleSt; } + const QList &rangeSt() const { return _rangeSt; } signals: - void systemsChanged (); - void chartsChanged (); void activeSystemChanged(); - void timeScalesChanged (); - void rangeListChanged (); + void chartsChanged(); + void rangeListChanged(); + void systemsChanged(); + void timeScalesChanged(); private slots: - void _receiveMessage (LinkInterface* link, mavlink_message_t message); - void _vehicleAdded (Vehicle* vehicle); - void _vehicleRemoved (Vehicle* vehicle); - void _setActiveVehicle (Vehicle* vehicle); - void _refreshFrequency (); - -private: - QGCMAVLinkSystem* _findVehicle (uint8_t id); + void _receiveMessage(LinkInterface *link, const mavlink_message_t &message); + void _refreshFrequency(); + void _setActiveVehicle(Vehicle *vehicle); + void _vehicleAdded(Vehicle *vehicle); + void _vehicleRemoved(Vehicle *vehicle); private: + QGCMAVLinkSystem *_findVehicle(uint8_t id); + uint8_t _selectedSystemID() const; + uint8_t _selectedComponentID() const; - uint8_t _selectedSystemID() const; - uint8_t _selectedComponentID() const; - - QStringList _timeScales; - QStringList _rangeList; - QGCMAVLinkSystem* _activeSystem = nullptr; - QTimer _updateFrequencyTimer; - QStringList _systemNames; - QmlObjectListModel _systems; ///< List of QGCMAVLinkSystem - QmlObjectListModel _charts; ///< List of MAVLinkCharts + QStringList _timeScales; + QStringList _rangeList; + QStringList _systemNames; QList_timeScaleSt; - QList _rangeSt; - + QList _rangeSt; + QGCMAVLinkSystem *_activeSystem = nullptr; + QTimer *_updateFrequencyTimer = nullptr; + QmlObjectListModel *_systems = nullptr; ///< List of QGCMAVLinkSystem + QmlObjectListModel *_charts = nullptr; ///< List of MAVLinkCharts }; diff --git a/src/AnalyzeView/MAVLinkInspectorPage.qml b/src/AnalyzeView/MAVLinkInspectorPage.qml index d33329287e4..ab7772b482e 100644 --- a/src/AnalyzeView/MAVLinkInspectorPage.qml +++ b/src/AnalyzeView/MAVLinkInspectorPage.qml @@ -20,46 +20,48 @@ import QGroundControl.Controls import QGroundControl.Controllers import QGroundControl.ScreenTools +import "." + AnalyzePage { id: root - headerComponent: headerComponent - pageComponent: pageComponent - allowPopout: true + headerComponent: headerComponent + pageComponent: pageComponent + allowPopout: true - property var curSystem: controller ? controller.activeSystem : null - property var curMessage: curSystem && curSystem.messages.count ? curSystem.messages.get(curSystem.selected) : null - property int curCompID: 0 - property real maxButtonWidth: 0 + property var curSystem: controller ? controller.activeSystem : null + property var curMessage: (curSystem && curSystem.messages.count) ? curSystem.messages.get(curSystem.selected) : null + property int curCompID: 0 + property real maxButtonWidth: 0 - MAVLinkInspectorController { - id: controller - } + MAVLinkInspectorController { id: controller } + QGCPalette { id: qgcPal; colorGroupEnabled: enabled } Component { id: headerComponent - //-- Header + RowLayout { - id: header - anchors.left: parent.left - anchors.right: parent.right - QGCLabel { - text: qsTr("Inspect real time MAVLink messages.") - } + anchors.left: parent.left + anchors.right: parent.right + + QGCLabel { text: qsTr("Inspect real time MAVLink messages.") } + RowLayout { - Layout.alignment: Qt.AlignRight - visible: curSystem ? controller.systemNames.length > 1 || curSystem.compIDsStr.length > 2 : false + Layout.alignment: Qt.AlignRight + visible: curSystem ? ((controller.systemNames.length > 1) || (curSystem.compIDsStr.length > 2)) : false + QGCComboBox { - id: systemCombo - model: controller.systemNames + id: systemCombo + model: controller.systemNames sizeToContents: true - visible: controller.systemNames.length > 1 - onActivated: (index) => { controller.setActiveSystem(controller.systems.get(index).id) } + visible: controller.systemNames.length > 1 + + onActivated: (index) => { controller.setActiveSystem(controller.systems.get(index).id) } Connections { target: controller onActiveSystemChanged: { - for (var systemIndex=0; systemIndex 2 : false + visible: curSystem ? (curSystem.compIDsStr.length > 2) : false + onActivated: (index) => { - if(curSystem && curSystem.compIDsStr.length > 1) { - if(index < 1) - curCompID = 0 - else - curCompID = curSystem.compIDs[index - 1] + if (curSystem && (curSystem.compIDsStr.length > 1)) { + curCompID = (index < 1) ? 0 : curSystem.compIDs[index - 1] } } } @@ -88,64 +89,69 @@ AnalyzePage { } Component { - id: pageComponent + id: pageComponent + Row { - width: availableWidth - height: availableHeight - spacing: ScreenTools.defaultFontPixelWidth - //-- Messages (Buttons) + width: availableWidth + height: availableHeight + spacing: ScreenTools.defaultFontPixelWidth + QGCFlickable { - id: buttonGrid + id: buttonGrid flickableDirection: Flickable.VerticalFlick - width: maxButtonWidth - height: parent.height - contentWidth: width - contentHeight: buttonCol.height + width: maxButtonWidth + height: parent.height + contentWidth: width + contentHeight: buttonCol.height + ColumnLayout { - id: buttonCol - anchors.left: parent.left - anchors.right: parent.right - spacing: ScreenTools.defaultFontPixelHeight * 0.25 + id: buttonCol + anchors.left: parent.left + anchors.right: parent.right + spacing: ScreenTools.defaultFontPixelHeight * 0.25 + Repeater { - model: curSystem ? curSystem.messages : [] - delegate: MAVLinkMessageButton { - text: object.name + (object.fieldSelected ? " *" : "") - compID: object.compId - checked: curSystem ? (curSystem.selected === index) : false - messageHz: object.actualRateHz - visible: curCompID === 0 || curCompID === compID - onClicked: { - curSystem.selected = index - } + model: curSystem ? curSystem.messages : [] + delegate: MAVLinkMessageButton { + text: object.name + (object.fieldSelected ? " *" : "") + compID: object.compId + checked: curSystem ? (curSystem.selected === index) : false + messageHz: object.actualRateHz + visible: ((curCompID === 0) || (curCompID === compID)) Layout.fillWidth: true + + onClicked: { curSystem.selected = index } } } } } - //-- Message Data + QGCFlickable { - id: messageGrid - visible: curMessage !== null && (curCompID === 0 || curCompID === curMessage.compId) + visible: curMessage && ((curCompID === 0) || (curCompID === curMessage.compId)) flickableDirection: Flickable.VerticalFlick - width: parent.width - buttonGrid.width - ScreenTools.defaultFontPixelWidth - height: parent.height - contentWidth: width - contentHeight: messageCol.height + width: parent.width - buttonGrid.width - ScreenTools.defaultFontPixelWidth + height: parent.height + contentWidth: width + contentHeight: messageCol.height + Column { - id: messageCol - width: parent.width - spacing: ScreenTools.defaultFontPixelHeight * 0.25 + id: messageCol + width: parent.width + spacing: ScreenTools.defaultFontPixelHeight * 0.25 + GridLayout { - columns: 2 - columnSpacing: ScreenTools.defaultFontPixelWidth - rowSpacing: ScreenTools.defaultFontPixelHeight * 0.25 + columns: 2 + columnSpacing: ScreenTools.defaultFontPixelWidth + rowSpacing: ScreenTools.defaultFontPixelHeight * 0.25 + QGCLabel { text: qsTr("Message:") Layout.minimumWidth: ScreenTools.defaultFontPixelWidth * 20 } + QGCLabel { color: qgcPal.buttonHighlight - text: curMessage ? curMessage.name + ' (' + curMessage.id + ')' : "" + text: curMessage ? (curMessage.name + ' (' + curMessage.id + ')') : "" } QGCLabel { text: qsTr("Component:") } @@ -181,109 +187,111 @@ AnalyzePage { ] Layout.alignment: Qt.AlignLeft sizeToContents: true + + onActivated: (index) => { controller.setMessageInterval(currentValue) } + Component.onCompleted: reset() - onActivated: (index) => controller.setMessageInterval(currentValue) + function reset() { currentIndex = indexOfValue(0) } + Connections { target: root function onCurMessageChanged() { msgRateCombo.reset() } } + Connections { target: curMessage function onTargetRateHzChanged() { const target_index = indexOfValue(curMessage.targetRateHz) - if(target_index != -1) { + if (target_index !== -1) { currentIndex = target_index } } } } } + Item { height: ScreenTools.defaultFontPixelHeight; width: 1 } - //--------------------------------------------------------- + GridLayout { - id: msgInfoGrid - columns: 5 - columnSpacing: ScreenTools.defaultFontPixelWidth * 0.25 - rowSpacing: ScreenTools.defaultFontPixelHeight * 0.25 - width: parent.width - QGCLabel { - text: qsTr("Name") - } - QGCLabel { - text: qsTr("Value") - } - QGCLabel { - text: qsTr("Type") - } - QGCLabel { - text: qsTr("Plot 1") - } - QGCLabel { - text: qsTr("Plot 2") - } + id: msgInfoGrid + columns: 5 + columnSpacing: ScreenTools.defaultFontPixelWidth * 0.25 + rowSpacing: ScreenTools.defaultFontPixelHeight * 0.25 + width: parent.width + + QGCLabel { text: qsTr("Name") } + QGCLabel { text: qsTr("Value") } + QGCLabel { text: qsTr("Type") } + QGCLabel { text: qsTr("Plot 1") } + QGCLabel { text: qsTr("Plot 2") } - //--------------------------------------------------------- Rectangle { - Layout.columnSpan: 5 - Layout.fillWidth: true - height: 1 - color: qgcPal.text + Layout.columnSpan: 5 + Layout.fillWidth: true + height: 1 + color: qgcPal.text } - //--------------------------------------------------------- Repeater { - model: curMessage ? curMessage.fields : [] - delegate: QGCLabel { - Layout.row: index + 2 - Layout.column: 0 + model: curMessage ? curMessage.fields : [] + delegate: QGCLabel { + Layout.row: index + 2 + Layout.column: 0 Layout.minimumWidth: ScreenTools.defaultFontPixelWidth * 20 - text: object.name + text: object.name } } + Repeater { - model: curMessage ? curMessage.fields : [] - delegate: QGCLabel { - Layout.row: index + 2 - Layout.column: 1 + model: curMessage ? curMessage.fields : [] + delegate: QGCLabel { + Layout.row: index + 2 + Layout.column: 1 Layout.minimumWidth: msgInfoGrid.width * 0.25 Layout.maximumWidth: msgInfoGrid.width * 0.25 - text: object.value - elide: Text.ElideRight + text: object.value + elide: Text.ElideRight } } + Repeater { - model: curMessage ? curMessage.fields : [] - delegate: QGCLabel { - Layout.row: index + 2 - Layout.column: 2 + model: curMessage ? curMessage.fields : [] + delegate: QGCLabel { + Layout.row: index + 2 + Layout.column: 2 Layout.minimumWidth: ScreenTools.defaultFontPixelWidth * 10 - text: object.type - elide: Text.ElideRight + text: object.type + elide: Text.ElideRight } } + Repeater { - model: curMessage ? curMessage.fields : [] - delegate: QGCCheckBox { - Layout.row: index + 2 - Layout.column: 3 - Layout.alignment: Qt.AlignHCenter + model: curMessage ? curMessage.fields : [] + delegate: QGCCheckBox { + Layout.row: index + 2 + Layout.column: 3 + Layout.alignment: Qt.AlignHCenter enabled: { - if(checked) + if (checked) { return true - if(!object.selectable) + } + if (!object.selectable) { return false - if(object.series !== null) + } + if (object.series) { return false - if(chart1.chartController !== null) { - if(chart1.chartController.chartFields.length >= chart1.seriesColors.length) + } + if (chart1.chartController) { + if (chart1.chartController.chartFields.length >= chart1.seriesColors.length) { return false + } } return true; } - checked: object.series !== null && object.chartIndex === 0 + checked: object.series && (object.chartIndex === 0) onClicked: { - if(checked) { + if (checked) { chart1.addDimension(object) } else { chart1.delDimension(object) @@ -291,28 +299,33 @@ AnalyzePage { } } } + Repeater { - model: curMessage ? curMessage.fields : [] - delegate: QGCCheckBox { + model: curMessage ? curMessage.fields : [] + delegate: QGCCheckBox { Layout.row: index + 2 Layout.column: 4 Layout.alignment: Qt.AlignHCenter enabled: { - if(checked) + if (checked) { return true - if(!object.selectable) + } + if (!object.selectable) { return false - if(object.series !== null) + } + if (object.series) { return false - if(chart2.chartController !== null) { - if(chart2.chartController.chartFields.length >= chart2.seriesColors.length) + } + if (chart2.chartController) { + if(chart2.chartController.chartFields.length >= chart2.seriesColors.length) { return false + } } return true; } - checked: object.series !== null && object.chartIndex === 1 + checked: object.series !== null && object.chartIndex === 1 onClicked: { - if(checked) { + if (checked) { chart2.addDimension(object) } else { chart2.delDimension(object) @@ -321,16 +334,19 @@ AnalyzePage { } } } + Item { height: ScreenTools.defaultFontPixelHeight * 0.25; width: 1 } + MAVLinkChart { - id: chart1 - height: ScreenTools.defaultFontPixelHeight * 20 - width: parent.width + id: chart1 + height: ScreenTools.defaultFontPixelHeight * 20 + width: parent.width } + MAVLinkChart { - id: chart2 - height: ScreenTools.defaultFontPixelHeight * 20 - width: parent.width + id: chart2 + height: ScreenTools.defaultFontPixelHeight * 20 + width: parent.width } } } diff --git a/src/AnalyzeView/MAVLinkMessage.cc b/src/AnalyzeView/MAVLinkMessage.cc index 421087743d5..09c16c45d10 100644 --- a/src/AnalyzeView/MAVLinkMessage.cc +++ b/src/AnalyzeView/MAVLinkMessage.cc @@ -10,21 +10,28 @@ #include "MAVLinkMessage.h" #include "MAVLinkMessageField.h" #include "QGCLoggingCategory.h" +#include "QmlObjectListModel.h" + +#include QGC_LOGGING_CATEGORY(MAVLinkMessageLog, "qgc.analyzeview.mavlinkmessage") -//----------------------------------------------------------------------------- -QGCMAVLinkMessage::QGCMAVLinkMessage(QObject *parent, mavlink_message_t* message) +QGCMAVLinkMessage::QGCMAVLinkMessage(const mavlink_message_t &message, QObject *parent) : QObject(parent) + , _message(message) + , _fields(new QmlObjectListModel(this)) { - _message = *message; - const mavlink_message_info_t* msgInfo = mavlink_get_message_info(message); + // qCDebug(MAVLinkMessageLog) << Q_FUNC_INFO << this; + + const mavlink_message_info_t *msgInfo = mavlink_get_message_info(&_message); if (!msgInfo) { - qCWarning(MAVLinkMessageLog) << QStringLiteral("QGCMAVLinkMessage NULL msgInfo msgid(%1)").arg(message->msgid); + qCWarning(MAVLinkMessageLog) << "QGCMAVLinkMessage NULL msgInfo msgid" << _message.msgid; return; } + _name = QString(msgInfo->name); qCDebug(MAVLinkMessageLog) << "New Message:" << _name; + for (unsigned int i = 0; i < msgInfo->num_fields; ++i) { QString type = QString("?"); switch (msgInfo->fields[i].type) { @@ -40,42 +47,40 @@ QGCMAVLinkMessage::QGCMAVLinkMessage(QObject *parent, mavlink_message_t* message case MAVLINK_TYPE_UINT64_T: type = QString("uint64_t"); break; case MAVLINK_TYPE_INT64_T: type = QString("int64_t"); break; } - QGCMAVLinkMessageField* f = new QGCMAVLinkMessageField(this, msgInfo->fields[i].name, type); - _fields.append(f); + + QGCMAVLinkMessageField *const field = new QGCMAVLinkMessageField(this, msgInfo->fields[i].name, type); + (void) _fields->append(field); } } -//----------------------------------------------------------------------------- QGCMAVLinkMessage::~QGCMAVLinkMessage() { - _fields.clearAndDeleteContents(); + _fields->clearAndDeleteContents(); + + // qCDebug(MAVLinkMessageLog) << Q_FUNC_INFO << this; } -//----------------------------------------------------------------------------- -void -QGCMAVLinkMessage::updateFieldSelection() +void QGCMAVLinkMessage::updateFieldSelection() { bool sel = false; - for (int i = 0; i < _fields.count(); ++i) { - QGCMAVLinkMessageField* f = qobject_cast(_fields.get(i)); - if(f) { - if(f->selected()) { - sel = true; - break; - } + + for (int i = 0; i < _fields->count(); ++i) { + const QGCMAVLinkMessageField *const field = qobject_cast(_fields->get(i)); + if(field && field->selected()) { + sel = true; + break; } } - if(sel != _fieldSelected) { + + if (sel != _fieldSelected) { _fieldSelected = sel; emit fieldSelectedChanged(); } } -//----------------------------------------------------------------------------- -void -QGCMAVLinkMessage::updateFreq() +void QGCMAVLinkMessage::updateFreq() { - quint64 msgCount = _count - _lastCount; + const quint64 msgCount = _count - _lastCount; _actualRateHz = (0.2 * _actualRateHz) + (0.8 * msgCount); _lastCount = _count; emit actualRateHzChanged(); @@ -92,250 +97,241 @@ void QGCMAVLinkMessage::setSelected(bool sel) void QGCMAVLinkMessage::setTargetRateHz(int32_t rate) { - if(rate != _targetRateHz) - { + if (rate != _targetRateHz) { _targetRateHz = rate; emit targetRateHzChanged(); } } -//----------------------------------------------------------------------------- -void -QGCMAVLinkMessage::update(mavlink_message_t* message) +void QGCMAVLinkMessage::update(const mavlink_message_t &message) { _count++; - _message = *message; + _message = message; if (_selected) { // Don't update field info unless selected to reduce perf hit of message processing _updateFields(); } + emit countChanged(); } -void QGCMAVLinkMessage::_updateFields(void) +void QGCMAVLinkMessage::_updateFields() { - const mavlink_message_info_t* msgInfo = mavlink_get_message_info(&_message); + const mavlink_message_info_t *const msgInfo = mavlink_get_message_info(&_message); if (!msgInfo) { - qWarning() << QStringLiteral("QGCMAVLinkMessage::update NULL msgInfo msgid(%1)").arg(_message.msgid); + qCWarning(MAVLinkMessageLog) << "QGCMAVLinkMessage::update NULL msgInfo msgid" << _message.msgid; return; } - if(_fields.count() != static_cast(msgInfo->num_fields)) { - qWarning() << QStringLiteral("QGCMAVLinkMessage::update msgInfo field count mismatch msgid(%1)").arg(_message.msgid); + + if (_fields->count() != static_cast(msgInfo->num_fields)) { + qCWarning(MAVLinkMessageLog) << "QGCMAVLinkMessage::update msgInfo field count mismatch msgid" << _message.msgid; return; } - uint8_t* m = reinterpret_cast(&_message.payload64[0]); + + uint8_t *const msg = reinterpret_cast(&_message.payload64[0]); for (unsigned int i = 0; i < msgInfo->num_fields; ++i) { - QGCMAVLinkMessageField* f = qobject_cast(_fields.get(static_cast(i))); - if(f) { - const unsigned int offset = msgInfo->fields[i].wire_offset; - const unsigned int array_length = msgInfo->fields[i].array_length; - static const unsigned int array_buffer_length = (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_CHECKSUM_BYTES + 7); - switch (msgInfo->fields[i].type) { - case MAVLINK_TYPE_CHAR: - f->setSelectable(false); - if (array_length > 0) { - char* str = reinterpret_cast(m + offset); - // Enforce null termination - str[array_length - 1] = '\0'; - QString v(str); - f->updateValue(v, 0); - } else { - // Single char - char b = *(reinterpret_cast(m + offset)); - QString v(b); - f->updateValue(v, 0); + QGCMAVLinkMessageField *const field = qobject_cast(_fields->get(static_cast(i))); + if (!field) { + continue; + } + + const unsigned int offset = msgInfo->fields[i].wire_offset; + const unsigned int array_length = msgInfo->fields[i].array_length; + static const unsigned int array_buffer_length = (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_CHECKSUM_BYTES + 7); + + switch (msgInfo->fields[i].type) { + case MAVLINK_TYPE_CHAR: + field->setSelectable(false); + if (array_length > 0) { + char *const str = reinterpret_cast(msg + offset); + str[array_length - 1] = '\0'; + QString value(str); + field->updateValue(value, 0); + } else { + char num = *(reinterpret_cast(msg + offset)); + QString value(num); + field->updateValue(value, 0); + } + break; + case MAVLINK_TYPE_UINT8_T: + if (array_length > 0) { + uint8_t *const nums = msg + offset; + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < array_length - 1; ++j) { + string += tmp.arg(nums[j]); } - break; - case MAVLINK_TYPE_UINT8_T: - if (array_length > 0) { - uint8_t* nums = m + offset; - QString tmp("%1, "); - QString string; - for (unsigned int j = 0; j < array_length - 1; ++j) { - string += tmp.arg(nums[j]); - } - string += QString::number(nums[array_length - 1]); - f->updateValue(string, static_cast(nums[0])); - } else { - // Single value - uint8_t u = *(m + offset); - f->updateValue(QString::number(u), static_cast(u)); + string += QString::number(nums[array_length - 1]); + field->updateValue(string, static_cast(nums[0])); + } else { + const uint8_t num = *(msg + offset); + field->updateValue(QString::number(num), static_cast(num)); + } + break; + case MAVLINK_TYPE_INT8_T: + if (array_length > 0) { + int8_t *const nums = reinterpret_cast(msg + offset); + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < array_length - 1; ++j) { + string += tmp.arg(nums[j]); } - break; - case MAVLINK_TYPE_INT8_T: - if (array_length > 0) { - int8_t* nums = reinterpret_cast(m + offset); - QString tmp("%1, "); - QString string; - for (unsigned int j = 0; j < array_length - 1; ++j) { - string += tmp.arg(nums[j]); - } - string += QString::number(nums[array_length - 1]); - f->updateValue(string, static_cast(nums[0])); - } else { - // Single value - int8_t n = *(reinterpret_cast(m + offset)); - f->updateValue(QString::number(n), static_cast(n)); + string += QString::number(nums[array_length - 1]); + field->updateValue(string, static_cast(nums[0])); + } else { + int8_t num = *(reinterpret_cast(msg + offset)); + field->updateValue(QString::number(num), static_cast(num)); + } + break; + case MAVLINK_TYPE_UINT16_T: + if (array_length > 0) { + uint16_t nums[array_buffer_length / sizeof(uint16_t)]; + (void) memcpy(nums, msg + offset, sizeof(uint16_t) * array_length); + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < array_length - 1; ++j) { + string += tmp.arg(nums[j]); } - break; - case MAVLINK_TYPE_UINT16_T: - if (array_length > 0) { - uint16_t nums[array_buffer_length / sizeof(uint16_t)]; - memcpy(nums, m + offset, sizeof(uint16_t) * array_length); - QString tmp("%1, "); - QString string; - for (unsigned int j = 0; j < array_length - 1; ++j) { - string += tmp.arg(nums[j]); - } - string += QString::number(nums[array_length - 1]); - f->updateValue(string, static_cast(nums[0])); - } else { - // Single value - uint16_t n; - memcpy(&n, m + offset, sizeof(uint16_t)); - f->updateValue(QString::number(n), static_cast(n)); + string += QString::number(nums[array_length - 1]); + field->updateValue(string, static_cast(nums[0])); + } else { + uint16_t num; + (void) memcpy(&num, msg + offset, sizeof(uint16_t)); + field->updateValue(QString::number(num), static_cast(num)); + } + break; + case MAVLINK_TYPE_INT16_T: + if (array_length > 0) { + int16_t nums[array_buffer_length / sizeof(int16_t)]; + (void) memcpy(nums, msg + offset, sizeof(int16_t) * array_length); + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < array_length - 1; ++j) { + string += tmp.arg(nums[j]); } - break; - case MAVLINK_TYPE_INT16_T: - if (array_length > 0) { - int16_t nums[array_buffer_length / sizeof(int16_t)]; - memcpy(nums, m + offset, sizeof(int16_t) * array_length); - QString tmp("%1, "); - QString string; - for (unsigned int j = 0; j < array_length - 1; ++j) { - string += tmp.arg(nums[j]); - } - string += QString::number(nums[array_length - 1]); - f->updateValue(string, static_cast(nums[0])); - } else { - // Single value - int16_t n; - memcpy(&n, m + offset, sizeof(int16_t)); - f->updateValue(QString::number(n), static_cast(n)); + string += QString::number(nums[array_length - 1]); + field->updateValue(string, static_cast(nums[0])); + } else { + int16_t num; + (void) memcpy(&num, msg + offset, sizeof(int16_t)); + field->updateValue(QString::number(num), static_cast(num)); + } + break; + case MAVLINK_TYPE_UINT32_T: + if (array_length > 0) { + uint32_t nums[array_buffer_length / sizeof(uint32_t)]; + (void) memcpy(nums, msg + offset, sizeof(uint32_t) * array_length); + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < array_length - 1; ++j) { + string += tmp.arg(nums[j]); } - break; - case MAVLINK_TYPE_UINT32_T: - if (array_length > 0) { - uint32_t nums[array_buffer_length / sizeof(uint32_t)]; - memcpy(nums, m + offset, sizeof(uint32_t) * array_length); - QString tmp("%1, "); - QString string; - for (unsigned int j = 0; j < array_length - 1; ++j) { - string += tmp.arg(nums[j]); - } - string += QString::number(nums[array_length - 1]); - f->updateValue(string, static_cast(nums[0])); + string += QString::number(nums[array_length - 1]); + field->updateValue(string, static_cast(nums[0])); + } else { + uint32_t num; + (void) memcpy(&num, msg + offset, sizeof(uint32_t)); + if (_message.msgid == MAVLINK_MSG_ID_SYSTEM_TIME) { + const QDateTime date = QDateTime::fromMSecsSinceEpoch(static_cast(num),Qt::UTC,0); + field->updateValue(date.toString("HH:mm:ss"), static_cast(num)); } else { - // Single value - uint32_t n; - memcpy(&n, m + offset, sizeof(uint32_t)); - //-- Special case - if(_message.msgid == MAVLINK_MSG_ID_SYSTEM_TIME) { - QDateTime d = QDateTime::fromMSecsSinceEpoch(static_cast(n),Qt::UTC,0); - f->updateValue(d.toString("HH:mm:ss"), static_cast(n)); - } else { - f->updateValue(QString::number(n), static_cast(n)); - } + field->updateValue(QString::number(num), static_cast(num)); } - break; - case MAVLINK_TYPE_INT32_T: - if (array_length > 0) { - int32_t nums[array_buffer_length / sizeof(int32_t)]; - memcpy(nums, m + offset, sizeof(int32_t) * array_length); - QString tmp("%1, "); - QString string; - for (unsigned int j = 0; j < array_length - 1; ++j) { - string += tmp.arg(nums[j]); - } - string += QString::number(nums[array_length - 1]); - f->updateValue(string, static_cast(nums[0])); - } else { - // Single value - int32_t n; - memcpy(&n, m + offset, sizeof(int32_t)); - f->updateValue(QString::number(n), static_cast(n)); + } + break; + case MAVLINK_TYPE_INT32_T: + if (array_length > 0) { + int32_t nums[array_buffer_length / sizeof(int32_t)]; + (void) memcpy(nums, msg + offset, sizeof(int32_t) * array_length); + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < array_length - 1; ++j) { + string += tmp.arg(nums[j]); } - break; - case MAVLINK_TYPE_FLOAT: - if (array_length > 0) { - float nums[array_buffer_length / sizeof(float)]; - memcpy(nums, m + offset, sizeof(float) * array_length); - QString tmp("%1, "); - QString string; - for (unsigned int j = 0; j < array_length - 1; ++j) { - string += tmp.arg(static_cast(nums[j])); - } - string += QString::number(static_cast(nums[array_length - 1])); - f->updateValue(string, static_cast(nums[0])); - } else { - // Single value - float fv; - memcpy(&fv, m + offset, sizeof(float)); - f->updateValue(QString::number(static_cast(fv)), static_cast(fv)); + string += QString::number(nums[array_length - 1]); + field->updateValue(string, static_cast(nums[0])); + } else { + int32_t num; + (void) memcpy(&num, msg + offset, sizeof(int32_t)); + field->updateValue(QString::number(num), static_cast(num)); + } + break; + case MAVLINK_TYPE_FLOAT: + if (array_length > 0) { + float nums[array_buffer_length / sizeof(float)]; + (void) memcpy(nums, msg + offset, sizeof(float) * array_length); + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < array_length - 1; ++j) { + string += tmp.arg(static_cast(nums[j])); } - break; - case MAVLINK_TYPE_DOUBLE: - if (array_length > 0) { - double nums[array_buffer_length / sizeof(double)]; - memcpy(nums, m + offset, sizeof(double) * array_length); - QString tmp("%1, "); - QString string; - for (unsigned int j = 0; j < array_length - 1; ++j) { - string += tmp.arg(nums[j]); - } - string += QString::number(static_cast(nums[array_length - 1])); - f->updateValue(string, static_cast(nums[0])); - } else { - // Single value - double d; - memcpy(&d, m + offset, sizeof(double)); - f->updateValue(QString::number(d), static_cast(d)); + string += QString::number(static_cast(nums[array_length - 1])); + field->updateValue(string, static_cast(nums[0])); + } else { + float num; + (void) memcpy(&num, msg + offset, sizeof(float)); + field->updateValue(QString::number(static_cast(num)), static_cast(num)); + } + break; + case MAVLINK_TYPE_DOUBLE: + if (array_length > 0) { + double nums[array_buffer_length / sizeof(double)]; + (void) memcpy(nums, msg + offset, sizeof(double) * array_length); + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < array_length - 1; ++j) { + string += tmp.arg(nums[j]); } - break; - case MAVLINK_TYPE_UINT64_T: - if (array_length > 0) { - uint64_t nums[array_buffer_length / sizeof(uint64_t)]; - memcpy(nums, m + offset, sizeof(uint64_t) * array_length); - QString tmp("%1, "); - QString string; - for (unsigned int j = 0; j < array_length - 1; ++j) { - string += tmp.arg(nums[j]); - } - string += QString::number(nums[array_length - 1]); - f->updateValue(string, static_cast(nums[0])); - } else { - // Single value - uint64_t n; - memcpy(&n, m + offset, sizeof(uint64_t)); - //-- Special case - if(_message.msgid == MAVLINK_MSG_ID_SYSTEM_TIME) { - QDateTime d = QDateTime::fromMSecsSinceEpoch(n/1000,Qt::UTC,0); - f->updateValue(d.toString("yyyy MM dd HH:mm:ss"), static_cast(n)); - } else { - f->updateValue(QString::number(n), static_cast(n)); - } + string += QString::number(static_cast(nums[array_length - 1])); + field->updateValue(string, static_cast(nums[0])); + } else { + double num; + (void) memcpy(&num, msg + offset, sizeof(double)); + field->updateValue(QString::number(num), static_cast(num)); + } + break; + case MAVLINK_TYPE_UINT64_T: + if (array_length > 0) { + uint64_t nums[array_buffer_length / sizeof(uint64_t)]; + (void) memcpy(nums, msg + offset, sizeof(uint64_t) * array_length); + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < array_length - 1; ++j) { + string += tmp.arg(nums[j]); } - break; - case MAVLINK_TYPE_INT64_T: - if (array_length > 0) { - int64_t nums[array_buffer_length / sizeof(int64_t)]; - memcpy(nums, m + offset, sizeof(int64_t) * array_length); - QString tmp("%1, "); - QString string; - for (unsigned int j = 0; j < array_length - 1; ++j) { - string += tmp.arg(nums[j]); - } - string += QString::number(nums[array_length - 1]); - f->updateValue(string, static_cast(nums[0])); + string += QString::number(nums[array_length - 1]); + field->updateValue(string, static_cast(nums[0])); + } else { + uint64_t num; + (void) memcpy(&num, msg + offset, sizeof(uint64_t)); + if (_message.msgid == MAVLINK_MSG_ID_SYSTEM_TIME) { + const QDateTime date = QDateTime::fromMSecsSinceEpoch(num / 1000, Qt::UTC, 0); + field->updateValue(date.toString("yyyy MM dd HH:mm:ss"), static_cast(num)); } else { - // Single value - int64_t n; - memcpy(&n, m + offset, sizeof(int64_t)); - f->updateValue(QString::number(n), static_cast(n)); + field->updateValue(QString::number(num), static_cast(num)); + } + } + break; + case MAVLINK_TYPE_INT64_T: + if (array_length > 0) { + int64_t nums[array_buffer_length / sizeof(int64_t)]; + (void) memcpy(nums, msg + offset, sizeof(int64_t) * array_length); + QString tmp("%1, "); + QString string; + for (unsigned int j = 0; j < array_length - 1; ++j) { + string += tmp.arg(nums[j]); } - break; + string += QString::number(nums[array_length - 1]); + field->updateValue(string, static_cast(nums[0])); + } else { + int64_t num; + (void) memcpy(&num, msg + offset, sizeof(int64_t)); + field->updateValue(QString::number(num), static_cast(num)); } + break; + default: + continue; } } } diff --git a/src/AnalyzeView/MAVLinkMessage.h b/src/AnalyzeView/MAVLinkMessage.h index 795c2636b22..6661a617ce4 100644 --- a/src/AnalyzeView/MAVLinkMessage.h +++ b/src/AnalyzeView/MAVLinkMessage.h @@ -14,54 +14,53 @@ #pragma once #include -#include #include +#include #include #include "MAVLinkLib.h" -#include "QmlObjectListModel.h" Q_DECLARE_LOGGING_CATEGORY(MAVLinkMessageLog) -//----------------------------------------------------------------------------- -/// MAVLink message +class QmlObjectListModel; + class QGCMAVLinkMessage : public QObject { Q_OBJECT QML_ELEMENT - -public: + Q_MOC_INCLUDE("QmlObjectListModel.h") Q_PROPERTY(quint32 id READ id CONSTANT) Q_PROPERTY(quint32 sysId READ sysId CONSTANT) Q_PROPERTY(quint32 compId READ compId CONSTANT) + Q_PROPERTY(quint64 count READ count NOTIFY countChanged) Q_PROPERTY(QString name READ name CONSTANT) Q_PROPERTY(qreal actualRateHz READ actualRateHz NOTIFY actualRateHzChanged) Q_PROPERTY(int32_t targetRateHz READ targetRateHz NOTIFY targetRateHzChanged) - Q_PROPERTY(quint64 count READ count NOTIFY countChanged) - Q_PROPERTY(QmlObjectListModel* fields READ fields CONSTANT) Q_PROPERTY(bool fieldSelected READ fieldSelected NOTIFY fieldSelectedChanged) Q_PROPERTY(bool selected READ selected NOTIFY selectedChanged) + Q_PROPERTY(QmlObjectListModel *fields READ fields CONSTANT) - QGCMAVLinkMessage (QObject* parent, mavlink_message_t* message); - ~QGCMAVLinkMessage (); +public: + QGCMAVLinkMessage(const mavlink_message_t &message, QObject *parent = nullptr); + ~QGCMAVLinkMessage(); - quint32 id () const { return _message.msgid; } - quint8 sysId () const { return _message.sysid; } - quint8 compId () const { return _message.compid; } - QString name () const { return _name; } - qreal actualRateHz () const { return _actualRateHz; } - int32_t targetRateHz () const { return _targetRateHz; } - quint64 count () const { return _count; } - quint64 lastCount () const { return _lastCount; } - QmlObjectListModel* fields () { return &_fields; } - bool fieldSelected () const { return _fieldSelected; } - bool selected () const { return _selected; } + quint32 id() const { return _message.msgid; } + quint8 sysId() const { return _message.sysid; } + quint8 compId() const { return _message.compid; } + quint64 count() const { return _count; } + QString name() const { return _name; } + qreal actualRateHz() const { return _actualRateHz; } + int32_t targetRateHz() const { return _targetRateHz; } + bool fieldSelected() const { return _fieldSelected; } + bool selected() const { return _selected; } + QmlObjectListModel *fields() { return _fields; } + quint64 lastCount() const { return _lastCount; } - void updateFieldSelection(); - void update (mavlink_message_t* message); - void updateFreq (); - void setSelected (bool sel); - void setTargetRateHz (int32_t rate); + void updateFieldSelection(); + void update(const mavlink_message_t &message); + void updateFreq(); + void setSelected(bool sel); + void setTargetRateHz(int32_t rate); signals: void countChanged(); @@ -71,15 +70,15 @@ class QGCMAVLinkMessage : public QObject void selectedChanged(); private: - void _updateFields(void); + void _updateFields(); - QmlObjectListModel _fields; - QString _name; - qreal _actualRateHz = 0.0; - int32_t _targetRateHz = 0; - uint64_t _count = 1; - uint64_t _lastCount = 0; - mavlink_message_t _message; - bool _fieldSelected = false; - bool _selected = false; + mavlink_message_t _message = {0}; + QmlObjectListModel *_fields = nullptr; + QString _name; + qreal _actualRateHz = 0.0; + int32_t _targetRateHz = 0; + uint64_t _count = 1; + uint64_t _lastCount = 0; + bool _fieldSelected = false; + bool _selected = false; }; diff --git a/src/AnalyzeView/MAVLinkMessageButton.qml b/src/AnalyzeView/MAVLinkMessageButton.qml new file mode 100644 index 00000000000..2e61305a743 --- /dev/null +++ b/src/AnalyzeView/MAVLinkMessageButton.qml @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick +import QtQuick.Controls +import QtQuick.Layouts + +import QGroundControl.Controls +import QGroundControl.Palette +import QGroundControl.ScreenTools + +Button { + id: control + autoExclusive: true + leftPadding: ScreenTools.defaultFontPixelWidth + rightPadding: leftPadding + + readonly property real _compIDWidth: ScreenTools.defaultFontPixelWidth * 3 + readonly property real _hzWidth: ScreenTools.defaultFontPixelWidth * 6 + readonly property real _nameWidth: nameLabel.contentWidth + property double messageHz: 0 + property int compID: 0 + + QGCPalette { id: qgcPal; colorGroupEnabled: enabled } + + background: Rectangle { + anchors.fill: parent + color: checked ? qgcPal.buttonHighlight : qgcPal.button + } + + contentItem: RowLayout { + id: rowLayout + spacing: ScreenTools.defaultFontPixelWidth + + QGCLabel { + text: control.compID + color: checked ? qgcPal.buttonHighlightText : qgcPal.buttonText + verticalAlignment: Text.AlignVCenter + Layout.minimumHeight: ScreenTools.isMobile ? (ScreenTools.defaultFontPixelHeight * 2) : (ScreenTools.defaultFontPixelHeight * 1.5) + Layout.minimumWidth: _compIDWidth + } + + QGCLabel { + id: nameLabel + text: control.text + color: checked ? qgcPal.buttonHighlightText : qgcPal.buttonText + Layout.fillWidth: true + Layout.alignment: Qt.AlignVCenter + } + + QGCLabel { + color: checked ? qgcPal.buttonHighlightText : qgcPal.buttonText + text: messageHz.toFixed(1) + 'Hz' + horizontalAlignment: Text.AlignRight + Layout.minimumWidth: _hzWidth + Layout.alignment: Qt.AlignVCenter + } + } + + Component.onCompleted: maxButtonWidth = Math.max(maxButtonWidth, _compIDWidth + _hzWidth + _nameWidth + (rowLayout.spacing * 2) + (control.leftPadding * 2)) +} diff --git a/src/AnalyzeView/MAVLinkMessageField.cc b/src/AnalyzeView/MAVLinkMessageField.cc index e464721a302..7fbeda14f8e 100644 --- a/src/AnalyzeView/MAVLinkMessageField.cc +++ b/src/AnalyzeView/MAVLinkMessageField.cc @@ -18,131 +18,146 @@ QGC_LOGGING_CATEGORY(MAVLinkMessageFieldLog, "qgc.analyzeview.mavlinkmessagefield") -//----------------------------------------------------------------------------- -QGCMAVLinkMessageField::QGCMAVLinkMessageField(QGCMAVLinkMessage *parent, QString name, QString type) +QGCMAVLinkMessageField::QGCMAVLinkMessageField(QGCMAVLinkMessage *parent, const QString &name, const QString &type) : QObject(parent) , _type(type) , _name(name) , _msg(parent) { + // qCDebug(MAVLinkMessageFieldLog) << Q_FUNC_INFO << this; qCDebug(MAVLinkMessageFieldLog) << "Field:" << name << type; } -//----------------------------------------------------------------------------- -void -QGCMAVLinkMessageField::addSeries(MAVLinkChartController* chart, QAbstractSeries* series) +QGCMAVLinkMessageField::~QGCMAVLinkMessageField() { - if(!_pSeries) { - _chart = chart; - _pSeries = series; - emit seriesChanged(); - _dataIndex = 0; - _msg->updateFieldSelection(); + // qCDebug(MAVLinkMessageFieldLog) << Q_FUNC_INFO << this; +} + +void QGCMAVLinkMessageField::addSeries(MAVLinkChartController *chart, QAbstractSeries *series) +{ + if (_pSeries) { + return; } + + _dataIndex = 0; + _chart = chart; + _pSeries = series; + emit seriesChanged(); + + _msg->updateFieldSelection(); } -//----------------------------------------------------------------------------- -void -QGCMAVLinkMessageField::delSeries() +void QGCMAVLinkMessageField::delSeries() { - if(_pSeries) { - _values.clear(); - QLineSeries* lineSeries = static_cast(_pSeries); - lineSeries->replace(_values); - _pSeries = nullptr; - _chart = nullptr; - emit seriesChanged(); - _msg->updateFieldSelection(); + if (!_pSeries) { + return; } + + _values.clear(); + QLineSeries *const lineSeries = static_cast(_pSeries); + (void) lineSeries->replace(_values); + _chart = nullptr; + _pSeries = nullptr; + emit seriesChanged(); + + _msg->updateFieldSelection(); } -//----------------------------------------------------------------------------- -QString -QGCMAVLinkMessageField::label() +QString QGCMAVLinkMessageField::label() { - //-- Label is message name + field name - return QString(_msg->name() + ": " + _name); + return (_msg->name() + QStringLiteral(": ") + _name); } -//----------------------------------------------------------------------------- -void -QGCMAVLinkMessageField::setSelectable(bool sel) +void QGCMAVLinkMessageField::setSelectable(bool sel) { - if(_selectable != sel) { + if (_selectable != sel) { _selectable = sel; emit selectableChanged(); } } -//----------------------------------------------------------------------------- -int -QGCMAVLinkMessageField::chartIndex() +int QGCMAVLinkMessageField::chartIndex() { - if(_chart) - return _chart->chartIndex(); - return 0; + return (_chart ? _chart->chartIndex() : 0); } -//----------------------------------------------------------------------------- -void -QGCMAVLinkMessageField::updateValue(QString newValue, qreal v) +void QGCMAVLinkMessageField::updateValue(const QString &newValue, qreal val) { - if(_value != newValue) { + if (_value != newValue) { _value = newValue; emit valueChanged(); } - if(_pSeries && _chart) { - int count = _values.count(); - //-- Arbitrary limit of 1 minute of data at 50Hz for now - if(count < (50 * 60)) { - QPointF p(QGC::bootTimeMilliseconds(), v); - _values.append(p); - } else { - if(_dataIndex >= count) _dataIndex = 0; - _values[_dataIndex].setX(QGC::bootTimeMilliseconds()); - _values[_dataIndex].setY(v); - _dataIndex++; + + if (!_pSeries || !_chart) { + return; + } + + const int count = _values.count(); + if (count < kDataLimit) { + QPointF point(QGC::bootTimeMilliseconds(), val); + (void) _values.append(point); + } else { + if (_dataIndex >= count) { + _dataIndex = 0; + } + + _values[_dataIndex].setX(QGC::bootTimeMilliseconds()); + _values[_dataIndex].setY(val); + _dataIndex++; + } + + //-- Auto Range + if (_chart->rangeYIndex() != 0) { + return; + } + + qreal vmin = std::numeric_limits::max(); + qreal vmax = std::numeric_limits::min(); + for (const QPointF point : _values) { + const qreal val = point.y(); + if (vmax < val) { + vmax = val; } - //-- Auto Range - if(_chart->rangeYIndex() == 0) { - qreal vmin = std::numeric_limits::max(); - qreal vmax = std::numeric_limits::min(); - for(int i = 0; i < _values.count(); i++) { - qreal v = _values[i].y(); - if(vmax < v) vmax = v; - if(vmin > v) vmin = v; - } - bool changed = false; - if(std::abs(_rangeMin - vmin) > 0.000001) { - _rangeMin = vmin; - changed = true; - } - if(std::abs(_rangeMax - vmax) > 0.000001) { - _rangeMax = vmax; - changed = true; - } - if(changed) { - _chart->updateYRange(); - } + if (vmin > val) { + vmin = val; } } + + bool changed = false; + if (qAbs(_rangeMin - vmin) > 0.000001) { + _rangeMin = vmin; + changed = true; + } + + if (qAbs(_rangeMax - vmax) > 0.000001) { + _rangeMax = vmax; + changed = true; + } + + if (changed) { + _chart->updateYRange(); + } } -//----------------------------------------------------------------------------- -void -QGCMAVLinkMessageField::updateSeries() +void QGCMAVLinkMessageField::updateSeries() { - int count = _values.count(); - if (count > 1) { - QList s; - s.reserve(count); - int idx = _dataIndex; - for(int i = 0; i < count; i++, idx++) { - if(idx >= count) idx = 0; - QPointF p(_values[idx]); - s.append(p); + const int count = _values.count(); + if (count <= 1) { + return; + } + + QList points; + points.reserve(count); + int idx = _dataIndex; + for (int i = 0; i < count; i++, idx++) { + if (idx >= count) { + idx = 0; } - QLineSeries* lineSeries = static_cast(_pSeries); - lineSeries->replace(s); + + QPointF point(_values[idx]); + (void) points.append(point); } + + QLineSeries *const lineSeries = static_cast(_pSeries); + lineSeries->replace(points); } diff --git a/src/AnalyzeView/MAVLinkMessageField.h b/src/AnalyzeView/MAVLinkMessageField.h index d9626b1de29..c6e8293b238 100644 --- a/src/AnalyzeView/MAVLinkMessageField.h +++ b/src/AnalyzeView/MAVLinkMessageField.h @@ -13,10 +13,10 @@ #pragma once +#include #include -#include #include -#include +#include #include Q_DECLARE_LOGGING_CATEGORY(MAVLinkMessageFieldLog) @@ -25,60 +25,61 @@ class QGCMAVLinkMessage; class MAVLinkChartController; class QAbstractSeries; -//----------------------------------------------------------------------------- -/// MAVLink message field class QGCMAVLinkMessageField : public QObject { Q_OBJECT QML_ELEMENT - Q_MOC_INCLUDE() + Q_MOC_INCLUDE("QtCharts/QAbstractSeries") + Q_PROPERTY(QString name READ name CONSTANT) + Q_PROPERTY(QString label READ label CONSTANT) + Q_PROPERTY(QString type READ type CONSTANT) + Q_PROPERTY(QString value READ value NOTIFY valueChanged) + Q_PROPERTY(bool selectable READ selectable NOTIFY selectableChanged) + Q_PROPERTY(int chartIndex READ chartIndex CONSTANT) + Q_PROPERTY(QAbstractSeries *series READ series NOTIFY seriesChanged) public: - Q_PROPERTY(QString name READ name CONSTANT) - Q_PROPERTY(QString label READ label CONSTANT) - Q_PROPERTY(QString type READ type CONSTANT) - Q_PROPERTY(QString value READ value NOTIFY valueChanged) - Q_PROPERTY(bool selectable READ selectable NOTIFY selectableChanged) - Q_PROPERTY(int chartIndex READ chartIndex CONSTANT) - Q_PROPERTY(QAbstractSeries* series READ series NOTIFY seriesChanged) + QGCMAVLinkMessageField(QGCMAVLinkMessage *parent, const QString &name, const QString &type); + ~QGCMAVLinkMessageField(); - QGCMAVLinkMessageField(QGCMAVLinkMessage* parent, QString name, QString type); + QString name() const { return _name; } + QString label(); + QString type() const { return _type; } + QString value() const { return _value; } + bool selectable() const { return _selectable; } + int chartIndex(); - QString name () { return _name; } - QString label (); - QString type () { return _type; } - QString value () { return _value; } - bool selectable () const{ return _selectable; } - bool selected () { return _pSeries != nullptr; } - QAbstractSeries*series () { return _pSeries; } - QList* values () { return &_values;} - qreal rangeMin () const{ return _rangeMin; } - qreal rangeMax () const{ return _rangeMax; } - int chartIndex (); + bool selected() const { return (_pSeries != nullptr); } + qreal rangeMin() const { return _rangeMin; } + qreal rangeMax() const { return _rangeMax; } + QAbstractSeries *series() { return _pSeries; } + QList *values() { return &_values;} - void setSelectable (bool sel); - void updateValue (QString newValue, qreal v); - - void addSeries (MAVLinkChartController* chart, QAbstractSeries* series); - void delSeries (); - void updateSeries (); + void setSelectable(bool sel); + void updateValue(const QString &newValue, qreal val); + void addSeries(MAVLinkChartController *chart, QAbstractSeries *series); + void delSeries(); + void updateSeries(); signals: - void seriesChanged (); - void selectableChanged (); - void valueChanged (); + void seriesChanged(); + void selectableChanged(); + void valueChanged(); private: - QString _type; - QString _name; - QString _value; - bool _selectable = true; - int _dataIndex = 0; - qreal _rangeMin = 0; - qreal _rangeMax = 0; + QString _type; + QString _name; + QGCMAVLinkMessage *_msg = nullptr; + + QString _value; + bool _selectable = true; + int _dataIndex = 0; + qreal _rangeMin = 0.; + qreal _rangeMax = 0.; + QList _values; + + QAbstractSeries *_pSeries = nullptr; + MAVLinkChartController *_chart = nullptr; - QAbstractSeries* _pSeries = nullptr; - QGCMAVLinkMessage* _msg = nullptr; - MAVLinkChartController* _chart = nullptr; - QList _values; + static constexpr int kDataLimit = (50 * 60); ///< Arbitrary limit of 1 minute of data at 50Hz for now }; diff --git a/src/AnalyzeView/MAVLinkSystem.cc b/src/AnalyzeView/MAVLinkSystem.cc index 7f87daa40cd..62e70f0ee07 100644 --- a/src/AnalyzeView/MAVLinkSystem.cc +++ b/src/AnalyzeView/MAVLinkSystem.cc @@ -10,141 +10,140 @@ #include "MAVLinkSystem.h" #include "MAVLinkMessage.h" #include "QGCLoggingCategory.h" +#include "QmlObjectListModel.h" QGC_LOGGING_CATEGORY(MAVLinkSystemLog, "qgc.analyzeview.mavlinksystem") -//----------------------------------------------------------------------------- -QGCMAVLinkSystem::QGCMAVLinkSystem(QObject* parent, quint8 id) +QGCMAVLinkSystem::QGCMAVLinkSystem(quint8 id, QObject *parent) : QObject(parent) , _id(id) + , _messages(new QmlObjectListModel(this)) { + // qCDebug(MAVLinkSystemLog) << Q_FUNC_INFO << this; + qCDebug(MAVLinkSystemLog) << "New Vehicle:" << id; } -//----------------------------------------------------------------------------- QGCMAVLinkSystem::~QGCMAVLinkSystem() { - _messages.clearAndDeleteContents(); + _messages->clearAndDeleteContents(); + + // qCDebug(MAVLinkSystemLog) << Q_FUNC_INFO << this; } -//----------------------------------------------------------------------------- -QGCMAVLinkMessage* -QGCMAVLinkSystem::findMessage(uint32_t id, uint8_t compId) +QGCMAVLinkMessage *QGCMAVLinkSystem::findMessage(uint32_t id, uint8_t compId) const { - for(int i = 0; i < _messages.count(); i++) { - QGCMAVLinkMessage* m = qobject_cast(_messages.get(i)); - if(m) { - if(m->id() == id && m->compId() == compId) { - return m; - } + for (int i = 0; i < _messages->count(); i++) { + QGCMAVLinkMessage *const msg = qobject_cast(_messages->get(i)); + if (msg && (msg->id() == id) && (msg->compId() == compId)) { + return msg; } } + return nullptr; } -//----------------------------------------------------------------------------- -int -QGCMAVLinkSystem::findMessage(QGCMAVLinkMessage* message) +int QGCMAVLinkSystem::findMessage(QGCMAVLinkMessage *message) const { - for(int i = 0; i < _messages.count(); i++) { - QGCMAVLinkMessage* m = qobject_cast(_messages.get(i)); - if(m && m == message) { + for (int i = 0; i < _messages->count(); i++) { + QGCMAVLinkMessage *const msg = qobject_cast(_messages->get(i)); + if(msg && (msg == message)) { return i; } } + return -1; } -//----------------------------------------------------------------------------- -void -QGCMAVLinkSystem::_resetSelection() +void QGCMAVLinkSystem::_resetSelection() { - for(int i = 0; i < _messages.count(); i++) { - QGCMAVLinkMessage* m = qobject_cast(_messages.get(i)); - if(m && m->selected()) { - m->setSelected(false); - emit m->selectedChanged(); + for (int i = 0; i < _messages->count(); i++) { + QGCMAVLinkMessage *const msg = qobject_cast(_messages->get(i)); + if (msg && msg->selected()) { + msg->setSelected(false); + emit msg->selectedChanged(); } } } -//----------------------------------------------------------------------------- -void -QGCMAVLinkSystem::setSelected(int sel) +void QGCMAVLinkSystem::setSelected(int sel) { - if(sel < _messages.count()) { - _selected = sel; - emit selectedChanged(); - _resetSelection(); - QGCMAVLinkMessage* m = qobject_cast(_messages.get(sel)); - if(m && !m->selected()) { - m->setSelected(true); - emit m->selectedChanged(); - } + if (sel >= _messages->count()) { + return; + } + + _selected = sel; + emit selectedChanged(); + + _resetSelection(); + + QGCMAVLinkMessage *const msg = qobject_cast(_messages->get(sel)); + if (msg && !msg->selected()) { + msg->setSelected(true); + emit msg->selectedChanged(); } } -QGCMAVLinkMessage* QGCMAVLinkSystem::selectedMsg() +QGCMAVLinkMessage *QGCMAVLinkSystem::selectedMsg() { - QGCMAVLinkMessage* selectedMsg = nullptr; - if(_messages.count()) - { - selectedMsg = qobject_cast(_messages.get(_selected)); - } - return selectedMsg; + return (_messages->count() ? qobject_cast(_messages->get(_selected)) : nullptr); } -//----------------------------------------------------------------------------- -static bool -messages_sort(QObject* a, QObject* b) +bool QGCMAVLinkSystem::_messagesSort(const QObject *a, const QObject *b) { - QGCMAVLinkMessage* aa = qobject_cast(a); - QGCMAVLinkMessage* bb = qobject_cast(b); - if(!aa || !bb) return false; - if(aa->name() == bb->name()) return aa->name() < bb->name(); - return aa->name() < bb->name(); + const QGCMAVLinkMessage *const aa = qobject_cast(a); + const QGCMAVLinkMessage *const bb = qobject_cast(b); + + if (!aa || !bb) { + return false; + } + + if (aa->name() == bb->name()) { + return (aa->name() < bb->name()); + } + + return (aa->name() < bb->name()); } -//----------------------------------------------------------------------------- -void -QGCMAVLinkSystem::append(QGCMAVLinkMessage* message) +void QGCMAVLinkSystem::append(QGCMAVLinkMessage *message) { - //-- Save selected message - QGCMAVLinkMessage* selectedMsg = nullptr; - if(_messages.count()) { - selectedMsg = qobject_cast(_messages.get(_selected)); + QGCMAVLinkMessage *selectedMsg = nullptr; + + if(_messages->count()) { + selectedMsg = qobject_cast(_messages->get(_selected)); } else { - //-- First message message->setSelected(true); } - _messages.append(message); + + (void) _messages->append(message); + //-- Sort messages by id and then compId - if (_messages.count() > 0) { - _messages.beginReset(); - std::sort(_messages.objectList()->begin(), _messages.objectList()->end(), messages_sort); - _messages.endReset(); + if (_messages->count() > 0) { + _messages->beginReset(); + std::sort(_messages->objectList()->begin(), _messages->objectList()->end(), _messagesSort); + _messages->endReset(); _checkCompID(message); } + //-- Remember selected message - if(selectedMsg) { - int idx = findMessage(selectedMsg); - if(idx >= 0) { + if (selectedMsg) { + const int idx = findMessage(selectedMsg); + if (idx >= 0) { _selected = idx; emit selectedChanged(); } } } -//----------------------------------------------------------------------------- -void -QGCMAVLinkSystem::_checkCompID(QGCMAVLinkMessage* message) +void QGCMAVLinkSystem::_checkCompID(QGCMAVLinkMessage *message) { - if(_compIDsStr.isEmpty()) { + if (_compIDsStr.isEmpty()) { _compIDsStr << tr("Comp All"); } - if(!_compIDs.contains(static_cast(message->compId()))) { - int compId = static_cast(message->compId()); - _compIDs.append(compId); + + if (!_compIDs.contains(static_cast(message->compId()))) { + const int compId = static_cast(message->compId()); + (void) _compIDs.append(compId); _compIDsStr << tr("Comp %1").arg(compId); emit compIDsChanged(); } diff --git a/src/AnalyzeView/MAVLinkSystem.h b/src/AnalyzeView/MAVLinkSystem.h index 5abffe91c6e..6a65daf0fa9 100644 --- a/src/AnalyzeView/MAVLinkSystem.h +++ b/src/AnalyzeView/MAVLinkSystem.h @@ -14,57 +14,55 @@ #pragma once #include -#include #include +#include #include -#include "QmlObjectListModel.h" Q_DECLARE_LOGGING_CATEGORY(MAVLinkSystemLog) class QGCMAVLinkMessage; +class QmlObjectListModel; -//----------------------------------------------------------------------------- -/// Vehicle MAVLink message belongs to class QGCMAVLinkSystem : public QObject { Q_OBJECT QML_ELEMENT + Q_MOC_INCLUDE("QmlObjectListModel.h") + Q_PROPERTY(quint8 id READ id CONSTANT) + Q_PROPERTY(QmlObjectListModel *messages READ messages CONSTANT) + Q_PROPERTY(QList compIDs READ compIDs NOTIFY compIDsChanged) + Q_PROPERTY(QStringList compIDsStr READ compIDsStr NOTIFY compIDsChanged) + Q_PROPERTY(int selected READ selected WRITE setSelected NOTIFY selectedChanged) public: - Q_PROPERTY(quint8 id READ id CONSTANT) - Q_PROPERTY(QmlObjectListModel* messages READ messages CONSTANT) - Q_PROPERTY(QList compIDs READ compIDs NOTIFY compIDsChanged) - Q_PROPERTY(QStringList compIDsStr READ compIDsStr NOTIFY compIDsChanged) - Q_PROPERTY(int selected READ selected WRITE setSelected NOTIFY selectedChanged) + QGCMAVLinkSystem(quint8 id, QObject *parent = nullptr); + ~QGCMAVLinkSystem(); - QGCMAVLinkSystem (QObject* parent, quint8 id); - ~QGCMAVLinkSystem (); + quint8 id() const { return _id; } + QmlObjectListModel *messages() { return _messages; } + QList compIDs() const { return _compIDs; } + QStringList compIDsStr() const { return _compIDsStr; } + int selected() const { return _selected; } - quint8 id () const{ return _id; } - QmlObjectListModel* messages () { return &_messages; } - QList compIDs () { return _compIDs; } - QStringList compIDsStr () { return _compIDsStr; } - int selected () const{ return _selected; } - - void setSelected (int sel); - QGCMAVLinkMessage* findMessage (uint32_t id, uint8_t compId); - int findMessage (QGCMAVLinkMessage* message); - void append (QGCMAVLinkMessage* message); - QGCMAVLinkMessage* selectedMsg (); + void setSelected(int sel); + QGCMAVLinkMessage *findMessage(uint32_t id, uint8_t compId) const; + int findMessage(QGCMAVLinkMessage *message) const; + void append(QGCMAVLinkMessage *message); + QGCMAVLinkMessage *selectedMsg(); signals: - void compIDsChanged (); - void selectedChanged (); + void compIDsChanged(); + void selectedChanged(); private: - void _checkCompID (QGCMAVLinkMessage *message); - void _resetSelection (); + void _checkCompID(QGCMAVLinkMessage *message); + void _resetSelection(); + static bool _messagesSort(const QObject *a, const QObject *b); -private: - quint8 _id; - QList _compIDs; - QStringList _compIDsStr; - QmlObjectListModel _messages; //-- List of QGCMAVLinkMessage - int _selected = 0; + quint8 _id = 0; + QmlObjectListModel *_messages = nullptr; ///< List of QGCMAVLinkMessage + QList _compIDs; + QStringList _compIDsStr; + int _selected = 0; }; diff --git a/src/QmlControls/MAVLinkChart.qml b/src/QmlControls/MAVLinkChart.qml deleted file mode 100644 index 6051c41d683..00000000000 --- a/src/QmlControls/MAVLinkChart.qml +++ /dev/null @@ -1,129 +0,0 @@ -import QtQuick -import QtQuick.Controls -import QtQuick.Layouts -import QtCharts - -import QGroundControl -import QGroundControl.Palette -import QGroundControl.Controls -import QGroundControl.Controllers -import QGroundControl.ScreenTools - -ChartView { - id: chartView - theme: ChartView.ChartThemeDark - antialiasing: true - animationOptions: ChartView.NoAnimation - legend.visible: false - backgroundColor: qgcPal.window - backgroundRoundness: 0 - margins.bottom: ScreenTools.defaultFontPixelHeight * 1.5 - margins.top: chartHeader.height + (ScreenTools.defaultFontPixelHeight * 2) - - property var chartController: null - property var seriesColors: ["#00E04B","#DE8500","#F32836","#BFBFBF","#536DFF","#EECC44"] - - function addDimension(field) { - if(!chartController) { - chartController = controller.createChart() - } - var color = chartView.seriesColors[chartView.count] - var serie = createSeries(ChartView.SeriesTypeLine, field.label) - serie.axisX = axisX - serie.axisY = axisY - serie.useOpenGL = true - serie.color = color - serie.width = 1 - chartController.addSeries(field, serie) - } - - function delDimension(field) { - if(chartController) { - chartView.removeSeries(field.series) - chartController.delSeries(field) - if(chartView.count === 0) { - controller.deleteChart(chartController) - chartController = null - } - } - } - - DateTimeAxis { - id: axisX - min: chartController ? chartController.rangeXMin : new Date() - max: chartController ? chartController.rangeXMax : new Date() - visible: chartController !== null - format: "
mm:ss.zzz" - tickCount: 5 - gridVisible: true - labelsFont.family: ScreenTools.fixedFontFamily - labelsFont.pointSize: ScreenTools.smallFontPointSize - labelsColor: qgcPal.text - } - - ValueAxis { - id: axisY - min: chartController ? chartController.rangeYMin : 0 - max: chartController ? chartController.rangeYMax : 0 - visible: chartController !== null - lineVisible: false - labelsFont.family: ScreenTools.fixedFontFamily - labelsFont.pointSize: ScreenTools.smallFontPointSize - labelsColor: qgcPal.text - } - - Row { - id: chartHeader - anchors.left: parent.left - anchors.leftMargin: ScreenTools.defaultFontPixelWidth * 4 - anchors.right: parent.right - anchors.rightMargin: ScreenTools.defaultFontPixelWidth * 4 - anchors.top: parent.top - anchors.topMargin: ScreenTools.defaultFontPixelHeight * 1.5 - spacing: ScreenTools.defaultFontPixelWidth * 2 - visible: chartController !== null - GridLayout { - columns: 2 - columnSpacing: ScreenTools.defaultFontPixelWidth - rowSpacing: ScreenTools.defaultFontPixelHeight * 0.25 - anchors.verticalCenter: parent.verticalCenter - QGCLabel { - text: qsTr("Scale:"); - Layout.alignment: Qt.AlignVCenter - } - QGCComboBox { - Layout.minimumWidth: ScreenTools.defaultFontPixelWidth * 10 - Layout.maximumWidth: ScreenTools.defaultFontPixelWidth * 10 - height: ScreenTools.defaultFontPixelHeight - model: controller.timeScales - currentIndex: chartController ? chartController.rangeXIndex : 0 - onActivated: (index) => { if(chartController) chartController.rangeXIndex = index; } - Layout.alignment: Qt.AlignVCenter - } - QGCLabel { - text: qsTr("Range:"); - Layout.alignment: Qt.AlignVCenter - } - QGCComboBox { - Layout.minimumWidth: ScreenTools.defaultFontPixelWidth * 10 - Layout.maximumWidth: ScreenTools.defaultFontPixelWidth * 10 - height: ScreenTools.defaultFontPixelHeight - model: controller.rangeList - currentIndex: chartController ? chartController.rangeYIndex : 0 - onActivated: (index) => { if(chartController) chartController.rangeYIndex = index; } - Layout.alignment: Qt.AlignVCenter - } - } - ColumnLayout { - anchors.verticalCenter: parent.verticalCenter - Repeater { - model: chartController ? chartController.chartFields : [] - QGCLabel { - text: modelData.label - color: chartView.series(index).color - font.pointSize: ScreenTools.smallFontPointSize - } - } - } - } -} diff --git a/src/QmlControls/MAVLinkMessageButton.qml b/src/QmlControls/MAVLinkMessageButton.qml deleted file mode 100644 index 1d3c7bd635b..00000000000 --- a/src/QmlControls/MAVLinkMessageButton.qml +++ /dev/null @@ -1,63 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2020 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - -import QtQuick -import QtQuick.Controls -import QtQuick.Layouts - -import QGroundControl.Palette -import QGroundControl.ScreenTools - -Button { - id: control - autoExclusive: true - leftPadding: ScreenTools.defaultFontPixelWidth - rightPadding: leftPadding - - property real _compIDWidth: ScreenTools.defaultFontPixelWidth * 3 - property real _hzWidth: ScreenTools.defaultFontPixelWidth * 6 - property real _nameWidth: nameLabel.contentWidth - - background: Rectangle { - anchors.fill: parent - color: checked ? qgcPal.buttonHighlight : qgcPal.button - } - - property double messageHz: 0 - property int compID: 0 - - contentItem: RowLayout { - id: rowLayout - spacing: ScreenTools.defaultFontPixelWidth - - QGCLabel { - text: control.compID - color: checked ? qgcPal.buttonHighlightText : qgcPal.buttonText - verticalAlignment: Text.AlignVCenter - Layout.minimumHeight: ScreenTools.isMobile ? (ScreenTools.defaultFontPixelHeight * 2) : (ScreenTools.defaultFontPixelHeight * 1.5) - Layout.minimumWidth: _compIDWidth - } - QGCLabel { - id: nameLabel - text: control.text - color: checked ? qgcPal.buttonHighlightText : qgcPal.buttonText - Layout.fillWidth: true - Layout.alignment: Qt.AlignVCenter - } - QGCLabel { - color: checked ? qgcPal.buttonHighlightText : qgcPal.buttonText - text: messageHz.toFixed(1) + 'Hz' - horizontalAlignment: Text.AlignRight - Layout.minimumWidth: _hzWidth - Layout.alignment: Qt.AlignVCenter - } - } - - Component.onCompleted: maxButtonWidth = Math.max(maxButtonWidth, _compIDWidth + _hzWidth + _nameWidth + (rowLayout.spacing * 2) + (control.leftPadding * 2)) -} diff --git a/src/QmlControls/QGroundControl/Controls/qmldir b/src/QmlControls/QGroundControl/Controls/qmldir index 816f521baca..a65a4bd922f 100644 --- a/src/QmlControls/QGroundControl/Controls/qmldir +++ b/src/QmlControls/QGroundControl/Controls/qmldir @@ -47,7 +47,6 @@ MainStatusIndicator 1.0 MainStatusIndicator.qml FlightModeMenuIndicator 1.0 FlightModeMenuIndicator.qml MainStatusIndicatorOfflinePage 1.0 MainStatusIndicatorOfflinePage.qml MainWindowSavedState 1.0 MainWindowSavedState.qml -MAVLinkMessageButton 1.0 MAVLinkMessageButton.qml MissionCommandDialog 1.0 MissionCommandDialog.qml MissionItemEditor 1.0 MissionItemEditor.qml MissionItemIndexLabel 1.0 MissionItemIndexLabel.qml @@ -124,4 +123,3 @@ ToolStrip 1.0 ToolStrip.qml ToolStripHoverButton 1.0 ToolStripHoverButton.qml VehicleRotationCal 1.0 VehicleRotationCal.qml VehicleSummaryRow 1.0 VehicleSummaryRow.qml -MAVLinkChart 1.0 MAVLinkChart.qml