gcs-nf/MavLinkNode/statusprocess.cpp

#include "statusprocess.h"

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

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

    QDateTime current = QDateTime::currentDateTime();

    HeartBeatTimeName = QString("./log/other/HeartBeat%1.csv").arg(current.toString("yyyyMMddHHmmss"));
    HeartBeatTimeFile = new QFile(HeartBeatTimeName);
    HeartBeatTimeFile->open(QIODevice::WriteOnly);



    if(HeartBeatTimeFile)
    {
        QString data;

        data.clear();
        data.append("date");                 data.append(",");
        data.append("year,mon,day,hour,min,sec,ms,");data.append(",");
        data.append("elapse");               data.append(",");
        data.append("custom_mode ");         data.append(",");
        data.append("type ");                data.append(",");
        data.append("autopilot ");           data.append(",");
        data.append("base_mode ");           data.append(",");
        data.append("system_status ");       data.append(",");
        data.append("mavlink_version ");     data.append("\n");

        QTextStream stream(HeartBeatTimeFile);
        stream << data;
        HeartBeatTimeFile->flush();
    }


    timer = new QTimer();

    timer->setInterval(1000);
    connect(timer,&QTimer::timeout,[=]{
       // qDebug() << "heart time out";
       emitSignal = true;
       status.m_Mode = TransmitMode;

    });



}

statusprocess::~statusprocess()
{
    if (HeartBeatTimeFile)
    {
        HeartBeatTimeFile->close();
        delete HeartBeatTimeFile;
        HeartBeatTimeFile = NULL;
    }

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

}


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

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

    //static int time = QTime::currentTime().msecsSinceStartOfDay();

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

    while (true)
    {
         count ++;



         switch(status.m_Mode)
         {
            default:
            case Nop_Mode     : QThread::msleep(10);break;
            case RecieveMode  : ReadStateMachine();break;//QApplication::processEvents();break;
            case TransmitMode :
             {
                 if(emitSignal)
                 {
                     heartbeat(m_heartbeat.custom_mode,
                               m_heartbeat.type,
                               m_heartbeat.autopilot,
                               m_heartbeat.base_mode,
                               m_heartbeat.system_status,
                               m_heartbeat.mavlink_version);


                     emit heartbeatTimeStamp(QTime::currentTime().msecsSinceStartOfDay() - timeCount);

                     //qDebug() << "time:" << QTime::currentTime().msecsSinceStartOfDay() - time;


                     if(HeartBeatTimeFile)
                     {
                         QString data;

                         data.clear();

                         QDateTime time = QDateTime::currentDateTime();

                         data.append(QString::number(time.date().year()));         data.append(",");
                         data.append(QString::number(time.date().month()));        data.append(",");
                         data.append(QString::number(time.date().day()));          data.append(",");
                         data.append(QString::number(time.time().hour()));         data.append(",");
                         data.append(QString::number(time.time().minute()));       data.append(",");
                         data.append(QString::number(time.time().second()));       data.append(",");
                         data.append(QString::number(time.time().msec()));         data.append(",");


                         data.append(QDateTime::currentDateTimeUtc().toString("yyyy.MM.dd HH:mm:ss:zzz"));            data.append(",");
                         data.append(QString::number(QTime::currentTime().msecsSinceStartOfDay() - timeCount));   data.append(",");
                         data.append(QString::number(m_heartbeat.custom_mode));    data.append(",");
                         data.append(QString::number(m_heartbeat.type));           data.append(",");
                         data.append(QString::number(m_heartbeat.autopilot));      data.append(",");
                         data.append(QString::number(m_heartbeat.base_mode));      data.append(",");
                         data.append(QString::number(m_heartbeat.system_status));  data.append(",");
                         data.append(QString::number(m_heartbeat.mavlink_version));data.append("\n");

                         QTextStream stream(HeartBeatTimeFile);

                         stream << data;

                         HeartBeatTimeFile->flush();

                     }


                     emitSignal = false;
                     status.m_Mode = Nop_Mode;

                     timeCount = QTime::currentTime().msecsSinceStartOfDay();
                 }


                 /*
                 //QApplication::processEvents();
                 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)
{
     //给指令赋值
     setRunFrq(frq.toFloat());
     //开启线程开始传输
     status.transmit.type = 0;

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

          if(timer)
          {
              timer->stop();
              timer->setInterval(1000.0/running_frq);
              timer->start();
              timeCount = QTime::currentTime().msecsSinceStartOfDay();
          }


     }
     else
     {

         if(timer)
         {
             timer->stop();
             emitSignal = false;
         }
          status.m_Mode = _modetype::Nop_Mode;//空闲模式
     }





}


void statusprocess::timeOut(void)
{
    qDebug() << "heart time out";
}

//读状态机
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);
}