rx_int16 IOBuffer::consumeI16LE() {
rx_int16 val = 0;
memcpy(&val, buffer+consumed, 2);
val = FromLE16(val);
consumed += 2;
return val;
}
FLARM::MessageType
FlarmDevice::WaitForACKOrNACK(uint16_t sequence_number,
AllocatedArray<uint8_t> &data, uint16_t &length,
OperationEnvironment &env, unsigned timeout_ms)
{
const TimeoutClock timeout(timeout_ms);
// Receive frames until timeout or expected frame found
while (!timeout.HasExpired()) {
// Wait until the next start byte comes around
if (!WaitForStartByte(env, timeout.GetRemainingOrZero()))
continue;
// Read the following FrameHeader
FLARM::FrameHeader header;
if (!ReceiveFrameHeader(header, env, timeout.GetRemainingOrZero()))
continue;
// Read and check length of the FrameHeader
length = header.GetLength();
if (length <= sizeof(header))
continue;
// Calculate payload length
length -= sizeof(header);
// Read payload and check length
data.GrowDiscard(length);
if (!ReceiveEscaped(data.begin(), length,
env, timeout.GetRemainingOrZero()))
continue;
// Verify CRC
if (header.GetCRC() != FLARM::CalculateCRC(header, data.begin(), length))
continue;
// Check message type
if (header.type != FLARM::MT_ACK && header.type != FLARM::MT_NACK)
continue;
// Check payload length
if (length < 2)
continue;
// Check whether the received ACK is for the right sequence number
if (FromLE16(*((const uint16_t *)(const void *)data.begin())) ==
sequence_number)
return (FLARM::MessageType)header.type;
}
return FLARM::MT_ERROR;
}
uint16_t GetLength() const {
return FromLE16(length);
}
uint16_t GetCRC() const {
return FromLE16(crc);
}
