#include "missionprocess.h"


MissionProcess::MissionProcess(QObject *parent) : ThreadTemplet(parent)
{
    setRunFrq(20);//默认20Hz频率运行

    mission_status.m_Mode = Nop_Mode;
}

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

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

         //QThread::msleep(1000/frq());

         switch(mission_status.m_Mode)
         {
            default:
            case Nop_Mode     : QThread::msleep(1000/frq());break;
            case RecieveMode  : ReadStateMachine();break;//QApplication::processEvents();break;
            case TransmitMode :
             {
                 //QApplication::processEvents();
                 if(mission_status.transmit.type == 0)//航线传输
                 {
                    WriteStateMachine();
                 }
                 else if(mission_status.transmit.type == 1)//当前点设置
                 {
                    setcurrent(mission_status.transmit.seq);
                    mission_status.m_Mode = Nop_Mode;
                    qDebug() << "set current point" << mission_status.transmit.seq;
                 }
             }break;
         }

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

    }
}


void MissionProcess::checkoutVehicle(int sysid)
{
    QMap<int,QMap<int,mavlink_mission_item_int_t >> missionGroup = missions.value(sysid);//读出一个飞机的所有航线

    QMap<int,mavlink_mission_item_int_t> group1 = missionGroup.value(1);
    QMap<int,mavlink_mission_item_int_t> group3 = missionGroup.value(3);
    QMap<int,mavlink_mission_item_int_t> group4 = missionGroup.value(4);
    QMap<int,mavlink_mission_item_int_t> group5 = missionGroup.value(5);
    QMap<int,mavlink_mission_item_int_t> group6 = missionGroup.value(6);


    foreach (mavlink_mission_item_int_t item, group1) {
        //把航点发出去
        emit vehicleChanged(item.param1,item.param2,item.param3,item.param4,
                           item.x,item.y,item.z,
                           item.seq,item.mission_type,
                           item.command,
                           item.target_system,
                           item.target_component,
                           item.frame,
                           item.current,
                           item.autocontinue,
                           item.mission_type);
    }

    foreach (mavlink_mission_item_int_t item, group3) {
        //把航点发出去
        emit vehicleChanged(item.param1,item.param2,item.param3,item.param4,
                           item.x,item.y,item.z,
                           item.seq,item.mission_type,
                           item.command,
                           item.target_system,
                           item.target_component,
                           item.frame,
                           item.current,
                           item.autocontinue,
                           item.mission_type);
    }

    foreach (mavlink_mission_item_int_t item, group4) {
        //把航点发出去
        emit vehicleChanged(item.param1,item.param2,item.param3,item.param4,
                           item.x,item.y,item.z,
                           item.seq,item.mission_type,
                           item.command,
                           item.target_system,
                           item.target_component,
                           item.frame,
                           item.current,
                           item.autocontinue,
                           item.mission_type);
    }

    foreach (mavlink_mission_item_int_t item, group5) {
        //把航点发出去
        emit vehicleChanged(item.param1,item.param2,item.param3,item.param4,
                           item.x,item.y,item.z,
                           item.seq,item.mission_type,
                           item.command,
                           item.target_system,
                           item.target_component,
                           item.frame,
                           item.current,
                           item.autocontinue,
                           item.mission_type);
    }

    foreach (mavlink_mission_item_int_t item, group6) {
        //把航点发出去
        emit vehicleChanged(item.param1,item.param2,item.param3,item.param4,
                           item.x,item.y,item.z,
                           item.seq,item.mission_type,
                           item.command,
                           item.target_system,
                           item.target_component,
                           item.frame,
                           item.current,
                           item.autocontinue,
                           item.mission_type);
    }

}




void MissionProcess::ReadCmd(uint8_t m_sysid, uint8_t m_compid,int m_group)
{
    if(mission_status.m_Mode == Nop_Mode)//没有任务正在下载
    {
        mission_status.m_Mode = RecieveMode;//接收模式

        mission_status.recieve.group = m_group;
        qDebug() << "read mission" << sysid << compid << m_group;

    }
}

