Skip to content

Commit

Permalink
MAVLinkInterface: add reparse to crc fail packets
Browse files Browse the repository at this point in the history
  • Loading branch information
@meee1
meee1 committed Jan 19, 2023
1 parent 0e49cec commit 75ad216
Showing 1 changed file with 70 additions and 37 deletions.
107 changes: 70 additions & 37 deletions ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4552,25 +4552,28 @@ public async Task<MAVLinkMessage> readPacketAsync()
int length = 0;
int readcount = 0;
MAVLinkMessage message = null;
bool skipheader = false;

//These need to be function wide, because of the extension of ReadLock try/catch
byte sysid;
byte compid;
byte packetSeqNo;
uint msgid;
bool packetSeemValid;
bool packetSeemValid = false;

DateTime start = DateTime.Now;

if (debug)
Console.WriteLine(DateTime.Now.Millisecond + " SR0 " + BaseStream?.BytesToRead);
{
await readlock.WaitAsync().ConfigureAwait(false);
}

await readlock.WaitAsync().ConfigureAwait(false);

try
{
if (debug)
Console.WriteLine(DateTime.Now.Millisecond + " SR1 " + BaseStream?.BytesToRead);

readloop:

while ((BaseStream != null && BaseStream.IsOpen) || logreadmode)
{
Expand Down Expand Up @@ -4598,40 +4601,48 @@ public async Task<MAVLinkMessage> readPacketAsync()
// time updated for internal reference
MAV.cs.datetime = DateTime.Now;

DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout);
if (!skipheader)
{
DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout);

if (debug)
Console.WriteLine(DateTime.Now.Millisecond + " SR1a " + BaseStream?.BytesToRead);
if (debug)
Console.WriteLine(DateTime.Now.Millisecond + " SR1a " + BaseStream?.BytesToRead);

while (BaseStream.IsOpen && BaseStream.BytesToRead <= 0)
{
if (DateTime.Now > to)
while (BaseStream.IsOpen && BaseStream.BytesToRead <= 0)
{
log.InfoFormat("MAVLINK: 1 wait time out btr {0} len {1}", BaseStream?.BytesToRead,
length);
throw new TimeoutException("Timeout");
if (DateTime.Now > to)
{
log.InfoFormat("MAVLINK: 1 wait time out btr {0} len {1}",
BaseStream?.BytesToRead,
length);
throw new TimeoutException("Timeout");
}

await Task.Delay(1).ConfigureAwait(false);
if (debug)
Console.WriteLine(DateTime.Now.Millisecond + " SR0b " +
BaseStream?.BytesToRead);
}

await Task.Delay(1).ConfigureAwait(false);
if (debug)
Console.WriteLine(DateTime.Now.Millisecond + " SR0b " + BaseStream?.BytesToRead);
}
Console.WriteLine(DateTime.Now.Millisecond + " SR1a " + BaseStream?.BytesToRead);
if (BaseStream.IsOpen)
{
BaseStream.Read(buffer, count, 1);
if (rawlogfile != null && rawlogfile.CanWrite)
rawlogfile.WriteByte(buffer[count]);
}

if (debug)
Console.WriteLine(DateTime.Now.Millisecond + " SR1a " + BaseStream?.BytesToRead);
if (BaseStream.IsOpen)
{
BaseStream.Read(buffer, count, 1);
if (rawlogfile != null && rawlogfile.CanWrite)
rawlogfile.WriteByte(buffer[count]);
if (debug)
Console.WriteLine(DateTime.Now.Millisecond + " SR1b " + BaseStream?.BytesToRead);
}

if (debug)
Console.WriteLine(DateTime.Now.Millisecond + " SR1b " + BaseStream?.BytesToRead);
skipheader = false;
}
}
catch (Exception e)
{
skipheader = false;
log.Info("MAVLink readpacket read error: " + e.ToString());
break;
}
Expand Down Expand Up @@ -4679,11 +4690,11 @@ public async Task<MAVLinkMessage> readPacketAsync()
int headerlengthstx = headerlength + 1;

// if we have the header, and no other chars, get the length and packet identifiers
if (count == 0 && !logreadmode)
if (count < headerlength && !logreadmode)
{
DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout);

while (BaseStream.IsOpen && BaseStream.BytesToRead < headerlength)
while (BaseStream.IsOpen && BaseStream.BytesToRead < headerlength - count)
{
if (DateTime.Now > to)
{
Expand All @@ -4698,15 +4709,15 @@ public async Task<MAVLinkMessage> readPacketAsync()
if (debug)
Console.WriteLine(DateTime.Now.Millisecond + " SR2a " + BaseStream?.BytesToRead);
var start1 = DateTime.Now;
int read = BaseStream.Read(buffer, 1, headerlength);
int read = BaseStream.Read(buffer, count + 1, headerlength - count);
var end = DateTime.Now - start1;
var lapse = end.TotalMilliseconds;
//Console.WriteLine("read: " + lapse);
if (debug)
Console.WriteLine(DateTime.Now.Millisecond + " SR2b " + BaseStream?.BytesToRead);
count = read;
if (rawlogfile != null && rawlogfile.CanWrite)
rawlogfile.Write(buffer, 1, read);
rawlogfile.Write(buffer, count + 1, read);
count += read;
}

// packet length
Expand Down Expand Up @@ -4814,7 +4825,8 @@ public async Task<MAVLinkMessage> readPacketAsync()
"bps {0} loss {1} left {2} mem {3} mav2 {4} sign {5} mav1 {6} mav2 {7} signed {8} \n",
_bps1,
MAV.synclost, btr,
GC.GetTotalMemory(false) / 1024 / 1024.0, MAV.mavlinkv2, MAV.signing, _mavlink1count,
GC.GetTotalMemory(false) / 1024 / 1024.0, MAV.mavlinkv2, MAV.signing,
_mavlink1count,
_mavlink2count, _mavlink2signed);
_bps2 = _bps1; // prev sec
_bps1 = 0; // current sec
Expand All @@ -4838,7 +4850,8 @@ public async Task<MAVLinkMessage> readPacketAsync()

