65 lines
1.8 KiB
C#
65 lines
1.8 KiB
C#
/// 串口
|
||
///
|
||
///
|
||
/// FRAME:<ADDRESS><LENGTH><TYPE><PALOAD><CRC>
|
||
// ADDRESS: FLIGHT_CONTROL 0XC8
|
||
// LENGTH: CONTAIN TYPE,PALOAD,CRC
|
||
// TYPE: LINK STATE 0X14
|
||
// CHANNEL 0X16
|
||
// CRC: CRC8_DVB_S2 1BYTE
|
||
|
||
using System;
|
||
using System.Collections.Concurrent;
|
||
using System.Linq;
|
||
using System.Threading;
|
||
|
||
namespace LoraGamepad.Util;
|
||
public class SerialPipeIn: IAsyncPipe<byte,byte[]>
|
||
{
|
||
private const int TpyeAndCrcLength = 2;
|
||
private const int HeaderAndLenLength = 2;
|
||
private const int FrameCrcLength = 1;
|
||
protected override void Process(ConcurrentQueue<byte> queue)
|
||
{
|
||
if (!queue.TryPeek(out var first))
|
||
{
|
||
SpinWait.SpinUntil(() => false, 1);
|
||
// 队列无数据
|
||
return;
|
||
}
|
||
|
||
if (0xC8 != first) {
|
||
// 未匹配到包头,扔掉继续
|
||
queue.TryDequeue(out _);
|
||
return;
|
||
}
|
||
|
||
if (queue.Count < 10) {
|
||
// 数据长度过低
|
||
return;
|
||
}
|
||
|
||
var packLength = queue.ElementAt(1);
|
||
if (queue.Count < packLength + HeaderAndLenLength)
|
||
{
|
||
return;
|
||
}
|
||
|
||
var pack = queue.Take(packLength + HeaderAndLenLength).ToArray();
|
||
var crc = pack[packLength + HeaderAndLenLength - FrameCrcLength];
|
||
if (crc != CrcUtil.CRC8_Calculate(pack.Skip(HeaderAndLenLength).SkipLast(FrameCrcLength).ToArray())) {
|
||
// crc校验失败，砍头
|
||
queue.TryDequeue(out _);
|
||
return;
|
||
}
|
||
|
||
// 队列消除整包数据
|
||
for (var i = 0; i < packLength + HeaderAndLenLength; i++) {
|
||
queue.TryDequeue(out _);
|
||
}
|
||
|
||
// 输出数据，砍头砍尾
|
||
// Console.WriteLine(BitConverter.ToString(pack));
|
||
OnOut(pack.Skip(HeaderAndLenLength).SkipLast(FrameCrcLength).ToArray());
|
||
}
|
||
} |