void MissionProcess::WriteCmd(uint8_t m_sysid, uint8_t m_compid ,uint32_t count ,int m_group)
{
    if(mission_status.m_Mode == Nop_Mode)//没有任务在上传
    {
        mission_status.transmit.type = 0;
        mission_status.transmit.count = count;
        mission_status.transmit.group = m_group;
        mission_status.m_Mode = TransmitMode;//发送模式

        qDebug() << "write mission" << m_sysid << m_compid << m_group;
    }
}


void MissionProcess::SetCurrentPoint(int seq)
{
    if(mission_status.m_Mode == Nop_Mode)//没有任务在上传
    {
        mission_status.transmit.type = 1;
        mission_status.m_Mode = TransmitMode;//发送模式
        mission_status.transmit.seq = seq - 1;
    }
}

void MissionProcess::sendFence(qreal Vertex,
                               qreal group,
                               qreal Lat,
                               qreal Lng,
                               uint16_t FecneSeq,
                               uint16_t command,
                               uint16_t sysid,
                               uint16_t compid,
                               uint16_t mission_type)
{

    mapcontrol::WayPointItem::_property item;



    item.param1 = Vertex;
    item.param2 = group;
    item.param3 = 0;
    item.param4 = 0;
    item.x = Lat * 10e6;
    item.y = Lng * 10e6;
    item.z = 0;
    item.seq = FecneSeq;
    item.group = 0;
    item.command = command;
    item.target_system = sysid;
    item.target_component = compid;
    item.frame = 0;
    item.current = 0;
    item.autocontinue = 0;
    item.mission_type = mission_type;

    items.insert(item.seq,item);

    qDebug() << Vertex
             << group
             << Lat
             << Lng
             << FecneSeq
             << command
             << sysid
             << compid
             << mission_type
             << items.size();

}



void MissionProcess::transmitPoint(float param1,float param2,float param3,float param4,
                     int32_t x,int32_t y,float z,
                     uint16_t seq,
                     uint16_t group,
                     uint16_t command,
                     uint8_t target_system,
                     uint8_t target_component,
                     uint8_t frame,
                     uint8_t current,
                     uint8_t autocontinue,
                     uint8_t mission_type)
{

    mapcontrol::WayPointItem::_property item;

    qDebug() << x << y << z;

    item.param1 = param1;
    item.param2 = param2;
    item.param3 = param3;
    item.param4 = param4;
    item.x = x;
    item.y = y;
    item.z = z;
    item.seq = seq-1;
    item.group = group;
    item.command = command;
    item.target_system = target_system;
    item.target_component = target_component;
    item.frame = frame;
    item.current = current;
    item.autocontinue = autocontinue;
    item.mission_type = mission_type;

    items.insert(item.seq,item);


/*
    foreach (mapcontrol::WayPointItem::_property item, items) {
        qDebug() << "Mission Thread: transmit seq"
                 << item.seq
                 << item.param1
                 << item.param2
                 << item.param3
                 << item.param4
                 << item.x
                 << item.y
                 << item.z
                 << item.seq
                 << item.group
                 << item.command
                 << item.target_system
                 << item.target_component
                 << item.frame
                 << item.current
                 << item.autocontinue
                 << item.mission_type;
    }
*/



}



