diff --git a/src/Participants/ParticipantUDP.cs b/src/Participants/ParticipantUDP.cs index 9144292..4c1e14c 100644 --- a/src/Participants/ParticipantUDP.cs +++ b/src/Participants/ParticipantUDP.cs @@ -305,37 +305,58 @@ namespace RoboidControl { } public void ReceiveData(byte[] data, Participant sender) { - byte msgId = data[0]; + int dataIx = 0; + byte msgId = data[dataIx]; if (msgId == 0xFF) { // Timeout return; } switch (msgId) { - case ParticipantMsg.Id: // 0xA0 / 160 - this.Process(sender, new ParticipantMsg(data)); + case ParticipantMsg.Id: { // 0xA0 / 160 + ParticipantMsg msg = new(data); + this.Process(sender, msg); + dataIx += ParticipantMsg.length; + } break; - case NetworkIdMsg.Id: // 0xA1 / 161 - this.Process(sender, new NetworkIdMsg(data)); + case NetworkIdMsg.Id: {// 0xA1 / 161 + NetworkIdMsg msg = new(data); + this.Process(sender, msg); + dataIx += NetworkIdMsg.length; + } break; case InvestigateMsg.Id: // 0x81 // result = await InvestigateMsg.Receive(dataStream, client, packetSize); break; - case ThingMsg.id: // 0x80 / 128 - this.Process(sender, new ThingMsg(data)); + case ThingMsg.id: { // 0x80 / 128 + ThingMsg msg = new(data); + this.Process(sender, msg); + dataIx += ThingMsg.length; + } break; - case NameMsg.Id: // 0x91 / 145 - this.Process(sender, new NameMsg(data)); + case NameMsg.Id: { // 0x91 / 145 + NameMsg msg = new(data); + this.Process(sender, msg); + dataIx += NameMsg.length + msg.nameLength; + } break; - case ModelUrlMsg.Id: // 0x90 / 144 - this.Process(sender, new ModelUrlMsg(data)); + case ModelUrlMsg.Id: { // 0x90 / 144 + ModelUrlMsg msg = new(data); + this.Process(sender, msg); + dataIx += ModelUrlMsg.length + msg.urlLength; + } break; - case PoseMsg.Id: // 0x10 / 16 - this.Process(sender, new PoseMsg(data)); - // result = await PoseMsg.Receive(dataStream, client, packetSize); + case PoseMsg.Id: { // 0x10 / 16 + PoseMsg msg = new(data); + this.Process(sender, msg); + dataIx += msg.length; + } break; - case BinaryMsg.Id: // 0xB1 / 177 - this.Process(sender, new BinaryMsg(data)); + case BinaryMsg.Id: { // 0xB1 / 177 + BinaryMsg msg = new(data); + this.Process(sender, msg); + dataIx += BinaryMsg.length + msg.dataLength; + } break; case TextMsg.Id: // 0xB0 / 176 // result = await TextMsg.Receive(dataStream, client, packetSize); @@ -347,6 +368,8 @@ namespace RoboidControl { default: break; } + if (dataIx < data.Length) + Console.WriteLine($"####### Buffer not fully read, remaining {data.Length - dataIx}"); } protected virtual void Process(Participant sender, ParticipantMsg msg) { @@ -401,7 +424,7 @@ namespace RoboidControl { protected virtual void Process(Participant sender, PoseMsg msg) { #if DEBUG -// Console.WriteLine($"{this.name}: Process PoseMsg [{msg.networkId}/{msg.thingId}] {msg.poseType}"); + Console.WriteLine($"{this.name}: Process PoseMsg [{msg.networkId}/{msg.thingId}] {msg.poseType}"); #endif Participant owner = Participant.GetParticipant(msg.networkId); if (owner == null)