From f6e29673d1665a6ca73bc417415bca389ac01f13 Mon Sep 17 00:00:00 2001 From: Zhuyahong Date: Tue, 8 Nov 2022 15:05:47 +0800 Subject: [PATCH] =?UTF-8?q?feat:=2016=E4=B8=AA=E9=80=9A=E9=81=93=E6=95=B0?= =?UTF-8?q?=E6=8D=AE=E4=BC=A0=E8=BE=93=E6=88=90=E5=8A=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- LoraGamepad/Models/CrsfChMsg.cs | 47 +++++++++++++++++++++++++ LoraGamepad/Util/CrcUtil.cs | 20 +++++++++-- LoraGamepad/Util/CrsfParserPipeIn.cs | 25 +++++++++++++ LoraGamepad/Util/SerialPipeIN.cs | 40 ++++++++++++++------- LoraGamepad/ViewModels/TProViewModel.cs | 15 +++++--- 5 files changed, 129 insertions(+), 18 deletions(-) create mode 100644 LoraGamepad/Models/CrsfChMsg.cs create mode 100644 LoraGamepad/Util/CrsfParserPipeIn.cs diff --git a/LoraGamepad/Models/CrsfChMsg.cs b/LoraGamepad/Models/CrsfChMsg.cs new file mode 100644 index 0000000..171d29b --- /dev/null +++ b/LoraGamepad/Models/CrsfChMsg.cs @@ -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 + }; + } +} \ No newline at end of file diff --git a/LoraGamepad/Util/CrcUtil.cs b/LoraGamepad/Util/CrcUtil.cs index 6e02bdf..edb236b 100644 --- a/LoraGamepad/Util/CrcUtil.cs +++ b/LoraGamepad/Util/CrcUtil.cs @@ -10,11 +10,27 @@ 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; + } /// /// CRC8——MAXIM校验表 diff --git a/LoraGamepad/Util/CrsfParserPipeIn.cs b/LoraGamepad/Util/CrsfParserPipeIn.cs new file mode 100644 index 0000000..ebce00e --- /dev/null +++ b/LoraGamepad/Util/CrsfParserPipeIn.cs @@ -0,0 +1,25 @@ +using System; +using System.Collections.Concurrent; +using System.Threading; +using LoraGamepad.Models; + +namespace LoraGamepad.Util; + +public class CrsfParserPipeIn: IAsyncPipe +{ + protected override void Process(ConcurrentQueue 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; + } + } +} \ No newline at end of file diff --git a/LoraGamepad/Util/SerialPipeIN.cs b/LoraGamepad/Util/SerialPipeIN.cs index 42d420d..f5271bd 100644 --- a/LoraGamepad/Util/SerialPipeIN.cs +++ b/LoraGamepad/Util/SerialPipeIN.cs @@ -1,12 +1,24 @@ +/// 串口 +/// +/// +/// FRAME:
+// 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 { + private const int TpyeAndCrcLength = 2; + private const int HeaderAndLenLength = 2; + private const int FrameCrcLength = 1; protected override void Process(ConcurrentQueue queue) { if (!queue.TryPeek(out var first)) @@ -15,35 +27,39 @@ public class SerialPipeIn: IAsyncPipe // 队列无数据 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()); } - - } \ No newline at end of file diff --git a/LoraGamepad/ViewModels/TProViewModel.cs b/LoraGamepad/ViewModels/TProViewModel.cs index 882c866..e9a0e26 100644 --- a/LoraGamepad/ViewModels/TProViewModel.cs +++ b/LoraGamepad/ViewModels/TProViewModel.cs @@ -20,7 +20,7 @@ public class TProViewModel : ViewModelBase public CBPortItem PortSelectItem { get; set; } private readonly SerialPipeIn _serialPipeIn; - + private readonly CrsfParserPipeIn _crsfParserPipeIn; public ReactiveCommand OpenPort { get; } public ReactiveCommand ClosePort { get; } @@ -29,13 +29,20 @@ 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); ClosePort = ReactiveCommand.Create(PortCloseButtonClick); } - + public void ReadThreadEntry() { while (true) @@ -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();