[PlaneCopter.cs]

1加入数据量统计
2降低飞机发到地面的数据频率，5hz改为1hz
3收数据不写日志（导致死机）
4为RTK数据单独组包，以减小包大小（发送大小为：RTK数据+11字节/包协议开销，自动拆分为110字节/包）

PlaneGcsSdk.Contract_Shared/Copters/ICopterActions.cs

@@ -110,6 +110,10 @@ namespace Plane.Copters
         Task SetParamAsync(string paramName, float value, int millisecondsTimeout = Timeout.Infinite);
         bool GetShowLEDAsync();
 
+        void GetCommunicationNumber(out int recnumber, out int sendnumber);
+       //重设数据量
+        void ResetCommunicationNumber();
+
         Task SetShowLEDAsync(bool Ledon);
         Task SetShowLEDFlashAsync(int Flashtype, int millisecondsTime);
 

PlaneGcsSdk_Shared/Copters/EmptyCopter.cs

@@ -54,6 +54,20 @@ namespace Plane.Copters
             return TaskUtils.CompletedTask;
         }
 
+        public void GetCommunicationNumber(out int recnumber, out int sendnumber)
+        {
+            recnumber = 0;
+            sendnumber = 0;
+        }
+
+
+        //重设数据量
+        public void ResetCommunicationNumber()
+        {
+            
+        }
+
+
         public Task SetParamAsync(string paramName, float value, int millisecondsTimeout = -1)
         {
             return TaskUtils.CompletedTask;

PlaneGcsSdk_Shared/Copters/FakeCopter.cs

@@ -260,6 +260,20 @@ namespace Plane.Copters
             return TaskUtils.CompletedTask;
         }
 
+        //得到收到的总数据量
+        public void GetCommunicationNumber(out int recnumber, out int sendnumber)
+        {
+            recnumber = 0;
+            sendnumber = 0;
+        }
+
+
+        //重设数据量
+        public void ResetCommunicationNumber()
+        {
+
+        }
+
         public Task SetParamAsync(string paramName, float value, int millisecondsTimeout = Timeout.Infinite)
         {
             // TODO: 林俊清, 20150807, 实现仿真的 SetParamAsync。

PlaneGcsSdk_Shared/Copters/PLCopter.cs

@@ -201,6 +201,21 @@ namespace Plane.Copters
                     await Task.Delay(5).ConfigureAwait(false);
                 }
         }
+
+        //得到收到的总数据量
+
+        public void GetCommunicationNumber(out int recnumber, out int sendnumber)
+        {
+            _internalCopter.GetCommunicationNumber(out recnumber,out sendnumber);
+        }
+
+
+        //重设数据量
+        public void ResetCommunicationNumber()
+        {
+            _internalCopter.ResetCommunicationNumber();
+        }
+
         public async Task SetParamAsync(string paramName, float paramValue, int millisecondsTimeout = Timeout.Infinite)
         {
             var stopwatch = Stopwatch.StartNew();

PlaneGcsSdk_Shared/Copters/PlaneCopter.cs

@@ -24,6 +24,8 @@ namespace Plane.Copters
     {
         uint iErrorData;
         ulong iDataCount;
+        private int _recnumber = 0;
+        private int _sendnumber = 0;
 
         public bool IsConnected { get { return Connection.IsOpen; } }
 
@@ -133,12 +135,21 @@ namespace Plane.Copters
             //停止所有数据流
             await StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM.ALL, 0).ConfigureAwait(false);
             //需要电池电压，电流，GPS数据
-
+            /*
-            await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS, 5).ConfigureAwait(false); //10 1电池电压、电流、电量,152-内存、62-导航控制输出、24-GPS数据、42-航点数据
+             *  目前接收的数据是
+             *  gps 38 字节
+                hud 28
+                SYS_STATUS  39
+                MISSION_CURRENT  10
+                HEARTBEAT  17
+                共132字节
+               5hz 592/s 太高
+             */
+            await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS, 1).ConfigureAwait(false); //10 1电池电压、电流、电量,152-内存、62-导航控制输出、24-GPS数据、42-航点数据
                 //不需要姿态信息
             //         await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTRA1, 1).ConfigureAwait(false); //5 请求 飞行姿态 roll、pitch、yaw、rollspeed、pitchspeed、yawspeed--- 30
             //需要高度和机头朝向数据
