gcs-nf/MavLinkNode/Terminal.cpp

#include "Terminal.h"



terminal::terminal(QObject *parent) : QObject(parent)
{
    setRunFrq(20);//默认50Hz频率运行

    status.m_Mode = Nop_Mode;
}


void terminal::setRunFrq(uint32_t frq)
{
    if((frq != 0)||(frq <= 1000))
    {
        running_frq = frq;
        qDebug() << "set mission thread running frquency:" <<frq <<"Hz";
    }
}

void terminal::start()
{
    if(thread == nullptr)
    {
        thread = new QThread();
        this->moveToThread(thread);
        connect(thread, &QThread::started, this, &terminal::process);
    }

    if(!thread->isRunning())
    {
        running_flag = true;
        thread->start();
        qDebug() << "thread start" << running_flag;
    }
    else
    {
        qDebug() << "thread has started";
    }
}

void terminal::stop()
{
    if(thread->isRunning())
    {
        running_flag = false;
        qDebug() << "thread stop"
                 << QThread::currentThreadId()
                 << QThread::currentThread()
                 << "running state:"
                 << running_flag;

        disconnect(thread, nullptr, nullptr, nullptr);
    }
    else
    {
        qDebug() << "thread is not running";
    }
}

void terminal::process()//线程函数
{
    uint8_t count = 0;
    while (running_flag)
    {
         count ++;
         //QThread::msleep(1000.0/running_frq);
         switch(status.m_Mode)
         {
            default:
            case Nop_Mode     : QThread::yieldCurrentThread();break;
            case RecieveMode  : ReadStateMachine();QApplication::processEvents();break;
            case TransmitMode : WriteStateMachine();QApplication::processEvents();break;
         }
        //qDebug() << count;
    }
    //退出线程
    disconnect(thread, nullptr, nullptr, nullptr);
    thread->quit();
    //thread->wait();//等待结束
    thread->deleteLater();
    thread = nullptr;
}

void terminal::setID(int m_sysid,int m_compid)
{
    sysid = m_sysid;
    compid = m_compid;
}



void terminal::SendMessage(mavlink_message_t msg)
{
    uint8_t  buff[256+20];
    uint16_t len = mavlink_msg_to_send_buffer(buff, &msg);

    emit SendMessageTo(0,buff, len);//使用信号和槽
}

void terminal::Transmit(QString msg)
{
    if(status.m_Mode == Nop_Mode)//没有任务在上传
    {
        SerialData.clear();
        SerialData.append(msg);
        status.transmit.type = 0;
        status.m_Mode = TransmitMode;//发送模式
        qDebug() << "terminal" << sysid << compid << SerialData;
        start();//开启线程
    }
}





//这个函数类似中断，专门处理接收到的状态
void terminal::Parse(mavlink_message_t msg)
{
    switch (msg.msgid) {
    case MAVLINK_MSG_ID_SERIAL_CONTROL :

       mavlink_serial_control_t serial_control;

       mavlink_msg_serial_control_decode(&msg,&serial_control);

       QByteArray str;//((char *)serial_control.data);

       //str = (char *)serial_control.data;

       str.append((char *)serial_control.data,serial_control.count);



       //str.setRawData((char *)serial_control.data,serial_control.count);

        //memcpy(str,serial_control.data,SerialData.size());

       //qDebug() << str;

       emit Recieve(str);

       //delete str;

    break;

    }

}


//读航线状态机
void terminal::ReadStateMachine(void)
{
   static uint8_t step = 0;
   static uint8_t timeout_count = 0;
   static int time = 0;

   status.m_Mode = _modetype::Nop_Mode;
}

//写航线状态机
void terminal::WriteStateMachine(void)
{

    serial_control();

    status.m_Mode = _modetype::Nop_Mode;

}

void terminal::serial_control(void)
{
   static mavlink_message_t msg;
   static mavlink_serial_control_t serial_control;

   serial_control.baudrate = 115200;
   serial_control.count = SerialData.size();
   //serial_control.data = SerialData.toLatin1();

   memcpy(serial_control.data,SerialData.toLocal8Bit().data(),SerialData.size());

   serial_control.flags = SERIAL_CONTROL_FLAG_EXCLUSIVE | SERIAL_CONTROL_FLAG_RESPOND | SERIAL_CONTROL_FLAG_MULTI;
   serial_control.device = SERIAL_CONTROL_DEV_SHELL;
   serial_control.timeout = 5000;

   mavlink_msg_serial_control_encode(Current_sysID,Current_CompID, &msg,&serial_control);
   SendMessage(msg);
}