public static Packet parse(Bytes data) { if (data.Length < 5) { return(null); } var crc_computed = CrcUtil.Crc8(data.Sub(0, data.Length - 1).ToArray()); var crc = data[data.Length - 1]; if (crc != crc_computed) { return(null); } var address = data.DWord(0); var d4 = data.Byte(4); var type = (RadioPacketType)(d4 & 0b11100000); var sequence = d4 & 0b00011111; var body = data.Sub(5); return(new Packet(address, type, sequence, body)); }
private Packet GetPacket(Bytes data) { if (data != null && data.Length > 1) { byte rssi = data[0]; try { var rp = Packet.parse(data.Sub(2)); if (rp != null) { rp.rssi = rssi; } return(rp); } catch { this.PacketLogger.Log($"RECV INVALID DATA {data}"); } } return(null); }
public List <Packet> get_radio_packets(int first_packet_sequence) { this.message_str_prefix = $"{this.address:%08X} {this.sequence:%02X} {this.expect_critical_followup} "; var message_body_len = 0; foreach (var p in this.parts) { var cmd_body = p.Item2; var nonce = p.Item3; message_body_len += (int)cmd_body.Length + 2; if (nonce != null) { message_body_len += 4; } } byte b0 = 0; if (this.expect_critical_followup) { b0 = 0x80; } b0 |= (byte)(this.sequence << 2); b0 |= (byte)((message_body_len >> 8) & 0x03); byte b1 = (byte)(message_body_len & 0xff); var message_body = new Bytes(this.address.Value); message_body.Append(b0); message_body.Append(b1); foreach (var p in this.parts) { var cmd_type = p.Item1; var cmd_body = p.Item2; var nonce = p.Item3; if (nonce == null) { if (cmd_type == (byte)PodResponse.Status) { message_body.Append(cmd_type); } else { message_body.Append(cmd_type); message_body.Append((byte)cmd_body.Length); } } else { message_body.Append(cmd_type); message_body.Append((byte)(cmd_body.Length + 4)); message_body.Append(nonce.Value); } message_body.Append(cmd_body[0]); } var crc_calculated = CrcUtil.Crc16(message_body.ToArray()); message_body.Append(crc_calculated); int index = 0; bool first_packet = true; int sequence = first_packet_sequence; int total_body_len = (int)message_body.Length; var radio_packets = new List <Packet>(); while (index < message_body.Length) { var to_write = Math.Min(31, message_body.Length - index); var packet_body = message_body.Sub(index, index + to_write); radio_packets.Add(new Packet(AckAddressOverride.Value, first_packet ? this.type.Value : RadioPacketType.CON, sequence, packet_body)); first_packet = false; sequence = (sequence + 2) % 32; index += to_write; } if (this.DoubleTake) { var fp = radio_packets[0]; radio_packets.Insert(0, fp); } return(radio_packets); }