// calc crc
var sigsize = (message.sig != null) ? MAVLINK_SIGNATURE_BLOCK_LEN : 0;
ushort crc = MavlinkCRC.crc_calculate(buffer, message.Length - sigsize - MAVLINK_NUM_CHECKSUM_BYTES);
ushort crc = MavlinkCRC.crc_calculate(buffer,
message.Length - sigsize - MAVLINK_NUM_CHECKSUM_BYTES);

// calc extra bit of crc for mavlink 1.0/2.0
if (message.header == MAVLINK_STX_MAVLINK1 || message.header == MAVLINK_STX)
Expand All @@ -4855,7 +4868,8 @@ public async Task<MAVLinkMessage> readPacketAsync()
}
else
{
log.InfoFormat("Mavlink Bad Packet (Len Fail) len {0} pkno {1}", buffer.Length, message.msgid);
log.InfoFormat("Mavlink Bad Packet (Len Fail) len {0} pkno {1}", buffer.Length,
message.msgid);
return MAVLinkMessage.Invalid;
}
}
Expand All @@ -4865,14 +4879,30 @@ public async Task<MAVLinkMessage> readPacketAsync()
(message.crc16 & 0xff) != (crc & 0xff))
{
if (buffer.Length > 5 && msginfo.name != null)
log.InfoFormat("Mavlink Bad Packet (crc fail) len {0} crc {1} vs {4} pkno {2} {3}", buffer.Length,
log.InfoFormat("Mavlink Bad Packet (crc fail) len {0} crc {1} vs {4} pkno {2} {3}",
buffer.Length,
crc, message.msgid, msginfo.name.ToString(),
message.crc16);
if (logreadmode)
log.InfoFormat("bad packet pos {0} ", logplaybackfile.BaseStream.Position);
// unknown packet
if (buffer.Length > 5 && String.IsNullOrEmpty(msginfo.name))
ProcessMirrorStream(buffer);

// look within the last 20 bytes
int offset = buffer.Search(new byte[] { MAVLINK_STX }, Math.Max(1, buffer.Length - 20));
if (offset > 0)
{
var newbuffer = new byte[MAVLINK_MAX_PACKET_LEN + 25];
Array.ConstrainedCopy(buffer, offset, newbuffer, 0, buffer.Length - offset);
count = buffer.Length - offset - 1;
log.InfoFormat($"rewind oldlen {buffer.Length} offset {offset} cnt {count}");
buffer = newbuffer;
message = null;
skipheader = true;
goto readloop;
}

return MAVLinkMessage.Invalid;
}

Expand Down Expand Up @@ -4955,6 +4985,7 @@ public async Task<MAVLinkMessage> readPacketAsync()
MAVlist[sysid, compid].packetslost = (MAVlist[sysid, compid].packetslost * 0.8f);
MAVlist[sysid, compid].packetsnotlost = (MAVlist[sysid, compid].packetsnotlost * 0.8f);
}

packetSeemValid = false;
//The following IF had to be splitted to accomodate the new place of Readlock end.
//packetSeemValid helps it.
Expand All @@ -4968,13 +4999,14 @@ public async Task<MAVLinkMessage> readPacketAsync()

{
// the second part is to work around a 3dr radio bug sending dup seqno's
if (packetSeqNo != expectedPacketSeqNo && packetSeqNo != MAVlist[sysid, compid].recvpacketcount)
if (packetSeqNo != expectedPacketSeqNo &&
packetSeqNo != MAVlist[sysid, compid].recvpacketcount)
{
MAVlist[sysid, compid].synclost++; // actual sync loss's
int numLost = 0;

if (packetSeqNo < ((MAVlist[sysid, compid].recvpacketcount + 1)))
// recvpacketcount = 255 then 10 < 256 = true if was % 0x100 this would fail
// recvpacketcount = 255 then 10 < 256 = true if was % 0x100 this would fail
{
numLost = 0x100 - expectedPacketSeqNo + packetSeqNo;
}
Expand All @@ -4987,7 +5019,8 @@ public async Task<MAVLinkMessage> readPacketAsync()
WhenPacketLost.OnNext(numLost);

if (!logreadmode)
log.InfoFormat("mav {2}-{4} seqno {0} exp {3} pkts lost {1}", packetSeqNo, numLost,
log.InfoFormat("mav {2}-{4} seqno {0} exp {3} pkts lost {1}", packetSeqNo,
numLost,
sysid,
expectedPacketSeqNo, compid);
}
Expand Down

0 comments on commit 75ad216

Please sign in to comment.