feat: 16个通道数据传输成功
parent
ef6f29e261
commit
f6e29673d1
|
|
@ -0,0 +1,47 @@
|
|||
using System;
|
||||
using System.Buffers.Binary;
|
||||
using System.Collections;
|
||||
using System.Collections.Specialized;
|
||||
using System.IO;
|
||||
using System.Linq;
|
||||
using Microsoft.VisualBasic;
|
||||
|
||||
namespace LoraGamepad.Models;
|
||||
|
||||
public class CrsfChMsg
|
||||
{
|
||||
public int[] channel = new int[16];
|
||||
|
||||
public int channel0 = 0;
|
||||
|
||||
public static CrsfChMsg FromBytes(byte[] payload)
|
||||
{
|
||||
var ch = new int[16];
|
||||
// Console.WriteLine($"Raw Data: 1:{payload[1]:X},2:{payload[2]:X}");//,2:{payload[3]:X},3{payload[4]:X},4{payload[5]:X},5{payload[6]:X}");
|
||||
var raw = payload.Skip(1).Reverse().ToArray();
|
||||
ch[15] = ((raw[0 ] & 0xFF) << 3 ) + ((raw[1 ] & 0xE0) >> 5); //res: 5
|
||||
ch[14] = ((raw[1 ] & 0x1F) << 6 ) + ((raw[2 ] & 0xFC) >> 2); //res: 3
|
||||
ch[13] = ((raw[2 ] & 0x03) << 9 ) + ((raw[3 ] & 0xFF) << 1)+ ((raw[4 ] & 0x80) >> 7);
|
||||
ch[12] = ((raw[4 ] & 0x7F) << 4 ) + ((raw[5 ] & 0xF0) >> 4);
|
||||
ch[11] = ((raw[5 ] & 0x0F) << 7 ) + ((raw[6 ] & 0xFE) >> 1);
|
||||
ch[10] = ((raw[6 ] & 0x01) << 10) + ((raw[7 ] & 0xFF) << 2) + ((raw[8 ] & 0xC0) >> 6);
|
||||
ch[9] = ((raw[8 ] & 0x3F) << 5 ) + ((raw[9 ] & 0xF8) >> 3);
|
||||
ch[8] = ((raw[9 ] & 0x07) << 8 ) + ((raw[10] & 0xFF) >> 0);
|
||||
|
||||
ch[7] = ((raw[11] & 0xFF) << 3 ) + ((raw[12] & 0xE0) >> 5); //res: 5
|
||||
ch[6] = ((raw[12] & 0x1F) << 6 ) + ((raw[13] & 0xFC) >> 2); //res: 3
|
||||
ch[5] = ((raw[13] & 0x03) << 9 ) + ((raw[14] & 0xFF) << 1)+ ((raw[15] & 0x80) >> 7);
|
||||
ch[4] = ((raw[15] & 0x7F) << 4 ) + ((raw[16] & 0xF0) >> 4);
|
||||
ch[3] = ((raw[16] & 0x0F) << 7 ) + ((raw[17] & 0xFE) >> 1);
|
||||
ch[2] = ((raw[17] & 0x01) << 10) + ((raw[18] & 0xFF) << 2) + ((raw[19] & 0xC0) >> 6);
|
||||
ch[1] = ((raw[19] & 0x3F) << 5 ) + ((raw[20] & 0xF8) >> 3);
|
||||
ch[0] = ((raw[20] & 0x07) << 8 ) + ((raw[21] & 0xFF) >> 0);
|
||||
// ch[1] = ((payload[2] & 0xF8) >> 3) + ((payload[3] & 0x3F) << 5); // res: 2
|
||||
// ch[2] = ((payload[3] & 0x03) << 9) + ((payload[4] & 0xFF) << 1) + ((payload[5] & 0x80) >> 7); // res: 16
|
||||
// ch[3] = (uint)(((payload[5] & 0x7F) << 4) + ((payload[6] & 0xF0) >> 4)); // res: 4
|
||||
return new CrsfChMsg()
|
||||
{
|
||||
channel = ch
|
||||
};
|
||||
}
|
||||
}
|
||||
|
|
@ -10,12 +10,28 @@ public class CrcUtil
|
|||
public static byte CRC8_Calculate(byte[] data)
|
||||
{
|
||||
byte crc8 = 0;
|
||||
for( int i = 0 ; i < data.Length; i++){
|
||||
crc8 = CRC8Table[crc8^data[i]];
|
||||
for( int i = 0 ; i < data.Length; ++i){
|
||||
crc8 = crc8_dvb_s2_buf(crc8,data[i]);
|
||||
}
|
||||
return(crc8);
|
||||
}
|
||||
|
||||
private static byte crc8_dvb_s2_buf(byte crc, byte dataIn)
|
||||
{
|
||||
crc ^= dataIn;
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
if ((crc & 0x80) != 0 )
|
||||
{
|
||||
var temp = (byte)(crc << 1);
|
||||
crc = (byte)( temp ^ 0xD5);
|
||||
} else {
|
||||
crc = (byte)(crc << 1);
|
||||
}
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// CRC8——MAXIM校验表
|
||||
/// </summary>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,25 @@
|
|||
using System;
|
||||
using System.Collections.Concurrent;
|
||||
using System.Threading;
|
||||
using LoraGamepad.Models;
|
||||
|
||||
namespace LoraGamepad.Util;
|
||||
|
||||
public class CrsfParserPipeIn: IAsyncPipe<byte[],CrsfChMsg>
|
||||
{
|
||||
protected override void Process(ConcurrentQueue<byte[]> queue)
|
||||
{
|
||||
if (!queue.TryDequeue(out var data))
|
||||
{
|
||||
SpinWait.SpinUntil(() => false, 1);
|
||||
return;
|
||||
}
|
||||
switch (data[0])
|
||||
{
|
||||
case 0x16:
|
||||
Console.WriteLine("Get Channel Message!");
|
||||
OnOut(CrsfChMsg.FromBytes(data));
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1,12 +1,24 @@
|
|||
/// 串口
|
||||
///
|
||||
///
|
||||
/// 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))
|
||||
|
|
@ -16,34 +28,38 @@ public class SerialPipeIn: IAsyncPipe<byte,byte[]>
|
|||
return;
|
||||
}
|
||||
|
||||
if (0xFE != first) {
|
||||
if (0xC8 != first) {
|
||||
// 未匹配到包头,扔掉继续
|
||||
queue.TryDequeue(out _);
|
||||
return;
|
||||
}
|
||||
|
||||
if (queue.Count < 14) {
|
||||
if (queue.Count < 10) {
|
||||
// 数据长度过低
|
||||
return;
|
||||
}
|
||||
|
||||
var pack = queue.Take(14).ToArray();
|
||||
if (pack[13] != CrcUtil.CRC8_Calculate(pack.Skip(1).SkipLast(1).ToArray())) {
|
||||
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 < 14; i++) {
|
||||
for (var i = 0; i < packLength + HeaderAndLenLength; i++) {
|
||||
queue.TryDequeue(out _);
|
||||
}
|
||||
|
||||
// 输出数据，砍头砍尾
|
||||
// Console.WriteLine(BitConverter.ToString(pack));
|
||||
OnOut(pack.Skip(1).SkipLast(1).ToArray());
|
||||
|
||||
OnOut(pack.Skip(HeaderAndLenLength).SkipLast(FrameCrcLength).ToArray());
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
|
@ -20,7 +20,7 @@ public class TProViewModel : ViewModelBase
|
|||
public CBPortItem PortSelectItem { get; set; }
|
||||
|
||||
private readonly SerialPipeIn _serialPipeIn;
|
||||
|
||||
private readonly CrsfParserPipeIn _crsfParserPipeIn;
|
||||
public ReactiveCommand<Unit, Unit> OpenPort { get; }
|
||||
public ReactiveCommand<Unit, Unit> ClosePort { get; }
|
||||
|
||||
|
|
@ -29,6 +29,13 @@ public class TProViewModel : ViewModelBase
|
|||
{
|
||||
PortList = CBPortItem.GetPortList();
|
||||
_serialPipeIn = new SerialPipeIn();
|
||||
_crsfParserPipeIn = new CrsfParserPipeIn();
|
||||
_serialPipeIn.OnOut += _crsfParserPipeIn.Push;
|
||||
_crsfParserPipeIn.OnOut += data =>
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"Get Channel 0:{data.channel[0]},1:{data.channel[1]},2:{data.channel[2]},3:{data.channel[3]}");
|
||||
};
|
||||
ReadThread = new Thread(ReadThreadEntry);
|
||||
|
||||
OpenPort = ReactiveCommand.Create(PortOpenButtonClick);
|
||||
|
|
@ -45,7 +52,7 @@ public class TProViewModel : ViewModelBase
|
|||
// Console.WriteLine(BitConverter.ToString(data));
|
||||
_serialPipeIn.Push(data);
|
||||
}
|
||||
Thread.Sleep(1);
|
||||
// Thread.Sleep(1);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -93,7 +100,7 @@ public class TProViewModel : ViewModelBase
|
|||
private void PortOpenButtonClick()
|
||||
{
|
||||
if (PortSelectItem.PortName == "") return;
|
||||
SerialPort.Me.Open(PortSelectItem.PortName,420000,1000);
|
||||
SerialPort.Me.Open(PortSelectItem.PortName,115200,1000);
|
||||
IsOpenPortButtonVisible = false;
|
||||
IsClosePortButtonVisible = true;
|
||||
ReadThread.Start();
|
||||
|
|
|
|||
Loading…
Reference in New Issue