//这个函数类似中断，专门处理接收到的状态
void MissionProcess::Parse(mavlink_message_t msg)
{
    switch (msg.msgid) {
    case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: {
       mavlink_msg_mission_request_list_decode(&msg,&mission_request_list);
    }break;
    case MAVLINK_MSG_ID_MISSION_COUNT: {
       mavlink_msg_mission_count_decode(&msg,&mission_count);

       qDebug() << "mission_count" << mission_count.count;
       mission_status.recieve.isWaitingforCount = false;
    }break;
    case MAVLINK_MSG_ID_MISSION_REQUEST_INT: {
       mavlink_msg_mission_request_int_decode(&msg,&mission_request_int);

       qDebug() << "recieve mission_request_int " << "mision type" << mission_request_int.mission_type << mission_item_int.seq;
       mission_item_int.seq ++;

       emit sendItemOK(mission_item_int.seq,true);
       mission_status.transmit.isWaiteforRequest = false;
    }break;
    case MAVLINK_MSG_ID_MISSION_REQUEST: {
       mavlink_msg_mission_request_decode(&msg,&mission_request);

       qDebug() << "recieve mission_request " << "mission type" << mission_request.mission_type;

       mission_item_int.seq ++;

       emit sendItemOK(mission_item_int.seq,true);
       mission_status.transmit.isWaiteforRequest = false;
    }break;
    case MAVLINK_MSG_ID_MISSION_ITEM_INT: {
       mavlink_msg_mission_item_int_decode(&msg,&mission_item_int);

       qDebug() << "recieve mission " << mission_item_int.seq;

       if(mission_item_int.seq == 0)
       {
           emit clearWaypoint();

           QMap<int,QMap<int,mavlink_mission_item_int_t >> missionGroup = missions.value(msg.sysid);//读出一个飞机的所有航线
           QMap<int,mavlink_mission_item_int_t > points = missionGroup.value(mission_item_int.mission_type);//读出一组

           points.clear();
           missionGroup.insert(mission_item_int.mission_type,points);
           missions.insert(msg.sysid,missionGroup);

       }

        //把航点发出去
        emit receivedPoint(mission_item_int.param1,mission_item_int.param2,mission_item_int.param3,mission_item_int.param4,
                           mission_item_int.x,mission_item_int.y,mission_item_int.z,
                           mission_item_int.seq,mission_item_int.mission_type,
                           mission_item_int.command,
                           mission_item_int.target_system,
                           mission_item_int.target_component,
                           mission_item_int.frame,
                           mission_item_int.current,
                           mission_item_int.autocontinue,
                           mission_item_int.mission_type);

       qDebug() << "recieve mission type" << mission_item_int.mission_type;


       QMap<int,QMap<int,mavlink_mission_item_int_t >> missionGroup = missions.value(msg.sysid);//读出一个飞机的所有航线
       QMap<int,mavlink_mission_item_int_t > points = missionGroup.value(mission_item_int.mission_type);//读出一组

       points.insert(mission_item_int.seq,mission_item_int);
       missionGroup.insert(mission_item_int.mission_type,points);
       missions.insert(msg.sysid,missionGroup);


       mission_status.recieve.isWaitingforItem = false;//已经收到航点，不用再等待
    }break;
    case MAVLINK_MSG_ID_MISSION_ITEM: {
       mavlink_msg_mission_item_decode(&msg,&mission_item);

       mission_status.recieve.isWaitingforItem = false;
    }break;
    case MAVLINK_MSG_ID_MISSION_ACK: {
       mavlink_msg_mission_ack_decode(&msg,&mission_ack);

       mission_status.transmit.isWaiteforACK = false;
    }break;
    case MAVLINK_MSG_ID_MISSION_CURRENT: {
       mavlink_msg_mission_current_decode(&msg,&mission_current);

       int group = mission_current.seq /1000;
       int seq   = mission_current.seq %1000;

       //emit currentGroup(group);
       // emit currentPoint(seq + 1);
       emit currentPoint(msg.sysid,mission_current.seq + 1);

    }break;
    case MAVLINK_MSG_ID_MISSION_SET_CURRENT: {
       mavlink_msg_mission_set_current_decode(&msg,&mission_set_current);
    }break;
    case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: {
       mavlink_msg_mission_clear_all_decode(&msg,&mission_clear_all);
    }break;
    case MAVLINK_MSG_ID_MISSION_ITEM_REACHED: {
       mavlink_msg_mission_item_reached_decode(&msg,&mission_item_reached);
    }break;
    case MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST: {
       mavlink_msg_mission_request_partial_list_decode(&msg,&mission_request_partial_list);
    }break;
    case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: {
       mavlink_msg_mission_write_partial_list_decode(&msg,&mission_write_partial_list);
    }break;

    }
}


