Commit d10e89f4 authored by DonLakeFlyer's avatar DonLakeFlyer

parent 92e15136
......@@ -179,6 +179,7 @@ QGCMAVLinkMessage::QGCMAVLinkMessage(QObject *parent, mavlink_message_t* message
QGCMAVLinkMessageField* f = new QGCMAVLinkMessageField(this, msgInfo->fields[i].name, type);
_fields.append(f);
}
update(message);
}
//-----------------------------------------------------------------------------
......@@ -222,10 +223,7 @@ void
QGCMAVLinkMessage::update(mavlink_message_t* message)
{
_count++;
//-- If we are not consuming this message, no need to parse it
if(!_selected && !_fieldSelected) {
return;
}
_message = *message;
const mavlink_message_info_t* msgInfo = mavlink_get_message_info(message);
if (!msgInfo) {
......@@ -454,7 +452,7 @@ QGCMAVLinkMessage::update(mavlink_message_t* message)
}
//-----------------------------------------------------------------------------
QGCMAVLinkVehicle::QGCMAVLinkVehicle(QObject* parent, quint8 id)
QGCMAVLinkSystem::QGCMAVLinkSystem(QObject* parent, quint8 id)
: QObject(parent)
, _id(id)
{
......@@ -462,14 +460,14 @@ QGCMAVLinkVehicle::QGCMAVLinkVehicle(QObject* parent, quint8 id)
}
//-----------------------------------------------------------------------------
QGCMAVLinkVehicle::~QGCMAVLinkVehicle()
QGCMAVLinkSystem::~QGCMAVLinkSystem()
{
_messages.clearAndDeleteContents();
}
//-----------------------------------------------------------------------------
QGCMAVLinkMessage*
QGCMAVLinkVehicle::findMessage(uint32_t id, uint8_t cid)
QGCMAVLinkSystem::findMessage(uint32_t id, uint8_t cid)
{
for(int i = 0; i < _messages.count(); i++) {
QGCMAVLinkMessage* m = qobject_cast<QGCMAVLinkMessage*>(_messages.get(i));
......@@ -484,7 +482,7 @@ QGCMAVLinkVehicle::findMessage(uint32_t id, uint8_t cid)
//-----------------------------------------------------------------------------
int
QGCMAVLinkVehicle::findMessage(QGCMAVLinkMessage* message)
QGCMAVLinkSystem::findMessage(QGCMAVLinkMessage* message)
{
for(int i = 0; i < _messages.count(); i++) {
QGCMAVLinkMessage* m = qobject_cast<QGCMAVLinkMessage*>(_messages.get(i));
......@@ -497,7 +495,7 @@ QGCMAVLinkVehicle::findMessage(QGCMAVLinkMessage* message)
//-----------------------------------------------------------------------------
void
QGCMAVLinkVehicle::_resetSelection()
QGCMAVLinkSystem::_resetSelection()
{
for(int i = 0; i < _messages.count(); i++) {
QGCMAVLinkMessage* m = qobject_cast<QGCMAVLinkMessage*>(_messages.get(i));
......@@ -510,7 +508,7 @@ QGCMAVLinkVehicle::_resetSelection()
//-----------------------------------------------------------------------------
void
QGCMAVLinkVehicle::setSelected(int sel)
QGCMAVLinkSystem::setSelected(int sel)
{
if(sel < _messages.count()) {
_selected = sel;
......@@ -537,7 +535,7 @@ messages_sort(QObject* a, QObject* b)
//-----------------------------------------------------------------------------
void
QGCMAVLinkVehicle::append(QGCMAVLinkMessage* message)
QGCMAVLinkSystem::append(QGCMAVLinkMessage* message)
{
//-- Save selected message
QGCMAVLinkMessage* selectedMsg = nullptr;
......@@ -572,15 +570,15 @@ QGCMAVLinkVehicle::append(QGCMAVLinkMessage* message)
//-----------------------------------------------------------------------------
void
QGCMAVLinkVehicle::_checkCompID(QGCMAVLinkMessage* message)
QGCMAVLinkSystem::_checkCompID(QGCMAVLinkMessage* message)
{
if(_compIDsStr.isEmpty()) {
_compIDsStr << tr("All");
_compIDsStr << tr("Comp All");
}
if(!_compIDs.contains(static_cast<int>(message->cid()))) {
int cid = static_cast<int>(message->cid());
_compIDs.append(cid);
_compIDsStr << QString::number(cid);
_compIDsStr << tr("Comp %1").arg(cid);
emit compIDsChanged();
}
}
......@@ -748,7 +746,7 @@ MAVLinkInspectorController::MAVLinkInspectorController()
MAVLinkInspectorController::~MAVLinkInspectorController()
{
_charts.clearAndDeleteContents();
_vehicles.clearAndDeleteContents();
_systems.clearAndDeleteContents();
}
//----------------------------------------------------------------------------------------
......@@ -780,24 +778,24 @@ void
MAVLinkInspectorController::_setActiveVehicle(Vehicle* vehicle)
{
if(vehicle) {
QGCMAVLinkVehicle* v = _findVehicle(static_cast<uint8_t>(vehicle->id()));
QGCMAVLinkSystem* v = _findVehicle(static_cast<uint8_t>(vehicle->id()));
if(v) {
_activeVehicle = v;
_activeSystem = v;
} else {
_activeVehicle = nullptr;
_activeSystem = nullptr;
}
} else {
_activeVehicle = nullptr;
_activeSystem = nullptr;
}
emit activeVehiclesChanged();
emit activeSystemChanged();
}
//-----------------------------------------------------------------------------
QGCMAVLinkVehicle*
QGCMAVLinkSystem*
MAVLinkInspectorController::_findVehicle(uint8_t id)
{
for(int i = 0; i < _vehicles.count(); i++) {
QGCMAVLinkVehicle* v = qobject_cast<QGCMAVLinkVehicle*>(_vehicles.get(i));
for(int i = 0; i < _systems.count(); i++) {
QGCMAVLinkSystem* v = qobject_cast<QGCMAVLinkSystem*>(_systems.get(i));
if(v) {
if(v->id() == id) {
return v;
......@@ -811,8 +809,8 @@ MAVLinkInspectorController::_findVehicle(uint8_t id)
void
MAVLinkInspectorController::_refreshFrequency()
{
for(int i = 0; i < _vehicles.count(); i++) {
QGCMAVLinkVehicle* v = qobject_cast<QGCMAVLinkVehicle*>(_vehicles.get(i));
for(int i = 0; i < _systems.count(); i++) {
QGCMAVLinkSystem* v = qobject_cast<QGCMAVLinkSystem*>(_systems.get(i));
if(v) {
for(int i = 0; i < v->messages()->count(); i++) {
QGCMAVLinkMessage* m = qobject_cast<QGCMAVLinkMessage*>(v->messages()->get(i));
......@@ -828,29 +826,29 @@ MAVLinkInspectorController::_refreshFrequency()
void
MAVLinkInspectorController::_vehicleAdded(Vehicle* vehicle)
{
QGCMAVLinkVehicle* v = _findVehicle(static_cast<uint8_t>(vehicle->id()));
QGCMAVLinkSystem* v = _findVehicle(static_cast<uint8_t>(vehicle->id()));
if(v) {
v->messages()->clearAndDeleteContents();
emit v->messagesChanged();
} else {
v = new QGCMAVLinkVehicle(this, static_cast<uint8_t>(vehicle->id()));
_vehicles.append(v);
_vehicleNames.append(tr("Vehicle %1").arg(vehicle->id()));
v = new QGCMAVLinkSystem(this, static_cast<uint8_t>(vehicle->id()));
_systems.append(v);
_systemNames.append(tr("System %1").arg(vehicle->id()));
}
emit vehiclesChanged();
emit systemsChanged();
}
//-----------------------------------------------------------------------------
void
MAVLinkInspectorController::_vehicleRemoved(Vehicle* vehicle)
{
QGCMAVLinkVehicle* v = _findVehicle(static_cast<uint8_t>(vehicle->id()));
QGCMAVLinkSystem* v = _findVehicle(static_cast<uint8_t>(vehicle->id()));
if(v) {
v->deleteLater();
_vehicles.removeOne(v);
QString vs = tr("Vehicle %1").arg(vehicle->id());
_vehicleNames.removeOne(vs);
emit vehiclesChanged();
_systems.removeOne(v);
QString vs = tr("System %1").arg(vehicle->id());
_systemNames.removeOne(vs);
emit systemsChanged();
}
}
......@@ -859,15 +857,15 @@ void
MAVLinkInspectorController::_receiveMessage(LinkInterface*, mavlink_message_t message)
{
QGCMAVLinkMessage* m = nullptr;
QGCMAVLinkVehicle* v = _findVehicle(message.sysid);
QGCMAVLinkSystem* v = _findVehicle(message.sysid);
if(!v) {
v = new QGCMAVLinkVehicle(this, message.sysid);
_vehicles.append(v);
_vehicleNames.append(tr("Vehicle %1").arg(message.sysid));
emit vehiclesChanged();
if(!_activeVehicle) {
_activeVehicle = v;
emit activeVehiclesChanged();
v = new QGCMAVLinkSystem(this, message.sysid);
_systems.append(v);
_systemNames.append(tr("System %1").arg(message.sysid));
emit systemsChanged();
if(!_activeSystem) {
_activeSystem = v;
emit activeSystemChanged();
}
} else {
m = v->findMessage(message.msgid, message.compid);
......@@ -924,3 +922,12 @@ MAVLinkInspectorController::Range_st::Range_st(QObject* parent, const QString& l
{
}
void MAVLinkInspectorController::setActiveSystem(int systemId)
{
QGCMAVLinkSystem* v = _findVehicle(systemId);
if (v != _activeSystem) {
_activeSystem = v;
emit activeSystemChanged();
}
}
......@@ -27,7 +27,7 @@ Q_DECLARE_LOGGING_CATEGORY(MAVLinkInspectorLog)
QT_CHARTS_USE_NAMESPACE
class QGCMAVLinkMessage;
class QGCMAVLinkVehicle;
class QGCMAVLinkSystem;
class MAVLinkChartController;
class MAVLinkInspectorController;
......@@ -137,7 +137,7 @@ private:
//-----------------------------------------------------------------------------
/// Vehicle MAVLink message belongs to
class QGCMAVLinkVehicle : public QObject {
class QGCMAVLinkSystem : public QObject {
Q_OBJECT
public:
Q_PROPERTY(quint8 id READ id CONSTANT)
......@@ -147,8 +147,8 @@ public:
Q_PROPERTY(int selected READ selected WRITE setSelected NOTIFY selectedChanged)
QGCMAVLinkVehicle (QObject* parent, quint8 id);
~QGCMAVLinkVehicle ();
QGCMAVLinkSystem (QObject* parent, quint8 id);
~QGCMAVLinkSystem ();
quint8 id () { return _id; }
QmlObjectListModel* messages () { return &_messages; }
......@@ -248,22 +248,23 @@ public:
MAVLinkInspectorController();
~MAVLinkInspectorController();
Q_PROPERTY(QStringList vehicleNames READ vehicleNames NOTIFY vehiclesChanged)
Q_PROPERTY(QmlObjectListModel* vehicles READ vehicles NOTIFY vehiclesChanged)
Q_PROPERTY(QmlObjectListModel* charts READ charts NOTIFY chartsChanged)
Q_PROPERTY(QGCMAVLinkVehicle* activeVehicle READ activeVehicle NOTIFY activeVehiclesChanged)
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_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_INVOKABLE MAVLinkChartController* createChart ();
Q_INVOKABLE void deleteChart (MAVLinkChartController* chart);
Q_INVOKABLE void setActiveSystem (int systemId);
QmlObjectListModel* vehicles () { return &_vehicles; }
QmlObjectListModel* charts () { return &_charts; }
QGCMAVLinkVehicle* activeVehicle () { return _activeVehicle; }
QStringList vehicleNames () { return _vehicleNames; }
QStringList timeScales ();
QStringList rangeList ();
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 {
public:
......@@ -283,33 +284,33 @@ public:
const QList<Range_st*>& rangeSt () { return _rangeSt; }
signals:
void vehiclesChanged ();
void chartsChanged ();
void activeVehiclesChanged ();
void timeScalesChanged ();
void rangeListChanged ();
void systemsChanged ();
void chartsChanged ();
void activeSystemChanged();
void timeScalesChanged ();
void rangeListChanged ();
private slots:
void _receiveMessage (LinkInterface* link, mavlink_message_t message);
void _vehicleAdded (Vehicle* vehicle);
void _vehicleRemoved (Vehicle* vehicle);
void _setActiveVehicle (Vehicle* vehicle);
void _refreshFrequency ();
void _receiveMessage (LinkInterface* link, mavlink_message_t message);
void _vehicleAdded (Vehicle* vehicle);
void _vehicleRemoved (Vehicle* vehicle);
void _setActiveVehicle (Vehicle* vehicle);
void _refreshFrequency ();
private:
QGCMAVLinkVehicle* _findVehicle (uint8_t id);
QGCMAVLinkSystem* _findVehicle (uint8_t id);
private:
int _selectedSystemID = 0; ///< Currently selected system
int _selectedComponentID = 0; ///< Currently selected component
int _selectedSystemID = 0; ///< Currently selected system
int _selectedComponentID = 0; ///< Currently selected component
QStringList _timeScales;
QStringList _rangeList;
QGCMAVLinkVehicle* _activeVehicle = nullptr;
QGCMAVLinkSystem* _activeSystem = nullptr;
QTimer _updateFrequencyTimer;
QStringList _vehicleNames;
QmlObjectListModel _vehicles; ///< List of QGCMAVLinkVehicle
QmlObjectListModel _charts; ///< List of MAVLinkCharts
QStringList _systemNames;
QmlObjectListModel _systems; ///< List of QGCMAVLinkSystem
QmlObjectListModel _charts; ///< List of MAVLinkCharts
QList<TimeScale_st*>_timeScaleSt;
QList<Range_st*> _rangeSt;
......
......@@ -24,8 +24,8 @@ AnalyzePage {
headerComponent: headerComponent
pageComponent: pageComponent
property var curVehicle: controller ? controller.activeVehicle : null
property var curMessage: curVehicle && curVehicle.messages.count ? curVehicle.messages.get(curVehicle.selected) : null
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
......@@ -45,21 +45,39 @@ AnalyzePage {
}
RowLayout {
Layout.alignment: Qt.AlignRight
visible: curVehicle ? curVehicle.compIDsStr.length > 2 : false
QGCLabel {
text: qsTr("Component ID:")
visible: curSystem ? controller.systemNames.length > 1 || curSystem.compIDsStr.length > 2 : false
QGCComboBox {
id: systemCombo
model: controller.systemNames
sizeToContents: true
visible: controller.systemNames.length > 1
onActivated: controller.setActiveSystem(controller.systems.get(index).id);
Connections {
target: controller
onActiveSystemChanged: {
for (var systemIndex=0; systemIndex<controller.systems.count; systemIndex++) {
if (controller.systems.get(systemIndex) == curSystem) {
systemCombo.currentIndex = systemIndex
curCompID = 0
cidCombo.currentIndex = 0
break
}
}
}
}
}
QGCComboBox {
id: cidCombo
model: curVehicle ? curVehicle.compIDsStr : []
Layout.minimumWidth: ScreenTools.defaultFontPixelWidth * 10
currentIndex: 0
model: curSystem ? curSystem.compIDsStr : []
sizeToContents: true
visible: curSystem ? curSystem.compIDsStr.length > 2 : false
onActivated: {
if(curVehicle && curVehicle.compIDsStr.length > 1) {
if(curSystem && curSystem.compIDsStr.length > 1) {
if(index < 1)
curCompID = 0
else
curCompID = curVehicle.compIDs[index - 1]
curCompID = curSystem.compIDs[index - 1]
}
}
}
......@@ -87,15 +105,15 @@ AnalyzePage {
anchors.right: parent.right
spacing: ScreenTools.defaultFontPixelHeight * 0.25
Repeater {
model: curVehicle ? curVehicle.messages : []
model: curSystem ? curSystem.messages : []
delegate: MAVLinkMessageButton {
text: object.name + (object.fieldSelected ? " *" : "")
compID: object.cid
checked: curVehicle ? (curVehicle.selected === index) : false
checked: curSystem ? (curSystem.selected === index) : false
messageHz: object.messageHz
visible: curCompID === 0 || curCompID === compID
onClicked: {
curVehicle.selected = index
curSystem.selected = index
}
Layout.fillWidth: true
}
......
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