gcs-nf/MavLinkNode/mavlinknode.cpp

#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;

    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);


    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(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;


        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);

         //解码从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();
}

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();
               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;

    for (QByteArray::const_iterator i = datagram.cbegin(); i != datagram.cend(); ++i)
    {
        if(MAVLINK_FRAMING_OK == mavlink_parse_char(src,*i,&msg,&status))
        {
            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)             //过滤地面站发过来的数据
            {
                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:{
                 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_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_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;

        }
}

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_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;

        }

        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);
    }
}