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