-            await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTRA2, 5).ConfigureAwait(false); //10 请求vfr airspeed(空速)、groundspeed(地速)、alt(高度)、climb(爬升率)、heading(头朝向)、throttle(实际油门) ---74
+            await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.EXTRA2, 1).ConfigureAwait(false); //10 请求vfr airspeed(空速)、groundspeed(地速)、alt(高度)、climb(爬升率)、heading(头朝向)、throttle(实际油门) ---74
             //不需要通道数据
             //await RequestDatastreamAsync(MAVLink.MAV_DATA_STREAM.RC_CHANNELS, 1).ConfigureAwait(false); //1 请求 通道数据
             //不需要原始数据
@@ -201,7 +212,7 @@ namespace Plane.Copters
                             autopilot = (byte)MAVLink.MAV_AUTOPILOT.GENERIC,
                             mavlink_version = 3,
                         };
-                        await SendPacketAsync(htb).ConfigureAwait(false);
+                        await SendPacketAsync(htb).ConfigureAwait(false);  //发心跳包
 #if DEBUG && LOG_PACKETS
                         if (!_sendHeartbeatStopwatch.IsRunning)
                         {
@@ -247,7 +258,7 @@ namespace Plane.Copters
                     {
                         PacketReceived?.Invoke(this, new PacketReceivedEventArgs(packet));
                         var handled = true;
-                        Debug.WriteLine(packet[5],"收到数据类型");
+                      //  Debug.WriteLine(packet[5],"收到数据类型");
                         switch (packet[5])
                         {
                             case MAVLink.MAVLINK_MSG_ID_HEARTBEAT: //0
@@ -359,26 +370,33 @@ namespace Plane.Copters
             }
 
             int length = 0;
-
+            int readnumber = 0;  //_recnumber;
             try
             {
-                if (await Connection.ReadAsync(_readBuffer, 0, 1) == 0)
+                readnumber = await Connection.ReadAsync(_readBuffer, 0, 1);
+                _recnumber += readnumber;
+                if (readnumber == 0)
                 {
                     return null;
                 }
                 if (_readBuffer[0] == MAVLink.MAVLINK_STX)
                 {
-                    if (await Connection.ReadAsync(_readBuffer, 1, 5) == 0)
+                    readnumber = await Connection.ReadAsync(_readBuffer, 1, 5);
+                    _recnumber += readnumber;
+                    if (readnumber == 0)
                     {
                         return null;
                     }
+                    
 #if DEBUG && LOG_SEQUENCE_NUMBER
                     // _sequenceLog.Append(DateTime.Now.ToString("HHmmss.fff")).Append('\t').Append(string.Format("{0,3}", _readBuffer[2].ToString())).Append('\t').Append(string.Format("{0,3}", _readBuffer[5].ToString()));
                     _sequenceLog.Append(DateTime.Now.ToString()).Append('\t').Append(string.Format("{0,3}", _readBuffer[2].ToString())).Append('\t').Append(string.Format("{0,3}", _readBuffer[5].ToString()));
 #endif
                     // packet length
                     length = 6 + _readBuffer[1] + 2; // header + data + checksum
-                    if (await Connection.ReadAsync(_readBuffer, 6, length - 6) == 0)
+                    readnumber = await Connection.ReadAsync(_readBuffer, 6, length - 6);
+                    _recnumber += readnumber;
+                    if (readnumber == 0)
                     {
                         return null;
                     }
@@ -534,7 +552,7 @@ namespace Plane.Copters
             {
                 data = MavlinkUtil.StructureToByteArrayBigEndian(indata);
             }
-
+           // data = MavlinkUtil.StructureToByteArrayBigEndian(indata);
             //Console.WriteLine(DateTime.Now + " PC Doing req "+ messageType + " " + this.BytesToRead);
             byte[] packet = new byte[data.Length + 6 + 2];
 
@@ -584,6 +602,84 @@ namespace Plane.Copters
                 try
                 {
                     await Connection.WriteAsync(packet, 0, i);
+                   // Console.WriteLine("sendout:" + i);
+                    _sendnumber += i; //统计发送数量
+                }
+                catch
+                {
+                    // 林俊清, 20151031, 异常已通过 IConnection.ExceptionThrown 事件传到外部，此处吞掉就行了。
+                }
+            }
+        }
+
+
+        public async Task GenerateRTKPacketAsync<TMavlinkPacket>(byte messageType, TMavlinkPacket indata,int rtklength)
+        {
+            byte[] data;
+            int rtkdataleng= rtklength + 3;
+            if (mavlinkversion == 3)
+            {
+                data = MavlinkUtil.StructureToByteArray(indata);
+            }
+            else
+            {
+                data = MavlinkUtil.StructureToByteArrayBigEndian(indata);
+            }
+            // data = MavlinkUtil.StructureToByteArrayBigEndian(indata);
+            //Console.WriteLine(DateTime.Now + " PC Doing req "+ messageType + " " + this.BytesToRead);
+ 
+            byte[] packet = new byte[rtkdataleng + 6 + 2];
+
+            if (mavlinkversion == 3 || mavlinkversion == 0)
+            {
+                packet[0] = MAVLink.MAVLINK_STX;
+            }
+            else if (mavlinkversion == 2)
+            {
+                packet[0] = (byte)'U';
+            }
+            packet[1] = (byte)(rtkdataleng);
+            packet[2] = (byte)packetcount;
+            //packet[2] = 35;
+            packetcount++;
+
+            packet[3] = 255; // this is always 255 - MYGCS
+
+            packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
+
+            packet[5] = messageType;
+
+            int i = 6;
+            foreach (byte b in data)
+            {
+                if (i < rtkdataleng + 6 )
+                    packet[i] = b;
+                else break;
+                i++;
+            }
+
+            ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);
+
+            if (mavlinkversion == 3 || mavlinkversion == 0)
+            {
+                checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[messageType], checksum);
+            }
+
+            byte ck_a = (byte)(checksum & 0xFF); ///< High byte
+            byte ck_b = (byte)(checksum >> 8); ///< Low byte
+
+            packet[i] = ck_a;
+            i += 1;
+            packet[i] = ck_b;
+            i += 1;
+
+            if (IsConnected)
+            {
+                try
+                {
+                    await Connection.WriteAsync(packet, 0, i);
+                    //Console.WriteLine("sendout:" + i);
+                    _sendnumber += i; //统计发送数量
                 }
                 catch
                 {
@@ -1108,7 +1204,7 @@ namespace Plane.Copters
                 return;
             }
 
-            //log.InfoFormat("Request stream {0} at {1} hz", Enum.Parse(typeof(MAV_DATA_STREAM), id.ToString()), hzrate);
+           // log.InfoFormat("Request stream {0} at {1} hz", Enum.Parse(typeof(MAV_DATA_STREAM), id.ToString()), hzrate);
             await GetDatastreamAsync(id, hzrate);
         }
 
@@ -1245,7 +1341,19 @@ namespace Plane.Copters
             await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req);
             await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req);
         }
