66 lines
2.8 KiB
C#
66 lines
2.8 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;
|
|
// OpenTx RC PWM (BF)
|
|
// min -1024 172( 22) 988us
|
|
// ctr 0 992(124) 1500us
|
|
// max 1024 1811(226) 2012us
|
|
public class CrsfChMsg
|
|
{
|
|
public int[] channel = new int[16];
|
|
|
|
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();
|
|
|
|
var bitRaw11 = new BitArray(raw);
|
|
|
|
// var rs=Enumerable.Range(0, 11).Select(_=>bitRaw11).ToArray().Reverse().ToList();
|
|
// for (var i = 0; i < rs.Count&&i<ch.Length; i++)
|
|
// {
|
|
// rs[i].CopyTo(ch,i);
|
|
// }
|
|
|
|
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
|
|
ScalingChannel(ch, out var sch);
|
|
return new CrsfChMsg()
|
|
{
|
|
channel = sch
|
|
};
|
|
}
|
|
|
|
private static void ScalingChannel(int[] datain, out int[] dataout)
|
|
{
|
|
dataout = datain.ToList().ConvertAll((a) => (a - 1000) / 9).ToArray();
|
|
}
|
|
} |