706 lines
20 KiB
C++
706 lines
20 KiB
C++
#include "mavlinknode.h"
|
|
|
|
/*
|
|
* mavlink 解析相关内容请参考如下
|
|
* https://mavlink.io/en/services/command.html
|
|
* 包含了各个函数、参数、状态机等及其说明
|
|
**/
|
|
|
|
|
|
MavLinkNode::MavLinkNode(QObject *parent) : QObject(parent)
|
|
{
|
|
QDir *temp = new QDir;
|
|
if(!temp->exists("./Tlog"))
|
|
{
|
|
qDebug() << "make dir tlog";
|
|
temp->mkdir("./Tlog");//如果文件夹不存在就新建
|
|
}
|
|
|
|
running_flag = false;
|
|
thread = new QThread();
|
|
this->moveToThread(thread);
|
|
connect(thread, &QThread::started, this, &MavLinkNode::process);
|
|
setRunFrq(10);//50
|
|
//初始化buff
|
|
initbuff();
|
|
|
|
//初始化ID
|
|
int Current_sysID = 0xFB;
|
|
int Current_CompID = MAV_COMP_ID_MISSIONPLANNER;
|
|
|
|
CommucationOverTimer = 1000;
|
|
|
|
timer = new QTimer();
|
|
//timer->moveToThread(thread);
|
|
timer->setInterval(CommucationOverTimer);
|
|
|
|
//connect(thread, SIGNAL(started()), timer, SLOT(start()),Qt::DirectConnection);
|
|
connect(timer,&QTimer::timeout,
|
|
this,&MavLinkNode::TimerOut,Qt::DirectConnection);
|
|
timer->start();
|
|
|
|
qDebug() << "start timer";
|
|
|
|
hasConneted = false;
|
|
//comm
|
|
|
|
replay = new Replay();
|
|
connect(replay,SIGNAL(readReady()),
|
|
this,SLOT(readPendingDatagramsReplay()),Qt::DirectConnection);
|
|
|
|
|
|
Mission = new MissionProcess();
|
|
Mission->setGCSID(Current_sysID,Current_CompID);
|
|
connect(Mission,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
|
|
this,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),Qt::DirectConnection);
|
|
|
|
connect(this,SIGNAL(setCurrentID(int,int)),
|
|
Mission,SLOT(setID(int,int)),Qt::DirectConnection);
|
|
|
|
connect(Mission,SIGNAL(showMessage(QString,int)),
|
|
this,SIGNAL(showMessage(QString,int)),Qt::DirectConnection);
|
|
|
|
Parameter = new ParameterProcess();
|
|
Parameter->setGCSID(Current_sysID,Current_CompID);
|
|
connect(Parameter,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
|
|
this,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),Qt::DirectConnection);
|
|
|
|
connect(Parameter,SIGNAL(showMessage(QString,int)),
|
|
this,SIGNAL(showMessage(QString,int)),Qt::DirectConnection);
|
|
|
|
Commander = new commandprocess();
|
|
Commander->setGCSID(Current_sysID,Current_CompID);
|
|
connect(Commander,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
|
|
this,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),Qt::DirectConnection);
|
|
|
|
connect(this,SIGNAL(setCurrentID(int,int)),
|
|
Commander,SLOT(setID(int,int)),Qt::DirectConnection);
|
|
|
|
connect(Commander,SIGNAL(showMessage(QString,int)),
|
|
this,SIGNAL(showMessage(QString,int)),Qt::DirectConnection);
|
|
|
|
isCommunicationLost = false;
|
|
|
|
//showMessage(tr("解锁才能有航迹，需要修正,示波器显示时间不对"));
|
|
}
|
|
|
|
MavLinkNode::~MavLinkNode()
|
|
{
|
|
//关闭文件
|
|
if (mavLogFile)
|
|
{
|
|
mavLogFile->close();
|
|
delete mavLogFile;
|
|
mavLogFile = NULL;
|
|
}
|
|
|
|
//停止回放
|
|
if(replay)
|
|
{
|
|
replay->stop();
|
|
delete replay;
|
|
replay = nullptr;
|
|
}
|
|
|
|
//停止任务
|
|
if(Mission)
|
|
{
|
|
if(Mission->isActive())
|
|
{
|
|
Mission->stop();
|
|
delete Mission;
|
|
Mission = nullptr;
|
|
}
|
|
}
|
|
|
|
//停止参数
|
|
if(Parameter)
|
|
{
|
|
if(Parameter->isActive())
|
|
{
|
|
Parameter->stop();
|
|
delete Parameter;
|
|
Parameter = nullptr;
|
|
}
|
|
}
|
|
|
|
if(Commander)
|
|
{
|
|
if(Commander->isActive())
|
|
{
|
|
Commander->stop();//可能还没停止线程，后面不能紧跟着删除
|
|
delete Commander;
|
|
Commander = nullptr;
|
|
}
|
|
}
|
|
|
|
if(timer)
|
|
{
|
|
timer->stop();
|
|
delete timer;
|
|
}
|
|
|
|
|
|
if(thread->isRunning())
|
|
{
|
|
thread->quit();
|
|
thread->wait();
|
|
}
|
|
|
|
delete thread;
|
|
thread = nullptr;
|
|
|
|
}
|
|
|
|
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(!thread->isRunning())
|
|
{
|
|
if (mavLogFile)
|
|
{
|
|
mavLogFile->close();
|
|
delete mavLogFile;
|
|
}
|
|
|
|
QDateTime current = QDateTime::currentDateTime();
|
|
mavLogFile = new QFile(QString("./Tlog/%1.tlog").arg(current.toString("yyyyMMddHHmmss")));
|
|
mavLogFile->open(QIODevice::WriteOnly);
|
|
|
|
//初始化buff
|
|
initbuff();
|
|
|
|
running_flag = true;
|
|
thread->start();
|
|
qDebug() << "MavLinkNode thread start" << running_flag;
|
|
|
|
//timer->start();
|
|
|
|
|
|
if(replay)
|
|
{
|
|
replay->start();
|
|
}
|
|
|
|
if(Mission)
|
|
{
|
|
Mission->start();
|
|
}
|
|
|
|
if(Commander)
|
|
{
|
|
Commander->start();
|
|
}
|
|
|
|
if(Parameter)
|
|
{
|
|
Parameter->start();
|
|
}
|
|
}
|
|
else
|
|
{
|
|
qDebug() << "MavLinkNode thread has started";
|
|
}
|
|
}
|
|
|
|
|
|
void MavLinkNode::stop()
|
|
{
|
|
if(thread->isRunning())
|
|
{
|
|
if (mavLogFile)
|
|
{
|
|
mavLogFile->close();
|
|
delete mavLogFile;
|
|
mavLogFile = NULL;
|
|
}
|
|
|
|
running_flag = false;
|
|
qDebug() << "thread stop" << running_flag;
|
|
}
|
|
else
|
|
{
|
|
qDebug() << "thread is not running";
|
|
}
|
|
}
|
|
|
|
//这里一直在解码，一直检查双缓冲里面是否有数据，有就解码，没有就休息
|
|
void MavLinkNode::process()//线程函数
|
|
{
|
|
uint8_t count = 0;
|
|
QByteArray datagram = nullptr;
|
|
while (running_flag)
|
|
{
|
|
count ++;
|
|
QThread::msleep(1000/running_frq);
|
|
|
|
//timer->start();
|
|
|
|
//解码从UDP来的
|
|
datagram.clear();
|
|
datagram = readbuff(SourceType::c_sock);//每次全部读取
|
|
if(!datagram.isEmpty())
|
|
{
|
|
//qDebug() << "client parse";
|
|
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);
|
|
}
|
|
|
|
}
|
|
running_flag = false;
|
|
//退出线程
|
|
disconnect(thread, nullptr, nullptr, nullptr);
|
|
thread->quit();
|
|
//thread->wait();
|
|
|
|
thread->deleteLater();
|
|
thread = nullptr;
|
|
|
|
}
|
|
|
|
void MavLinkNode::TimerOut(void)
|
|
{
|
|
//qDebug() << "timeout" << "communication lost";
|
|
|
|
if((parserSuccess + parserFailure) > 0)
|
|
{
|
|
rssi = (float)(parserSuccess * 100.0f) / (parserSuccess + parserFailure);
|
|
}
|
|
else
|
|
{
|
|
rssi = 0;
|
|
}
|
|
|
|
if(hasConneted)
|
|
{
|
|
CommucationOverCount --;
|
|
|
|
if(CommucationOverCount <= 0)
|
|
{
|
|
CommucationOverCount = 0;
|
|
isCommunicationLost = true;
|
|
emit CommuniationLost(isCommunicationLost);
|
|
}
|
|
else
|
|
{
|
|
emit CommuniationLost(isCommunicationLost);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
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)
|
|
{
|
|
static qint64 last = QDateTime::currentMSecsSinceEpoch();
|
|
|
|
bitcount += data.size();
|
|
|
|
if((QDateTime::currentMSecsSinceEpoch() - last) >= 1000)
|
|
{
|
|
bitrate = bitcount;
|
|
bittotal += bitcount;
|
|
|
|
bitcount = 0;
|
|
|
|
last = QDateTime::currentMSecsSinceEpoch();
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
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();
|
|
qDebug() << "client_buff.buff " << client_buff.select <<" OVERFLOW";
|
|
}
|
|
client_buff.buff[client_buff.select].append(data);
|
|
|
|
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 " << serial_buff.select <<" OVERFLOW";
|
|
}
|
|
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.clear();
|
|
datagram.append(client_buff.buff[1]);
|
|
//datagram.setRawData(client_buff.buff[1],client_buff.buff[1].size());
|
|
//清除这个未选择的buff
|
|
client_buff.buff[1].clear();
|
|
//读取完成，可以往这个内存里面写数了
|
|
client_buff.select = 1;
|
|
}
|
|
else if(client_buff.select == 1)
|
|
{
|
|
datagram.clear();
|
|
datagram.append(client_buff.buff[0]);
|
|
//datagram.setRawData(client_buff.buff[0],client_buff.buff[0].size());
|
|
//清除这个未选择的buff
|
|
client_buff.buff[0].clear();
|
|
//读取完成，可以往这个内存里面写数了
|
|
client_buff.select = 0;
|
|
}
|
|
break;
|
|
case SourceType::s_port:
|
|
if(serial_buff.select == 0)
|
|
{
|
|
datagram.clear();
|
|
datagram.append(serial_buff.buff[1]);
|
|
//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.clear();
|
|
datagram.append(serial_buff.buff[0]);
|
|
//datagram.setRawData(serial_buff.buff[0],serial_buff.buff[0].size());
|
|
//读取完成，可以往这个内存里面写数了
|
|
serial_buff.buff[0].clear();
|
|
serial_buff.select = 0;
|
|
}
|
|
break;
|
|
}
|
|
return datagram;
|
|
}
|
|
|
|
|
|
void MavLinkNode::Mavlinkparse(quint32 src,QByteArray datagram)
|
|
{
|
|
static int count = 0;
|
|
mavlink_message_t msg;
|
|
mavlink_status_t status;
|
|
|
|
hasConneted = true;
|
|
|
|
for (QByteArray::const_iterator i = datagram.cbegin(); i != datagram.cend(); ++i)
|
|
{
|
|
|
|
|
|
if(MAVLINK_FRAMING_OK == mavlink_parse_char(src,*i,&msg,&status))
|
|
{
|
|
parserSuccess += 1;
|
|
|
|
uint8_t buff[MAVLINK_MAX_PACKET_LEN+sizeof(quint64)];
|
|
quint64 currentTimestamp = ((quint64)QDateTime::currentMSecsSinceEpoch()) * 1000;
|
|
qToBigEndian(currentTimestamp, buff);
|
|
uint16_t len = mavlink_msg_to_send_buffer(buff+sizeof(quint64), &msg);
|
|
if (mavLogFile)
|
|
{
|
|
mavLogFile->write((const char *)buff, len+sizeof(quint64));
|
|
}
|
|
|
|
count++;
|
|
if(msg.sysid < 250) //过滤地面站发过来的数据
|
|
{
|
|
//timer->start();
|
|
|
|
CommucationOverCount = 5;
|
|
isCommunicationLost = false;
|
|
|
|
//emit CommuniationLost(isCommunicationLost);
|
|
|
|
MAVLinkRcv_Handler(msg); //接收完一帧数据并处理
|
|
emit recievemsg(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:{
|
|
parserFailure += 1;
|
|
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)
|
|
{
|
|
//用于给参数添加设备，便于读取
|
|
|
|
CheckVehicle(msg.sysid,msg.compid);
|
|
|
|
vehicle.sysid = msg.sysid;
|
|
vehicle.compid = msg.compid;
|
|
|
|
switch (msg.msgid) {
|
|
//航线部分
|
|
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
|
|
case MAVLINK_MSG_ID_MISSION_COUNT:
|
|
case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
|
|
case MAVLINK_MSG_ID_MISSION_REQUEST:
|
|
case MAVLINK_MSG_ID_MISSION_ITEM_INT:
|
|
case MAVLINK_MSG_ID_MISSION_ITEM:
|
|
case MAVLINK_MSG_ID_MISSION_ACK:
|
|
case MAVLINK_MSG_ID_MISSION_CURRENT:
|
|
case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
|
|
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
|
|
case MAVLINK_MSG_ID_MISSION_ITEM_REACHED:
|
|
case MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST:
|
|
case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST:
|
|
Mission->Parse(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:
|
|
Parameter->Parse(msg);
|
|
break;
|
|
|
|
//命令
|
|
case MAVLINK_MSG_ID_COMMAND_INT:
|
|
case MAVLINK_MSG_ID_COMMAND_LONG:
|
|
case MAVLINK_MSG_ID_COMMAND_ACK:
|
|
Commander->Parse(msg);
|
|
break;
|
|
|
|
//状态
|
|
default:
|
|
StatusParse(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);
|
|
//qDebug() << "recieve heartbeat";
|
|
emit beep();
|
|
}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_INS1: {
|
|
mavlink_msg_ins1_decode(&msg,&vehicle.ins1);
|
|
}break;
|
|
case MAVLINK_MSG_ID_INS2: {
|
|
mavlink_msg_ins2_decode(&msg,&vehicle.ins2);
|
|
}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;
|
|
case MAVLINK_MSG_ID_EMB_ATMO_COM: {
|
|
mavlink_msg_emb_atmo_com_decode(&msg,&vehicle.emb_atom_com);
|
|
}break;
|
|
case MAVLINK_MSG_ID_TurbineState: {
|
|
mavlink_msg_turbinestate_decode(&msg,&vehicle.turbinstate);
|
|
}break;
|
|
case MAVLINK_MSG_ID_BMUState: {
|
|
mavlink_msg_bmustate_decode(&msg,&vehicle.bmustate);
|
|
}break;
|
|
case MAVLINK_MSG_ID_CCMState: {
|
|
mavlink_msg_ccmstate_decode(&msg,&vehicle.ccmstate);
|
|
}break;
|
|
}
|
|
|
|
emit state_updated();
|
|
|
|
}
|
|
|
|
|
|
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::CheckVehicle(int sysid,int compid)
|
|
{
|
|
if(!vehicleList.contains(sysid))
|
|
{
|
|
vehicleList.insert(sysid,compid);
|
|
|
|
emit addVehicles(sysid,compid);
|
|
|
|
for (QHash<int,int>::iterator i = vehicleList.begin();i!= vehicleList.end();++i)
|
|
{
|
|
qDebug() << "add vehicle" << i.key() << i.value();
|
|
}
|
|
}
|
|
}
|
|
|
|
void MavLinkNode::setCurrentSelected(int sysid,int compid)
|
|
{
|
|
//qDebug() << "CurrentSelected" << sysid << compid;
|
|
|
|
Current_sysID = sysid;
|
|
Current_CompID = compid;
|
|
|
|
emit setCurrentID(sysid,compid);
|
|
|
|
}
|
|
|
|
void MavLinkNode::readPendingDatagramsReplay(void)
|
|
{
|
|
if(replay)
|
|
{
|
|
QByteArray datagram = replay->readAll();
|
|
setbuff(SourceType::c_sock,datagram);
|
|
}
|
|
}
|
|
|
|
void MavLinkNode::setLogfile(QString file)
|
|
{
|
|
if(replay)
|
|
{
|
|
replay->setLogfile(file);
|
|
}
|
|
}
|