//读航线状态机
void MissionProcess::ReadStateMachine(void)
{
   static uint8_t step = 0;
   static uint8_t timeout_count = 0;
   static int time = 0;

   if(step == 0)
   {
       qDebug() << "start reading mission";
       mission_status.recieve.isWaitingforCount = true;
       request_list(mission_status.recieve.group);
       step++;
       time = QTime::currentTime().msecsSinceStartOfDay();
   }
   else if(step == 1)//等待收到Count
   {
       //如果超时，那么重新发一次指令
       if((QTime::currentTime().msecsSinceStartOfDay() - time) > readTimeout)
       {
          step = 0;//如果超时没有收到就返回上一个步骤
          timeout_count ++;
          qDebug() << "mission count time out " << timeout_count << "times, retry again";
          if(timeout_count > 5)//如果指令发5次还没发出去，那么结束读取，并报错
          {
             step = 0;
             mission_status.m_Mode = Nop_Mode;//切换到什么都不做
             timeout_count = 0;
             qDebug() << "mission read fail";
          }
       }
       else//如果没有超时，那么就进入下一个阶段
       {
           if(mission_status.recieve.isWaitingforCount == false)//收到count
           {
              qDebug() << "mission count reccieved";
              timeout_count = 0;

              if(mission_count.count > 0)
              {
                  request_int(0,mission_status.recieve.group);//读取0点
                  time = QTime::currentTime().msecsSinceStartOfDay();
                  mission_status.recieve.isWaitingforItem = true;
                  step ++;

                  qDebug() << "group" << mission_status.recieve.group;
              }
              else
              {
                  step = 0;
                  mission_status.m_Mode = Nop_Mode;//切换到什么都不做
                  timeout_count = 0;
                  qDebug() << "no mission to read";
                  emit showMessage(tr("this mission group is empty"));
              }
           }
       }

   }
   else if(step ==2)//请求航点第0个航点
   {
       //超过时间
       if((QTime::currentTime().msecsSinceStartOfDay() - time) > readTimeout)
       {
          timeout_count ++;
          qDebug() << "mission request 0 time out " << timeout_count << "times, retry again";
          if(timeout_count > 5)//如果指令发5次还没发出去，那么结束读取，并报错
          {
              step = 0;
              mission_status.m_Mode= Nop_Mode;//切换到什么都不做
              timeout_count = 0;
              qDebug() << "mission read fail";
          }
          step = 1;//返回上一个步骤重新请求一次
       }
       else
       {
           if(mission_status.recieve.isWaitingforItem == false)
           {
               timeout_count = 0;
               time = QTime::currentTime().msecsSinceStartOfDay();
               mission_status.recieve.isWaitingforItem = false;
               step ++;
           }
       }
   }
   else if(step ==3)//请求航点
   {
       //超过时间
       if((QTime::currentTime().msecsSinceStartOfDay() - time) > readTimeout)
       {
          timeout_count ++;
          qDebug() << "mission request " << (mission_item_int.seq+1) <<  " time out " << timeout_count << "times, retry again";
          if(timeout_count > 5)//如果指令发5次还没发出去，那么结束读取，并报错
          {
              step = 0;
              mission_item_int.seq = 0;
              mission_status.m_Mode= Nop_Mode;//切换到什么都不做
              timeout_count = 0;
              qDebug() << "mission read fail";
          }
          mission_status.recieve.isWaitingforItem = false;//如果超时了，那么就重新请求一次
       }
       else
       {
           if(mission_status.recieve.isWaitingforItem == false)
           {
               //分成两类，int和不带int
               if((mission_item_int.seq+1) < mission_count.count)
               {
                   qDebug() << "request mission_item_int.seq+1" << (mission_item_int.seq+1);

                  mission_status.recieve.isWaitingforItem = true;
                  request_int(mission_item_int.seq+1,mission_status.recieve.group);
                  timeout_count = 0;
                  time = QTime::currentTime().msecsSinceStartOfDay();

               }
               else
               {
                   qDebug() << "mission recieve all";
                   step++;
               }
           }
       }
   }
   else if(step == 4)//航线传输结束，发送ack
   {
      qDebug() << "step" << step << "transmit ok";

      ack();
      step = 0;
      mission_status.m_Mode = Nop_Mode;//切换到什么都不做
      timeout_count = 0;
      mission_item_int.seq = 0;
      mission_status.recieve.isWaitingforCount = true;
      mission_status.recieve.isWaitingforItem = true;
   }
}

