inline uint8_t RadioTask::_parsePacketBuffer(byte packetBuffer[], uint8_t packetBufferLength) {
// Have we received a whole packet yet?
bool receivedWholePacket =
packetBufferLength >= 5 + 1 && // Has to have at least one byte payload
packetBuffer[packetBufferLength - 1] >= 48 && packetBuffer[packetBufferLength - 1] <= 57 && // Digit
packetBuffer[packetBufferLength - 2] >= 48 && packetBuffer[packetBufferLength - 2] <= 57 && // Digit
packetBuffer[packetBufferLength - 3] >= 48 && packetBuffer[packetBufferLength - 3] <= 57 && // Digit
packetBuffer[packetBufferLength - 4] == '-' &&
packetBuffer[packetBufferLength - 5] == '|';
// If the data looks like a packet we can start parsing it
if (receivedWholePacket) {
// read rssi and then ignore the packet footer
uint8_t rssi = (packetBuffer[packetBufferLength - 1] - 48) +
(packetBuffer[packetBufferLength - 2] - 48) * 10 +
(packetBuffer[packetBufferLength - 3] - 48) * 100;
packetBufferLength -= 5;
if (_radioState == RADIO_STATE_DISCOVERY) {
_handleDiscoveryPacket(packetBuffer, packetBufferLength, rssi);
} else if (_radioState == RADIO_STATE_RECEIVE) {
_handleReceivePacket(packetBuffer, packetBufferLength);
}
packetBufferLength = 0;
} else if (packetBufferLength == RADIO_PACKET_WITH_RSSI_LENGTH) {
debug::log("RadioTask: Packet does not conform");
//debug::logByteArray(packetBuffer, 58);
// Something's wrong, we received enough bytes but it's not formated correctly.
packetBufferLength = 0;
}
return packetBufferLength;
}
inline uint8_t RadioReceiveTask::_parsePacketBuffer(byte packetBuffer[], uint8_t packetBufferLength) {
// Have we received a whole packet yet?
bool receivedWholePacket =
packetBufferLength >= 5 + 1 && // Has to have at least one byte payload
packetBuffer[packetBufferLength - 1] >= 48 && packetBuffer[packetBufferLength - 1] <= 57 && // Digit
packetBuffer[packetBufferLength - 2] >= 48 && packetBuffer[packetBufferLength - 2] <= 57 && // Digit
packetBuffer[packetBufferLength - 3] >= 48 && packetBuffer[packetBufferLength - 3] <= 57 && // Digit
packetBuffer[packetBufferLength - 4] == '-' &&
packetBuffer[packetBufferLength - 5] == '|';
// If the data looks like a packet we can start parsing it
if (receivedWholePacket) {
// read rssi and then ignore the packet footer
_rssi = (packetBuffer[packetBufferLength - 1] - 48) +
(packetBuffer[packetBufferLength - 2] - 48) * 10 +
(packetBuffer[packetBufferLength - 3] - 48) * 100;
packetBufferLength -= 5;
#ifdef RADIO_DEBUG_MODE
debug::log("Packet!");
#endif
if (_radioState == RADIO_STATE_DISCOVERY) {
_handleDiscoveryPacket(packetBuffer, packetBufferLength, _rssi);
} else if (_radioState == RADIO_STATE_RECEIVE) {
_handleReceivePacket(packetBuffer, packetBufferLength);
}
packetBufferLength = 0;
} else if (packetBufferLength == RADIO_PACKET_WITH_RSSI_LENGTH) {
#ifdef RADIO_DEBUG_MODE
debug::log("RadioReceiveTask: Packet does not conform");
debug::logByteArray(packetBuffer, 58);
#endif
// Something's wrong, we received enough bytes but it's not formated correctly.
// Let's sleep a while and clear the buffer. Hopefully that'll fix it.
vTaskDelay(7);
_clearSerialBuffer();
packetBufferLength = 0;
}
return packetBufferLength;
}