+        public void GetCommunicationNumber(out int recnumber, out int sendnumber)
+        {
+            recnumber=_recnumber;
+            sendnumber = _sendnumber;
+        }
 
+
+        //重设数据量
+        public void ResetCommunicationNumber()
+        {
+            _recnumber = 0;
+            _sendnumber = 0;
+        }
         async Task StopGetDatastreamAsync(MAVLink.MAV_DATA_STREAM id, byte hzrate)
         {
             MAVLink.mavlink_request_data_stream_t req = new MAVLink.mavlink_request_data_stream_t();
@@ -1333,16 +1441,18 @@ namespace Plane.Copters
         {
             MAVLink.mavlink_gps_inject_data_t gps = new MAVLink.mavlink_gps_inject_data_t();
             var msglen = 110;
+            int datalen = 0;
             var len = (length % msglen) == 0 ? length / msglen : (length / msglen) + 1;
             for (int a = 0; a < len; a++)
             {
+                datalen = Math.Min(length - a * msglen, msglen);
                 gps.data = new byte[msglen];
-                int copy = Math.Min(length - a * msglen, msglen);
+                Array.Copy(data, a * msglen, gps.data, 0, datalen);
-                Array.Copy(data, a * msglen, gps.data, 0, copy);
+                gps.len = (byte)datalen;
-                gps.len = (byte)copy;
                 gps.target_component = compid;
                 gps.target_system = sysid;
-                await GeneratePacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps);
+                await GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len);
+               // Console.WriteLine("send:" + (ushort)gps.len);
             }
         }