输出协议完成并测试

MavLinkNode/mavlinknode.cpp

@@ -909,6 +909,25 @@ void MavLinkNode::StatusParse(mavlink_message_t msg)
 
             emit signal_ins2(vehicle.ins2);
 
+            _showinfo info;
+
+            info.flag = 0x23F8;
+            info.id   = msg.sysid;//1
+            info.time = vehicle.ins2.time;//(second+min*60+hour*3600)*1000
+            info.lng  = vehicle.ins2.lon * 10000000;//*10000000
+            info.lat  = vehicle.ins2.lat * 10000000;//*10000000
+            info.alt  = vehicle.ins2.alt * 10;//*10
+            info.ve   = vehicle.ins2.v_east;
+            info.vn   = vehicle.ins2.v_north;
+            info.vu   = vehicle.ins2.v_up;
+            info.v    = sqrt(vehicle.ins2.v_east * vehicle.ins2.v_east +
+                                 vehicle.ins2.v_north * vehicle.ins2.v_north +
+                                 vehicle.ins2.v_up * vehicle.ins2.v_up);
+            info.course = vehicle.ins2.yaw * 10;//*10
+
+
+            infoExport(info);
+
          }break;
          case MAVLINK_MSG_ID_GPS_RAW_INT: {
             mavlink_msg_gps_raw_int_decode(&msg,&vehicle.gps_raw_int);
@@ -1203,3 +1222,143 @@ void MavLinkNode::setLogfile(QString file)
     }
 }
 