//写航线状态机
void MissionProcess::WriteStateMachine(void)
{
   static uint8_t step = 0;
   static uint8_t timeout_count = 0;
   static uint32_t time = 0;

   if(step == 0)
   {
       //向其他线程或者自己读取航点的数量
       qDebug() << "start send count" << mission_status.transmit.count;
       count(mission_status.transmit.count,mission_status.transmit.group);//发送count
       mission_status.transmit.isWaiteforRequest = true;
       time = QTime::currentTime().msecsSinceStartOfDay();
       step++;//下一个阶段
   }
   else if(step == 1)//等待收到Request
   {
       //如果超时，那么重新发一次指令
       if((QTime::currentTime().msecsSinceStartOfDay() - time) > sendTimeout)
       {
          step = 0;//返回上一个阶段
          timeout_count ++;
          if(timeout_count > 5)//如果指令发5次还没发出去，那么结束读取，并报错
          {
             emit sendItemOK(mission_item_int.seq,false);
             mission_item_int.seq = 0;
             step = 0;
             mission_status.m_Mode = Nop_Mode;
             timeout_count = 0;
             qDebug() << "waitfor request time out,abort transmit";

          }
          qDebug() << "waitfor request time out" << timeout_count;

       }
       else//如果没有超时，那么就进入下一个阶段
       {
          if(mission_status.transmit.isWaiteforRequest == false)
          {
              qDebug() << "recieve item Request";
              mission_item_int.seq = 0;
              step ++;
              time = QTime::currentTime().msecsSinceStartOfDay();
          }
       }

   }
   else if(step == 2)
   {
       if((QTime::currentTime().msecsSinceStartOfDay() - time) > sendTimeout)
       {
          timeout_count ++;
          if(timeout_count > 5)//如果指令发5次还没发出去，那么结束读取，并报错
          {
             emit sendItemOK(mission_item_int.seq,false);
             mission_item_int.seq = 0;
             step = 0;
             mission_status.m_Mode = Nop_Mode;
             timeout_count = 0;

             qDebug() << "send item time out,abort transmit";
          }

          time = QTime::currentTime().msecsSinceStartOfDay();
          //如果超时了，重新发一次当前航点
          qDebug() << "send item time out" << timeout_count;
          mission_status.transmit.isWaiteforRequest = false;

       }
       else
       {
           if(mission_status.transmit.isWaiteforRequest == false)//收到Request
           {
               //分成两类，int和不带int
               if(mission_item_int.seq < mission_status.transmit.count)
               {
                  qDebug() << "send item" << mission_item_int.seq;

                  mapcontrol::WayPointItem::_property i;

                  i = *items.find(mission_item_int.seq);


                  item_int(i.param1,i.param2,i.param3,i.param4,
                           i.x,i.y,i.z,
                           i.seq,i.command,i.frame,
                           i.current,i.autocontinue,i.mission_type);//发送航点

                  qDebug() << i.param1
                           << i.param2
                           << i.param3
                           << i.param4
                           << i.x
                           << i.y
                           << i.z
                           << i.seq
                           << i.mission_type;



                  /*
                  item(i.param1,i.param2,i.param3,i.param4,
                           i.x * 10e-7,i.y * 10e-7,i.z * 10e-4,
                           i.seq,i.command,i.frame,
                           i.current,i.autocontinue,i.mission_type);//发送航点
                           */


                  if(mission_item_int.seq < (mission_status.transmit.count - 1))
                  {
                      mission_status.transmit.isWaiteforRequest = true;
                  }
                  else
                  {
                      step++;
                      mission_status.transmit.isWaiteforACK = true;
                  }

                  time = QTime::currentTime().msecsSinceStartOfDay();
               }
               else
               {
                   step++;
                   mission_status.transmit.isWaiteforACK = true;
                   time = QTime::currentTime().msecsSinceStartOfDay();
               }
           }
       }
   }
   else if(step == 3)//航线传输结束，等待ack，如果没接收到就算了，超时也结束
   {
       if((QTime::currentTime().msecsSinceStartOfDay() - time) > sendTimeout)
       {
          timeout_count ++;
          if(timeout_count > 5)//如果指令发5次还没发出去，那么结束读取
          {
             emit sendItemOK(mission_item_int.seq,false);
             qDebug() << "wait for ack time out ,transmit compelet";
             step = 0;
             mission_item_int.seq = 0;
             mission_status.m_Mode = Nop_Mode;
             mission_status.transmit.isWaiteforACK = false;
             timeout_count = 0;
             //清除存储的航线
             items.clear();
          }
          qDebug() << "wait for ack time out";
       }
       else
       {
           //收到ACK
           if(mission_status.transmit.isWaiteforACK == false)
           {
               emit sendItemOK(mission_item_int.seq+1,true);
               qDebug() << "recieve ack ,transmit compelet";
               step = 0;
               mission_item_int.seq = 0;
               mission_status.m_Mode = Nop_Mode;
               mission_status.transmit.isWaiteforRequest = false;

               //清除存储的航线
               items.clear();

           }
       }
   }
}






