gcs-nf/MavLinkNode/commandprocess.cpp

#include "commandprocess.h"


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

commandprocess::commandprocess(QObject *parent) : QObject(parent)
{
    setRunFrq(500);
}


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

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

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

void commandprocess::stop()
{
    if(thread->isRunning())
    {
        running_flag = false;
        qDebug() << "thread stop" << running_flag;
    }
    else
    {
        qDebug() << "thread is not running";
    }
}

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

    while (running_flag)
    {
         count ++;
         QThread::msleep(1000/running_frq);

         switch(status.m_Mode)
         {
            default:
            case Nop_Mode     :break;
            case RecieveMode  : ReadStateMachine();break;
            case TransmitMode : WriteStateMachine();break;
         }

    }
    //退出线程
    thread->quit();
    thread->wait(200);//等待结束
    thread->deleteLater();
    thread = nullptr;
}

void commandprocess::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 commandprocess::WriteCmd(uint8_t m_sysid, uint8_t m_compid ,
                              float param1,
                              float param2,
                              float param3,
                              float param4,
                              int32_t x,
                              int32_t y,
                              float z, uint16_t command, uint8_t frame, uint8_t current, uint8_t autocontinue)
{
     sysid = m_sysid;
     compid = m_compid;

     //给指令赋值
     cmd_int.param1 = param1;
     cmd_int.param2 = param2;
     cmd_int.param3 = param3;
     cmd_int.param4 = param4;
     cmd_int.x = x;
     cmd_int.y = y;
     cmd_int.z = z;

     cmd_int.command = command;
     cmd_int.frame = frame;
     cmd_int.current = current;
     cmd_int.autocontinue = autocontinue;

     //开启线程开始传输

     status.m_Mode = TransmitMode;//发送模式

     start();//开启线程

}









//这个函数类似中断，专门处理接收到的状态
void commandprocess::Parse(mavlink_message_t msg)
{
    mavlink_command_int_t command_int;
    mavlink_command_long_t command_long;
    mavlink_command_ack_t command_ack;

    switch (msg.msgid) {
    case MAVLINK_MSG_ID_COMMAND_INT://几乎不可能收到
         mavlink_msg_command_int_decode(&msg,&command_int);
    break;
    case MAVLINK_MSG_ID_COMMAND_LONG://几乎不可能收到
         mavlink_msg_command_long_decode(&msg,&command_long);
    break;
    case MAVLINK_MSG_ID_COMMAND_ACK:
         mavlink_msg_command_ack_decode(&msg,&command_ack);

         if(command_ack.result == MAV_RESULT_ACCEPTED)
         {
             status.transmit.isWaitingforACK = false;
         }
         else
         {
             //拒绝该指令
         }
    break;
    }
}

//读状态机
void commandprocess::ReadStateMachine(void)//没有读指令这个说法
{
   static uint8_t step = 0;
   //static uint8_t timeout_count = 0;
   static int time = 0;


   if(step == 0)
   {
       qDebug() << "start reading parameter";
       status.recieve.isWaitingforValue = true;

       //request_list();

       step++;
       time = QTime::currentTime().msecsSinceStartOfDay();
   }

}

