diff --git a/PlaneGcsSdk.Contract_Shared/Copters/ICopterActions.cs b/PlaneGcsSdk.Contract_Shared/Copters/ICopterActions.cs index d579501..d6ec450 100644 --- a/PlaneGcsSdk.Contract_Shared/Copters/ICopterActions.cs +++ b/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); diff --git a/PlaneGcsSdk_Shared/Copters/EmptyCopter.cs b/PlaneGcsSdk_Shared/Copters/EmptyCopter.cs index 6af04e8..4e74f16 100644 --- a/PlaneGcsSdk_Shared/Copters/EmptyCopter.cs +++ b/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; diff --git a/PlaneGcsSdk_Shared/Copters/FakeCopter.cs b/PlaneGcsSdk_Shared/Copters/FakeCopter.cs index 203b91c..c5dde58 100644 --- a/PlaneGcsSdk_Shared/Copters/FakeCopter.cs +++ b/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。 diff --git a/PlaneGcsSdk_Shared/Copters/PLCopter.cs b/PlaneGcsSdk_Shared/Copters/PLCopter.cs index ea28c49..acbc1ae 100644 --- a/PlaneGcsSdk_Shared/Copters/PLCopter.cs +++ b/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(); diff --git a/PlaneGcsSdk_Shared/Copters/PlaneCopter.cs b/PlaneGcsSdk_Shared/Copters/PlaneCopter.cs index e48a757..b34527c 100644 --- a/PlaneGcsSdk_Shared/Copters/PlaneCopter.cs +++ b/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(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, copy); - gps.len = (byte)copy; + Array.Copy(data, a * msglen, gps.data, 0, datalen); + gps.len = (byte)datalen; 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); } }