+
+
+void MavLinkNode::infoExport(_showinfo info)
+{
+    uint8_t buff[250];
+    int dataCount = 0;
+
+    union {uint8_t B[4];uint16_t D[2];int32_t H;}src;
+
+#if 0
+    src.D[0] = info.flag;
+    buff[dataCount++] = src.B[1];
+    buff[dataCount++] = src.B[0];
+
+    src.D[0] = info.id;
+    buff[dataCount++] = src.B[1];
+    buff[dataCount++] = src.B[0];
+
+    src.H = info.time;
+    buff[dataCount++] = src.B[3];
+    buff[dataCount++] = src.B[2];
+    buff[dataCount++] = src.B[1];
+    buff[dataCount++] = src.B[0];
+
+    src.H = info.lng;
+    buff[dataCount++] = src.B[3];
+    buff[dataCount++] = src.B[2];
+    buff[dataCount++] = src.B[1];
+    buff[dataCount++] = src.B[0];
+
+    src.H = info.lat;
+    buff[dataCount++] = src.B[3];
+    buff[dataCount++] = src.B[2];
+    buff[dataCount++] = src.B[1];
+    buff[dataCount++] = src.B[0];
+
+    src.H = info.alt;
+    buff[dataCount++] = src.B[3];
+    buff[dataCount++] = src.B[2];
+    buff[dataCount++] = src.B[1];
+    buff[dataCount++] = src.B[0];
+
+    src.H = info.ve;
+    buff[dataCount++] = src.B[3];
+    buff[dataCount++] = src.B[2];
+    buff[dataCount++] = src.B[1];
+    buff[dataCount++] = src.B[0];
+
+    src.H = info.vn;
+    buff[dataCount++] = src.B[3];
+    buff[dataCount++] = src.B[2];
+    buff[dataCount++] = src.B[1];
+    buff[dataCount++] = src.B[0];
+
+    src.H = info.vu;
+    buff[dataCount++] = src.B[3];
+    buff[dataCount++] = src.B[2];
+    buff[dataCount++] = src.B[1];
+    buff[dataCount++] = src.B[0];
+
+    src.H = info.v;
+    buff[dataCount++] = src.B[3];
+    buff[dataCount++] = src.B[2];
+    buff[dataCount++] = src.B[1];
+    buff[dataCount++] = src.B[0];
+
+    src.H = info.course;
+    buff[dataCount++] = src.B[3];
+    buff[dataCount++] = src.B[2];
+    buff[dataCount++] = src.B[1];
+    buff[dataCount++] = src.B[0];
+#else
+    src.D[0] = info.flag;
+    buff[dataCount++] = src.B[0];
+    buff[dataCount++] = src.B[1];
+
+    src.D[0] = info.id;
+    buff[dataCount++] = src.B[0];
+    buff[dataCount++] = src.B[1];
+
+    src.H = info.time;
+    buff[dataCount++] = src.B[0];
+    buff[dataCount++] = src.B[1];
+    buff[dataCount++] = src.B[2];
+    buff[dataCount++] = src.B[3];
+
+    src.H = info.lng;
+    buff[dataCount++] = src.B[0];
+    buff[dataCount++] = src.B[1];
+    buff[dataCount++] = src.B[2];
+    buff[dataCount++] = src.B[3];
+
+    src.H = info.lat;
+    buff[dataCount++] = src.B[0];
+    buff[dataCount++] = src.B[1];
+    buff[dataCount++] = src.B[2];
+    buff[dataCount++] = src.B[3];
+
+    src.H = info.alt;
+    buff[dataCount++] = src.B[0];
+    buff[dataCount++] = src.B[1];
+    buff[dataCount++] = src.B[2];
+    buff[dataCount++] = src.B[3];
+
+    src.H = info.ve;
+    buff[dataCount++] = src.B[0];
+    buff[dataCount++] = src.B[1];
+    buff[dataCount++] = src.B[2];
+    buff[dataCount++] = src.B[3];
+
+    src.H = info.vn;
+    buff[dataCount++] = src.B[0];
+    buff[dataCount++] = src.B[1];
+    buff[dataCount++] = src.B[2];
+    buff[dataCount++] = src.B[3];
+
+    src.H = info.vu;
+    buff[dataCount++] = src.B[0];
+    buff[dataCount++] = src.B[1];
+    buff[dataCount++] = src.B[2];
+    buff[dataCount++] = src.B[3];
+
+    src.H = info.v;
+    buff[dataCount++] = src.B[0];
+    buff[dataCount++] = src.B[1];
+    buff[dataCount++] = src.B[2];
+    buff[dataCount++] = src.B[3];
+
+    src.H = info.course;
+    buff[dataCount++] = src.B[0];
+    buff[dataCount++] = src.B[1];
+    buff[dataCount++] = src.B[2];
+    buff[dataCount++] = src.B[3];
+
+#endif
+
+    emit SendMessageToExport(0,buff,40);
+}
+
+

MavLinkNode/mavlinknode.h

@@ -14,7 +14,7 @@
 #include "statusprocess.h"
 #include "Terminal.h"
 #include "sbusparser.h"
-//#include "QAxObject"
+#include "QUdpSocket"
 
 #include "rcprocess.h"
 #include "rtkprocess.h"
@@ -64,7 +64,20 @@ public:
 
     }_vehicle;
 
-
+    typedef struct
+    {
+        uint16_t flag;//0x23f8
+        uint16_t id;//1
+        int32_t time;//(second+min*60+hour*3600)*1000
+        int32_t lng;//*10000000
+        int32_t lat;//*10000000
+        int32_t alt;//*10
+        int32_t ve;
+        int32_t vn;
+        int32_t vu;
+        int32_t v;
+        int32_t course;//*10
+    }_showinfo;//192.168.5.8:10038
 
 
     explicit MavLinkNode(QObject *parent = nullptr);
@@ -200,7 +213,7 @@ signals:
     void signal_vehicle(MavLinkNode::_vehicle vehicle);
 
 
-
+    void SendMessageToExport(quint8 ch, quint8 *data,quint16 len);
 
     void updateDlink(float rssi,uint64_t in,uint64_t out);
     void CommuniationLost(bool);
@@ -236,6 +249,7 @@ public slots:
 
     void setGCSID(int id);
 
+    void infoExport(_showinfo info);
 
 private slots:
    //线程私有接口