gcs-nf/MavLinkNode/Terminal.cpp

#include "Terminal.h"



terminal::terminal(QObject *parent) : ThreadTemplet(parent)
{
    setRunFrq(1);//默认10Hz频率运行

    status.m_Mode = Nop_Mode;
}

terminal::~terminal()
{
    qDebug() << "stop terminal" << QThread::currentThreadId();
}

void terminal::process()//线程函数
{
    QThread::msleep(5000);//5s后再发送
    uint8_t count = 0;
    while (true)
    {
         count ++;

         switch(status.m_Mode)
         {
            default:
            case Nop_Mode     : QThread::msleep(1000/frq());break;
            case RecieveMode  : ReadStateMachine();QApplication::processEvents();break;
            case TransmitMode : WriteStateMachine();QApplication::processEvents();break;
         }

         if(isInterruptionRequested())//退出
         {
             break;
         }
    }
}

void terminal::Transmit(QString msg)
{
    if(status.m_Mode == Nop_Mode)//没有任务在上传
    {
        SerialData.clear();
        SerialData.append(msg);
        status.transmit.type = 0;
        status.m_Mode = TransmitMode;//发送模式
    }
}

//这个函数类似中断，专门处理接收到的状态
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;

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

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

   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(GCS_SysID,GCS_CompID, &msg,&serial_control);
   Send(msg);
}