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); }