Exemple #1
0
        public static ushort compute(byte[] bytes, int len)
        {
            var temp  = new TransferCRC();
            var total = len;

            while (len > 0)
            {
                temp.add(bytes[total - len]);
                len--;
            }

            return(temp.get());
        }
Exemple #2
0
        public string PackageMessage(byte destNode, byte priority, byte transferID, IUAVCANSerialize msg)
        {
            var state = new statetracking();

            msg.encode(uavcan_transmit_chunk_handler, state);

            var msgtype = uavcan.MSG_INFO.First(a => a.Item1 == msg.GetType());

            CANFrame cf = new CANFrame(new byte[4]);

            cf.SourceNode = SourceNode;
            cf.Priority   = priority;

            if (msg.GetType().FullName.EndsWith("_res") || msg.GetType().FullName.EndsWith("_req"))
            {
                // service
                cf.IsServiceMsg       = true;
                cf.SvcDestinationNode = destNode;
                cf.SvcIsRequest       = msg.GetType().FullName.EndsWith("_req") ? true : false;
                cf.SvcTypeID          = (byte)msgtype.Item2;
            }
            else
            {
                // message
                cf.MsgTypeID = (ushort)msgtype.Item2;
            }

            string ans = "";

            var payloaddata = state.ToBytes();

            if (payloaddata.Length > 7)
            {
                var dt_sig = BitConverter.GetBytes(msgtype.Item3);

                var crcprocess = new TransferCRC();
                crcprocess.add(dt_sig, 8);
                crcprocess.add(payloaddata, payloaddata.Length);
                var crc = crcprocess.get();

                var buffer = new byte[8];
                var toogle = false;
                var size   = 7;
                for (int a = 0; a < payloaddata.Length; a += size)
                {
                    if (a == 0)
                    {
                        buffer[0] = (byte)(crc & 0xff);
                        buffer[1] = (byte)(crc >> 8);
                        size      = 5;
                        Array.ConstrainedCopy(payloaddata, a, buffer, 2, 5);
                    }
                    else
                    {
                        size = payloaddata.Length - a <= 7 ? payloaddata.Length - a : 7;
                        Array.ConstrainedCopy(payloaddata, a, buffer, 0, size);
                        if (buffer.Length != size + 1)
                        {
                            Array.Resize(ref buffer, size + 1);
                        }
                    }
                    CANPayload payload = new CANPayload(buffer);
                    payload.SOT        = a == 0 ? true : false;
                    payload.EOT        = a + size == payloaddata.Length ? true : false;
                    payload.TransferID = (byte)transferID;
                    payload.Toggle     = toogle;
                    toogle             = !toogle;

                    ans += String.Format("T{0}{1}{2}\r", cf.ToHex(), a == 0 ? 8 : size + 1, payload.ToHex());
                }
            }
            else
            {
                var buffer = new byte[payloaddata.Length + 1];
                Array.Copy(payloaddata, buffer, payloaddata.Length);
                CANPayload payload = new CANPayload(buffer);
                payload.SOT        = payload.EOT = true;
                payload.TransferID = (byte)transferID;

                ans = String.Format("T{0}{1}{2}\r", cf.ToHex(), buffer.Length, payload.ToHex());
            }

            //Console.Write("TX " + ans.Replace("\r", "\r\n"));
            //Console.WriteLine("TX " + msgtype.Item1 + " " + JsonConvert.SerializeObject(msg));
            return(ans);
        }