Commit e21e22cf authored by Lorenz Meier's avatar Lorenz Meier

Merge pull request #636 from mavlink/stream_rate_config

Added support for stream rate config to MAVLink inspector. Allows to con...
parents a48cbdec f991dd2e
......@@ -12,6 +12,7 @@ const unsigned int QGCMAVLinkInspector::updateInterval = 1000U;
QGCMAVLinkInspector::QGCMAVLinkInspector(MAVLinkProtocol* protocol, QWidget *parent) :
QWidget(parent),
_protocol(protocol),
selectedSystemID(0),
selectedComponentID(0),
ui(new Ui::QGCMAVLinkInspector)
......@@ -36,13 +37,23 @@ QGCMAVLinkInspector::QGCMAVLinkInspector(MAVLinkProtocol* protocol, QWidget *par
header << tr("Type");
ui->treeWidget->setHeaderLabels(header);
// Set up the column headers for the rate listing
QStringList rateHeader;
rateHeader << tr("Name");
rateHeader << tr("#ID");
rateHeader << tr("Rate");
ui->rateTreeWidget->setHeaderLabels(rateHeader);
connect(ui->rateTreeWidget, SIGNAL(itemChanged(QTreeWidgetItem*,int)),
this, SLOT(rateTreeItemChanged(QTreeWidgetItem*,int)));
ui->rateTreeWidget->hide();
// Connect the UI
connect(ui->systemComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(selectDropDownMenuSystem(int)));
connect(ui->componentComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(selectDropDownMenuComponent(int)));
connect(ui->clearButton, SIGNAL(clicked()), this, SLOT(clearView()));
// Connect external connections
connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addSystem(UASInterface*)));
// connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addSystem(UASInterface*)));
connect(protocol, SIGNAL(messageReceived(LinkInterface*,mavlink_message_t)), this, SLOT(receiveMessage(LinkInterface*,mavlink_message_t)));
// Attach the UI's refresh rate to a timer.
......@@ -59,29 +70,44 @@ void QGCMAVLinkInspector::selectDropDownMenuSystem(int dropdownid)
{
selectedSystemID = ui->systemComboBox->itemData(dropdownid).toInt();
rebuildComponentList();
if (selectedSystemID != 0 && selectedComponentID != 0) {
ui->rateTreeWidget->show();
} else {
ui->rateTreeWidget->hide();
}
}
void QGCMAVLinkInspector::selectDropDownMenuComponent(int dropdownid)
{
selectedComponentID = ui->componentComboBox->itemData(dropdownid).toInt();
if (selectedSystemID != 0 && selectedComponentID != 0) {
ui->rateTreeWidget->show();
} else {
ui->rateTreeWidget->hide();
}
}
void QGCMAVLinkInspector::rebuildComponentList()
{
ui->componentComboBox->clear();
components.clear();
// Fill
UASInterface* uas = UASManager::instance()->getUASForId(selectedSystemID);
if (uas)
{
QMap<int, QString> components = uas->getComponents();
ui->componentComboBox->addItem(tr("All"), 0);
foreach (int id, components.keys())
{
QString name = components.value(id);
ui->componentComboBox->addItem(name, id);
}
}
// // Fill
// UASInterface* uas = UASManager::instance()->getUASForId(selectedSystemID);
// if (uas)
// {
// QMap<int, QString> components = uas->getComponents();
// foreach (int id, components.keys())
// {
// QString name = components.value(id);
// ui->componentComboBox->addItem(name, id);
// }
// }
}
void QGCMAVLinkInspector::addComponent(int uas, int component, const QString& name)
......@@ -100,10 +126,12 @@ void QGCMAVLinkInspector::addComponent(int uas, int component, const QString& na
void QGCMAVLinkInspector::clearView()
{
memset(receivedMessages, 0xFF, sizeof(mavlink_message_t)*256);
onboardMessageInterval.clear();
lastMessageUpdate.clear();
messagesHz.clear();
treeWidgetItems.clear();
ui->treeWidget->clear();
ui->rateTreeWidget->clear();
}
void QGCMAVLinkInspector::refreshView()
......@@ -145,6 +173,45 @@ void QGCMAVLinkInspector::refreshView()
updateField(msg->msgid, i, message->child(i));
}
}
if (selectedSystemID == 0 || selectedComponentID == 0)
{
return;
}
for (int i = 0; i < 256; ++i)//mavlink_message_t msg, receivedMessages)
{
const char* msgname = messageInfo[i].name;
int namelen = strnlen(msgname, 5);
if (namelen < 3) {
continue;
}
if (!strcmp(msgname, "EMPTY")) {
continue;
}
// Update the tree view
QString messageName("%1");
messageName = messageName.arg(msgname);
if (!rateTreeWidgetItems.contains(i))
{
QStringList fields;
fields << messageName;
fields << QString("%1").arg(i);
fields << "OFF / --- Hz";
QTreeWidgetItem* widget = new QTreeWidgetItem(fields);
widget->setFlags(widget->flags() | Qt::ItemIsEditable);
rateTreeWidgetItems.insert(i, widget);
ui->rateTreeWidget->addTopLevelItem(widget);
}
// Set Hz
//QTreeWidgetItem* message = rateTreeWidgetItems.value(i);
//message->setData(0, Qt::DisplayRole, QVariant(messageName));
}
}
void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t message)
......@@ -155,6 +222,17 @@ void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t m
// Only overwrite if system filter is set
memcpy(receivedMessages+message.msgid, &message, sizeof(mavlink_message_t));
// Add not yet seen systems / components
if (!systems.contains(message.sysid)) {
systems.insert(message.sysid, message.sysid);
ui->systemComboBox->addItem(tr("MAV%1").arg(message.sysid), message.sysid);
}
if (!components.contains(message.compid)) {
components.insert(message.compid, message.compid);
ui->componentComboBox->addItem(tr("COMP%1").arg(message.compid), message.compid);
}
quint64 receiveTime = QGC::groundTimeMilliseconds();
if (lastMessageUpdate.contains(message.msgid))
{
......@@ -163,6 +241,54 @@ void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t m
}
lastMessageUpdate.insert(message.msgid, receiveTime);
if (selectedSystemID == 0 || selectedComponentID == 0) {
return;
}
switch (message.msgid) {
case MAVLINK_MSG_ID_DATA_STREAM:
{
mavlink_data_stream_t stream;
mavlink_msg_data_stream_decode(&message, &stream);
onboardMessageInterval.insert(stream.stream_id, stream.message_rate);
}
break;
}
}
void QGCMAVLinkInspector::changeStreamInterval(int msgid, int interval) {
//REQUEST_DATA_STREAM
if (selectedSystemID == 0 || selectedComponentID == 0) {
return;
}
mavlink_request_data_stream_t stream;
stream.target_system = selectedSystemID;
stream.target_component = selectedComponentID;
stream.req_stream_id = msgid;
stream.req_message_rate = interval;
stream.start_stop = (interval > 0);
mavlink_message_t msg;
mavlink_msg_request_data_stream_encode(_protocol->getSystemId(), _protocol->getComponentId(), &msg, &stream);
_protocol->sendMessage(msg);
}
void QGCMAVLinkInspector::rateTreeItemChanged(QTreeWidgetItem* paramItem, int column)
{
if (paramItem && column > 0) {
int key = paramItem->data(1, Qt::DisplayRole).toInt();
QVariant value = paramItem->data(2, Qt::DisplayRole);
float interval = 1000 / value.toFloat();
qDebug() << "Stream " << key << "interval" << interval;
changeStreamInterval(key, interval);
}
}
QGCMAVLinkInspector::~QGCMAVLinkInspector()
......
......@@ -35,14 +35,21 @@ public slots:
/** @Brief Select a component through the drop down menu */
void selectDropDownMenuComponent(int dropdownid);
void rateTreeItemChanged(QTreeWidgetItem* paramItem, int column);
protected:
MAVLinkProtocol *_protocol; ///< MAVLink instance
int selectedSystemID; ///< Currently selected system
int selectedComponentID; ///< Currently selected component
QMap<int, int> systems; ///< Already observed systems
QMap<int, int> components; ///< Already observed components
QMap<int, quint64> lastMessageUpdate; ///< Used to switch between highlight and non-highlighting color
QMap<int, float> messagesHz; ///< Used to store update rate in Hz
QMap<int, float> onboardMessageInterval; ///< Stores the onboard selected data rate
QMap<int, unsigned int> messageCount; ///< Used to store the message count
mavlink_message_t receivedMessages[256]; ///< Available / known messages
QMap<int, QTreeWidgetItem*> treeWidgetItems; ///< Available tree widget items
QMap<int, QTreeWidgetItem*> rateTreeWidgetItems; ///< Available rate tree widget items
QTimer updateTimer; ///< Only update at 1 Hz to not overload the GUI
mavlink_message_info_t messageInfo[256]; // Store the metadata for all available MAVLink messages.
......@@ -50,6 +57,8 @@ protected:
void updateField(int msgid, int fieldid, QTreeWidgetItem* item);
/** @brief Rebuild the list of components */
void rebuildComponentList();
/** @brief Change the stream interval */
void changeStreamInterval(int msgid, int interval);
static const unsigned int updateInterval;
static const float updateHzLowpass;
......
......@@ -6,30 +6,33 @@
<rect>
<x>0</x>
<y>0</y>
<width>400</width>
<width>658</width>
<height>300</height>
</rect>
</property>
<property name="windowTitle">
<string>Form</string>
</property>
<layout class="QGridLayout" name="gridLayout" columnstretch="2,8,0,0,0">
<property name="margin">
<layout class="QGridLayout" name="gridLayout" columnstretch="2,8,2,8,3">
<property name="leftMargin">
<number>6</number>
</property>
<item row="0" column="0">
<widget class="QLabel" name="label">
<property name="topMargin">
<number>6</number>
</property>
<property name="rightMargin">
<number>6</number>
</property>
<property name="bottomMargin">
<number>6</number>
</property>
<item row="0" column="4">
<widget class="QPushButton" name="clearButton">
<property name="text">
<string>System</string>
<string>Clear</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QComboBox" name="systemComboBox"/>
</item>
<item row="0" column="3">
<widget class="QComboBox" name="componentComboBox"/>
</item>
<item row="0" column="2">
<widget class="QLabel" name="label_2">
<property name="text">
......@@ -37,10 +40,10 @@
</property>
</widget>
</item>
<item row="0" column="4">
<widget class="QPushButton" name="clearButton">
<item row="0" column="0">
<widget class="QLabel" name="label">
<property name="text">
<string>Clear</string>
<string>System</string>
</property>
</widget>
</item>
......@@ -53,6 +56,21 @@
</column>
</widget>
</item>
<item row="3" column="0" colspan="5">
<widget class="QTreeWidget" name="rateTreeWidget">
<column>
<property name="text">
<string notr="true">1</string>
</property>
</column>
</widget>
</item>
<item row="0" column="1">
<widget class="QComboBox" name="systemComboBox"/>
</item>
<item row="0" column="3">
<widget class="QComboBox" name="componentComboBox"/>
</item>
</layout>
</widget>
<resources/>
......
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