47 lines
2.2 KiB
C#
47 lines
2.2 KiB
C#
|
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
|
||
|
};
|
||
|
}
|
||
|
}
|