TProPCMonitor/LoraGamepad/Models/CrsfChMsg.cs

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);
       //  }
       Console.WriteLine(BitConverter.ToString(raw));
        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();
    }
}