b

MavLinkNode/missionprocess.cpp

@@ -63,7 +63,19 @@ void MissionProcess::process()//线程函数
             default:
             case Nop_Mode     :break;
             case RecieveMode  : ReadStateMachine();break;
-            case TransmitMode : WriteStateMachine();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;
     }
@@ -87,6 +99,15 @@ 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);//使用信号和槽
 }
 
@@ -102,12 +123,23 @@ void MissionProcess::ReadCmd(uint8_t m_sysid, uint8_t m_compid)
 
 void MissionProcess::WriteCmd(uint8_t m_sysid, uint8_t m_compid ,uint32_t count )
 {
+    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)
+{
+    mission_status.transmit.type = 1;
+    mission_status.m_Mode = TransmitMode;//发送模式
+    mission_status.transmit.seq = seq;
+    start();//开启线程
+}
+
+
 void MissionProcess::transmitPoint(float param1,float param2,float param3,float param4,
                      int32_t x,int32_t y,float z,
                      uint16_t seq,
@@ -225,6 +257,13 @@ void MissionProcess::Parse(mavlink_message_t msg)
     }break;
     case MAVLINK_MSG_ID_MISSION_CURRENT: {
        mavlink_msg_mission_current_decode(&msg,&mission_current);
+       //if(mission_current.seq == mission_status.transmit.seq)
+       {
+           //收到设置当前点，对比如果一致，那么成功，否则失败
+           //qDebug() << "mission_current" << mission_current.seq;
+           emit currentPoint(mission_current.seq);
+       }
+
     }break;
     case MAVLINK_MSG_ID_MISSION_SET_CURRENT: {
        mavlink_msg_mission_set_current_decode(&msg,&mission_set_current);
@@ -329,6 +368,7 @@ void MissionProcess::ReadStateMachine(void)
           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";
@@ -367,6 +407,7 @@ void MissionProcess::ReadStateMachine(void)
       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;
    }
@@ -519,6 +560,11 @@ void MissionProcess::WriteStateMachine(void)
    }
 }
 
+
+
+
+
+
 /*All msg
  *
  *  MISSION_REQUEST_LIST
@@ -679,11 +725,16 @@ void MissionProcess::current(void)
     SendMessage(msg);
 }
 
-void MissionProcess::setcurrent(void)
+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(2,MAV_COMP_ID_MISSIONPLANNER, &msg,&mission_set_current);
     SendMessage(msg);
 }