/*All msg
 *
 *  MISSION_REQUEST_LIST
 *  MISSION_COUNT
 *  MISSION_REQUEST_INT
 *  MISSION_REQUEST
 *  MISSION_ITEM_INT
 *  MISSION_ITEM
 *  MISSION_ACK
 *  MISSION_CURRENT
 *  MISSION_SET_CURRENT
 * STATUSTEXT
 *  MISSION_CLEAR_ALL
 *  MISSION_ITEM_REACHED
 *  MISSION_REQUEST_PARTIAL_LIST
 *  MISSION_WRITE_PARTIAL_LIST
 **/
void MissionProcess::request_list(int missiontype)//读取整列请求
{
    static mavlink_message_t msg;
    static mavlink_mission_request_list_t mission_request_list;

     mission_request_list.mission_type = missiontype;//3，4，5，6对应 1，2，3，4
     mission_request_list.target_system = sysid;
     mission_request_list.target_component = compid;

    mavlink_msg_mission_request_list_encode(GCS_SysID,GCS_CompID, &msg,&mission_request_list);
    Send(msg);
}

void MissionProcess::count(uint16_t count,int missiontype)//计数值
{
    static mavlink_message_t msg;
    static mavlink_mission_count_t mission_count;

     mission_count.count = count;
     mission_count.mission_type = missiontype;
     mission_count.target_system = sysid;
     mission_count.target_component = compid;

    mavlink_msg_mission_count_encode(GCS_SysID,GCS_CompID, &msg,&mission_count);
    Send(msg);
}

void MissionProcess::request_int(uint16_t seq,int missiontype)//读取请求
{
    static mavlink_message_t msg;
    static mavlink_mission_request_int_t mission_request_int;

     mission_request_int.seq = seq;
     mission_request_int.mission_type = missiontype;
     mission_request_int.target_system = sysid;
     mission_request_int.target_component = compid;

    mavlink_msg_mission_request_int_encode(GCS_SysID,GCS_CompID, &msg,&mission_request_int);
    Send(msg);
}

void MissionProcess::request(uint16_t seq,int missiontype)//读取请求
{
    static mavlink_message_t msg;
    static mavlink_mission_request_t mission_request;

    mission_request.mission_type = missiontype;
    mission_request.seq = seq;
    mission_request.target_system = sysid;
    mission_request.target_component = compid;
    mavlink_msg_mission_request_encode(GCS_SysID,GCS_CompID, &msg,&mission_request);
    Send(msg);
}

