bool ResponseCollector::matchExpected(DataBuffer& data)
{
//make a save point so we can revert if need be
ReadBufferSavePoint savepoint(&data);
size_t startBytesRemaining = 0;
//get a lock on the response mutex
mutex_lock_guard lock(m_responseMutex);
//need to loop over all of the bytes to check for responses
while(data.moreToRead())
{
//log the bytes remaining in the data before we attempt to match
startBytesRemaining = data.bytesRemaining();
//look through all the expected responses
for(auto itr = m_expectedResponses.begin(); itr < m_expectedResponses.end(); itr++)
{
//don't try to match bytes that were in the buffer before the command was sent
if(data.readPosition() < itr->minBytePosition)
{
continue;
}
//if we found a match
if(itr->pattern->match(data))
{
//match functions move the data pointer on success (even if not fully matched)
//need to make sure we don't roll back to our previous savepoint.
savepoint.commit();
if(itr->pattern->fullyMatched())
{
//unregister the response
m_expectedResponses.erase(itr);
return true;
}
}
//if any match function moved the data pointer, we don't want to move it back here
if(startBytesRemaining != data.bytesRemaining())
{
savepoint.commit();
}
}
//if we can move to the next byte and the byte position hasn't been moved elsewhere (internally by a match function).
if(data.moreToRead() && startBytesRemaining == data.bytesRemaining())
{
//move to the next byte and check for the response again
data.read_uint8();
}
}
return false;
}
bool WirelessParser::findPacketInBytes(DataBuffer& data, WirelessTypes::Frequency freq)
{
//create a read save point for the DataBuffer
ReadBufferSavePoint savePoint(&data);
std::size_t lastReadPosition;
//while there are enough bytes remaining to make an ASPP response packet
while(data.bytesRemaining() > WirelessPacket::ASPP_MIN_RESPONSE_PACKET_SIZE)
{
lastReadPosition = data.readPosition();
//move to the next byte
data.read_uint8();
WirelessPacket packet;
//if we found a packet within the bytes
if(parseAsPacket(data, packet, freq) == parsePacketResult_completePacket)
{
//commit the data that was read
savePoint.commit();
//process the packet
processPacket(packet, lastReadPosition);
return true;
}
}
//we didn't find any packet in the bytes buffer
return false;
}
bool BaseStation_SetBeacon::Response::match(DataBuffer& data)
{
const uint16 TOTAL_SUCCESS_BYTES = 2;
//create a save point with the data
ReadBufferSavePoint savePoint(&data);
//if there aren't enough bytes in the buffer to match the response
if(data.bytesRemaining() < TOTAL_SUCCESS_BYTES)
{
return false;
}
//if it doesn't match the command Id
if(data.read_uint16() != COMMAND_ID)
{
return false;
}
//if we made it this far, we successfully matched everything
//commit the current read position
savePoint.commit();
//set the result to success
m_success = true;
//we have fully matched the response
m_fullyMatched = true;
//notify that the response was matched
m_matchCondition.notify();
return true;
}
bool InertialParser::findPacketInBytes(DataBuffer& data)
{
//create a read save point for the DataBuffer
ReadBufferSavePoint savePoint(&data);
//while there are enough bytes remaining to make a MIP packet
while(data.bytesRemaining() > InertialPacketInfo::MIP_MIN_PACKET_SIZE)
{
//move to the next byte
data.read_uint8();
InertialPacket packet;
//if we found a packet within the bytes
if(parseAsPacket(data, packet) == inertialParserResult_completePacket)
{
//commit the data that was read
savePoint.commit();
//process the packet
processPacket(packet);
return true;
}
}
//we didn't find any packet in the bytes buffer
return false;
}
bool ReadSingleSensor::Response::match(DataBuffer& data)
{
const uint16 TOTAL_BYTES = 5;
//if there aren't enough bytes in the buffer to match the response
if(data.bytesRemaining() < TOTAL_BYTES)
{
//not a good response
m_success = false;
return false;
}
//create a save point with the data
ReadBufferSavePoint savePoint(&data);
//verify the command id
if(data.read_uint8() != 0x03)
{
//not a good response
m_success = false;
return false;
}
uint16 sensorVal = data.read_uint16();
ChecksumBuilder checksum;
checksum.append_uint16(sensorVal); //value of the requested channel
//verify the checksum (only a checksum on the actual data value)
if(checksum.simpleChecksum() != data.read_uint16())
{
//not a good response
m_success = false;
return false;
}
//if we made it this far, the bytes match the expected response
m_success = true;
m_sensorValue = sensorVal;
//commit the current read position
savePoint.commit();
//we have fully matched the response
m_fullyMatched = true;
//notify that the response was matched
m_matchCondition.notify();
return true;
}
bool ShortPing::Response::match(DataBuffer& data)
{
const uint16 TOTAL_BYTES = 1;
//if there aren't enough bytes in the buffer to match the response
if(data.bytesRemaining() < TOTAL_BYTES)
{
//not a good response
m_success = false;
return false;
}
//create a save point with the data
ReadBufferSavePoint savePoint(&data);
uint8 resultByte = data.read_uint8();
//single byte 0x21 is a fail response
if(resultByte == 0x21)
{
//legitimate fail response
m_success = false;
}
//single byte 0x02 is a success response
else if(resultByte == WirelessProtocol::cmdId_shortPing)
{
//success response
m_success = true;
}
else
{
//non-matching response
m_success = false;
return false;
}
//if we made it this far, the bytes match the expected response (success or fail)
//commit the current read position
savePoint.commit();
//we have fully matched the response
m_fullyMatched = true;
//notify that the response was matched
m_matchCondition.notify();
return true;
}
WirelessParser::ParsePacketResult WirelessParser::parseAsPacket(DataBuffer& data, WirelessPacket& packet, WirelessTypes::Frequency freq)
{
std::size_t totalBytesAvailable = data.bytesRemaining();
if(totalBytesAvailable == 0)
{
//Not Enough Data to tell if valid packet
return parsePacketResult_notEnoughData;
}
//choose the correct ASPP version parser
switch(data.peekByte())
{
case WirelessPacket::ASPP_V1_START_OF_PACKET_BYTE:
return parseAsPacket_ASPP_v1(data, packet, freq);
case WirelessPacket::ASPP_V2_START_OF_PACKET_BYTE:
return parseAsPacket_ASPP_v2(data, packet, freq);
default:
return parsePacketResult_invalidPacket;
}
}
WirelessParser::ParsePacketResult WirelessParser::parseAsPacket_ASPP_v2(DataBuffer& data, WirelessPacket& packet, WirelessTypes::Frequency freq)
{
//Assume we are at the start of the packet, read the packet header
//byte 1 - Start Of Packet
//byte 2 - Delivery Stop Flag
//byte 3 - App Data Type
//byte 4 - 7 - Node Address (uint32)
//byte 8 - 9 - Payload Length
//byte 10 to N-4 - Payload
//byte N-3 - Node RSSI
//byte N-2 - Base RSSI
//byte N-1 - Fletcher Checksum (MSB)
//byte N - Fletcher Checksum (LSB)
//create a save point for the DataBuffer
ReadBufferSavePoint savePoint(&data);
std::size_t totalBytesAvailable = data.bytesRemaining();
//we need at least 13 bytes for any ASPP v2 packet (if empty payload)
if(totalBytesAvailable < 13)
{
//Not Enough Data to tell if valid packet
return parsePacketResult_notEnoughData;
}
//read byte 1
uint8 startOfPacket = data.read_uint8(); //Start Of Packet
//verify that the first byte is the Start Of Packet
if(startOfPacket != WirelessPacket::ASPP_V2_START_OF_PACKET_BYTE)
{
//Invalid Packet
return parsePacketResult_invalidPacket;
}
//read byte 2
uint8 deliveryStopFlag = data.read_uint8(); //Delivery Stop Flag
//read byte 3
uint8 appDataType = data.read_uint8(); //App Data Type
//read bytes 4 - 7
uint32 nodeAddress = data.read_uint32(); //Node Address
//read bytes 8 and 9
uint16 payloadLength = data.read_uint16(); //Payload Length
//determine the full packet length
size_t packetLength = payloadLength + WirelessPacket::ASPP_V2_NUM_BYTES_BEFORE_PAYLOAD + WirelessPacket::ASPP_V2_NUM_BYTES_AFTER_PAYLOAD;
//the DataBuffer must be large enough to hold the rest of the packet
if(totalBytesAvailable < packetLength)
{
//Not Enough Data to tell if valid packet
return parsePacketResult_notEnoughData;
}
//create the Bytes vector to hold the payload bytes
Bytes payload;
payload.reserve(payloadLength);
//loop through the payload
for(uint16 payloadItr = 0; payloadItr < payloadLength; payloadItr++)
{
//store the payload bytes
payload.push_back(data.read_uint8()); //Payload Bytes
}
//read the node RSSI
uint8 nodeRSSI = data.read_uint8(); //Node RSSI
//read the base station rssi
uint8 baseRSSI = data.read_uint8(); //Base RSSI
//get the checksum sent in the packet
uint16 checksum = data.read_uint16(); //Checksum
//build the checksum to calculate from all the bytes
ChecksumBuilder calcChecksum;
calcChecksum.append_uint8(startOfPacket);
calcChecksum.append_uint8(deliveryStopFlag);
calcChecksum.append_uint8(appDataType);
calcChecksum.append_uint32(nodeAddress);
calcChecksum.append_uint16(payloadLength);
calcChecksum.appendBytes(payload);
calcChecksum.append_uint8(nodeRSSI);
calcChecksum.append_uint8(baseRSSI);
//verify that the returned checksum is the same as the one we calculated
if(checksum != calcChecksum.fletcherChecksum())
{
//Bad Checksum
return parsePacketResult_badChecksum;
}
DeliveryStopFlags flags = DeliveryStopFlags::fromByte(deliveryStopFlag);
//add all the info about the packet to the WirelessPacket reference passed in
packet.deliveryStopFlags(flags);
packet.type(static_cast<WirelessPacket::PacketType>(appDataType));
packet.nodeAddress(nodeAddress);
packet.payload(payload);
packet.nodeRSSI(static_cast<int16>(nodeRSSI) - 205);
packet.baseRSSI(static_cast<int16>(baseRSSI) - 205);
packet.frequency(freq);
//Correct the packet type if it is incorrect
WirelessPacketUtils::correctPacketType(packet);
//make sure the packet is valid based on its specific type
if(!WirelessPacketUtils::packetIntegrityCheck(packet))
{
//not a valid packet, failed integrity check
return parsePacketResult_invalidPacket;
}
//check if the packet is a duplicate
if(isDuplicate(packet))
{
//even though it is a duplicate, we still have a complete packet so commit the bytes to skip over them
savePoint.commit();
//duplicate packet
return parsePacketResult_duplicate;
}
//we have a complete packet, commit the bytes that we just read (move the read pointer)
savePoint.commit();
return parsePacketResult_completePacket;
}
void WirelessParser::parse(DataBuffer& data, WirelessTypes::Frequency freq)
{
ParsePacketResult parseResult; //holds the result of verifying whether it was a valid ASPP packet or not
WirelessPacket packet;
size_t bytesRemaining; //holds how many bytes we have remaining, helps determine if the buffer has been moved by an external function
//make a save point so we can revert if need be
ReadBufferSavePoint savepoint(&data);
std::size_t lastReadPosition;
//while there is more data to be read in the DataBuffer
while(data.moreToRead())
{
lastReadPosition = data.readPosition();
//read the next byte (doesn't move data's read position)
uint8 currentByte = data.peekByte();
//skipByte is set to false when we don't want to skip to the next byte after we are done looking at the current byte
bool moveToNextByte = true;
//notEnoughData is true when the bytes could be a valid packet, but there isn't enough bytes to be sure
bool notEnoughData = false;
//if this is any ASPP Start of Packet byte
if(currentByte == WirelessPacket::ASPP_V1_START_OF_PACKET_BYTE ||
currentByte == WirelessPacket::ASPP_V2_START_OF_PACKET_BYTE)
{
//check if the packet is a valid ASPP packet, starting at this byte
parseResult = parseAsPacket(data, packet, freq);
//check the result of the parseAsPacket command
switch(parseResult)
{
//good packet, process it and then look for the next
case parsePacketResult_completePacket:
processPacket(packet, lastReadPosition);
savepoint.commit();
continue; //packet has been processed, move to the next byte after the packet
case parsePacketResult_duplicate:
savepoint.commit();
continue; //packet is a duplicate, but byte position has been moved. Move to the next byte after the packet
//somethings incorrect in the packet, move passed the AA and start looking for the next packet
case parsePacketResult_invalidPacket:
case parsePacketResult_badChecksum:
savepoint.commit();
break;
//ran out of data, return and wait for more
case parsePacketResult_notEnoughData:
moveToNextByte = false;
notEnoughData = true;
break;
default:
assert(false); //unhandled verifyResult, need to add a case for it
}
}
//data is not a packet at this point
bytesRemaining = data.bytesRemaining();
//check if the bytes we currently have match an expected response
// This isn't perfect (could possibly use part of a partial ASPP packet as a cmd response),
// but unfortunately its the best we can do with single byte responses being part of our protocol
if(findMatchingResponse(data))
{
//the bytes have already moved, don't move to the next byte in the next iteration
savepoint.commit();
moveToNextByte = false;
}
else
{
//failed to match
//if we didn't have enough data for a full packet, and it didn't match any expected responses
if(notEnoughData)
{
//look for packets after the current byte.
// Even though this looks like it could be the start of an ASPP packet,
// if we find any full ASPP packets inside of the these bytes, we need
// to pick them up and move on.
if(!findPacketInBytes(data, freq))
{
//if the read position in the bytes has been moved external to this function
if(data.bytesRemaining() != bytesRemaining)
{
//read position has moved somewhere else, so bytes have been committed. Commit in our local savepoint as well.
savepoint.commit();
}
//we didn't find a packet within this, so return from this function as we need to wait for more data
return;
}
else
{
savepoint.commit();
}
}
}
//if we need to move to the next byte
if (moveToNextByte)
{
//if the read position in the bytes has been moved external to this function
if(data.bytesRemaining() != bytesRemaining)
{
//read position has moved somewhere else, so bytes have been committed. Commit in our local savepoint as well.
savepoint.commit();
}
else
{
//move to the next byte
data.read_uint8();
}
}
}
}
InertialParseResult InertialParser::parseAsPacket(DataBuffer& data, InertialPacket& packet)
{
//Assume that we are at the start of a packet
//byte 1 - Start of Packet 1 (0x75)
//byte 2 - Start of Packet 2 (0x65)
//byte 3 - Descriptor Set
//byte 4 - Payload Length
//byte 5 to N-2 - Payload
//byte N-1 - Checksum (MSB)
//byte N - Checksum (LSB)
//create a save point for the DataBuffer
ReadBufferSavePoint savePoint(&data);
std::size_t totalBytesAvailable = data.bytesRemaining();
//make sure we have enough bytes to even be a MIP packet
if(totalBytesAvailable < InertialPacketInfo::MIP_MIN_PACKET_SIZE)
{
//Not Enough Data to tell if valid packet
return inertialParserResult_notEnoughData;
}
//read the first 2 bytes
uint16 startOfPacket = data.read_uint16(); //Start of Packet
//verify that the Start of Packet value is correct
if(startOfPacket != InertialPacketInfo::INERTIAL_PACKET_START_OF_PACKET)
{
//Invalid Packet
return inertialParserResult_invalidPacket;
}
//read byte 3
uint8 descriptorSet = data.read_uint8(); //Descriptor Set
//read byte 4
uint8 payloadLen = data.read_uint8(); //Payload Length
//determine the full packet length
uint32 packetLength = payloadLen + InertialPacketInfo::MIP_NUM_BYTES_BEFORE_PAYLOAD + InertialPacketInfo::MIP_NUM_BYTES_AFTER_PAYLOAD;
//the DataBuffer must be large enough to hold the rest of the packet
if(totalBytesAvailable < packetLength)
{
//Not Enough Data to tell if valid packet
return inertialParserResult_notEnoughData;
}
//create the Bytes vector to hold the payload bytes
Bytes payload;
payload.reserve(payloadLen);
//loop through the payload
for(uint8 payloadItr = 0; payloadItr < payloadLen; payloadItr++)
{
//store the payload bytes
payload.push_back(data.read_uint8()); //Payload Bytes
}
//get the checksum sent in the packet
uint16 checksum = data.read_uint16(); //Checksum
//build the checksum to calculate from all the bytes
ChecksumBuilder calcChecksum;
calcChecksum.append_uint16(startOfPacket);
calcChecksum.append_uint8(descriptorSet);
calcChecksum.append_uint8(payloadLen);
calcChecksum.appendBytes(payload);
if(checksum != calcChecksum.fletcherChecksum())
{
//Bad Checksum
return inertialParserResult_badChecksum;
}
//add all the info about the packet to the InertialPacket reference passed in
packet.descriptorSet(descriptorSet);
packet.payload(payload);
//we have a complete packet, commit the bytes that we just read (move the read pointer)
savePoint.commit();
return inertialParserResult_completePacket;
}
