hm/gcs-nf
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

缓存bug修复，状态机完成

Browse Source
This commit is contained in:
hm
2020-03-18 18:49:18 +08:00
parent f5b3fa5a09
commit 082ed2d316
13 changed files with 1232 additions and 190 deletions
Download Patch File Download Diff File Expand all files Collapse all files
App/App.pro
+102 -101
View File
@@ -1,101 +1,102 @@
#-------------------------------------------------
#
# Project created by QtCreator 2019-12-13T23:38:22
#
#-------------------------------------------------
QT += core gui network
QT += serialport
greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
win32:
{
VERSION = 1.2.3.1
QMAKE_TARGET_PRODUCT = ground control softwear
QMAKE_TARGET_COMPANY = company
QMAKE_TARGET_DESCRIPTION = UAV Ground Control Station
QMAKE_TARGET_COPYRIGHT = hm copyright
}
TARGET = GCS
TEMPLATE = app
# The following define makes your compiler emit warnings if you use
# any feature of Qt which has been marked as deprecated (the exact warnings
# depend on your compiler). Please consult the documentation of the
# deprecated API in order to know how to port your code away from it.
DEFINES += QT_DEPRECATED_WARNINGS
# You can also make your code fail to compile if you use deprecated APIs.
# In order to do so, uncomment the following line.
# You can also select to disable deprecated APIs only up to a certain version of Qt.
#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000 # disables all the APIs deprecated before Qt 6.0.0
CONFIG += c++11
SOURCES += \
main.cpp \
mainwindow.cpp
HEADERS += \
mainwindow.h
# Default rules for deployment.
qnx: target.path = /tmp/$${TARGET}/bin
else: unix:!android: target.path = /opt/$${TARGET}/bin
!isEmpty(target.path): INSTALLS += target
#添加程序运行的第三方库
INCLUDEPATH += $$PWD/../thirdpart/include
#添加mavlink目录(给mavlinknode模块打补丁)
INCLUDEPATH += $$PWD/../mavlink \
$$PWD/../mavlink/ardupilotmega \
$$PWD/../mavlink/common \
$$PWD/../mavlink/icarous \
$$PWD/../mavlink/uAvionix \
$$PWD/../mavlink/XYK \
$$PWD/../mavlink/message_definitions
LIBS += -L$$PWD/../thirdpart/lib -lSkin
LIBS += -L$$PWD/../thirdpart/lib -lSerialPort
LIBS += -L$$PWD/../thirdpart/lib -lClientLink
LIBS += -L$$PWD/../thirdpart/lib -lqNavigation
LIBS += -L$$PWD/../thirdpart/lib -lCockpit
LIBS += -L$$PWD/../thirdpart/lib -lMavLinkNode
LIBS += -L$$PWD/../thirdpart/lib -lcore
LIBS += -L$$PWD/../thirdpart/lib -linternals
LIBS += -L$$PWD/../thirdpart/lib -lopmapwidget
LIBS += -L$$PWD/../thirdpart/lib -lDlink
#temp file
DESTDIR = $$PWD/../app_bin
MOC_DIR = $$PWD/../build
OBJECTS_DIR = $$PWD/../build
#复制运行库到程序目录
win32|win64 {
src_dir = $$PWD\\..\\thirdpart\\lib\\*.dll
dst_dir = $$PWD\\..\\app_bin\\
# dst_dir 最后的 \\ 是必须的，用来标示 xcopy 到一个文件夹，若不存在，创建之
# Replace slashes in paths with backslashes for Windows
src_dir ~= s,/,\\,g
dst_dir ~= s,/,\\,g
#system(xcopy $$src_dir $$dst_dir /y /e)
}
#编译HDMI版本，方便适配大屏显示
DEFINES += BUILD_WITH_HDMI
TRANSLATIONS = qt_zh.ts qt_en.ts
#-------------------------------------------------
#
# Project created by QtCreator 2019-12-13T23:38:22
#
#-------------------------------------------------
QT += core gui network
QT += serialport
greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
win32:
{
VERSION = 1.2.3.1
QMAKE_TARGET_PRODUCT = ground control softwear
QMAKE_TARGET_COMPANY = company
QMAKE_TARGET_DESCRIPTION = UAV Ground Control Station
QMAKE_TARGET_COPYRIGHT = hm copyright
}
TARGET = GCS
TEMPLATE = app
# The following define makes your compiler emit warnings if you use
# any feature of Qt which has been marked as deprecated (the exact warnings
# depend on your compiler). Please consult the documentation of the
# deprecated API in order to know how to port your code away from it.
DEFINES += QT_DEPRECATED_WARNINGS
# You can also make your code fail to compile if you use deprecated APIs.
# In order to do so, uncomment the following line.
# You can also select to disable deprecated APIs only up to a certain version of Qt.
#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000 # disables all the APIs deprecated before Qt 6.0.0
CONFIG += c++11
SOURCES += \
main.cpp \
mainwindow.cpp
HEADERS += \
mainwindow.h
# Default rules for deployment.
qnx: target.path = /tmp/$${TARGET}/bin
else: unix:!android: target.path = /opt/$${TARGET}/bin
!isEmpty(target.path): INSTALLS += target
#添加程序运行的第三方库
INCLUDEPATH += $$PWD/../thirdpart/include
#添加mavlink目录(给mavlinknode模块打补丁)
INCLUDEPATH += $$PWD/../mavlink \
$$PWD/../mavlink/ardupilotmega \
$$PWD/../mavlink/common \
$$PWD/../mavlink/icarous \
$$PWD/../mavlink/uAvionix \
$$PWD/../mavlink/XYK \
$$PWD/../mavlink/message_definitions
LIBS += -L$$PWD/../thirdpart/lib -lSkin
LIBS += -L$$PWD/../thirdpart/lib -lSerialPort
LIBS += -L$$PWD/../thirdpart/lib -lClientLink
LIBS += -L$$PWD/../thirdpart/lib -lqNavigation
LIBS += -L$$PWD/../thirdpart/lib -lCockpit
LIBS += -L$$PWD/../thirdpart/lib -lMavLinkNode
LIBS += -L$$PWD/../thirdpart/lib -lMavLinkInspector
LIBS += -L$$PWD/../thirdpart/lib -lcore
LIBS += -L$$PWD/../thirdpart/lib -linternals
LIBS += -L$$PWD/../thirdpart/lib -lopmapwidget
LIBS += -L$$PWD/../thirdpart/lib -lDlink
#temp file
DESTDIR = $$PWD/../app_bin
MOC_DIR = $$PWD/../build
OBJECTS_DIR = $$PWD/../build
#复制运行库到程序目录
win32|win64 {
src_dir = $$PWD\\..\\thirdpart\\lib\\*.dll
dst_dir = $$PWD\\..\\app_bin\\
# dst_dir 最后的 \\ 是必须的，用来标示 xcopy 到一个文件夹，若不存在，创建之
# Replace slashes in paths with backslashes for Windows
src_dir ~= s,/,\\,g
dst_dir ~= s,/,\\,g
#system(xcopy $$src_dir $$dst_dir /y /e)
}
#编译HDMI版本，方便适配大屏显示
DEFINES += BUILD_WITH_HDMI
TRANSLATIONS = qt_zh.ts qt_en.ts
App/mainwindow.cpp
+18
View File
@@ -223,6 +223,24 @@ void MainWindow::onTabIndexChanged(const int &index)
{
client_triggered();
}
else if (index == 3)
{
QAction* action;
MAVLinkInspector *inspector = new MAVLinkInspector("mavlink inspector",action,this);
qRegisterMetaType<mavlink_message_t>("mavlink_message_t");
connect(dlink->mavlinknode,SIGNAL(recievemsg(mavlink_message_t)),
inspector,SLOT(receiveMessage(mavlink_message_t)));
/*
connect(dlink->mavlinknode,&MavLinkNode::recievemsg,
inspector,&MAVLinkInspector::receiveMessage);
*/
inspector->show();
}
}
App/mainwindow.h
+1
View File
@@ -19,6 +19,7 @@
#include "dlink.h"
#include "clientlinkdialog.h"
#include "mavlinknode.h"
#include "MAVLinkInspector.h"
#pragma execution_character_set("utf-8")
GCS.pro
+3
View File
@@ -7,6 +7,9 @@ CONFIG += ordered
SUBDIRS += MavLinkNode
SUBDIRS += MAVLinkInspector
SUBDIRS += SerialPortDialog
SUBDIRS += ClientLinkDialog
SUBDIRS += Skin
MAVLinkInspector/MAVLinkInspector.cc
+690
View File
@@ -0,0 +1,690 @@
#include "MAVLinkInspector.h"
#include "ui_MAVLinkInspector.h"
#include <QList>
#include <QDebug>
const float MAVLinkInspector::updateHzLowpass = 0.2f;
const unsigned int MAVLinkInspector::updateInterval = 1000U;
MAVLinkInspector::MAVLinkInspector(const QString& title, QAction* action,QWidget *parent) :
selectedSystemID(0),
selectedComponentID(0),
ui(new Ui::MAVLinkInspector)
{
ui->setupUi(this);
setWindowTitle(title);
// Make sure "All" is an option for both the system and components
ui->systemComboBox->addItem(tr("All"), 0);
ui->componentComboBox->addItem(tr("All"), 0);
// Set up the column headers for the message listing
QStringList header;
header << tr("Name");
header << tr("Value");
header << tr("Type");
ui->treeWidget->setHeaderLabels(header);
// Connect the UI
connect(ui->systemComboBox, static_cast<void (QComboBox::*)(int)>(&QComboBox::currentIndexChanged),
this, &MAVLinkInspector::selectDropDownMenuSystem);
connect(ui->componentComboBox, static_cast<void (QComboBox::*)(int)>(&QComboBox::currentIndexChanged),
this, &MAVLinkInspector::selectDropDownMenuComponent);
connect(ui->clearButton, &QPushButton::clicked, this, &MAVLinkInspector::clearView);
// Connect external connections
//connect(qApp()->toolbox()->multiVehicleManager(), &MAVLinkInspector::vehicleAdded, this, &QGCMAVLinkInspector::_vehicleAdded);
//connect(protocol, &MAVLinkInspector::messageReceived, this, &MAVLinkInspector::receiveMessage);
// Attach the UI's refresh rate to a timer.
connect(&updateTimer, &QTimer::timeout, this, &MAVLinkInspector::refreshView);
updateTimer.start(updateInterval);
//loadSettings();
}
void MAVLinkInspector::_vehicleAdded(quint32 id)
{
ui->systemComboBox->addItem(tr("Vehicle %1").arg(id), id);
// Add a tree for a new UAS
addUAStoTree(id);
}
void MAVLinkInspector::selectDropDownMenuSystem(int dropdownid)
{
selectedSystemID = ui->systemComboBox->itemData(dropdownid).toInt();
rebuildComponentList();
}
void MAVLinkInspector::selectDropDownMenuComponent(int dropdownid)
{
selectedComponentID = ui->componentComboBox->itemData(dropdownid).toInt();
}
void MAVLinkInspector::rebuildComponentList()
{
ui->componentComboBox->clear();
components.clear();
ui->componentComboBox->addItem(tr("All"), 0);
// Fill
/*
Vehicle* vehicle = qgcApp()->toolbox()->multiVehicleManager()->getVehicleById(selectedSystemID);
if (vehicle)
{
UASInterface* uas = vehicle->uas();
QMap<int, QString> components = uas->getComponents();
foreach (int id, components.keys())
{
QString name = components.value(id);
ui->componentComboBox->addItem(name, id);
}
}
*/
}
void MAVLinkInspector::addComponent(int uas, int component, const QString& name)
{
Q_UNUSED(component);
Q_UNUSED(name);
if (uas != selectedSystemID) return;
rebuildComponentList();
}
/**
* Reset the view. This entails clearing all data structures and resetting data from already-
* received messages.
*/
void MAVLinkInspector::clearView()
{
QMap<int, mavlink_message_t* >::iterator ite;
for(ite=uasMessageStorage.begin(); ite!=uasMessageStorage.end();++ite)
{
delete ite.value();
ite.value() = nullptr;
}
uasMessageStorage.clear();
QMap<int, QMap<int, QTreeWidgetItem*>* >::iterator iteMsg;
for (iteMsg=uasMsgTreeItems.begin(); iteMsg!=uasMsgTreeItems.end();++iteMsg)
{
QMap<int, QTreeWidgetItem*>* msgTreeItems = iteMsg.value();
QList<int> groupKeys = msgTreeItems->uniqueKeys();
QList<int>::iterator listKeys;
for (listKeys=groupKeys.begin();listKeys!=groupKeys.end();++listKeys)
{
delete msgTreeItems->take(*listKeys);
}
}
uasMsgTreeItems.clear();
QMap<int, QTreeWidgetItem* >::iterator iteTree;
for(iteTree=uasTreeWidgetItems.begin(); iteTree!=uasTreeWidgetItems.end();++iteTree)
{
delete iteTree.value();
iteTree.value() = NULL;
}
uasTreeWidgetItems.clear();
QMap<int, QMap<int, float>* >::iterator iteHz;
for (iteHz=uasMessageHz.begin(); iteHz!=uasMessageHz.end();++iteHz)
{
iteHz.value()->clear();
delete iteHz.value();
iteHz.value() = NULL;
}
uasMessageHz.clear();
QMap<int, QMap<int, unsigned int>* >::iterator iteCount;
for(iteCount=uasMessageCount.begin(); iteCount!=uasMessageCount.end();++iteCount)
{
iteCount.value()->clear();
delete iteCount.value();
iteCount.value() = NULL;
}
uasMessageCount.clear();
QMap<int, QMap<int, quint64>* >::iterator iteLast;
for(iteLast=uasLastMessageUpdate.begin(); iteLast!=uasLastMessageUpdate.end();++iteLast)
{
iteLast.value()->clear();
delete iteLast.value();
iteLast.value() = NULL;
}
uasLastMessageUpdate.clear();
onboardMessageInterval.clear();
ui->treeWidget->clear();
}
void MAVLinkInspector::refreshView()
{
QMap<int, mavlink_message_t* >::const_iterator ite;
for(ite=uasMessageStorage.constBegin(); ite!=uasMessageStorage.constEnd();++ite)
{
mavlink_message_t* msg = ite.value();
const mavlink_message_info_t* msgInfo = mavlink_get_message_info(msg);
if (!msgInfo) {
qWarning() << QStringLiteral("MAVLinkInspector::refreshView NULL msgInfo msgid(%1)").arg(msg->msgid);
continue;
}
// Ignore NULL values
if (msg->msgid == 0xFF) continue;
// Update the message frenquency
// Get the previous frequency for low-pass filtering
float msgHz = 0.0f;
QMap<int, QMap<int, float>* >::const_iterator iteHz = uasMessageHz.find(msg->sysid);
QMap<int, float>* uasMsgHz = iteHz.value();
while((iteHz != uasMessageHz.end()) && (iteHz.key() == msg->sysid))
{
if(iteHz.value()->contains(msg->msgid))
{
uasMsgHz = iteHz.value();
msgHz = iteHz.value()->value(msg->msgid);
break;
}
++iteHz;
}
// Get the number of message received
float msgCount = 0;
QMap<int, QMap<int, unsigned int> * >::const_iterator iter = uasMessageCount.find(msg->sysid);
QMap<int, unsigned int>* uasMsgCount = iter.value();
while((iter != uasMessageCount.end()) && (iter.key()==msg->sysid))
{
if(iter.value()->contains(msg->msgid))
{
msgCount = (float) iter.value()->value(msg->msgid);
uasMsgCount = iter.value();
break;
}
++iter;
}
// Compute the new low-pass filtered frequency and update the message count
msgHz = (1.0f-updateHzLowpass)* msgHz + updateHzLowpass*msgCount/((float)updateInterval/1000.0f);
uasMsgHz->insert(msg->msgid,msgHz);
uasMsgCount->insert(msg->msgid,(unsigned int) 0);
// Update the tree view
QString messageName("%1 (%2 Hz, #%3)");
messageName = messageName.arg(msgInfo->name).arg(msgHz, 3, 'f', 1).arg(msg->msgid);
addUAStoTree(msg->sysid);
// Look for the tree for the UAS sysid
QMap<int, QTreeWidgetItem*>* msgTreeItems = uasMsgTreeItems.value(msg->sysid);
if (!msgTreeItems)
{
// The UAS tree has not been created yet, no update
return;
}
// Add the message with msgid to the tree if not done yet
if(!msgTreeItems->contains(msg->msgid))
{
QStringList fields;
fields << messageName;
QTreeWidgetItem* widget = new QTreeWidgetItem();
for (unsigned int i = 0; i < msgInfo->num_fields; ++i)
{
QTreeWidgetItem* field = new QTreeWidgetItem();
widget->addChild(field);
}
msgTreeItems->insert(msg->msgid,widget);
QList<int> groupKeys = msgTreeItems->uniqueKeys();
int insertIndex = groupKeys.indexOf(msg->msgid);
uasTreeWidgetItems.value(msg->sysid)->insertChild(insertIndex,widget);
}
// Update the message
QTreeWidgetItem* message = msgTreeItems->value(msg->msgid);
if(message)
{
message->setFirstColumnSpanned(true);
message->setData(0, Qt::DisplayRole, QVariant(messageName));
for (unsigned int i = 0; i < msgInfo->num_fields; ++i)
{
updateField(msg, msgInfo, i, message->child(i));
}
}
}
}
void MAVLinkInspector::addUAStoTree(int sysId)
{
if(!uasTreeWidgetItems.contains(sysId))
{
// Add the UAS to the main tree after it has been created
//Vehicle* vehicle = qgcApp()->toolbox()->multiVehicleManager()->getVehicleById(sysId);
if (1)//vehicle)
{
//UASInterface* uas = vehicle->uas();
QStringList idstring;
idstring << QString("Vehicle %1").arg(sysId);//uas->getUASID());
QTreeWidgetItem* uasWidget = new QTreeWidgetItem(idstring);
uasWidget->setFirstColumnSpanned(true);
uasTreeWidgetItems.insert(sysId,uasWidget);
ui->treeWidget->addTopLevelItem(uasWidget);
uasMsgTreeItems.insert(sysId,new QMap<int, QTreeWidgetItem*>());
}
}
}
void MAVLinkInspector::receiveMessage(mavlink_message_t message)
{
//Q_UNUSED(link);
quint64 receiveTime;
if (selectedSystemID != 0 && selectedSystemID != message.sysid) return;
if (selectedComponentID != 0 && selectedComponentID != message.compid) return;
// Create dynamically an array to store the messages for each UAS
if (!uasMessageStorage.contains(message.sysid))
{
mavlink_message_t* msg = new mavlink_message_t;
*msg = message;
uasMessageStorage.insertMulti(message.sysid,msg);
}
bool msgFound = false;
QMap<int, mavlink_message_t* >::const_iterator iteMsg = uasMessageStorage.find(message.sysid);
mavlink_message_t* uasMessage = iteMsg.value();
while((iteMsg != uasMessageStorage.end()) && (iteMsg.key() == message.sysid))
{
if (iteMsg.value()->msgid == message.msgid)
{
msgFound = true;
uasMessage = iteMsg.value();
break;
}
++iteMsg;
}
if (!msgFound)
{
mavlink_message_t* msgIdMessage = new mavlink_message_t;
*msgIdMessage = message;
uasMessageStorage.insertMulti(message.sysid,msgIdMessage);
}
else
{
*uasMessage = message;
}
// Looking if this message has already been received once
msgFound = false;
QMap<int, QMap<int, quint64>* >::const_iterator ite = uasLastMessageUpdate.find(message.sysid);
QMap<int, quint64>* lastMsgUpdate = ite.value();
while((ite != uasLastMessageUpdate.end()) && (ite.key() == message.sysid))
{
if(ite.value()->contains(message.msgid))
{
msgFound = true;
// Point to the found message
lastMsgUpdate = ite.value();
break;
}
++ite;
}
//输入时间
receiveTime = QDateTime::currentMSecsSinceEpoch();//0;//QGC::groundTimeMilliseconds();
// If the message doesn't exist, create a map for the frequency, message count and time of reception
if(!msgFound)
{
// Create a map for the message frequency
QMap<int, float>* messageHz = new QMap<int,float>;
messageHz->insert(message.msgid,0.0f);
uasMessageHz.insertMulti(message.sysid,messageHz);
// Create a map for the message count
QMap<int, unsigned int>* messagesCount = new QMap<int, unsigned int>;
messagesCount->insert(message.msgid,0);
uasMessageCount.insertMulti(message.sysid,messagesCount);
// Create a map for the time of reception of the message
QMap<int, quint64>* lastMessage = new QMap<int, quint64>;
lastMessage->insert(message.msgid,receiveTime);
uasLastMessageUpdate.insertMulti(message.sysid,lastMessage);
// Point to the created message
lastMsgUpdate = lastMessage;
}
else
{
// The message has been found/created
if ((lastMsgUpdate->contains(message.msgid))&&(uasMessageCount.contains(message.sysid)))
{
// Looking for and updating the message count
unsigned int count = 0;
QMap<int, QMap<int, unsigned int>* >::const_iterator iter = uasMessageCount.find(message.sysid);
QMap<int, unsigned int> * uasMsgCount = iter.value();
while((iter != uasMessageCount.end()) && (iter.key() == message.sysid))
{
if(iter.value()->contains(message.msgid))
{
uasMsgCount = iter.value();
count = uasMsgCount->value(message.msgid,0);
uasMsgCount->insert(message.msgid,count+1);
break;
}
++iter;
}
}
lastMsgUpdate->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;
}
}
MAVLinkInspector::~MAVLinkInspector()
{
clearView();
delete ui;
}
void MAVLinkInspector::updateField(mavlink_message_t* msg, const mavlink_message_info_t* msgInfo, int fieldid, QTreeWidgetItem* item)
{
// Add field tree widget item
item->setData(0, Qt::DisplayRole, QVariant(msgInfo->fields[fieldid].name));
bool msgFound = false;
QMap<int, mavlink_message_t* >::const_iterator iteMsg = uasMessageStorage.find(msg->sysid);
mavlink_message_t* uasMessage = iteMsg.value();
while((iteMsg != uasMessageStorage.end()) && (iteMsg.key() == msg->sysid))
{
if (iteMsg.value()->msgid == msg->msgid)
{
msgFound = true;
uasMessage = iteMsg.value();
break;
}
++iteMsg;
}
if (!msgFound)
{
return;
}
uint8_t* m = (uint8_t*)&uasMessage->payload64[0];
switch (msgInfo->fields[fieldid].type)
{
case MAVLINK_TYPE_CHAR:
if (msgInfo->fields[fieldid].array_length > 0)
{
char* str = (char*)(m+msgInfo->fields[fieldid].wire_offset);
// Enforce null termination
str[msgInfo->fields[fieldid].array_length-1] = '\0';
QString string(str);
item->setData(2, Qt::DisplayRole, "char");
item->setData(1, Qt::DisplayRole, string);
}
else
{
// Single char
char b = *((char*)(m+msgInfo->fields[fieldid].wire_offset));
item->setData(2, Qt::DisplayRole, QString("char[%1]").arg(msgInfo->fields[fieldid].array_length));
item->setData(1, Qt::DisplayRole, b);
}
break;
case MAVLINK_TYPE_UINT8_T:
if (msgInfo->fields[fieldid].array_length > 0)
{
uint8_t* nums = m+msgInfo->fields[fieldid].wire_offset;
// Enforce null termination
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
string += tmp.arg(nums[j]);
}
item->setData(2, Qt::DisplayRole, QString("uint8_t[%1]").arg(msgInfo->fields[fieldid].array_length));
item->setData(1, Qt::DisplayRole, string);
}
else
{
// Single value
uint8_t u = *(m+msgInfo->fields[fieldid].wire_offset);
item->setData(2, Qt::DisplayRole, "uint8_t");
item->setData(1, Qt::DisplayRole, u);
}
break;
case MAVLINK_TYPE_INT8_T:
if (msgInfo->fields[fieldid].array_length > 0)
{
int8_t* nums = (int8_t*)(m+msgInfo->fields[fieldid].wire_offset);
// Enforce null termination
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
string += tmp.arg(nums[j]);
}
item->setData(2, Qt::DisplayRole, QString("int8_t[%1]").arg(msgInfo->fields[fieldid].array_length));
item->setData(1, Qt::DisplayRole, string);
}
else
{
// Single value
int8_t n = *((int8_t*)(m+msgInfo->fields[fieldid].wire_offset));
item->setData(2, Qt::DisplayRole, "int8_t");
item->setData(1, Qt::DisplayRole, n);
}
break;
case MAVLINK_TYPE_UINT16_T:
if (msgInfo->fields[fieldid].array_length > 0)
{
uint16_t* nums = (uint16_t*)(m+msgInfo->fields[fieldid].wire_offset);
// Enforce null termination
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
string += tmp.arg(nums[j]);
}
item->setData(2, Qt::DisplayRole, QString("uint16_t[%1]").arg(msgInfo->fields[fieldid].array_length));
item->setData(1, Qt::DisplayRole, string);
}
else
{
// Single value
uint16_t n = *((uint16_t*)(m+msgInfo->fields[fieldid].wire_offset));
item->setData(2, Qt::DisplayRole, "uint16_t");
item->setData(1, Qt::DisplayRole, n);
}
break;
case MAVLINK_TYPE_INT16_T:
if (msgInfo->fields[fieldid].array_length > 0)
{
int16_t* nums = (int16_t*)(m+msgInfo->fields[fieldid].wire_offset);
// Enforce null termination
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
string += tmp.arg(nums[j]);
}
item->setData(2, Qt::DisplayRole, QString("int16_t[%1]").arg(msgInfo->fields[fieldid].array_length));
item->setData(1, Qt::DisplayRole, string);
}
else
{
// Single value
int16_t n = *((int16_t*)(m+msgInfo->fields[fieldid].wire_offset));
item->setData(2, Qt::DisplayRole, "int16_t");
item->setData(1, Qt::DisplayRole, n);
}
break;
case MAVLINK_TYPE_UINT32_T:
if (msgInfo->fields[fieldid].array_length > 0)
{
uint32_t* nums = (uint32_t*)(m+msgInfo->fields[fieldid].wire_offset);
// Enforce null termination
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
string += tmp.arg(nums[j]);
}
item->setData(2, Qt::DisplayRole, QString("uint32_t[%1]").arg(msgInfo->fields[fieldid].array_length));
item->setData(1, Qt::DisplayRole, string);
}
else
{
// Single value
float n = *((uint32_t*)(m+msgInfo->fields[fieldid].wire_offset));
item->setData(2, Qt::DisplayRole, "uint32_t");
item->setData(1, Qt::DisplayRole, n);
}
break;
case MAVLINK_TYPE_INT32_T:
if (msgInfo->fields[fieldid].array_length > 0)
{
int32_t* nums = (int32_t*)(m+msgInfo->fields[fieldid].wire_offset);
// Enforce null termination
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
string += tmp.arg(nums[j]);
}
item->setData(2, Qt::DisplayRole, QString("int32_t[%1]").arg(msgInfo->fields[fieldid].array_length));
item->setData(1, Qt::DisplayRole, string);
}
else
{
// Single value
int32_t n = *((int32_t*)(m+msgInfo->fields[fieldid].wire_offset));
item->setData(2, Qt::DisplayRole, "int32_t");
item->setData(1, Qt::DisplayRole, n);
}
break;
case MAVLINK_TYPE_FLOAT:
if (msgInfo->fields[fieldid].array_length > 0)
{
float* nums = (float*)(m+msgInfo->fields[fieldid].wire_offset);
// Enforce null termination
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
string += tmp.arg(nums[j]);
}
item->setData(2, Qt::DisplayRole, QString("float[%1]").arg(msgInfo->fields[fieldid].array_length));
item->setData(1, Qt::DisplayRole, string);
}
else
{
// Single value
float f = *((float*)(m+msgInfo->fields[fieldid].wire_offset));
item->setData(2, Qt::DisplayRole, "float");
item->setData(1, Qt::DisplayRole, f);
}
break;
case MAVLINK_TYPE_DOUBLE:
if (msgInfo->fields[fieldid].array_length > 0)
{
double* nums = (double*)(m+msgInfo->fields[fieldid].wire_offset);
// Enforce null termination
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
string += tmp.arg(nums[j]);
}
item->setData(2, Qt::DisplayRole, QString("double[%1]").arg(msgInfo->fields[fieldid].array_length));
item->setData(1, Qt::DisplayRole, string);
}
else
{
// Single value
double f = *((double*)(m+msgInfo->fields[fieldid].wire_offset));
item->setData(2, Qt::DisplayRole, "double");
item->setData(1, Qt::DisplayRole, f);
}
break;
case MAVLINK_TYPE_UINT64_T:
if (msgInfo->fields[fieldid].array_length > 0)
{
uint64_t* nums = (uint64_t*)(m+msgInfo->fields[fieldid].wire_offset);
// Enforce null termination
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
string += tmp.arg(nums[j]);
}
item->setData(2, Qt::DisplayRole, QString("uint64_t[%1]").arg(msgInfo->fields[fieldid].array_length));
item->setData(1, Qt::DisplayRole, string);
}
else
{
// Single value
uint64_t n = *((uint64_t*)(m+msgInfo->fields[fieldid].wire_offset));
item->setData(2, Qt::DisplayRole, "uint64_t");
item->setData(1, Qt::DisplayRole, (quint64) n);
}
break;
case MAVLINK_TYPE_INT64_T:
if (msgInfo->fields[fieldid].array_length > 0)
{
int64_t* nums = (int64_t*)(m+msgInfo->fields[fieldid].wire_offset);
// Enforce null termination
QString tmp("%1, ");
QString string;
for (unsigned int j = 0; j < msgInfo->fields[fieldid].array_length; ++j)
{
string += tmp.arg(nums[j]);
}
item->setData(2, Qt::DisplayRole, QString("int64_t[%1]").arg(msgInfo->fields[fieldid].array_length));
item->setData(1, Qt::DisplayRole, string);
}
else
{
// Single value
int64_t n = *((int64_t*)(m+msgInfo->fields[fieldid].wire_offset));
item->setData(2, Qt::DisplayRole, "int64_t");
item->setData(1, Qt::DisplayRole, (qint64) n);
}
break;
}
}
MAVLinkInspector/MAVLinkInspector.h
+92
View File
@@ -0,0 +1,92 @@
#ifndef MAVLINKINSPECTOR_H
#define MAVLINKINSPECTOR_H
#define MAVLINK_USE_MESSAGE_INFO
#include <QMap>
#include <QTimer>
#include "QDateTime"
#include "QWidget"
#include "mavlink.h"
#include "mavlink_get_info.h"
#ifdef QtMavlinkInspector
#include <mavlinkinspectorglobal.h>
namespace Ui {
class MAVLinkInspector;
}
class MAVLINKINSPECTORSHARED_EXPORT QTreeWidgetItem;
class MAVLINKINSPECTORSHARED_EXPORT UASInterface;
class MAVLINKINSPECTORSHARED_EXPORT MAVLinkInspector : public QWidget{
#else
namespace Ui {
class MAVLinkInspector;
}
class QTreeWidgetItem;
class UASInterface;
class MAVLinkInspector : public QWidget
{
#endif
Q_OBJECT
public:
explicit MAVLinkInspector(const QString& title, QAction* action, QWidget *parent = nullptr);
~MAVLinkInspector();
public slots:
void receiveMessage(mavlink_message_t message);
/** @brief Clear all messages */
void clearView();
/** @brief Update view */
void refreshView();
/** @brief Add component to the list */
void addComponent(int uas, int component, const QString& name);
/** @Brief Select a system through the drop down menu */
void selectDropDownMenuSystem(int dropdownid);
/** @Brief Select a component through the drop down menu */
void selectDropDownMenuComponent(int dropdownid);
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, float> onboardMessageInterval; ///< Stores the onboard selected data rate
QTimer updateTimer; ///< Only update at 1 Hz to not overload the GUI
QMap<int, QTreeWidgetItem* > uasTreeWidgetItems; ///< Tree of available uas with their widget
QMap<int, QMap<int, QTreeWidgetItem*>* > uasMsgTreeItems; ///< Stores the widget of the received message for each UAS
QMap<int, mavlink_message_t* > uasMessageStorage; ///< Stores the messages for every UAS
QMap<int, QMap<int, float>* > uasMessageHz; ///< Stores the frequency of each message of each UAS
QMap<int, QMap<int, unsigned int>* > uasMessageCount; ///< Stores the message count of each message of each UAS
QMap<int, QMap<int, quint64>* > uasLastMessageUpdate; ///< Stores the time of the last message for each message of each UAS
/* @brief Update one message field */
void updateField(mavlink_message_t* msg, const mavlink_message_info_t* msgInfo, int fieldid, QTreeWidgetItem* item);
/** @brief Rebuild the list of components */
void rebuildComponentList();
/* @brief Create a new tree for a new UAS */
void addUAStoTree(int sysId);
static const unsigned int updateInterval; ///< The update interval of the refresh function
static const float updateHzLowpass; ///< The low-pass filter value for the frequency of each message
private slots:
void _vehicleAdded(quint32 id);
private:
Ui::MAVLinkInspector *ui;
};
#endif // QGCMAVLINKINSPECTOR_H
MAVLinkInspector/MAVLinkInspector.pri
+8
View File
@@ -0,0 +1,8 @@
FORMS += \
$$PWD/MAVLinkInspector.ui
HEADERS += \
$$PWD/MAVLinkInspector.h
SOURCES += \
$$PWD/MAVLinkInspector.cc
MAVLinkInspector/MAVLinkInspector.pro
+91
View File
@@ -0,0 +1,91 @@
QT += core gui network
greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
TARGET = MAVLinkInspector
TEMPLATE = lib
DEFINES += MAVLINKINSPECTOR_LIBRARY
CONFIG += resources_big
# The following define makes your compiler emit warnings if you use
# any feature of Qt which has been marked as deprecated (the exact warnings
# depend on your compiler). Please consult the documentation of the
# deprecated API in order to know how to port your code away from it.
DEFINES += QT_DEPRECATED_WARNINGS
# You can also make your code fail to compile if you use deprecated APIs.
# In order to do so, uncomment the following line.
# You can also select to disable deprecated APIs only up to a certain version of Qt.
#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000 # disables all the APIs deprecated before Qt 6.0.0
DESTDIR = $$PWD/../thirdpart/lib
MOC_DIR = $$PWD/../build
OBJECTS_DIR = $$PWD/../build
DEFINES += QtMavlinkInspector
#添加mavlink目录
INCLUDEPATH += $$PWD/../mavlink \
$$PWD/../mavlink/ardupilotmega \
$$PWD/../mavlink/common \
$$PWD/../mavlink/icarous \
$$PWD/../mavlink/uAvionix \
$$PWD/../mavlink/XYK \
$$PWD/../mavlink/message_definitions
FORMS += \
$$PWD/MAVLinkInspector.ui
HEADERS += \
$$PWD/MAVLinkInspector.h \
MAVLinkInspectorGlobal.h
SOURCES += \
$$PWD/MAVLinkInspector.cc
win32|win64 {
src_dir = $$PWD\\*.h
dst_dir = $$PWD\\..\\thirdpart\\include\\
# dst_dir 最后的 \\ 是必须的，用来标示 xcopy 到一个文件夹，若不存在，创建之
# Replace slashes in paths with backslashes for Windows
src_dir ~= s,/,\\,g
dst_dir ~= s,/,\\,g
system(xcopy $$src_dir $$dst_dir /y /e)
}
unix {
src_dir = $$PWD/*.h
dst_dir = $$PWD/../thirdpart/include/
!exists($$dst_dir): system(mkdir -p $$dst_dir)
system(cp $$src_dir $$dst_dir -arf )
}
MAVLinkInspector/MAVLinkInspector.ui
+75
View File
@@ -0,0 +1,75 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>MAVLinkInspector</class>
<widget class="QWidget" name="MAVLinkInspector">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>658</width>
<height>444</height>
</rect>
</property>
<property name="windowTitle">
<string>MAVLink Inspector</string>
</property>
<layout class="QGridLayout" name="gridLayout" columnstretch="2,0,0,0,0">
<property name="leftMargin">
<number>6</number>
</property>
<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="0">
<widget class="QLabel" name="label">
<property name="maximumSize">
<size>
<width>100</width>
<height>16777215</height>
</size>
</property>
<property name="text">
<string>System</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">
<string>Component</string>
</property>
</widget>
</item>
<item row="0" column="4">
<widget class="QPushButton" name="clearButton">
<property name="text">
<string>Clear</string>
</property>
</widget>
</item>
<item row="2" column="0" colspan="5">
<widget class="QTreeWidget" name="treeWidget">
<column>
<property name="text">
<string notr="true">1</string>
</property>
</column>
</widget>
</item>
</layout>
</widget>
<resources/>
<connections/>
</ui>
MAVLinkInspector/MAVLinkInspectorGlobal.h
+18
View File
@@ -0,0 +1,18 @@
#ifndef MAVLINKINSPECTORGLOBAL_H
#define MAVLINKINSPECTORGLOBAL_H
#include <QtCore/qglobal.h>
#ifdef QtMavlinkInspector
#if defined(MAVLINKINSPECTOR_LIBRARY)
# define MAVLINKINSPECTORSHARED_EXPORT Q_DECL_EXPORT
#else
# define MAVLINKINSPECTORSHARED_EXPORT Q_DECL_IMPORT
#endif
#endif
#endif // MAVLINKINSPECTORGLOBAL_H
MavLinkNode/mavlinknode.cpp
+8 -3
View File
@@ -7,12 +7,12 @@ MavLinkNode::MavLinkNode(QObject *parent) : QObject(parent)
Nodethread = new QThread();
this->moveToThread(Nodethread);
connect(Nodethread, &QThread::started, this, &MavLinkNode::process);
setRunFrq(50);//50
setRunFrq(10);//50
//初始化buff
initbuff();
Mission = new MissionProcess(this);
Mission->setRunFrq(200);
@@ -130,15 +130,17 @@ void MavLinkNode::setbuff(quint32 src,QByteArray data)
if(client_buff.buff[client_buff.select].size() >= client_buff.max_size)
{
client_buff.buff[client_buff.select].clear();
qDebug() << "client_buff.buff" << client_buff.select << "OVERFLOW";
}
client_buff.buff[client_buff.select].append(data);
qDebug() << "buff" << client_buff.select << client_buff.buff[client_buff.select].size() ;
break;
case SourceType::s_port:
//当前的buff超过10M字节之后就清除，防爆机制，不然buff太大后容易卡死
if(serial_buff.buff[serial_buff.select].size() >= serial_buff.max_size)
{
serial_buff.buff[serial_buff.select].clear();
qDebug() << "serial_buff.buff" << client_buff.select << "OVERFLOW";
}
serial_buff.buff[serial_buff.select].append(data);
break;
@@ -165,6 +167,7 @@ QByteArray MavLinkNode::readbuff(quint32 src)
datagram.setRawData(client_buff.buff[0],client_buff.buff[0].size());
//清除这个未选择的buff
client_buff.buff[0].clear();
//读取完成，可以往这个内存里面写数了
client_buff.select = 0;
}
@@ -174,12 +177,14 @@ QByteArray MavLinkNode::readbuff(quint32 src)
{
datagram.setRawData(serial_buff.buff[1],serial_buff.buff[1].size());
//读取完成，可以往这个内存里面写数了
serial_buff.buff[1].clear();
serial_buff.select = 1;
}
else if(serial_buff.select == 1)
{
datagram.setRawData(serial_buff.buff[0],serial_buff.buff[0].size());
//读取完成，可以往这个内存里面写数了
serial_buff.buff[0].clear();
serial_buff.select = 0;
}
break;
MavLinkNode/missionprocess.cpp
+85 -72
View File
@@ -3,7 +3,7 @@
MissionProcess::MissionProcess(QObject *parent) : QObject(parent)
{
setRunFrq(200);//默认200Hz频率运行
}
@@ -58,12 +58,12 @@ void MissionProcess::process()//线程函数
count ++;
QThread::msleep(1000/running_frq);
switch(m_TransmitMode)
switch(mission_status.m_Mode)
{
default:
case 0:break;
case 1: ReadStateMachine();break;
case 2: WriteStateMachine();break;
case Nop_Mode :break;
case RecieveMode : ReadStateMachine();break;
case TransmitMode : WriteStateMachine();break;
}
}
@@ -86,11 +86,22 @@ void MissionProcess::ReadCmd(uint8_t m_sysid, uint8_t m_compid)
{
sysid = m_sysid;
compid = m_compid;
m_TransmitMode = 1;//接收模式
ismTimerTimeOut = false;
mission_status.m_Mode = RecieveMode;//接收模式
start();//开启线程
}
void MissionProcess::WriteCmd(uint8_t m_sysid, uint8_t m_compid ,uint32_t count )
{
sysid = m_sysid;
compid = m_compid;
mission_status.transmit.count = count;
mission_status.m_Mode = TransmitMode;//接收模式
start();//开启线程
}
//这个函数类似中断，专门处理接收到的状态
void MissionProcess::MissionParse(mavlink_message_t msg)
{
@@ -101,25 +112,23 @@ void MissionProcess::MissionParse(mavlink_message_t msg)
case MAVLINK_MSG_ID_MISSION_COUNT: {
mavlink_msg_mission_count_decode(&msg,&mission_count);
//qDebug() << "mission_count" << mission_count.count;
isRecieveCount = true;
mission_status.recieve.isWaitingforCount = false;
}break;
case MAVLINK_MSG_ID_MISSION_REQUEST_INT: {
mavlink_msg_mission_request_int_decode(&msg,&mission_request_int);
isRecieveRequest = true;
mission_status.transmit.isWaiteforRequest = false;
}break;
case MAVLINK_MSG_ID_MISSION_REQUEST: {
mavlink_msg_mission_request_decode(&msg,&mission_request);
isRecieveRequest = true;
mission_status.transmit.isWaiteforRequest = false;
}break;
case MAVLINK_MSG_ID_MISSION_ITEM_INT: {
mavlink_msg_mission_item_int_decode(&msg,&mission_item_int);
//qDebug() << "recieve item int:" << mission_item_int.seq;
isWaitingforItem = false;//已经收到航点，不用再等待
mission_status.recieve.isWaitingforItem = false;//已经收到航点，不用再等待
}break;
case MAVLINK_MSG_ID_MISSION_ITEM: {
mavlink_msg_mission_item_decode(&msg,&mission_item);
//qDebug() << "recieve item:" << mission_item.seq;
isWaitingforItem = false;
mission_status.recieve.isWaitingforItem = false;
}break;
case MAVLINK_MSG_ID_MISSION_ACK: {
mavlink_msg_mission_ack_decode(&msg,&mission_ack);
@@ -157,11 +166,10 @@ void MissionProcess::ReadStateMachine(void)
if(step == 0)
{
qDebug() << "start reading mission";
isRecieveCount = false;
request_list();//请求读取某一个设备的航线
step++;//下一个阶段
mission_status.recieve.isWaitingforCount = true;
request_list();
step++;
time = QTime::currentTime().msecsSinceStartOfDay();
}
else if(step == 1)//等待收到Count
{
@@ -174,23 +182,19 @@ void MissionProcess::ReadStateMachine(void)
if(timeout_count > 5)//如果指令发5次还没发出去，那么结束读取，并报错
{
step = 0;
m_TransmitMode = 0;//切换到什么都不做
mission_status.m_Mode = Nop_Mode;//切换到什么都不做
timeout_count = 0;
qDebug() << "mission read fail";
}
}
else//如果没有超时，那么就进入下一个阶段
{
if(isRecieveCount == true)//收到count
if(mission_status.recieve.isWaitingforCount == false)//收到count
{
qDebug() << "step" << step << "isRecieveCount" << isRecieveCount;
timeout_count = 0;
isRecieveCount = false;
isWaitingforItem = true;
mission_status.recieve.isWaitingforItem = true;
request_int(0);//读取0点
time = QTime::currentTime().msecsSinceStartOfDay();
step ++;
}
}
@@ -198,47 +202,42 @@ void MissionProcess::ReadStateMachine(void)
}
else if(step ==2)//请求航点
{
if(isWaitingforItem == false)
//超过时间
if((QTime::currentTime().msecsSinceStartOfDay() - time) > 1000)
{
//分成两类，int和不带int
if((mission_item_int.seq+1) < mission_count.count)
{
qDebug() << "mission_item_int.seq+1" << (mission_item_int.seq+1);
isWaitingforItem = true;
request_int(mission_item_int.seq+1);
//定时
time = QTime::currentTime().msecsSinceStartOfDay();
}
else
{
qDebug() << "mission recieve all";
step++;
}
timeout_count ++;
qDebug() << "mission request time out " << timeout_count << "times, retry again";
if(timeout_count > 5)//如果指令发5次还没发出去，那么结束读取，并报错
{
step = 0;
mission_status.m_Mode= Nop_Mode;//切换到什么都不做
timeout_count = 0;
qDebug() << "mission read fail";
}
mission_status.recieve.isWaitingforItem = false;//如果超时了，那么就重新请求一次
}
else
{
//超过时间
if((QTime::currentTime().msecsSinceStartOfDay() - time) > 1000)
{
timeout_count ++;
qDebug() << "mission request time out " << timeout_count << "times, retry again";
if(timeout_count > 5)//如果指令发5次还没发出去，那么结束读取，并报错
{
step = 0;
m_TransmitMode = 0;//切换到什么都不做
timeout_count = 0;
qDebug() << "mission read fail";
}
isWaitingforItem = false;//重新发送一次
}
else
if(mission_status.recieve.isWaitingforItem == false)
{
//分成两类，int和不带int
if((mission_item_int.seq+1) < mission_count.count)
{
qDebug() << "mission_item_int.seq+1" << (mission_item_int.seq+1);
mission_status.recieve.isWaitingforItem = true;
request_int(mission_item_int.seq+1);
time = QTime::currentTime().msecsSinceStartOfDay();
}
else
{
qDebug() << "mission recieve all";
step++;
}
}
}
}
else if(step == 3)//航线传输结束，发送ack
{
@@ -246,10 +245,10 @@ void MissionProcess::ReadStateMachine(void)
ack();
step = 0;
m_TransmitMode = 0;//切换到什么都不做
mission_status.m_Mode = Nop_Mode;//切换到什么都不做
timeout_count = 0;
isRecieveCount = false;
isWaitingforItem = false;
mission_status.recieve.isWaitingforCount = true;
mission_status.recieve.isWaitingforItem = true;
}
}
@@ -263,7 +262,7 @@ void MissionProcess::WriteStateMachine(void)
if(step == 0)
{
//向其他线程或者自己读取航点的数量
count(20);//发送count
count(mission_status.transmit.count);//发送count
time = QTime::currentTime().msecsSinceStartOfDay();
step++;//下一个阶段
}
@@ -277,20 +276,21 @@ void MissionProcess::WriteStateMachine(void)
if(timeout_count > 5)//如果指令发5次还没发出去，那么结束读取，并报错
{
step = 0;
m_TransmitMode = 0;
mission_status.m_Mode = Nop_Mode;
timeout_count = 0;
}
}
else//如果没有超时，那么就进入下一个阶段
{
if(isRecieveRequest == true)
if(mission_status.transmit.isWaiteforRequest == false)
{
step ++;
time = QTime::currentTime().msecsSinceStartOfDay();
}
}
}
else if(step == 3)
else if(step == 2)
{
if((QTime::currentTime().msecsSinceStartOfDay() - time) > 1000)
{
@@ -299,23 +299,28 @@ void MissionProcess::WriteStateMachine(void)
if(timeout_count > 5)//如果指令发5次还没发出去，那么结束读取，并报错
{
step = 0;
m_TransmitMode = 0;
mission_status.m_Mode = Nop_Mode;
timeout_count = 0;
}
}
else
{
if(isRecieveRequest == true)//收到Request
if(mission_status.transmit.isWaiteforRequest == true)//收到Request
{
//分成两类，int和不带int
if(mission_item_int.seq < mission_count.count)
{
//item_int(mission_item_int.seq+1);//发送航点
isRecieveRequest = false;
mission_status.transmit.isWaiteforRequest = false;
time = QTime::currentTime().msecsSinceStartOfDay();
}
else
{
step++;
mission_status.transmit.isWaiteforACK == true;
}
}
}
@@ -323,9 +328,17 @@ void MissionProcess::WriteStateMachine(void)
else if(step == 4)//航线传输结束，等待ack，如果没接收到就算了，超时也结束
{
//ack();//wait for ack
step = 0;
isReadMission = false;
isRecieveCount = false;
if(mission_status.transmit.isWaiteforACK == true)
{
}
else
{
step = 0;
mission_status.m_Mode = Nop_Mode;
mission_status.transmit.isWaiteforRequest = false;
}
}
}
MavLinkNode/missionprocess.h
+41 -14
View File
@@ -17,13 +17,43 @@ class MissionProcess : public QObject
{
#endif
Q_OBJECT
enum {
Nop_Mode = 0,
RecieveMode,
TransmitMode
}_modetype;
typedef struct {
bool isWaitingforCount;
bool isWaitingforItem;
}_recieve;
typedef struct {
uint32_t count;
bool isWaiteforRequest;
bool isWaiteforACK;
}_transmit;
typedef struct
{
_recieve recieve;
_transmit transmit;
_modetype m_Mode;
}_mission_;
public:
explicit MissionProcess(QObject *parent = nullptr);
_mission_ mission_status;
//读取航线的指令
void ReadCmd(uint8_t m_sysid, uint8_t m_compid);
void WriteCmd(uint8_t m_sysid, uint8_t m_compid ,uint32_t count );
public slots:
//线程对外接口
void setRunFrq(uint32_t frq);
@@ -106,23 +136,20 @@ private:
QThread *Missionthread = nullptr;
uint8_t m_TransmitMode = 0;
bool isRecieveCount = false;
bool isRecieveRequest = false;
bool isWaitingforItem = false;
bool isReadMission = false;
bool isWriteMission = false;
bool ismTimerTimeOut = false;
//bool isReadMission = false;
//bool isWriteMission = false;
//bool ismTimerTimeOut = false;
};