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

接入数据，但是死机

Browse Source
This commit is contained in:
hm
2020-02-27 22:26:02 +08:00
parent 2e379ee660
commit 9b4ce69c41
6 changed files with 301 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitmodules
+1 -1
View File
@@ -1,3 +1,3 @@
[submodule "mavlink"]
path = mavlink
url = https://hm@git.matthewgong.com/matt/c_library_v2.git
url = https://git.matthewgong.com/matt/c_library_v2.git
App/App.pro
+9 -1
View File
@@ -55,6 +55,15 @@ else: unix:!android: target.path = /opt/$${TARGET}/bin
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
@@ -70,7 +79,6 @@ DESTDIR = $$PWD/../app_bin
MOC_DIR = $$PWD/../build
OBJECTS_DIR = $$PWD/../build
#复制运行库到程序目录
win32|win64 {
src_dir = $$PWD\\..\\thirdpart\\lib\\*.dll
MavLinkNode/mavlinknode.cpp
+207 -16
View File
@@ -6,12 +6,12 @@ MavLinkNode::MavLinkNode(QObject *parent) : QObject(parent)
Nodethread = new QThread();
this->moveToThread(Nodethread);
connect(Nodethread, &QThread::started, this, &MavLinkNode::process);
//connect(Nodethread, &QThread::finished, Nodethread, &QObject::deleteLater);
qDebug() << "MavLink " << QThread::currentThreadId();
setRunFrq(200);//200Hz
//初始化buff
initbuff();
}
MavLinkNode::~MavLinkNode()
@@ -37,6 +37,9 @@ void MavLinkNode::start()
{
if(!Nodethread->isRunning())
{
//初始化buff
initbuff();
running_flag = true;
Nodethread->start();
qDebug() << "thread start" << running_flag;
@@ -67,33 +70,221 @@ void MavLinkNode::stop()
void MavLinkNode::process()//线程函数
{
uint8_t count = 0;
while (running_flag) {
while (running_flag)
{
count ++;
QThread::msleep(1000/running_frq);//50Hz
//qDebug() << "mavlink Process " << QThread::currentThreadId();
count ++;
//qDebug() << "run " << count << "ms";
QThread::msleep(1000/running_frq);//50Hz
QByteArray datagram = NULL;
//解码从UDP来的
datagram.clear();
datagram = readbuff(SourceType::c_sock);//每次全部读取
if(!datagram.isEmpty())
{
Mavlinkparse(SourceType::c_sock,datagram);
}
//解码从串口来的
datagram.clear();
datagram = readbuff(SourceType::c_sock);//每次全部读取
if(!datagram.isEmpty())
{
Mavlinkparse(SourceType::c_sock,datagram);
}
}
//退出线程
Nodethread->quit();
}
void MavLinkNode::setbuff(uint32_t src,QByteArray data)
void MavLinkNode::initbuff(void)
{
if(buff_select == 1)
{
RawData2.append(data);
client_buff.max_size = 10 * 1024 *1024;//10M
client_buff.buff[0].clear();
client_buff.buff[1].clear();
client_buff.select = 0;
serial_buff.max_size = 10 * 1024 *1024;
serial_buff.buff[0].clear();
serial_buff.buff[1].clear();
serial_buff.select = 0;
}
void MavLinkNode::setbuff(quint32 src,QByteArray data)
{
switch (src) {
default:
case 0:
//当前的buff超过10M字节之后就清除，防爆机制，不然buff太大后容易卡死
if(client_buff.buff[client_buff.select].size() >= client_buff.max_size)
{
client_buff.buff[client_buff.select].clear();
}
client_buff.buff[client_buff.select].append(data);
break;
case 1:
//当前的buff超过10M字节之后就清除，防爆机制，不然buff太大后容易卡死
if(serial_buff.buff[serial_buff.select].size() >= serial_buff.max_size)
{
serial_buff.buff[serial_buff.select].clear();
}
serial_buff.buff[serial_buff.select].append(data);
break;
}
else if(buff_select == 2)
{
RawData1.append(data);
}
QByteArray MavLinkNode::readbuff(quint32 src)
{
QByteArray datagram;
//从buff里面读取
switch (src) {
default:
case SourceType::c_sock:
if(client_buff.select == 0)
{
datagram.setRawData(client_buff.buff[1],client_buff.buff[1].size());
//读取完成，可以往这个内存里面写数了
client_buff.select = 1;
}
else if(client_buff.select == 1)
{
datagram.setRawData(client_buff.buff[1],client_buff.buff[1].size());
//读取完成，可以往这个内存里面写数了
client_buff.select = 0;
}
break;
case SourceType::s_port:
if(serial_buff.select == 0)
{
datagram.setRawData(serial_buff.buff[1],serial_buff.buff[1].size());
//读取完成，可以往这个内存里面写数了
serial_buff.select = 1;
}
else if(serial_buff.select == 1)
{
datagram.setRawData(serial_buff.buff[1],serial_buff.buff[1].size());
//读取完成，可以往这个内存里面写数了
serial_buff.select = 0;
}
break;
}
return datagram;
}
void MavLinkNode::Mavlinkparse(quint32 src,QByteArray datagram)
{
mavlink_message_t msg;
mavlink_status_t status;
for (QByteArray::const_iterator i = datagram.cbegin(); i != datagram.cend(); ++i)
{
if(MAVLINK_FRAMING_OK == mavlink_parse_char(src,*i,&msg,&status))
{
MAVLinkRcv_Handler(msg); //接收完一帧数据并处理
//qDebug() << "re";
}
else
{
if (status.parse_state == MAVLINK_PARSE_STATE_GOT_BAD_CRC1)
{
qDebug() << "mavlink recieve crc error";
}
}
}
}
void MavLinkNode::MAVLinkRcv_Handler(mavlink_message_t msg)
{
//vehicle.sysid = msg.sysid;
//vehicle.compid = MAV_COMP_ID_AUTOPILOT1;//msg.compid;
switch (msg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: {
mavlink_msg_heartbeat_decode(&msg,&vehicle.heartbeat);
}break;
case MAVLINK_MSG_ID_PING: {
mavlink_msg_ping_decode(&msg,&vehicle.ping);
}break;
case MAVLINK_MSG_ID_ATTITUDE: {
mavlink_msg_attitude_decode(&msg,&vehicle.attitude);
}break;
case MAVLINK_MSG_ID_GPS_RAW_INT: {
mavlink_msg_gps_raw_int_decode(&msg,&vehicle.gps_raw_int);
}break;
case MAVLINK_MSG_ID_AIRSPEED_AUTOCAL: {
mavlink_msg_airspeed_autocal_decode(&msg,&vehicle.airspeed_autocal);
}break;
case MAVLINK_MSG_ID_VFR_HUD: {
mavlink_msg_vfr_hud_decode(&msg,&vehicle.vfr_hud);
}break;
//航线部分
case MAVLINK_MSG_ID_MISSION_COUNT: {//飞控发来计数值，然后开始读取
mavlink_msg_mission_count_decode(&msg,&vehicle.mission_count);
//Mavlink_msg_mission_request_int(0);
}break;
case MAVLINK_MSG_ID_MISSION_ITEM: {
mavlink_msg_mission_item_decode(&msg,&vehicle.mission_item);
}break;
case MAVLINK_MSG_ID_MISSION_ITEM_INT: {
mavlink_msg_mission_item_int_decode(&msg,&vehicle.mission_item_int);
//emit mission_recieve_item(vehicle.mission_item_int);
if((vehicle.mission_count.count-1) >= (vehicle.mission_item_int.seq+1))
{
//Mavlink_msg_mission_request_int(vehicle.mission_item_int.seq + 1);
}
else {
//Mavlink_msg_mission_ack();
}
}break;
case MAVLINK_MSG_ID_MISSION_REQUEST: {
mavlink_msg_mission_request_decode(&msg,&vehicle.mission_request);
//emit mission_item_request(vehicle.mission_request.seq);
}break;
case MAVLINK_MSG_ID_MISSION_REQUEST_INT: {
mavlink_msg_mission_request_int_decode(&msg,&vehicle.mission_request_int);
//emit mission_item_request_int(vehicle.mission_request_int.seq);
}break;
case MAVLINK_MSG_ID_MISSION_ACK: {
mavlink_msg_mission_ack_decode(&msg,&vehicle.mission_ack);
}break;
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
mavlink_msg_param_request_list_decode(&msg,&vehicle.param_request_list);
}break;
case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
mavlink_msg_param_request_read_decode(&msg,&vehicle.param_request_read);
}break;
case MAVLINK_MSG_ID_PARAM_SET: {
mavlink_msg_param_set_decode(&msg,&vehicle.param_set);
}break;
case MAVLINK_MSG_ID_PARAM_VALUE: {//分类解码
mavlink_msg_param_value_decode(&msg,&vehicle.param_value);
//要判断是否是全部读取，如果不是那么就不发送那么多
//emit recieveParamater(vehicle.param_value);//发送收到参数
if(vehicle.param_value.param_index < (vehicle.param_value.param_count-1))
{
// Mavlink_param_request_read(vehicle.param_value.param_index + 1);
}
}break;
}
//最后更新一下界面
//emit vehicleUpdate();
qDebug() << vehicle.heartbeat.type;
}
MavLinkNode/mavlinknode.h
+83 -15
View File
@@ -6,7 +6,7 @@
#include "QDebug"
//#include "mavlink.h"
#include "mavlink.h"
#ifdef QtMavlinkNode
#include <mavlinknodeglobal.h>
@@ -15,41 +15,109 @@ class MAVLINKNODESHARED_EXPORT MavLinkNode : public QObject {
class MavLinkNode : public QObject
{
#endif
Q_OBJECT
typedef struct {
qint32 max_size;
quint8 select;
QByteArray buff[2];
}_buffdef;
typedef struct {
uint8_t sysid; /*< ID of message sender system/aircraft */
uint8_t compid; /*< ID of the message sender component */
mavlink_heartbeat_t heartbeat_old;
mavlink_heartbeat_t heartbeat;
mavlink_ping_t ping;
mavlink_attitude_t attitude;
mavlink_gps_raw_int_t gps_raw_int;
mavlink_battery_status_t battery_status;
mavlink_rpm_t rpm;
mavlink_rc_channels_raw_t rc_channels_raw;
mavlink_servo_output_raw_t servo_output_raw;
mavlink_scaled_pressure_t scaled_pressure;
mavlink_vfr_hud_t vfr_hud;
mavlink_airspeed_autocal_t airspeed_autocal;
mavlink_command_int_t command_int;
mavlink_mission_count_t mission_count;
mavlink_mission_item_t mission_item;
mavlink_mission_item_int_t mission_item_int;
mavlink_mission_request_t mission_request;
mavlink_mission_request_int_t mission_request_int;
mavlink_mission_ack_t mission_ack;
mavlink_param_set_t param_set;//
mavlink_param_union_t param_union;
mavlink_param_value_t param_value;//
mavlink_param_map_rc_t param_map;
mavlink_param_ext_ack_t param_ext_ack;
mavlink_param_ext_set_t param_ext_set;
mavlink_param_ext_value_t param_ext_value;
mavlink_param_request_list_t param_request_list;//
mavlink_param_request_read_t param_request_read;//
mavlink_param_union_double_t param_union_double;
mavlink_param_ext_request_list_t param_ext_request_list;
mavlink_param_ext_request_read_t param_ext_request_read;
}_vehicle;
public:
explicit MavLinkNode(QObject *parent = nullptr);
~MavLinkNode();
void setRunFrq(uint32_t frq);
_vehicle vehicle;
signals:
public slots:
void process();
//线程对外接口
void setRunFrq(uint32_t frq);
void start();
void stop();
//对外的填充数据的接口
void setbuff(uint32_t src, QByteArray data);
//缓存对外接口
void setbuff(quint32 src, QByteArray data);
private slots:
//线程私有接口
void process();
//缓存私有接口
void initbuff(void);
QByteArray readbuff(quint32 src);
//解析
void Mavlinkparse(quint32 src,QByteArray datagram);
void MAVLinkRcv_Handler(mavlink_message_t msg);
protected:
enum SourceType{
s_port = 0,
c_sock = 1
};
bool running_flag = false;
quint32 running_frq = 10;//10Hz
quint32 running_frq = 200;//200Hz
QThread *Nodethread;
QByteArray RawData1;
QByteArray RawData2;
uint8_t buff_select;
_buffdef serial_buff;
_buffdef client_buff;
};
#endif // MAVLINKNODE_H
dlink/dlink.cpp
-26
View File
@@ -154,19 +154,6 @@ void DLink::readPendingDatagramsSerialPort(void)
mavlinknode->setbuff(SourceType::s_port,datagram);
/*
for (QByteArray::const_iterator i = datagram.cbegin(); i != datagram.cend(); ++i)
{
ret = mavlink_parse_char(MAVLINK_COMM_0,*i,&msg,&status);
if(MAVLINK_FRAMING_OK == ret)
{
MAVLinkRcv_Handler(msg); //接收完一帧数据并处理
}
}
*/
}
void DLink::readPendingDatagramsClient(void)
@@ -178,19 +165,6 @@ void DLink::readPendingDatagramsClient(void)
Clientsock->readDatagram(datagram.data(), datagram.size());
mavlinknode->setbuff(SourceType::c_sock,datagram);
//SendMessageTo(ret,(uint8_t *)datagram.data(),datagram.size());
/*
for (QByteArray::const_iterator i = datagram.cbegin(); i != datagram.cend(); ++i)
{
ret = mavlink_parse_char(MAVLINK_COMM_0,*i,&msg,&status);
if(MAVLINK_FRAMING_OK == ret)
{
MAVLinkRcv_Handler(msg); //接收完一帧数据并处理
}
}
*/
}
}
dlink/dlink.h
+1 -45
View File
@@ -31,50 +31,6 @@ public:
explicit DLink(QObject *parent = nullptr);
~DLink();
typedef struct {
uint8_t sysid; /*< ID of message sender system/aircraft */
uint8_t compid; /*< ID of the message sender component */
/*
mavlink_heartbeat_t heartbeat_old;
mavlink_heartbeat_t heartbeat;
mavlink_ping_t ping;
mavlink_attitude_t attitude;
mavlink_gps_raw_int_t gps_raw_int;
mavlink_battery_status_t battery_status;
mavlink_rpm_t rpm;
mavlink_rc_channels_raw_t rc_channels_raw;
mavlink_servo_output_raw_t servo_output_raw;
mavlink_scaled_pressure_t scaled_pressure;
mavlink_vfr_hud_t vfr_hud;
mavlink_command_int_t command_int;
mavlink_mission_count_t mission_count;
mavlink_mission_item_t mission_item;
mavlink_mission_item_int_t mission_item_int;
mavlink_mission_request_t mission_request;
mavlink_mission_request_int_t mission_request_int;
mavlink_mission_ack_t mission_ack;
mavlink_param_set_t param_set;//
mavlink_param_union_t param_union;
mavlink_param_value_t param_value;//
mavlink_param_map_rc_t param_map;
mavlink_param_ext_ack_t param_ext_ack;
mavlink_param_ext_set_t param_ext_set;
mavlink_param_ext_value_t param_ext_value;
mavlink_param_request_list_t param_request_list;//
mavlink_param_request_read_t param_request_read;//
mavlink_param_union_double_t param_union_double;
mavlink_param_ext_request_list_t param_ext_request_list;
mavlink_param_ext_request_read_t param_ext_request_read;
*/
}_vehicle;
_vehicle vehicle;
void setupPort(const QString port, qint32 baudrate, QSerialPort::Parity parity);
bool statesPort();
void stopPort();
@@ -99,7 +55,7 @@ public slots:
private:
protected:
enum SourceType{
s_port = 0,