#include "statusprocess.h"

//留出一个接口给界面去注册指令
//收到ACK后发送一个信号

statusprocess::statusprocess(QObject *parent) : ThreadTemplet(parent)
{
    setRunFrq(1);
    status.m_Mode = _modetype::Nop_Mode;
}

statusprocess::~statusprocess()
{
    qDebug() << "stop status" << QThread::currentThreadId();
     status.m_Mode = _modetype::Nop_Mode;

}


void statusprocess::process()//线程函数
{
    uint8_t count = 0;
    static int time = 0;

    QThread::msleep(5000);//5s后再发送

    while (true)
    {
         count ++;


         switch(status.m_Mode)
         {
            default:
            case Nop_Mode     : QThread::msleep(1000.0/frq());break;
            case RecieveMode  : ReadStateMachine();break;
            case TransmitMode :
             {
                 if((QTime::currentTime().msecsSinceStartOfDay() - time) > (1000.0/running_frq))
                 {
                     heartbeat(m_heartbeat.custom_mode,
                               m_heartbeat.type,
                               m_heartbeat.autopilot,
                               m_heartbeat.base_mode,
                               m_heartbeat.system_status,
                               m_heartbeat.mavlink_version);
                     time = QTime::currentTime().msecsSinceStartOfDay();
                 }
                 else
                 {
                     QThread::msleep(1000.0/frq());
                     //QThread::yieldCurrentThread();
                 }
             }
         }

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

    }
}

//发送函数
void statusprocess::setHeartbeat(QVariant state,QVariant frq)
{
     //给指令赋值

     //开启线程开始传输
     status.transmit.type = 0;

     if(state.toBool() == true)
     {
          status.m_Mode = _modetype::TransmitMode;//发送模式
     }
     else
     {
          status.m_Mode = _modetype::Nop_Mode;//空闲模式
     }

     setRunFrq(frq.toFloat());
}




//读状态机
void statusprocess::ReadStateMachine(void)//没有读指令这个说法
{

}

//写状态机
void statusprocess::WriteStateMachine(void)
{


}


/*all msg
  heartbeat
*/



void statusprocess::heartbeat(uint32_t custom_mode,uint8_t type,uint8_t autopilot,uint8_t base_mode,uint8_t system_status,uint8_t mavlink_version)
{
    static mavlink_message_t msg;
    static mavlink_heartbeat_t heartbeat;

    heartbeat.custom_mode = custom_mode;
    heartbeat.type = type;

    heartbeat.autopilot = autopilot;
    heartbeat.base_mode = base_mode;
    heartbeat.system_status = system_status;
    heartbeat.mavlink_version = mavlink_version;

    mavlink_msg_heartbeat_encode(GCS_SysID,GCS_CompID, &msg,&heartbeat);
    Send(msg);
}