void MissionProcess::item_int(float param1,float param2,float param3,float param4,
                                     int32_t x,int32_t y,float z,
                                     uint16_t seq,uint16_t command,
                                     uint8_t frame,uint8_t current,
                                     uint8_t autocontinue,uint8_t mission_type)
{
    static mavlink_message_t msg;
    static mavlink_mission_item_int_t mission_item_int;

    mission_item_int.target_system = sysid;
    mission_item_int.target_component = compid;

    mission_item_int.param1 = param1;
    mission_item_int.param2 = param2;
    mission_item_int.param3 = param3;
    mission_item_int.param4 = param4;

    mission_item_int.x = x;
    mission_item_int.y = y;
    mission_item_int.z = z;

    mission_item_int.seq = seq;
    mission_item_int.command = command;
    mission_item_int.frame = frame;
    mission_item_int.current = current;
    mission_item_int.autocontinue = autocontinue;
    mission_item_int.mission_type = mission_type;

    mavlink_msg_mission_item_int_encode(GCS_SysID,GCS_CompID, &msg,&mission_item_int);
    Send(msg);

    //emit showMessage(tr("发送航点 %1").arg(seq + 1));
}

void MissionProcess::item(float param1,float param2,float param3,float param4,
                                     float x,float y,float z,
                                     uint16_t seq,uint16_t command,
                                     uint8_t frame,uint8_t current,
                                     uint8_t autocontinue,uint8_t mission_type)
{
    static mavlink_message_t msg;
    static mavlink_mission_item_t mission_item;

    mission_item.target_system = sysid;
    mission_item.target_component = compid;

    mission_item.param1 = param1;
    mission_item.param2 = param2;
    mission_item.param3 = param3;
    mission_item.param4 = param4;

    mission_item.x = x;
    mission_item.y = y;
    mission_item.z = z;

    mission_item.seq = seq;
    mission_item.command = command;
    mission_item.frame = frame;
    mission_item.current = current;
    mission_item.autocontinue = autocontinue;
    mission_item.mission_type = mission_type;

    mavlink_msg_mission_item_encode(GCS_SysID,GCS_CompID, &msg,&mission_item);
    Send(msg);
}

void MissionProcess::ack(void)
{
    static mavlink_message_t msg;
    static mavlink_mission_ack_t mission_ack;

    mission_ack.target_system = sysid;
    mission_ack.target_component = compid;
    mission_ack.mission_type = 0;
    mission_ack.type = 0;

    mavlink_msg_mission_ack_encode(GCS_SysID,GCS_CompID, &msg,&mission_ack);
    Send(msg);


}

void MissionProcess::current(void)
{
    static mavlink_message_t msg;
    static mavlink_mission_current_t mission_current;

    mavlink_msg_mission_current_encode(GCS_SysID,GCS_CompID, &msg,&mission_current);
    Send(msg);
}

void MissionProcess::setcurrent(uint16_t seq)
{
    static mavlink_message_t msg;
    static mavlink_mission_set_current_t mission_set_current;

    mission_set_current.target_system = sysid;
    mission_set_current.target_component = compid;

    mission_set_current.seq = seq;

    mavlink_msg_mission_set_current_encode(GCS_SysID,GCS_CompID, &msg,&mission_set_current);
    Send(msg);
}

void MissionProcess::clear_all(void)
{
    static mavlink_message_t msg;
    static mavlink_mission_clear_all_t mission_clear_all;

    mission_clear_all.target_system = sysid;
    mission_clear_all.target_component = compid;
    mission_clear_all.mission_type = 0;

    mavlink_msg_mission_clear_all_encode(GCS_SysID,GCS_CompID, &msg,&mission_clear_all);
    Send(msg);
}

void MissionProcess::item_reached(void)
{
    static mavlink_message_t msg;
    static mavlink_mission_item_reached_t mission_item_reached;

    mavlink_msg_mission_item_reached_encode(GCS_SysID,GCS_CompID, &msg,&mission_item_reached);
    Send(msg);
}

void MissionProcess::request_partial_list(void)
{
     static mavlink_message_t msg;
     static mavlink_mission_request_partial_list_t mission_request_partial_list;

     mavlink_msg_mission_request_partial_list_encode(GCS_SysID,GCS_CompID, &msg,&mission_request_partial_list);
     Send(msg);
}

void MissionProcess::write_partial_list(void)
{
     static mavlink_message_t msg;
     static mavlink_mission_write_partial_list_t mission_write_partial_list;

     mavlink_msg_mission_write_partial_list_encode(GCS_SysID,GCS_CompID, &msg,&mission_write_partial_list);
     Send(msg);
}