//写状态机
void commandprocess::WriteStateMachine(void)
{
   static uint8_t step = 0;
   static uint8_t timeout_count = 0;
   static int time = 0;

   if(step == 0)
   {
       status.transmit.isWaitingforACK = true;

       _int(cmd_int.param1,cmd_int.param2,cmd_int.param3,cmd_int.param4,
            cmd_int.x,cmd_int.y,cmd_int.z,
            cmd_int.command,cmd_int.frame,cmd_int.current,cmd_int.autocontinue);//发送一个指令
       time = QTime::currentTime().msecsSinceStartOfDay();
       step++;//下一个阶段
   }
   else if(step == 1)//等待返回
   {
       if((QTime::currentTime().msecsSinceStartOfDay() - time) > 1000)
       {
          timeout_count ++;
          step = 0;//返回上一个步骤
          if(timeout_count > 5)//如果指令发5次还没发出去，那么结束发送
          {
             qDebug() << "command set value fail";
             step = 0;
             status.m_Mode = Nop_Mode;
             status.transmit.isWaitingforACK = false;
             timeout_count = 0;

             //清除指令
             cmd_int.param1 = 0;
             cmd_int.param2 = 0;
             cmd_int.param3 = 0;
             cmd_int.param4 = 0;
             cmd_int.x = 0;
             cmd_int.y = 0;
             cmd_int.z = 0;
             cmd_int.command = 0;
             cmd_int.frame = 0;
             cmd_int.current = 0;
             cmd_int.autocontinue = 0;

          }
       }
       else
       {
           //收到Value
           if(status.transmit.isWaitingforACK == false)
           {
               step = 0;
               status.m_Mode = Nop_Mode;
               status.transmit.isWaitingforACK = false;
               timeout_count = 0;

               //清除指令
               cmd_int.param1 = 0;
               cmd_int.param2 = 0;
               cmd_int.param3 = 0;
               cmd_int.param4 = 0;
               cmd_int.x = 0;
               cmd_int.y = 0;
               cmd_int.z = 0;
               cmd_int.command = 0;
               cmd_int.frame = 0;
               cmd_int.current = 0;
               cmd_int.autocontinue = 0;
           }
       }
   }

}


/*all msg
  MAVLINK_MSG_ID_COMMAND_INT
  MAVLINK_MSG_ID_COMMAND_LONG
  MAVLINK_MSG_ID_COMMAND_ACK
*/


void commandprocess::_int(float param1,
                          float param2,
                          float param3,
                          float param4,
                          int32_t x,
                          int32_t y,
                          float z,
                          uint16_t command,
                          uint8_t frame,
                          uint8_t current,
                          uint8_t autocontinue)
{
    static mavlink_message_t msg;
    static mavlink_command_int_t command_int;

    command_int.target_system = sysid;
    command_int.target_component = compid;


    command_int.param1 = param1;
    command_int.param2 = param2;
    command_int.param3 = param3;
    command_int.param4 = param4;
    command_int.x = x;
    command_int.y = y;
    command_int.z = z;
    command_int.command = command;
    command_int.frame = frame;
    command_int.current = current;
    command_int.autocontinue = autocontinue;

    mavlink_msg_command_int_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&command_int);
    SendMessage(msg);
}

void commandprocess::_long(float param1,
                           float param2,
                           float param3,
                           float param4,
                           float param5,
                           float param6,
                           float param7,
                           uint16_t command,
                           uint8_t confirmation)
{
    static mavlink_message_t msg;
    static mavlink_command_long_t command_long;

    command_long.target_system = sysid;
    command_long.target_component = compid;

    command_long.param1 = param1;
    command_long.param2 = param2;
    command_long.param3 = param3;
    command_long.param4 = param4;
    command_long.param5 = param5;
    command_long.param6 = param6;
    command_long.param7 = param7;
    command_long.command = command;
    command_long.confirmation = confirmation;

    mavlink_msg_command_long_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&command_long);
    SendMessage(msg);
}


/*
   MAV_RESULT_ACCEPTED=0,
   MAV_RESULT_TEMPORARILY_REJECTED=1,
   MAV_RESULT_DENIED=2,
   MAV_RESULT_UNSUPPORTED=3,
   MAV_RESULT_FAILED=4,
   MAV_RESULT_IN_PROGRESS=5,
 **/


void commandprocess::ack(uint16_t command,uint8_t result,uint8_t progress,int32_t result_param2)//地面站几乎不可能放这个函数
{
    static mavlink_message_t msg;
    static mavlink_command_ack_t command_ack;

    command_ack.target_system = sysid;
    command_ack.target_component = compid;

    command_ack.command = command;
    command_ack.result = result;
    command_ack.progress = progress;
    command_ack.result_param2 = result_param2;

    mavlink_msg_command_ack_encode(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&command_ack);
    SendMessage(msg);
}