public void Serialize(ref Span <byte> buffer) { var fillBuffer = buffer.Slice(0); var payloadBuffer = buffer.Slice(10); var payloadSize = Payload.Serialize(ref payloadBuffer); Debug.Assert(payloadSize <= byte.MaxValue, $"Wrong payload serialize size (must be {byte.MaxValue} size)"); BinSerialize.WriteByte(ref fillBuffer, PacketV2Helper.MagicMarkerV2); BinSerialize.WriteByte(ref fillBuffer, (byte)payloadSize); BinSerialize.WriteByte(ref fillBuffer, IncompatFlags); BinSerialize.WriteByte(ref fillBuffer, CompatFlags); BinSerialize.WriteByte(ref fillBuffer, Sequence); BinSerialize.WriteByte(ref fillBuffer, SystemId); BinSerialize.WriteByte(ref fillBuffer, ComponenId); BinSerialize.WriteByte(ref fillBuffer, (byte)((MessageId) & 0xFF)); BinSerialize.WriteByte(ref fillBuffer, (byte)((MessageId >> 8) & 0xFF)); BinSerialize.WriteByte(ref fillBuffer, (byte)((MessageId >> 16) & 0xFF)); var crcBuff = buffer.Slice(1, 9); var crc = X25Crc.Accumulate(ref crcBuff, X25Crc.CrcSeed); crcBuff = buffer.Slice(10, payloadSize); crc = X25Crc.Accumulate(ref crcBuff, crc); crc = X25Crc.Accumulate(GetCrcEtra(), crc); fillBuffer = fillBuffer.Slice(payloadSize); BinSerialize.WriteUShort(ref fillBuffer, crc); if (Signature.IsPresent) { Signature.Serialize(ref fillBuffer); } // Debug.Assert((payloadSize + Signature.ByteSize + PacketV2Helper.PacketV2FrameSize) == (buffer.Length - fillBuffer.Length)); buffer = fillBuffer; }
private static ushort CalcCrc(byte[] buffer, int inx, byte crcEtra, byte payloadSize) { var crc = X25Crc.Accumulate(buffer, X25Crc.CrcSeed, inx + 1, 9); crc = X25Crc.Accumulate(buffer, crc, 10, payloadSize); crc = X25Crc.Accumulate(crcEtra, crc); return(crc); }
public void Deserialize(ref ReadOnlySpan <byte> buffer) { var crcBuffer = buffer.Slice(1); var stx = BinSerialize.ReadByte(ref buffer); if (stx != PacketV2Helper.MagicMarkerV2) { throw new MavlinkException(string.Format(RS.WheelKnownConstant_VerifyStx_Unknown_STX_value, PacketV2Helper.MagicMarkerV2, stx)); } var payloadSize = BinSerialize.ReadByte(ref buffer); IncompatFlags = BinSerialize.ReadByte(ref buffer); CompatFlags = BinSerialize.ReadByte(ref buffer); Sequence = BinSerialize.ReadByte(ref buffer); SystemId = BinSerialize.ReadByte(ref buffer); ComponenId = BinSerialize.ReadByte(ref buffer); int messageId = BinSerialize.ReadByte(ref buffer); messageId |= BinSerialize.ReadByte(ref buffer) << 8; messageId |= BinSerialize.ReadByte(ref buffer) << 16; if (messageId != MessageId) { throw new MavlinkException(string.Format(RS.PacketV2_Deserialize_Error_message_id_type, MessageId, messageId)); } var crcStartBuffer = buffer.Slice(payloadSize, 2); var crc = BinSerialize.ReadUShort(ref crcStartBuffer); var originSize = Payload.GetMinByteSize(); if (payloadSize < originSize) { // this is Empty-Byte Payload Truncation https://mavlink.io/en/guide/serialization.html#payload_truncation var data = ArrayPool <byte> .Shared.Rent(originSize); var span = new Span <byte>(data, 0, originSize); buffer.Slice(0, payloadSize).CopyTo(span); var zeroByteNotEnough = originSize - payloadSize; for (var i = 0; i < zeroByteNotEnough; i++) { data[payloadSize + i] = 0; } try { var readOnly = new ReadOnlySpan <byte>(data, 0, originSize); Payload.Deserialize(ref readOnly, originSize); // Debug.Assert(readOnly.Length == 0); } finally { ArrayPool <byte> .Shared.Return(data); } } else { var payloadBuffer = buffer.Slice(0, payloadSize); Payload.Deserialize(ref payloadBuffer, payloadSize); //Debug.Assert(payloadBuffer.Length == 0); } var crcBuffer1 = crcBuffer.Slice(0, 9); var crcBuffer2 = crcBuffer.Slice(9, payloadSize); var calcCrc = X25Crc.Accumulate(ref crcBuffer1, X25Crc.CrcSeed); calcCrc = X25Crc.Accumulate(ref crcBuffer2, calcCrc); calcCrc = X25Crc.Accumulate(GetCrcEtra(), calcCrc); if (crc != calcCrc) { throw new MavlinkException(string.Format(RS.PacketV2Helper_VerifyCrc_Bad_X25Crc, calcCrc, crc)); } if ((IncompatFlags & 0x01) != 0) { Signature.Deserialize(ref buffer); } }