private async Task AcknowledgeEndOfMessage(RadioPacket ackPacket)
{
try
{
Debug.WriteLine("Sending final ack");
await SendPacket(ackPacket);
this.Pod.PacketSequence++;
Debug.WriteLine("Message exchange finalized");
}
catch (Exception)
{
Debug.WriteLine("Final ack failed, ignoring.");
}
}
private RadioPacket GetPacket(Bytes data)
{
if (data != null && data.Length > 1)
{
byte rssi = data[0];
try
{
var rp = RadioPacket.parse(data.Sub(2));
if (rp != null)
{
rp.rssi = rssi;
}
return(rp);
}
catch
{
Debug.WriteLine($"RECV INVALID DATA {data}");
}
}
return(null);
}
public bool WithRadioPacket(RadioPacket radio_packet)
{
if (radio_packet.type == PacketType.POD || radio_packet.type == PacketType.PDM)
{
ResponseMessage.type = radio_packet.type;
ResponseMessage.address = radio_packet.body.DWord(0);
var r4 = radio_packet.body.Byte(4);
ResponseMessage.sequence = (r4 >> 2) & 0x0f;
ResponseMessage.expect_critical_followup = (r4 & 0x80) > 0;
ResponseMessage.body_length = ((r4 & 0x03) << 8) | radio_packet.body.Byte(5);
ResponseMessage.body_prefix = radio_packet.body.Sub(0, 6);
ResponseMessage.body = radio_packet.body.Sub(6);
}
else
{
if (radio_packet.type == PacketType.CON)
{
ResponseMessage.body.Append(radio_packet.body);
}
else
{
throw new ErosProtocolException("Packet type invalid");
}
}
if (ResponseMessage.body_length == ResponseMessage.body.Length - 2)
{
var bodyWithoutCrc = ResponseMessage.body.Sub(0, ResponseMessage.body.Length - 2);
var crc = ResponseMessage.body.Word(ResponseMessage.body.Length - 2);
var crc_calculated = CrcUtil.Crc16(new Bytes(ResponseMessage.body_prefix, bodyWithoutCrc).ToArray());
if (crc == crc_calculated)
{
ResponseMessage.body = bodyWithoutCrc;
var bi = 0;
while (bi < ResponseMessage.body.Length)
{
var response_type = (PartType)ResponseMessage.body[bi];
Bytes response_body;
if (response_type == PartType.ResponseStatus)
{
response_body = ResponseMessage.body.Sub(bi + 1);
bi = ResponseMessage.body.Length;
}
else
{
var response_len = ResponseMessage.body[bi + 1];
response_body = ResponseMessage.body.Sub(bi + 2, bi + 2 + response_len);
bi += response_len + 2;
}
ResponseMessage.parts.Add(new ErosResponse()
{
PartType = response_type, PartData = response_body
});
}
return(true);
}
else
{
throw new ErosProtocolException("Message crc error");
}
}
else
{
return(false);
}
}
public ErosProtocolException(string message = "Unknown protocol error", RadioPacket received = null) : base(message)
{
this.ReceivedPacket = received;
}
public async Task <IMessage> GetResponse(IMessage requestMessage, IMessageProgress messageExchangeProgress, CancellationToken ct)
{
this.Started = DateTime.UtcNow;
if (MessageExchangeParameters.TransmissionLevelOverride.HasValue)
{
RileyLink.SetTxLevel(MessageExchangeParameters.TransmissionLevelOverride.Value);
}
var erosRequestMessage = requestMessage as ErosMessage;
var packets = GetRadioPackets(erosRequestMessage);
RadioPacket received = null;
var packet_count = packets.Count;
this.unique_packets = packet_count * 2;
for (int part = 0; part < packet_count; part++)
{
var packet = packets[part];
int repeat_count = -1;
int timeout = 10000;
while (true)
{
repeat_count++;
if (repeat_count == 0)
{
Debug.WriteLine($"Sending PDM message part {part + 1}/{packet_count}");
}
else
{
Debug.WriteLine($"Sending PDM message part {part + 1}/{packet_count} (Repeat: {repeat_count})");
}
PacketType expected_type;
if (part == packet_count - 1)
{
expected_type = PacketType.POD;
}
else
{
expected_type = PacketType.ACK;
}
try
{
received = await this.ExchangePackets(packet.with_sequence(this.Pod.PacketSequence), expected_type, timeout);
break;
}
catch (OmniCoreTimeoutException)
{
Debug.WriteLine("Trying to recover from timeout error");
if (part == 0)
{
if (repeat_count == 0)
{
timeout = 15000;
continue;
}
else if (repeat_count == 1)
{
timeout = 10000;
Thread.Sleep(2000);
continue;
}
else if (repeat_count == 2)
{
await RileyLink.Reset();
timeout = 15000;
continue;
}
else
{
Debug.WriteLine("Failed recovery");
Pod.PacketSequence = 0;
throw;
}
}
else if (part < packet_count - 1)
{
if (repeat_count < 2)
{
timeout = 20000;
continue;
}
else
{
throw;
}
}
else
{
if (repeat_count < 10)
{
timeout = 20000;
continue;
}
else
{
throw;
}
}
}
catch (PacketRadioException)
{
Debug.WriteLine("Trying to recover from radio error");
this.radio_errors++;
if (part == 0)
{
if (repeat_count < 2)
{
await RileyLink.Reset();
continue;
}
else if (repeat_count < 4)
{
await RileyLink.Reset();
timeout = 10000;
Thread.Sleep(2000);
continue;
}
else
{
Debug.WriteLine("Failed recovery");
throw;
}
}
else if (part < packet_count - 1)
{
if (repeat_count < 6)
{
await RileyLink.Reset();
timeout = 10000;
Thread.Sleep(2000);
continue;
}
else
{
Debug.WriteLine("Failed recovery");
throw;
}
}
else
{
if (repeat_count < 10)
{
await RileyLink.Reset();
timeout = 10000;
Thread.Sleep(2000);
continue;
}
else
{
Debug.WriteLine("Failed recovery");
Pod.PacketSequence = 0;
throw;
}
}
}
catch (ErosProtocolException pe)
{
if (pe.ReceivedPacket != null && expected_type == PacketType.POD && pe.ReceivedPacket.type == PacketType.ACK)
{
Debug.WriteLine("Trying to recover from protocol error");
this.Pod.PacketSequence++;
return(await GetResponse(requestMessage, messageExchangeProgress, ct));
}
else
{
throw pe;
}
}
catch (Exception) { throw; }
}
part++;
this.Pod.PacketSequence = (received.sequence + 1) % 32;
}
Debug.WriteLine($"SENT MSG {requestMessage}");
var part_count = 0;
if (received.type == PacketType.POD)
{
part_count = 1;
Debug.WriteLine($"Received POD message part {part_count}");
}
var responseBuilder = new ErosResponseBuilder();
var radioAddress = Pod.RadioAddress;
if (MessageExchangeParameters.AddressOverride.HasValue)
{
radioAddress = MessageExchangeParameters.AddressOverride.Value;
}
var ackAddress = radioAddress;
if (MessageExchangeParameters.AckAddressOverride.HasValue)
{
ackAddress = MessageExchangeParameters.AckAddressOverride.Value;
}
while (!responseBuilder.WithRadioPacket(received))
{
var ackPacket = this.InterimAckPacket(ackAddress, (received.sequence + 1) % 32);
received = await this.ExchangePackets(ackPacket, PacketType.CON);
part_count++;
Debug.WriteLine($"Received POD message part {part_count}");
}
var podResponse = responseBuilder.Build();
Debug.WriteLine($"RCVD MSG {podResponse}");
Debug.WriteLine("Send and receive completed.");
this.Pod.MessageSequence = (podResponse.sequence.Value + 1) % 16;
this.Pod.PacketSequence = (received.sequence + 1) % 32;
var finalAckPacket = this.FinalAckPacket(ackAddress, (received.sequence + 1) % 32);
FinalAckTask = Task.Run(() => AcknowledgeEndOfMessage(finalAckPacket));
return(podResponse);
}
private async Task SendPacket(RadioPacket packet_to_send, int timeout = 25000)
{
int start_time = 0;
this.unique_packets++;
Bytes received = null;
while (start_time == 0 || Environment.TickCount - start_time < timeout)
{
try
{
Debug.WriteLine($"SEND PKT {packet_to_send}");
try
{
received = await RileyLink.SendAndGetPacket(packet_to_send.get_data(), 0, 0, 300, 3, 300);
}
catch (OmniCoreTimeoutException)
{
Debug.WriteLine("Silence fell.");
this.Pod.PacketSequence = (this.Pod.PacketSequence + 1) % 32;
return;
}
if (start_time == 0)
{
start_time = Environment.TickCount;
}
var p = this.GetPacket(received);
if (p == null)
{
this.bad_packets++;
RileyLink.TxLevelDown();
continue;
}
if (p.address != this.Pod.RadioAddress)
{
this.bad_packets++;
Debug.WriteLine("RECV PKT ADDR MISMATCH");
RileyLink.TxLevelDown();
continue;
}
this.last_packet_timestamp = Environment.TickCount;
if (this.last_received_packet != null && p.type == this.last_received_packet.type &&
p.sequence == this.last_received_packet.sequence)
{
this.repeated_receives++;
Debug.WriteLine("RECV PKT previous");
RileyLink.TxLevelUp();
continue;
}
Debug.WriteLine($"RECV PKT {p}");
Debug.WriteLine($"RECEIVED unexpected packet");
this.protocol_errors++;
this.last_received_packet = p;
this.Pod.PacketSequence = (p.sequence + 1) % 32;
packet_to_send.with_sequence(this.Pod.PacketSequence);
start_time = Environment.TickCount;
continue;
}
catch (PacketRadioException pre)
{
this.radio_errors++;
Debug.WriteLine($"Radio error during send, retrying {pre}");
await RileyLink.Reset();
start_time = Environment.TickCount;
}
catch (Exception) { throw; }
}
Debug.WriteLine("Exceeded timeout while waiting for silence to fall");
}
private async Task <RadioPacket> ExchangePackets(RadioPacket packet_to_send, PacketType expected_type, int timeout = 10000)
{
int start_time = 0;
Bytes received = null;
Debug.WriteLine($"SEND PKT {packet_to_send}");
while (start_time == 0 || Environment.TickCount - start_time < timeout)
{
try
{
if (this.last_packet_timestamp == 0 || (Environment.TickCount - this.last_packet_timestamp) > 2000)
{
received = await RileyLink.SendAndGetPacket(packet_to_send.get_data(), 0, 0, 300, 1, 300);
}
else
{
received = await RileyLink.SendAndGetPacket(packet_to_send.get_data(), 0, 0, 120, 0, 40);
}
}
catch (OmniCoreTimeoutException)
{
received = null;
}
finally
{
if (start_time == 0)
{
start_time = Environment.TickCount;
}
else
{
this.repeated_sends += 1;
}
}
if (received == null)
{
this.receive_timeouts++;
Debug.WriteLine("RECV PKT None");
RileyLink.TxLevelUp();
continue;
}
var p = this.GetPacket(received);
if (p == null)
{
this.bad_packets++;
RileyLink.TxLevelDown();
continue;
}
Debug.WriteLine($"RECV PKT {p}");
if (p.address != this.Pod.RadioAddress)
{
this.bad_packets++;
Debug.WriteLine("RECV PKT ADDR MISMATCH");
RileyLink.TxLevelDown();
continue;
}
this.last_packet_timestamp = Environment.TickCount;
if (this.last_received_packet != null && p.sequence == this.last_received_packet.sequence &&
p.type == this.last_received_packet.type)
{
this.repeated_receives++;
Debug.WriteLine("RECV PKT previous");
RileyLink.TxLevelUp();
continue;
}
this.last_received_packet = p;
this.Pod.PacketSequence = (p.sequence + 1) % 32;
if (p.type != expected_type)
{
Debug.WriteLine("RECV PKT unexpected type");
this.protocol_errors++;
throw new ErosProtocolException("Unexpected packet type received", p);
}
if (p.sequence != (packet_to_send.sequence + 1) % 32)
{
this.Pod.PacketSequence = (p.sequence + 1) % 32;
Debug.WriteLine("RECV PKT unexpected sequence");
this.last_received_packet = p;
this.protocol_errors++;
throw new ErosProtocolException("Incorrect packet sequence received", p);
}
return(p);
}
throw new OmniCoreTimeoutException("Exceeded timeout while send and receive");
}
