Commit 47e80a09 authored by Gus Grubba's avatar Gus Grubba

Completed MAVLink Inspector

parent af2a3970
......@@ -84,6 +84,7 @@
<file alias="LogDownloadIcon">src/AnalyzeView/LogDownloadIcon.svg</file>
<file alias="LowBattery.svg">src/AutoPilotPlugins/PX4/Images/LowBattery.svg</file>
<file alias="LowBatteryLight.svg">src/AutoPilotPlugins/PX4/Images/LowBatteryLight.svg</file>
<file alias="MAVLinkInspector">src/AnalyzeView/MAVLinkInspector.svg</file>
<file alias="MotorComponentIcon.svg">src/AutoPilotPlugins/Common/Images/MotorComponentIcon.svg</file>
<file alias="PowerComponentBattery_01cell.svg">src/AutoPilotPlugins/PX4/Images/PowerComponentBattery_01cell.svg</file>
<file alias="PowerComponentBattery_02cell.svg">src/AutoPilotPlugins/PX4/Images/PowerComponentBattery_02cell.svg</file>
......
......@@ -77,6 +77,7 @@
<file alias="QGroundControl/Controls/GeoFenceMapVisuals.qml">src/PlanView/GeoFenceMapVisuals.qml</file>
<file alias="QGroundControl/Controls/IndicatorButton.qml">src/QmlControls/IndicatorButton.qml</file>
<file alias="QGroundControl/Controls/JoystickThumbPad.qml">src/QmlControls/JoystickThumbPad.qml</file>
<file alias="QGroundControl/Controls/MAVLinkMessageButton.qml">src/QmlControls/MAVLinkMessageButton.qml</file>
<file alias="QGroundControl/Controls/MissionCommandDialog.qml">src/QmlControls/MissionCommandDialog.qml</file>
<file alias="QGroundControl/Controls/MissionItemEditor.qml">src/PlanView/MissionItemEditor.qml</file>
<file alias="QGroundControl/Controls/MissionItemIndexLabel.qml">src/QmlControls/MissionItemIndexLabel.qml</file>
......
......@@ -108,7 +108,7 @@ Rectangle {
pageSource: "MavlinkConsolePage.qml"
}
ListElement {
buttonImage: "/qmlimages/MavlinkConsoleIcon"
buttonImage: "/qmlimages/MAVLinkInspector"
buttonText: qsTr("MAVLink Inspector")
pageSource: "MAVLinkInspectorPage.qml"
}
......
<?xml version="1.0" encoding="utf-8"?>
<!-- Generator: Adobe Illustrator 23.0.3, SVG Export Plug-In . SVG Version: 6.00 Build 0) -->
<svg version="1.1" id="Layer_1" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" x="0px" y="0px"
viewBox="0 0 72 72" style="enable-background:new 0 0 72 72;" xml:space="preserve">
<style type="text/css">
.st0{fill:#FFFFFF;}
.st1{fill:none;stroke:#FFFFFF;stroke-width:3;stroke-miterlimit:10;}
.st2{fill:none;stroke:#FFFFFF;stroke-width:4;stroke-linecap:round;stroke-miterlimit:10;}
</style>
<path class="st0" d="M15.187,36.541c-1.96,0-3.555,1.563-3.555,3.477c0,1.92,1.595,3.483,3.555,3.483s3.555-1.563,3.555-3.483
C18.742,38.103,17.147,36.541,15.187,36.541z"/>
<path class="st0" d="M15.187,26.097c-1.96,0-3.555,1.563-3.555,3.477c0,1.92,1.595,3.483,3.555,3.483s3.555-1.563,3.555-3.483
C18.742,27.66,17.147,26.097,15.187,26.097z"/>
<path class="st0" d="M15.187,15.66c-1.96,0-3.555,1.563-3.555,3.477c0,1.92,1.595,3.483,3.555,3.483s3.555-1.563,3.555-3.483
S17.147,15.66,15.187,15.66z"/>
<path class="st0" d="M24.352,16.294h24.312v4.329H24.352C24.352,20.623,24.352,16.294,24.352,16.294z"/>
<path class="st0" d="M24.352,27.413h24.312v4.335H24.352C24.352,31.748,24.352,27.413,24.352,27.413z"/>
<path class="st0" d="M24.352,38.532h9.72v4.329h-9.72C24.352,42.861,24.352,38.532,24.352,38.532z"/>
<path class="st0" d="M34.135,55.113H9.193c-2.614,0-4.741-2.085-4.741-4.646V8.694c0-2.561,2.128-4.646,4.741-4.646h41.449
c2.614,0,4.741,2.085,4.741,4.646v25.454c1.173,0.448,2.289,1.011,3.315,1.702V8.694C58.704,4.336,55.089,0.8,50.642,0.8H9.193
c-4.442,0-8.056,3.536-8.056,7.894v41.773c0,4.358,3.608,7.894,8.056,7.894h26.554C35.093,57.35,34.556,56.261,34.135,55.113z"/>
<circle class="st1" cx="48.325" cy="48.098" r="11.557"/>
<line class="st2" x1="56.87" y1="57.207" x2="68.863" y2="69.2"/>
</svg>
......@@ -27,7 +27,6 @@ QGCMAVLinkMessage::QGCMAVLinkMessage(QObject* parent, mavlink_message_t* message
: QObject(parent)
{
_message = *message;
_messageHz = QGC::groundTimeMilliseconds();
const mavlink_message_info_t* msgInfo = mavlink_get_message_info(message);
if (!msgInfo) {
qWarning() << QStringLiteral("QGCMAVLinkMessage NULL msgInfo msgid(%1)").arg(message->msgid);
......@@ -55,13 +54,22 @@ QGCMAVLinkMessage::QGCMAVLinkMessage(QObject* parent, mavlink_message_t* message
}
}
//-----------------------------------------------------------------------------
void
QGCMAVLinkMessage::updateFreq()
{
quint64 msgCount = _count - _lastCount;
_messageHz = (0.2 * _messageHz) + (0.8 * msgCount);
_lastCount = _count;
emit freqChanged();
}
//-----------------------------------------------------------------------------
void
QGCMAVLinkMessage::update(mavlink_message_t* message)
{
_message = *message;
_count++;
_messageHz = QGC::groundTimeMilliseconds();
const mavlink_message_info_t* msgInfo = mavlink_get_message_info(message);
if (!msgInfo) {
qWarning() << QStringLiteral("QGCMAVLinkMessage::update NULL msgInfo msgid(%1)").arg(message->msgid);
......@@ -167,7 +175,13 @@ QGCMAVLinkMessage::update(mavlink_message_t* message)
} else {
// Single value
uint32_t n = *(reinterpret_cast<uint32_t*>(m + msgInfo->fields[i].wire_offset));
f->updateValue(QString::number(n));
//-- Special case
if(_message.msgid == MAVLINK_MSG_ID_SYSTEM_TIME) {
QDateTime d = QDateTime::fromMSecsSinceEpoch(static_cast<qint64>(n),Qt::UTC,0);
f->updateValue(d.toString("HH:mm:ss"));
} else {
f->updateValue(QString::number(n));
}
}
break;
case MAVLINK_TYPE_INT32_T:
......@@ -231,7 +245,13 @@ QGCMAVLinkMessage::update(mavlink_message_t* message)
} else {
// Single value
uint64_t n = *(reinterpret_cast<uint64_t*>(m + msgInfo->fields[i].wire_offset));
f->updateValue(QString::number(n));
//-- 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"));
} else {
f->updateValue(QString::number(n));
}
}
break;
case MAVLINK_TYPE_INT64_T:
......@@ -266,12 +286,12 @@ QGCMAVLinkVehicle::QGCMAVLinkVehicle(QObject* parent, quint8 id)
//-----------------------------------------------------------------------------
QGCMAVLinkMessage*
QGCMAVLinkVehicle::findMessage(uint32_t id)
QGCMAVLinkVehicle::findMessage(uint32_t id, uint8_t cid)
{
for(int i = 0; i < _messages.count(); i++) {
QGCMAVLinkMessage* m = qobject_cast<QGCMAVLinkMessage*>(_messages.get(i));
if(m) {
if(m->id() == id) {
if(m->id() == id && m->cid() == cid) {
return m;
}
}
......@@ -279,11 +299,32 @@ QGCMAVLinkVehicle::findMessage(uint32_t id)
return nullptr;
}
//-----------------------------------------------------------------------------
static bool
messages_sort(QObject* a, QObject* b)
{
QGCMAVLinkMessage* aa = qobject_cast<QGCMAVLinkMessage*>(a);
QGCMAVLinkMessage* bb = qobject_cast<QGCMAVLinkMessage*>(b);
if(!aa || !bb) return false;
if(aa->id() == bb->id()) return aa->cid() < bb->cid();
return aa->id() < bb->id();
}
//-----------------------------------------------------------------------------
void
QGCMAVLinkVehicle::append(QGCMAVLinkMessage* message)
{
_messages.append(message);
//-- Sort messages by id and then cid
if(_messages.count() > 0) {
std::sort(_messages.objectList()->begin(), _messages.objectList()->end(), messages_sort);
for(int i = 0; i < _messages.count(); i++) {
QGCMAVLinkMessage* m = qobject_cast<QGCMAVLinkMessage*>(_messages.get(i));
if(m) {
emit m->indexChanged();
}
}
}
emit messagesChanged();
}
......@@ -295,6 +336,10 @@ MAVLinkInspectorController::MAVLinkInspectorController()
connect(multiVehicleManager, &MultiVehicleManager::vehicleRemoved, this, &MAVLinkInspectorController::_vehicleRemoved);
MAVLinkProtocol* mavlinkProtocol = qgcApp()->toolbox()->mavlinkProtocol();
connect(mavlinkProtocol, &MAVLinkProtocol::messageReceived, this, &MAVLinkInspectorController::_receiveMessage);
connect(&_updateTimer, &QTimer::timeout, this, &MAVLinkInspectorController::_refreshFrequency);
_updateTimer.start(1000);
MultiVehicleManager *manager = qgcApp()->toolbox()->multiVehicleManager();
connect(manager, &MultiVehicleManager::activeVehicleChanged, this, &MAVLinkInspectorController::_setActiveVehicle);
}
//-----------------------------------------------------------------------------
......@@ -303,6 +348,24 @@ MAVLinkInspectorController::~MAVLinkInspectorController()
_reset();
}
//----------------------------------------------------------------------------------------
void
MAVLinkInspectorController::_setActiveVehicle(Vehicle* vehicle)
{
if(vehicle) {
QGCMAVLinkVehicle* v = _findVehicle(static_cast<uint8_t>(vehicle->id()));
if(v) {
_activeVehicle = v;
} else {
_activeVehicle = nullptr;
}
} else {
_activeVehicle = nullptr;
}
emit activeVehiclesChanged();
}
//-----------------------------------------------------------------------------
QGCMAVLinkVehicle*
MAVLinkInspectorController::_findVehicle(uint8_t id)
......@@ -318,6 +381,23 @@ MAVLinkInspectorController::_findVehicle(uint8_t id)
return nullptr;
}
//-----------------------------------------------------------------------------
void
MAVLinkInspectorController::_refreshFrequency()
{
for(int i = 0; i < _vehicles.count(); i++) {
QGCMAVLinkVehicle* v = qobject_cast<QGCMAVLinkVehicle*>(_vehicles.get(i));
if(v) {
for(int i = 0; i < v->messages()->count(); i++) {
QGCMAVLinkMessage* m = qobject_cast<QGCMAVLinkMessage*>(v->messages()->get(i));
if(m) {
m->updateFreq();
}
}
}
}
}
//-----------------------------------------------------------------------------
void
MAVLinkInspectorController::_vehicleAdded(Vehicle* vehicle)
......@@ -360,8 +440,12 @@ MAVLinkInspectorController::_receiveMessage(LinkInterface* link, mavlink_message
_vehicles.append(v);
_vehicleNames.append(QString(tr("Vehicle %1")).arg(message.sysid));
emit vehiclesChanged();
if(!_activeVehicle) {
_activeVehicle = v;
emit activeVehiclesChanged();
}
} else {
m = v->findMessage(message.msgid);
m = v->findMessage(message.msgid, message.compid);
}
if(!m) {
m = new QGCMAVLinkMessage(this, &message);
......
......@@ -47,51 +47,58 @@ private:
//-----------------------------------------------------------------------------
class QGCMAVLinkMessage : public QObject {
Q_OBJECT
Q_PROPERTY(quint32 id READ id CONSTANT)
Q_PROPERTY(QString name READ name CONSTANT)
Q_PROPERTY(quint64 messageHz READ messageHz NOTIFY messageChanged)
Q_PROPERTY(quint32 id READ id NOTIFY indexChanged)
Q_PROPERTY(quint32 cid READ cid NOTIFY indexChanged)
Q_PROPERTY(QString name READ name NOTIFY indexChanged)
Q_PROPERTY(qreal messageHz READ messageHz NOTIFY freqChanged)
Q_PROPERTY(quint64 count READ count NOTIFY messageChanged)
Q_PROPERTY(QmlObjectListModel* fields READ fields CONSTANT)
Q_PROPERTY(QmlObjectListModel* fields READ fields NOTIFY indexChanged)
public:
QGCMAVLinkMessage(QObject* parent, mavlink_message_t* message);
quint32 id () { return _message.msgid; }
quint32 id () { return _message.msgid; }
quint8 cid () { return _message.compid; }
QString name () { return _name; }
quint64 messageHz () { return _messageHz; }
qreal messageHz () { return _messageHz; }
quint64 count () { return _count; }
quint64 lastCount () { return _lastCount; }
QmlObjectListModel* fields () { return &_fields; }
void update (mavlink_message_t* message);
void updateFreq ();
signals:
void messageChanged ();
void freqChanged ();
void indexChanged ();
private:
QmlObjectListModel _fields;
QString _name;
uint64_t _messageHz = 0;
qreal _messageHz = 0.0;
uint64_t _count = 0;
uint64_t _lastCount = 0;
mavlink_message_t _message; //-- List of QGCMAVLinkMessageField
};
//-----------------------------------------------------------------------------
class QGCMAVLinkVehicle : public QObject {
Q_OBJECT
Q_PROPERTY(quint8 id READ id CONSTANT)
Q_PROPERTY(QmlObjectListModel* messages READ messages NOTIFY messagesChanged)
Q_PROPERTY(quint8 id READ id CONSTANT)
Q_PROPERTY(QmlObjectListModel* messages READ messages NOTIFY messagesChanged)
public:
QGCMAVLinkVehicle(QObject* parent, quint8 id);
quint8 id () { return _id; }
QmlObjectListModel* messages () { return &_messages; }
quint8 id () { return _id; }
QmlObjectListModel* messages () { return &_messages; }
QGCMAVLinkMessage* findMessage (uint32_t id);
void append (QGCMAVLinkMessage* message);
QGCMAVLinkMessage* findMessage (uint32_t id, uint8_t cid);
void append (QGCMAVLinkMessage* message);
signals:
void messagesChanged ();
void messagesChanged ();
private:
quint8 _id;
......@@ -108,17 +115,22 @@ public:
Q_PROPERTY(QStringList vehicleNames READ vehicleNames NOTIFY vehiclesChanged)
Q_PROPERTY(QmlObjectListModel* vehicles READ vehicles NOTIFY vehiclesChanged)
Q_PROPERTY(QGCMAVLinkVehicle* activeVehicle READ activeVehicle NOTIFY activeVehiclesChanged)
QmlObjectListModel* vehicles () { return &_vehicles; }
QStringList vehicleNames () { return _vehicleNames; }
QmlObjectListModel* vehicles () { return &_vehicles; }
QGCMAVLinkVehicle* activeVehicle () { return _activeVehicle; }
QStringList vehicleNames () { return _vehicleNames; }
signals:
void vehiclesChanged ();
void activeVehiclesChanged ();
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:
void _reset ();
......@@ -129,6 +141,8 @@ private:
int _selectedSystemID = 0; ///< Currently selected system
int _selectedComponentID = 0; ///< Currently selected component
QGCMAVLinkVehicle* _activeVehicle = nullptr;
QTimer _updateTimer;
QStringList _vehicleNames;
QmlObjectListModel _vehicles; //-- List of QGCMAVLinkVehicle
};
......@@ -22,10 +22,7 @@ Item {
anchors.fill: parent
anchors.margins: ScreenTools.defaultFontPixelWidth
readonly property real _butttonWidth: ScreenTools.defaultFontPixelWidth * 30
property int curVehicleIndex: 0
property var curVehicle: controller.vehicles.count > 0 ? controller.vehicles.get(curVehicleIndex) : null
property var curVehicle: controller ? controller.activeVehicle : null
property int curMessageIndex: 0
property var curMessage: curVehicle && curVehicle.messages.count ? curVehicle.messages.get(curMessageIndex) : null
......@@ -38,39 +35,21 @@ Item {
}
//-- Header
ColumnLayout {
id: header
width: parent.width
spacing: ScreenTools.defaultFontPixelHeight
QGCLabel {
text: qsTr("Analyze real time MAVLink messages.")
}
RowLayout {
Layout.fillWidth: true
spacing: ScreenTools.defaultFontPixelWidth
QGCLabel {
text: qsTr("Vehicle:")
}
QGCComboBox {
id: vehicleSelector
model: controller.vehicleNames
enabled: controller.vehicles.count > 0
onActivated: curVehicleIndex = index
Layout.minimumWidth: ScreenTools.defaultFontPixelWidth * 16
}
}
Item {
height: ScreenTools.defaultFontPixelHeight
width: 1
}
QGCLabel {
id: header
text: qsTr("Inspect real time MAVLink messages.")
anchors.top: parent.top
anchors.left: parent.left
}
//-- Messages
//-- Messages (Buttons)
QGCFlickable {
id: buttonGrid
anchors.top: header.bottom
anchors.topMargin: ScreenTools.defaultFontPixelHeight
anchors.bottom: parent.bottom
anchors.left: parent.left
width: ScreenTools.defaultFontPixelWidth * 32
width: buttonCol.width
contentWidth: buttonCol.width
contentHeight: buttonCol.height
ColumnLayout {
......@@ -78,10 +57,12 @@ Item {
spacing: ScreenTools.defaultFontPixelHeight * 0.25
Repeater {
model: curVehicle ? curVehicle.messages : []
delegate: QGCButton {
text: object.name
onClicked: curMessageIndex = index
Layout.minimumWidth: _butttonWidth
delegate: MAVLinkMessageButton {
text: object.name
checked: curMessageIndex === index
messageHz: object.messageHz
onClicked: curMessageIndex = index
Layout.minimumWidth: ScreenTools.defaultFontPixelWidth * 36
}
}
}
......@@ -89,16 +70,17 @@ Item {
//-- Message Data
QGCFlickable {
id: messageGrid
anchors.top: header.bottom
visible: curMessage !== null
anchors.top: buttonGrid.top
anchors.bottom: parent.bottom
anchors.left: buttonGrid.right
anchors.leftMargin: ScreenTools.defaultFontPixelWidth * 2
anchors.right: parent.right
contentWidth: messageCol.width
contentHeight: messageCol.height
Column {
ColumnLayout {
id: messageCol
spacing: ScreenTools.defaultFontPixelHeight
spacing: ScreenTools.defaultFontPixelHeight * 0.25
GridLayout {
columns: 2
columnSpacing: ScreenTools.defaultFontPixelWidth
......@@ -109,21 +91,31 @@ Item {
}
QGCLabel {
color: qgcPal.buttonHighlight
text: curMessage ? curMessage.name : ""
text: curMessage ? curMessage.name + ' (' + curMessage.id + ') ' + curMessage.messageHz.toFixed(1) + 'Hz' : ""
}
QGCLabel {
text: qsTr("Count:")
text: qsTr("Component:")
}
QGCLabel {
text: curMessage ? curMessage.count : ""
text: curMessage ? curMessage.cid : ""
}
QGCLabel {
text: qsTr("Frequency:")
text: qsTr("Count:")
}
QGCLabel {
text: curMessage ? curMessage.messageHz + 'Hz' : ""
text: curMessage ? curMessage.count : ""
}
}
Item { height: ScreenTools.defaultFontPixelHeight; width: 1 }
QGCLabel {
text: qsTr("Message Fields:")
}
Rectangle {
Layout.fillWidth: true
height: 1
color: qgcPal.text
}
Item { height: ScreenTools.defaultFontPixelHeight * 0.25; width: 1 }
GridLayout {
columns: 3
columnSpacing: ScreenTools.defaultFontPixelWidth
......
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
import QtQuick 2.11
import QtQuick.Controls 2.4
import QtQuick.Layouts 1.11
import QGroundControl.Palette 1.0
import QGroundControl.ScreenTools 1.0
Button {
id: control
height: ScreenTools.defaultFontPixelHeight * 2
autoExclusive: true
background: Rectangle {
anchors.fill: parent
color: checked ? qgcPal.buttonHighlight : qgcPal.button
}
property double messageHz: 0
contentItem: RowLayout {
QGCLabel {
text: control.text
color: checked ? qgcPal.buttonHighlightText : qgcPal.buttonText
Layout.minimumWidth: ScreenTools.defaultFontPixelWidth * 26
}
QGCLabel {
color: checked ? qgcPal.buttonHighlightText : qgcPal.buttonText
text: messageHz.toFixed(1) + 'Hz'
Layout.alignment: Qt.AlignRight
}
}
}
......@@ -20,6 +20,7 @@ GeoFenceMapVisuals 1.0 GeoFenceMapVisuals.qml
HackFileDialog 1.0 HackFileDialog.qml
IndicatorButton 1.0 IndicatorButton.qml
JoystickThumbPad 1.0 JoystickThumbPad.qml
MAVLinkMessageButton 1.0 MAVLinkMessageButton.qml
MissionCommandDialog 1.0 MissionCommandDialog.qml
MissionItemEditor 1.0 MissionItemEditor.qml
MissionItemIndexLabel 1.0 MissionItemIndexLabel.qml
......
......@@ -24,8 +24,6 @@ import QGroundControl.FlightMap 1.0
/// Native QML top level window
ApplicationWindow {
id: mainWindow
width: ScreenTools.isMobile ? Screen.width : Math.min(250 * Screen.pixelDensity, Screen.width)
height: ScreenTools.isMobile ? Screen.height : Math.min(150 * Screen.pixelDensity, Screen.height)
minimumWidth: ScreenTools.isMobile ? Screen.width : Math.min(215 * Screen.pixelDensity, Screen.width)
minimumHeight: ScreenTools.isMobile ? Screen.height : Math.min(120 * Screen.pixelDensity, Screen.height)
visible: true
......@@ -33,6 +31,9 @@ ApplicationWindow {
Component.onCompleted: {
if(ScreenTools.isMobile) {
mainWindow.showFullScreen()
} else {
width = ScreenTools.isMobile ? Screen.width : Math.min(250 * Screen.pixelDensity, Screen.width)
height = ScreenTools.isMobile ? Screen.height : Math.min(150 * Screen.pixelDensity, Screen.height)
}
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment