group/Plane.Sdk3
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

[PlaneCopter.cs]

Browse Source 
1加入数据量统计
2降低飞机发到地面的数据频率，5hz改为1hz
3收数据不写日志（导致死机）
4为RTK数据单独组包，以减小包大小（发送大小为：RTK数据+11字节/包协议开销，自动拆分为110字节/包）
This commit is contained in:
panxu 2018-05-12 23:24:43 +08:00
parent fef98ae096
commit 22b8f46359
5 changed files with 172 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
PlaneGcsSdk.Contract_Shared/Copters/ICopterActions.cs
View File

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

14
PlaneGcsSdk_Shared/Copters/EmptyCopter.cs
View File

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

14
PlaneGcsSdk_Shared/Copters/FakeCopter.cs
View File

@ -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。

15
PlaneGcsSdk_Shared/Copters/PLCopter.cs
View File

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

138
PlaneGcsSdk_Shared/Copters/PlaneCopter.cs
View File

@ -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
{
@ -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);
}
}