添加航点的线程
This commit is contained in:
+448
-434
@@ -1,434 +1,448 @@
|
||||
#include "mavlinknode.h"
|
||||
|
||||
MavLinkNode::MavLinkNode(QObject *parent) : QObject(parent)
|
||||
{
|
||||
running_flag = false;
|
||||
Nodethread = new QThread();
|
||||
|
||||
this->moveToThread(Nodethread);
|
||||
connect(Nodethread, &QThread::started, this, &MavLinkNode::process);
|
||||
|
||||
setRunFrq(50);//50
|
||||
|
||||
//初始化buff
|
||||
initbuff();
|
||||
}
|
||||
|
||||
MavLinkNode::~MavLinkNode()
|
||||
{
|
||||
if(Nodethread->isRunning())
|
||||
{
|
||||
Nodethread->quit();
|
||||
Nodethread->wait();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void MavLinkNode::setRunFrq(uint32_t frq)
|
||||
{
|
||||
if((frq != 0)||(frq <= 1000))
|
||||
{
|
||||
running_frq = frq;
|
||||
qDebug() << "set Mavlink Node running frquency:" <<frq <<"Hz";
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void MavLinkNode::start()
|
||||
{
|
||||
if(!Nodethread->isRunning())
|
||||
{
|
||||
//初始化buff
|
||||
initbuff();
|
||||
|
||||
running_flag = true;
|
||||
Nodethread->start();
|
||||
qDebug() << "thread start" << running_flag;
|
||||
}
|
||||
else
|
||||
{
|
||||
qDebug() << "thread has started";
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void MavLinkNode::stop()
|
||||
{
|
||||
if(Nodethread->isRunning())
|
||||
{
|
||||
running_flag = false;
|
||||
qDebug() << "thread stop" << running_flag;
|
||||
}
|
||||
else
|
||||
{
|
||||
qDebug() << "thread is not running";
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
//这里一直在解码，一直检查双缓冲里面是否有数据，有就解码，没有就休息
|
||||
void MavLinkNode::process()//线程函数
|
||||
{
|
||||
uint8_t count = 0;
|
||||
QByteArray datagram = NULL;
|
||||
while (running_flag)
|
||||
{
|
||||
count ++;
|
||||
QThread::msleep(1000/running_frq);
|
||||
|
||||
//解码从UDP来的
|
||||
datagram.clear();
|
||||
datagram = readbuff(SourceType::c_sock);//每次全部读取
|
||||
if(!datagram.isEmpty())
|
||||
{
|
||||
Mavlinkparse(SourceType::c_sock,datagram);
|
||||
}
|
||||
|
||||
//解码从串口来的
|
||||
datagram.clear();
|
||||
datagram = readbuff(SourceType::s_port);//每次全部读取
|
||||
//qDebug() << "serial port parse";
|
||||
if(!datagram.isEmpty())
|
||||
{
|
||||
//qDebug() << "serial port parse";
|
||||
Mavlinkparse(SourceType::s_port,datagram);
|
||||
}
|
||||
|
||||
}
|
||||
//退出线程
|
||||
Nodethread->quit();
|
||||
}
|
||||
|
||||
void MavLinkNode::initbuff(void)
|
||||
{
|
||||
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 SourceType::c_sock:
|
||||
//当前的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);
|
||||
//qDebug() << "set buff";
|
||||
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();
|
||||
}
|
||||
serial_buff.buff[serial_buff.select].append(data);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
QByteArray MavLinkNode::readbuff(quint32 src)
|
||||
{
|
||||
QByteArray datagram;
|
||||
|
||||
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[0],client_buff.buff[0].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[0],serial_buff.buff[0].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); //接收完一帧数据并处理
|
||||
}
|
||||
else
|
||||
{
|
||||
switch(status.parse_state)
|
||||
{
|
||||
case MAVLINK_PARSE_STATE_UNINIT:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_UNINIT";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_IDLE:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_IDLE";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_GOT_STX:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_GOT_STX";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_GOT_LENGTH:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_GOT_LENGTH";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_GOT_SEQ:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_GOT_SEQ";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_GOT_SYSID:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_GOT_SYSID";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_GOT_COMPID:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_GOT_COMPID";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_GOT_MSGID1:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_GOT_MSGID1";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_GOT_MSGID2:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_GOT_MSGID2";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_GOT_MSGID3:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_GOT_MSGID3";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_GOT_PAYLOAD:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_GOT_PAYLOAD";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_GOT_CRC1:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_GOT_CRC1";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_GOT_BAD_CRC1:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_GOT_BAD_CRC1";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_SIGNATURE_WAIT:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_SIGNATURE_WAIT";
|
||||
}break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void MavLinkNode::MAVLinkRcv_Handler(mavlink_message_t msg)
|
||||
{
|
||||
switch (msg.msgid) {
|
||||
//状态
|
||||
case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
|
||||
case MAVLINK_MSG_ID_SYS_STATUS:
|
||||
case MAVLINK_MSG_ID_HEARTBEAT:
|
||||
case MAVLINK_MSG_ID_PING:
|
||||
case MAVLINK_MSG_ID_ATTITUDE:
|
||||
case MAVLINK_MSG_ID_GPS_RAW_INT:
|
||||
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
|
||||
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
|
||||
case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
|
||||
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
|
||||
case MAVLINK_MSG_ID_AIRSPEED_AUTOCAL:
|
||||
case MAVLINK_MSG_ID_RPM:
|
||||
case MAVLINK_MSG_ID_SCALED_PRESSURE:
|
||||
case MAVLINK_MSG_ID_EXTENDED_SYS_STATE:
|
||||
case MAVLINK_MSG_ID_BATTERY_STATUS:
|
||||
case MAVLINK_MSG_ID_VIBRATION:
|
||||
case MAVLINK_MSG_ID_EngineState:
|
||||
case MAVLINK_MSG_ID_VFR_HUD:
|
||||
StatusParse(msg);
|
||||
break;
|
||||
|
||||
//航线部分
|
||||
case MAVLINK_MSG_ID_MISSION_COUNT:
|
||||
case MAVLINK_MSG_ID_MISSION_CURRENT:
|
||||
//case MAVLINK_MSG_ID_MISSION_ITEM:
|
||||
case MAVLINK_MSG_ID_MISSION_ITEM_INT:
|
||||
//case MAVLINK_MSG_ID_MISSION_REQUEST:
|
||||
case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
|
||||
case MAVLINK_MSG_ID_MISSION_ACK:
|
||||
MissionParse(msg);
|
||||
break;
|
||||
//参数
|
||||
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
|
||||
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
|
||||
case MAVLINK_MSG_ID_PARAM_SET:
|
||||
case MAVLINK_MSG_ID_PARAM_VALUE:
|
||||
ParamParse(msg);
|
||||
break;
|
||||
|
||||
//命令
|
||||
case MAVLINK_MSG_ID_COMMAND_ACK:
|
||||
CommandParse(msg);
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void MavLinkNode::StatusParse(mavlink_message_t msg)
|
||||
{
|
||||
switch (msg.msgid) {
|
||||
case MAVLINK_MSG_ID_AUTOPILOT_VERSION: {
|
||||
mavlink_msg_autopilot_version_decode(&msg,&vehicle.autopilot_version);
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_SYS_STATUS: {
|
||||
mavlink_msg_sys_status_decode(&msg,&vehicle.sys_status);
|
||||
}break;
|
||||
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);
|
||||
emit state_updated();
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_GPS_RAW_INT: {
|
||||
mavlink_msg_gps_raw_int_decode(&msg,&vehicle.gps_raw_int);
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
|
||||
mavlink_msg_global_position_int_decode(&msg,&vehicle.global_position_int);
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: {
|
||||
mavlink_msg_servo_output_raw_decode(&msg,&vehicle.servo_output_raw);
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: {
|
||||
mavlink_msg_rc_channels_raw_decode(&msg,&vehicle.rc_channels_raw);
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: {
|
||||
mavlink_msg_nav_controller_output_decode(&msg,&vehicle.nav_controller_output);
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_AIRSPEED_AUTOCAL: {
|
||||
mavlink_msg_airspeed_autocal_decode(&msg,&vehicle.airspeed_autocal);
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_RPM: {
|
||||
mavlink_msg_rpm_decode(&msg,&vehicle.rpm);
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_SCALED_PRESSURE: {
|
||||
mavlink_msg_scaled_pressure_decode(&msg,&vehicle.scaled_pressure);
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_EXTENDED_SYS_STATE: {
|
||||
mavlink_msg_extended_sys_state_decode(&msg,&vehicle.extended_sys_state);
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_BATTERY_STATUS: {
|
||||
mavlink_msg_battery_status_decode(&msg,&vehicle.battery_status);
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_VIBRATION: {
|
||||
mavlink_msg_vibration_decode(&msg,&vehicle.vibration);
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_EngineState: {
|
||||
mavlink_msg_enginestate_decode(&msg,&vehicle.enginestate);
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_VFR_HUD: {
|
||||
mavlink_msg_vfr_hud_decode(&msg,&vehicle.vfr_hud);
|
||||
}break;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
void MavLinkNode::ParamParse(mavlink_message_t msg)
|
||||
{
|
||||
switch (msg.msgid) {
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
void MavLinkNode::CommandParse(mavlink_message_t msg)
|
||||
{
|
||||
switch (msg.msgid) {
|
||||
case MAVLINK_MSG_ID_COMMAND_ACK: {
|
||||
mavlink_command_ack_t ack;
|
||||
mavlink_msg_command_ack_decode(&msg,&ack);
|
||||
}break;
|
||||
}
|
||||
}
|
||||
|
||||
void MavLinkNode::MissionParse(mavlink_message_t msg)
|
||||
{
|
||||
switch (msg.msgid) {
|
||||
//航线部分
|
||||
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;
|
||||
}
|
||||
}
|
||||
#include "mavlinknode.h"
|
||||
|
||||
MavLinkNode::MavLinkNode(QObject *parent) : QObject(parent)
|
||||
{
|
||||
//main thread
|
||||
running_flag = false;
|
||||
Nodethread = new QThread();
|
||||
this->moveToThread(Nodethread);
|
||||
connect(Nodethread, &QThread::started, this, &MavLinkNode::process);
|
||||
setRunFrq(50);//50
|
||||
//初始化buff
|
||||
initbuff();
|
||||
|
||||
Mission = new MissionProcess();
|
||||
Mission->setRunFrq(200);
|
||||
Mission->start();
|
||||
|
||||
|
||||
|
||||
Parameterthread = new QThread();
|
||||
|
||||
|
||||
}
|
||||
|
||||
MavLinkNode::~MavLinkNode()
|
||||
{
|
||||
if(Nodethread->isRunning())
|
||||
{
|
||||
Nodethread->quit();
|
||||
Nodethread->wait();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void MavLinkNode::setRunFrq(uint32_t frq)
|
||||
{
|
||||
if((frq != 0)||(frq <= 1000))
|
||||
{
|
||||
running_frq = frq;
|
||||
qDebug() << "set Mavlink Node running frquency:" <<frq <<"Hz";
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void MavLinkNode::start()
|
||||
{
|
||||
if(!Nodethread->isRunning())
|
||||
{
|
||||
//初始化buff
|
||||
initbuff();
|
||||
|
||||
running_flag = true;
|
||||
Nodethread->start();
|
||||
qDebug() << "thread start" << running_flag;
|
||||
}
|
||||
else
|
||||
{
|
||||
qDebug() << "thread has started";
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void MavLinkNode::stop()
|
||||
{
|
||||
if(Nodethread->isRunning())
|
||||
{
|
||||
running_flag = false;
|
||||
qDebug() << "thread stop" << running_flag;
|
||||
}
|
||||
else
|
||||
{
|
||||
qDebug() << "thread is not running";
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
//这里一直在解码，一直检查双缓冲里面是否有数据，有就解码，没有就休息
|
||||
void MavLinkNode::process()//线程函数
|
||||
{
|
||||
uint8_t count = 0;
|
||||
QByteArray datagram = NULL;
|
||||
while (running_flag)
|
||||
{
|
||||
count ++;
|
||||
QThread::msleep(1000/running_frq);
|
||||
|
||||
//解码从UDP来的
|
||||
datagram.clear();
|
||||
datagram = readbuff(SourceType::c_sock);//每次全部读取
|
||||
if(!datagram.isEmpty())
|
||||
{
|
||||
Mavlinkparse(SourceType::c_sock,datagram);
|
||||
}
|
||||
|
||||
//解码从串口来的
|
||||
datagram.clear();
|
||||
datagram = readbuff(SourceType::s_port);//每次全部读取
|
||||
//qDebug() << "serial port parse";
|
||||
if(!datagram.isEmpty())
|
||||
{
|
||||
//qDebug() << "serial port parse";
|
||||
Mavlinkparse(SourceType::s_port,datagram);
|
||||
}
|
||||
|
||||
}
|
||||
//退出线程
|
||||
Nodethread->quit();
|
||||
}
|
||||
|
||||
void MavLinkNode::initbuff(void)
|
||||
{
|
||||
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 SourceType::c_sock:
|
||||
//当前的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);
|
||||
//qDebug() << "set buff";
|
||||
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();
|
||||
}
|
||||
serial_buff.buff[serial_buff.select].append(data);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
QByteArray MavLinkNode::readbuff(quint32 src)
|
||||
{
|
||||
QByteArray datagram;
|
||||
|
||||
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[0],client_buff.buff[0].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[0],serial_buff.buff[0].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); //接收完一帧数据并处理
|
||||
}
|
||||
else
|
||||
{
|
||||
switch(status.parse_state)
|
||||
{
|
||||
case MAVLINK_PARSE_STATE_UNINIT:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_UNINIT";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_IDLE:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_IDLE";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_GOT_STX:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_GOT_STX";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_GOT_LENGTH:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_GOT_LENGTH";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_GOT_SEQ:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_GOT_SEQ";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_GOT_SYSID:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_GOT_SYSID";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_GOT_COMPID:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_GOT_COMPID";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_GOT_MSGID1:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_GOT_MSGID1";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_GOT_MSGID2:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_GOT_MSGID2";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_GOT_MSGID3:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_GOT_MSGID3";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_GOT_PAYLOAD:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_GOT_PAYLOAD";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_GOT_CRC1:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_GOT_CRC1";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_GOT_BAD_CRC1:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_GOT_BAD_CRC1";
|
||||
}break;
|
||||
case MAVLINK_PARSE_STATE_SIGNATURE_WAIT:{
|
||||
qDebug() << "MAVLINK_PARSE_STATE_SIGNATURE_WAIT";
|
||||
}break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void MavLinkNode::MAVLinkRcv_Handler(mavlink_message_t msg)
|
||||
{
|
||||
switch (msg.msgid) {
|
||||
//状态
|
||||
case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
|
||||
case MAVLINK_MSG_ID_SYS_STATUS:
|
||||
case MAVLINK_MSG_ID_HEARTBEAT:
|
||||
case MAVLINK_MSG_ID_PING:
|
||||
case MAVLINK_MSG_ID_ATTITUDE:
|
||||
case MAVLINK_MSG_ID_GPS_RAW_INT:
|
||||
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
|
||||
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
|
||||
case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
|
||||
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
|
||||
case MAVLINK_MSG_ID_AIRSPEED_AUTOCAL:
|
||||
case MAVLINK_MSG_ID_RPM:
|
||||
case MAVLINK_MSG_ID_SCALED_PRESSURE:
|
||||
case MAVLINK_MSG_ID_EXTENDED_SYS_STATE:
|
||||
case MAVLINK_MSG_ID_BATTERY_STATUS:
|
||||
case MAVLINK_MSG_ID_VIBRATION:
|
||||
case MAVLINK_MSG_ID_EngineState:
|
||||
case MAVLINK_MSG_ID_VFR_HUD:
|
||||
StatusParse(msg);
|
||||
break;
|
||||
|
||||
//航线部分
|
||||
case MAVLINK_MSG_ID_MISSION_COUNT:
|
||||
case MAVLINK_MSG_ID_MISSION_CURRENT:
|
||||
//case MAVLINK_MSG_ID_MISSION_ITEM:
|
||||
case MAVLINK_MSG_ID_MISSION_ITEM_INT:
|
||||
//case MAVLINK_MSG_ID_MISSION_REQUEST:
|
||||
case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
|
||||
case MAVLINK_MSG_ID_MISSION_ACK:
|
||||
MissionParse(msg);
|
||||
break;
|
||||
//参数
|
||||
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
|
||||
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
|
||||
case MAVLINK_MSG_ID_PARAM_SET:
|
||||
case MAVLINK_MSG_ID_PARAM_VALUE:
|
||||
ParamParse(msg);
|
||||
break;
|
||||
|
||||
//命令
|
||||
case MAVLINK_MSG_ID_COMMAND_ACK:
|
||||
CommandParse(msg);
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void MavLinkNode::StatusParse(mavlink_message_t msg)
|
||||
{
|
||||
switch (msg.msgid) {
|
||||
case MAVLINK_MSG_ID_AUTOPILOT_VERSION: {
|
||||
mavlink_msg_autopilot_version_decode(&msg,&vehicle.autopilot_version);
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_SYS_STATUS: {
|
||||
mavlink_msg_sys_status_decode(&msg,&vehicle.sys_status);
|
||||
}break;
|
||||
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);
|
||||
emit state_updated();
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_GPS_RAW_INT: {
|
||||
mavlink_msg_gps_raw_int_decode(&msg,&vehicle.gps_raw_int);
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
|
||||
mavlink_msg_global_position_int_decode(&msg,&vehicle.global_position_int);
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: {
|
||||
mavlink_msg_servo_output_raw_decode(&msg,&vehicle.servo_output_raw);
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: {
|
||||
mavlink_msg_rc_channels_raw_decode(&msg,&vehicle.rc_channels_raw);
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: {
|
||||
mavlink_msg_nav_controller_output_decode(&msg,&vehicle.nav_controller_output);
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_AIRSPEED_AUTOCAL: {
|
||||
mavlink_msg_airspeed_autocal_decode(&msg,&vehicle.airspeed_autocal);
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_RPM: {
|
||||
mavlink_msg_rpm_decode(&msg,&vehicle.rpm);
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_SCALED_PRESSURE: {
|
||||
mavlink_msg_scaled_pressure_decode(&msg,&vehicle.scaled_pressure);
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_EXTENDED_SYS_STATE: {
|
||||
mavlink_msg_extended_sys_state_decode(&msg,&vehicle.extended_sys_state);
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_BATTERY_STATUS: {
|
||||
mavlink_msg_battery_status_decode(&msg,&vehicle.battery_status);
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_VIBRATION: {
|
||||
mavlink_msg_vibration_decode(&msg,&vehicle.vibration);
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_EngineState: {
|
||||
mavlink_msg_enginestate_decode(&msg,&vehicle.enginestate);
|
||||
}break;
|
||||
case MAVLINK_MSG_ID_VFR_HUD: {
|
||||
mavlink_msg_vfr_hud_decode(&msg,&vehicle.vfr_hud);
|
||||
}break;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
void MavLinkNode::ParamParse(mavlink_message_t msg)
|
||||
{
|
||||
switch (msg.msgid) {
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
void MavLinkNode::CommandParse(mavlink_message_t msg)
|
||||
{
|
||||
switch (msg.msgid) {
|
||||
case MAVLINK_MSG_ID_COMMAND_ACK: {
|
||||
mavlink_command_ack_t ack;
|
||||
mavlink_msg_command_ack_decode(&msg,&ack);
|
||||
}break;
|
||||
}
|
||||
}
|
||||
|
||||
void MavLinkNode::MissionParse(mavlink_message_t msg)
|
||||
{
|
||||
switch (msg.msgid) {
|
||||
//航线部分
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user