gcs-nf/MavLinkNode/commandprocess.cpp

#include "commandprocess.h"


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

commandprocess::commandprocess(QObject *parent) : ThreadTemplet(parent)
{
    setRunFrq(10);

    status.m_Mode = Nop_Mode;

}

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

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

     qDebug() << "command" << QThread::currentThreadId();

    while (true)
    {
         //QThread::msleep((status.m_Mode != 0)?(0):(-1));
         count ++;
         switch(status.m_Mode)
         {
            default:
            case Nop_Mode     : QThread::msleep(1000.0/frq());break;
            case RecieveMode  : ReadStateMachine();break;//QApplication::processEvents();break;
            case TransmitMode : WriteStateMachine();break;//QApplication::processEvents();break;
         }

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

//发送函数
void commandprocess::WriteCmd_int(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)
{
    if(status.m_Mode == Nop_Mode)
    {
     //给指令赋值
     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.transmit.type = 0;
     status.m_Mode = TransmitMode;//发送模式

    }
    else
    {
        emit commandAccepted(true,command,0xFF);//广播指令反馈
        //emit showMessage("Please do not operate too fast");
    }

}



void commandprocess::WriteCmd_long(float param1, float param2,  float param3, float param4, float param5, float param6, float param7, uint16_t command, uint8_t confirmation)
{
    if(status.m_Mode == Nop_Mode)
    {
        //给指令赋值
        cmd_long.param1 = param1;
        cmd_long.param2 = param2;
        cmd_long.param3 = param3;
        cmd_long.param4 = param4;
        cmd_long.param5 = param5;
        cmd_long.param6 = param6;
        cmd_long.param7 = param7;

        cmd_long.command = command;
        cmd_long.confirmation = confirmation;

        //开启线程开始传输
        status.m_Mode = TransmitMode;//发送模式
        status.transmit.type = 1;

    }
    else
    {
        emit commandAccepted(true,command,0xFF);//广播指令反馈
        //emit showMessage("Please do not operate too fast");
    }
}

//这个函数类似中断，专门处理接收到的状态
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.target_system != GCS_SysID)
         {
             qDebug() << command_ack.target_system << "this msg is'nt mine";
             //break;//如果目标系统不是自己，那么就抛弃该指令
         }

         //qDebug() << "command_ack.result" << command_ack.result;

         qDebug() << "recieve cmd"
                  <<  command_ack.target_system
                  << command_ack.result
                  << cmd_long.command
                  << command_ack.command;

         if(command_ack.result == MAV_RESULT_ACCEPTED)
         {
             if(cmd_long.command != command_ack.command)
             {
                 break;
             }

             status.transmit.isWaitingforACK = false;
             emit commandAccepted(true,command_ack.command,command_ack.result);//广播指令反馈
         }
         else
         {
             //拒绝该指令
             emit commandAccepted(false,command_ack.command,command_ack.result);//广播指令反馈
         }
    break;
    }
}

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

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

       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;

       if(status.transmit.type == 0)
       {
           qDebug() << "send cmd int" << cmd_int.command;
           _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);//发送一个指令
       }
       else if(status.transmit.type == 1)
       {
           qDebug() << "send cmd long" << cmd_long.command;
           _long(cmd_long.param1,cmd_long.param2,cmd_long.param3,cmd_long.param4,
                cmd_long.param5,cmd_long.param6,cmd_long.param7,
                cmd_long.command,cmd_long.confirmation);//发送一个指令
       }

       time = QTime::currentTime().msecsSinceStartOfDay();
       step++;//下一个阶段
   }
   else if(step == 1)//等待返回
   {
       if((QTime::currentTime().msecsSinceStartOfDay() - time) > sendTimeout)
       {
          timeout_count ++;
          step = 0;//返回上一个步骤
          if(timeout_count > 0)//如果指令发1次还没发出去，那么结束发送
          {
             qDebug() << "command set value time out fail";
             step = 0;
             status.m_Mode = Nop_Mode;
             status.transmit.isWaitingforACK = false;
             timeout_count = 0;

             //清除指令
             if(status.transmit.type == 0)
             {
                 emit commandAccepted(false,cmd_int.command,0xFE);//超时

                 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 if(status.transmit.type == 1)
             {
                 emit commandAccepted(false,cmd_long.command,0xFE);//超时

                 cmd_long.param1 = 0;
                 cmd_long.param2 = 0;
                 cmd_long.param3 = 0;
                 cmd_long.param4 = 0;
                 cmd_long.param5 = 0;
                 cmd_long.param6 = 0;
                 cmd_long.param7 = 0;
                 cmd_long.command = 0;
                 cmd_long.confirmation = 0;
             }

          }
       }
       else
       {
           //收到Value
           if(status.transmit.isWaitingforACK == false)
           {
               qDebug() << "command set value ok";
               step = 0;
               status.m_Mode = Nop_Mode;
               status.transmit.isWaitingforACK = false;
               timeout_count = 0;

               //清除指令
               if(status.transmit.type == 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 if(status.transmit.type == 1)
               {
                   cmd_long.param1 = 0;
                   cmd_long.param2 = 0;
                   cmd_long.param3 = 0;
                   cmd_long.param4 = 0;
                   cmd_long.param5 = 0;
                   cmd_long.param6 = 0;
                   cmd_long.param7 = 0;
                   cmd_long.command = 0;
                   cmd_long.confirmation = 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(GCS_SysID,GCS_CompID, &msg,&command_int);
    Send(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(GCS_SysID,GCS_CompID, &msg,&command_long);
    Send(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(GCS_SysID,GCS_CompID, &msg,&command_ack);
    Send(msg);
}