#include "missionprocess.h"


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


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

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

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

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

void MissionProcess::process()//线程函数
{
    uint8_t count = 0;
    while (running_flag)
    {
         count ++;
         QThread::msleep(1000.0/running_frq);

         switch(mission_status.m_Mode)
         {
            default:
            case Nop_Mode     :break;
            case RecieveMode  : ReadStateMachine();break;
            case TransmitMode :
             {
                 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;
         }
        //qDebug() << count;
    }
    //退出线程
    disconnect(thread, nullptr, nullptr, nullptr);
    thread->quit();
    thread->wait();//等待结束
    thread->deleteLater();
    thread = nullptr;
}

void MissionProcess::setID(int m_sysid,int m_compid)
{
    sysid = (uint8_t)m_sysid;
    compid = (uint8_t)m_compid;
}



void MissionProcess::SendMessage(mavlink_message_t msg)
{
    uint8_t  buff[256+20];
    uint16_t len = mavlink_msg_to_send_buffer(buff, &msg);

    /*
    QString num;
    for (int i = 0; i < len; ++i) {
        num.append(QString::number(buff[i],16).toUpper());
        num.append(" ");
    }
    qDebug() << num;
    */

    emit SendMessageTo(0,buff, len);//使用信号和槽
}


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

        qDebug() << "read mission" << sysid << compid;

        start();//开启线程
    }
}

void MissionProcess::WriteCmd(uint8_t m_sysid, uint8_t m_compid ,uint32_t count )
{
    if(mission_status.m_Mode != TransmitMode)//没有任务在上传
    {
        mission_status.transmit.type = 0;
        mission_status.transmit.count = count;
        mission_status.m_Mode = TransmitMode;//发送模式
        qDebug() << "write mission" << sysid << compid;
        start();//开启线程
    }
}


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


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() << "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);

       if(mission_count.target_system != Current_sysID)
       {
           qDebug() << mission_count.target_system << "this msg is'nt mine";
           //break;//如果目标系统不是自己，那么就抛弃该指令
       }

       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);

       if(mission_count.target_system != Current_sysID)
       {
           qDebug() << mission_count.target_system << "this msg is'nt mine";
           //break;//如果目标系统不是自己，那么就抛弃该指令
       }

       qDebug() << "recieve mission_request_int ";
       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);

       if(mission_request.target_system != Current_sysID)
       {
           qDebug() << mission_request.target_system << "this msg is'nt mine";
           //break;//如果目标系统不是自己，那么就抛弃该指令
       }

       qDebug() << "recieve mission_request ";

       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);

       if(mission_item_int.target_system != Current_sysID)
       {
           qDebug() << mission_item_int.target_system << "this msg is'nt mine";
           //break;//如果目标系统不是自己，那么就抛弃该指令
       }

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

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

       //把航点发出去
        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,0,
                           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);

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

       if(mission_item.target_system != Current_sysID)
       {
           qDebug() << mission_item.target_system << "this msg is'nt mine";
           //break;//如果目标系统不是自己，那么就抛弃该指令
       }

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

       if(mission_ack.target_system != Current_sysID)
       {
           qDebug() << mission_ack.target_system << "this msg is'nt mine";
           //break;//如果目标系统不是自己，那么就抛弃该指令
       }

       mission_status.transmit.isWaiteforACK = false;
    }break;
    case MAVLINK_MSG_ID_MISSION_CURRENT: {
       mavlink_msg_mission_current_decode(&msg,&mission_current);
       emit currentPoint(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();
       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;

              request_int(0);//读取0点
              time = QTime::currentTime().msecsSinceStartOfDay();
              mission_status.recieve.isWaitingforItem = true;
              step ++;
           }
       }

   }
   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);
                  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);//发送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;



                  /*
                  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(void)//读取整列请求
{
    static mavlink_message_t msg;
    static mavlink_mission_request_list_t mission_request_list;

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

    mavlink_msg_mission_request_list_encode(Current_sysID,Current_CompID, &msg,&mission_request_list);
    SendMessage(msg);
}

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

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

    mavlink_msg_mission_count_encode(Current_sysID,Current_CompID, &msg,&mission_count);
    SendMessage(msg);
}

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

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

    mavlink_msg_mission_request_int_encode(Current_sysID, Current_CompID, &msg,&mission_request_int);
    SendMessage(msg);
}

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

    mission_request.mission_type = 0;
    mission_request.seq = seq;
    mission_request.target_system = sysid;
    mission_request.target_component = compid;
    mavlink_msg_mission_request_encode(Current_sysID, Current_CompID, &msg,&mission_request);
    SendMessage(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(Current_sysID,Current_CompID, &msg,&mission_item_int);
    SendMessage(msg);
}

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(Current_sysID,Current_CompID, &msg,&mission_item);
    SendMessage(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(Current_sysID,Current_CompID, &msg,&mission_ack);
    SendMessage(msg);


}

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

    mavlink_msg_mission_current_encode(Current_sysID,Current_CompID, &msg,&mission_current);
    SendMessage(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(Current_sysID,Current_CompID, &msg,&mission_set_current);
    SendMessage(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(Current_sysID,Current_CompID, &msg,&mission_clear_all);
    SendMessage(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(Current_sysID,Current_CompID, &msg,&mission_item_reached);
    SendMessage(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(Current_sysID,Current_CompID, &msg,&mission_request_partial_list);
     SendMessage(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(Current_sysID,Current_CompID, &msg,&mission_write_partial_list);
     SendMessage(msg);
}