bool BaseStation_SetBeacon_v2::Response::matchFailResponse(const WirelessPacket& packet) { WirelessPacket::Payload payload = packet.payload(); //check the main bytes of the packet if(packet.deliveryStopFlags().toInvertedByte() != 0x07 || //delivery stop flag packet.type() != WirelessPacket::packetType_baseErrorReply || //app data type packet.nodeAddress() != WirelessProtocol::BASE_STATION_ADDRESS || //node address payload.size() != 0x07 || //payload length payload.read_uint16(0) != WirelessProtocol::cmdId_base_setBeacon || //command ID payload.read_uint32(2) != m_beaconStartTime //beacon timestamp ) { //failed to match some of the bytes return false; } //Not doing anything with the error code as of now //uint8 errorCode = payload.read_uint8(6); //set the result to failure m_success = false; return true; }
bool BaseStation_RfSweepStart::Response::matchSuccessResponse(const WirelessPacket& packet) { WirelessPacket::Payload payload = packet.payload(); //check the main bytes of the packet if(packet.deliveryStopFlags().toInvertedByte() != 0x07 || //delivery stop flag packet.type() != WirelessPacket::packetType_baseSuccessReply || //app data type packet.nodeAddress() != WirelessProtocol::BASE_STATION_ADDRESS || //node address payload.size() != 16 || //payload length payload.read_uint16(0) != WirelessProtocol::cmdId_base_rfScan || //command ID payload.read_uint16(2) != m_options || payload.read_uint32(4) != m_min || payload.read_uint32(8) != m_max || payload.read_uint32(12) != m_interval ) { //failed to match some of the bytes return false; } //set the result to success m_success = true; return true; }
bool StartNonSyncSampling_v2::Response::match(const WirelessPacket& packet) { WirelessPacket::Payload payload = packet.payload(); //check the main bytes of the packet if( packet.deliveryStopFlags().toInvertedByte() != 0x07 || //delivery stop flag packet.type() != WirelessPacket::packetType_nodeSuccessReply || //app data type packet.nodeAddress() != m_nodeAddress || //node address payload.size() != 0x02 || //payload length payload.read_uint16(0) != WirelessProtocol::cmdId_startLdc_v2 //Command ID ) { //failed to match some of the bytes return false; } //if we made it here, the packet matches the response pattern m_success = true; //we have fully matched the response m_fullyMatched = true; //notify that the response was matched m_matchCondition.notify(); return true; }
bool LongPing::Response::match(const WirelessPacket& packet) { WirelessPacket::Payload payload = packet.payload(); //check the main bytes of the packet if( packet.deliveryStopFlags().toInvertedByte() != 0x07 || //delivery stop flag packet.type() != 0x02 || //app data type packet.nodeAddress() != m_nodeAddress || //node address payload.size() != 0x02 || //payload length payload.read_uint16(0) != 0x0000 ) { //failed to match some of the bytes return false; } //if we made it here, the packet matches the response pattern //store the node and base RSSI values with the PingResponse m_result = PingResponse::ResponseSuccess(packet.nodeRSSI(), packet.baseRSSI()); //we have fully matched the response m_fullyMatched = true; //notify that the response was matched m_matchCondition.notify(); m_success = true; return true; }
bool ReadEeprom_v2::Response::matchFailResponse(const WirelessPacket& packet) { WirelessPacket::Payload payload = packet.payload(); //check the main bytes of the packet if(packet.deliveryStopFlags().toInvertedByte() != 0x07 || //delivery stop flag packet.type() != WirelessPacket::packetType_nodeErrorReply || //app data type packet.nodeAddress() != m_nodeAddress || //node address payload.size() != 0x05 || //payload length payload.read_uint16(0) != WirelessProtocol::cmdId_readEeprom_v2 || //command ID payload.read_uint16(2) != m_eepromAddress //eeprom address ) { //failed to match some of the bytes return false; } //if we made it here, the packet matches the response pattern //get the error code from the response m_success = false; m_errorCode = static_cast<WirelessPacket::ResponseErrorCode>(packet.payload().read_uint8(4)); return true; }
bool ReadEeprom_v2::Response::matchSuccessResponse(const WirelessPacket& packet) { WirelessPacket::Payload payload = packet.payload(); uint8 dsf = packet.deliveryStopFlags().toInvertedByte(); //check the main bytes of the packet if((dsf != 0x07 && dsf != 0x00) || //delivery stop flag (Unfortunately some nodes report 0x00 and some report 0x07) packet.type() != 0x00 || //app data type packet.nodeAddress() != m_nodeAddress || //node address payload.size() != 0x06 || //payload length payload.read_uint16(0) != WirelessProtocol::cmdId_readEeprom_v2 || //command ID payload.read_uint16(2) != m_eepromAddress //eeprom address ) { //failed to match some of the bytes return false; } //if we made it here, the packet matches the response pattern //get the eeprom value from the response m_success = true; m_errorCode = WirelessPacket::error_none; m_eepromValue = packet.payload().read_uint16(4); return true; }
bool BaseStation_ReadEeprom_v2::Response::matchFailResponse(const WirelessPacket& packet) { WirelessPacket::Payload payload = packet.payload(); //check the main bytes of the packet if(packet.deliveryStopFlags().toInvertedByte() != 0x07 || //delivery stop flag packet.type() != WirelessPacket::packetType_baseErrorReply || //app data type packet.nodeAddress() != WirelessProtocol::BASE_STATION_ADDRESS || //node address payload.size() != 0x05 || //payload length payload.read_uint16(0) != WirelessProtocol::cmdId_base_readEeprom_v2 || //command ID payload.read_uint16(2) != m_eepromAddress //eeprom address ) { //failed to match some of the bytes return false; } //read the error code from the response m_errorCode = static_cast<WirelessPacket::ResponseErrorCode>(payload.read_uint8(4)); //set the result to failure m_success = false; return true; }
bool HclSmartBearing_CalPacket::integrityCheck(const WirelessPacket& packet) { //verify the delivery stop flags are what we expected if(!packet.deliveryStopFlags().pc) { //packet not intended for the PC return false; } //verify the packet type is correct if(packet.type() != packetType_HclSmartBearing_Calibrated) { //packet is not a Sync Sampling packet return false; } const WirelessPacket::Payload& payload = packet.payload(); //verify the payload size if(payload.size() < 42) { return false; } return true; }
bool AutoCal::Response::match_nodeReceived(const WirelessPacket& packet) { WirelessPacket::Payload payload = packet.payload(); //check the main bytes of the packet if(packet.deliveryStopFlags().toByte() != 0x07 || //delivery stop flag packet.type() != 0x20 || //app data type packet.nodeAddress() != m_nodeAddress || //node address payload.size() != 0x07 //payload length ) { //failed to match some of the bytes return false; } //Command ID if(payload.read_uint16(0) != 0x0064) { return false; } //if the status flag is success (0) if(payload.read_uint8(2) == 0) { m_calStarted = true; //only want to read the time until completion if the cal has started m_timeUntilCompletion = payload.read_float(3); } return true; }
bool BaseStation_Ping_v2::Response::match(const WirelessPacket& packet) { WirelessPacket::Payload payload = packet.payload(); //check the main bytes of the packet if(packet.deliveryStopFlags().toInvertedByte() != 0x07 || //delivery stop flag packet.type() != 0x31 || //app data type packet.nodeAddress() != WirelessProtocol::BASE_STATION_ADDRESS || //node address payload.size() != 0x02 || //payload length payload.read_uint16(0) != WirelessProtocol::cmdId_basePing_v2 //command id ) { //failed to match some of the bytes return false; } //if we made it here, the packet matches the response pattern //the ping was a success m_success = true; //we have fully matched the response m_fullyMatched = true; //notify that the response was matched m_matchCondition.notify(); return true; }
DiagnosticPacket::DiagnosticPacket(const WirelessPacket& packet) { //construct the data packet from the wireless packet passed in m_nodeAddress = packet.nodeAddress(); m_deliveryStopFlags = packet.deliveryStopFlags(); m_type = packet.type(); m_nodeRSSI = WirelessTypes::UNKNOWN_RSSI; m_baseRSSI = packet.baseRSSI(); m_frequency = packet.frequency(); m_payload = packet.payload(); //parse the data sweeps in the packet parseSweeps(); }
bool WirelessParser::isDuplicate(const WirelessPacket& packet) { uint16 uniqueId; uint32 packetsNode = packet.nodeAddress(); //check the packet type switch(packet.type()) { //get the unique id depending on the type of packet case WirelessPacket::packetType_LDC: uniqueId = LdcPacket::getUniqueId(packet); break; case WirelessPacket::packetType_SyncSampling: uniqueId = SyncSamplingPacket::getUniqueId(packet); break; case WirelessPacket::packetType_BufferedLDC: uniqueId = BufferedLdcPacket::getUniqueId(packet); break; case WirelessPacket::packetType_LDC_16ch: uniqueId = LdcPacket_16ch::getUniqueId(packet); break; case WirelessPacket::packetType_SyncSampling_16ch: uniqueId = SyncSamplingPacket_16ch::getUniqueId(packet); break; case WirelessPacket::packetType_BufferedLDC_16ch: uniqueId = BufferedLdcPacket_16ch::getUniqueId(packet); break; case WirelessPacket::packetType_AsyncDigital: uniqueId = AsyncDigitalPacket::getUniqueId(packet); break; case WirelessPacket::packetType_AsyncDigitalAnalog: uniqueId = AsyncDigitalAnalogPacket::getUniqueId(packet); break; case WirelessPacket::packetType_diagnostic: uniqueId = DiagnosticPacket::getUniqueId(packet); break; case WirelessPacket::packetType_roller: uniqueId = RollerPacket::getUniqueId(packet); break; //isn't a valid data packet that has a unique id, so we can't check for duplicates case WirelessPacket::packetType_nodeDiscovery: case WirelessPacket::packetType_nodeDiscovery_v2: case WirelessPacket::packetType_nodeDiscovery_v3: case WirelessPacket::packetType_nodeDiscovery_v4: case WirelessPacket::packetType_SHM: case WirelessPacket::packetType_beaconEcho: case WirelessPacket::packetType_rfScanSweep: default: return false; } //if we found the packet's node address in the lastPacketMap if(m_lastPacketMap.find(packetsNode) != m_lastPacketMap.end()) { //if the unique id in the lastPacketMap matches the uniqueId from this packet if(m_lastPacketMap[packetsNode] == uniqueId) { //it is a duplicate packet return true; } } //update or set m_lastPacketMap's uniqueId for this node m_lastPacketMap[packetsNode] = uniqueId; //it is not a duplicate packet return false; }
HclSmartBearing_RawPacket::HclSmartBearing_RawPacket(const WirelessPacket& packet) { //construct the data packet from the wireless packet passed in m_nodeAddress = packet.nodeAddress(); m_deliveryStopFlags = packet.deliveryStopFlags(); m_type = packet.type(); m_nodeRSSI = packet.nodeRSSI(); m_baseRSSI = packet.baseRSSI(); m_frequency = packet.frequency(); m_payload = packet.payload(); m_payloadOffsetChannelData = 0; //not used for these packets //parse the data sweeps in the packet parseSweeps(); }
WirelessPacket buildWriteEepromResponse(int nodeAddress) { Bytes payload; payload.push_back(0x00); payload.push_back(0x04); //build the correct packet response first WirelessPacket packet; packet.deliveryStopFlags(DeliveryStopFlags::fromByte(0x00)); packet.type(static_cast<WirelessPacket::PacketType>(0x00)); packet.nodeAddress(nodeAddress); packet.payload(payload); return packet; }
BufferedLdcPacket::BufferedLdcPacket(const WirelessPacket& packet) { //construct the data packet from the wireless packet passed in m_nodeAddress = packet.nodeAddress(); m_deliveryStopFlags = packet.deliveryStopFlags(); m_type = packet.type(); m_nodeRSSI = WirelessTypes::UNKNOWN_RSSI; m_baseRSSI = packet.baseRSSI(); m_frequency = packet.frequency(); m_payload = packet.payload(); m_payloadOffsetChannelData = PAYLOAD_OFFSET_CHANNEL_DATA; //parse the data sweeps in the packet parseSweeps(); }
WirelessPacket buildAutoCalNodeRecResponse(int nodeAddress) { ByteStream payload; payload.append_uint16(0x0064); //cmd id payload.append_uint8(0x00); //status flag payload.append_float(5.0f); //time to completion WirelessPacket packet; packet.deliveryStopFlags(DeliveryStopFlags::fromByte(0x07)); packet.type(WirelessPacket::packetType_NodeReceived); packet.nodeAddress(nodeAddress); packet.payload(payload.data()); return packet; }
bool AsyncDigitalAnalogPacket::integrityCheck(const WirelessPacket& packet) { WirelessPacket::Payload payload = packet.payload(); //verify the payload size if(payload.size() < PAYLOAD_OFFSET_CHANNEL_DATA) { //payload is too small to be valid return false; } //verify the delivery stop flags are what we expected if(!packet.deliveryStopFlags().pc) { //packet not intended for the PC return false; } //read the data type uint8 dataType = payload.read_uint8(PAYLOAD_OFFSET_DATA_TYPE); //verify the data type if(dataType < WirelessTypes::dataType_first || dataType > WirelessTypes::dataType_last) { //the data type is invalid return false; } //verify the packet type is correct if(packet.type() != packetType_AsyncDigitalAnalog) { //packet is not an Async Digital packet return false; } //calculate the number of active channels uint32 channels = ChannelMask(payload.read_uint16(PAYLOAD_OFFSET_CHANNEL_MASK)).count(); //check that there are active channels if(channels == 0) { //no active channels return false; } //packet looks valid return true; }
WirelessPacket buildSyncSamplingResponse(uint16 nodeAddress) { Bytes payload; payload.push_back(0x00); payload.push_back(0x3B); payload.push_back(0x00); //build the correct packet response first WirelessPacket packet; packet.deliveryStopFlags(DeliveryStopFlags::fromInvertedByte(0x07)); packet.type(static_cast<WirelessPacket::PacketType>(0x00)); packet.nodeAddress(nodeAddress); packet.payload(payload); return packet; }
bool AutoBalance_v2::Response::match(const WirelessPacket& packet) { WirelessPacket::Payload payload = packet.payload(); //check the main bytes of the packet if(packet.deliveryStopFlags().toInvertedByte() != 0x07 || //delivery stop flag packet.type() != WirelessPacket::packetType_nodeSuccessReply || //app data type packet.nodeAddress() != m_nodeAddress || //node address payload.size() != 0x10 || //payload length payload.read_uint16(0) != WirelessProtocol::cmdId_autoBalance_v2 || //command id payload.read_uint8(2) != m_channelNumber || //channel number (echo) payload.read_float(3) != m_targetPercent //target percent (echo) ) { //failed to match some of the bytes return false; } //if we made it here, the packet matches the response pattern //error code m_result.m_errorCode = static_cast<WirelessTypes::AutoBalanceErrorFlag>(payload.read_uint8(7)); //sampled value m_result.m_percentAchieved = payload.read_float(8); //hardware offset m_result.m_hardwareOffset = static_cast<uint16>(payload.read_uint32(12)); switch(m_result.m_errorCode) { case WirelessTypes::autobalance_success: case WirelessTypes::autobalance_maybeInvalid: m_success = true; default: m_success = false; } //we have fully matched the response m_fullyMatched = true; //notify that the response was matched m_matchCondition.notify(); return true; }
bool HclSmartBearing_RawPacket::integrityCheck(const WirelessPacket& packet) { //verify the delivery stop flags are what we expected if(!packet.deliveryStopFlags().pc) { //packet not intended for the PC return false; } //verify the packet type is correct if(packet.type() != packetType_HclSmartBearing_Raw) { //packet is not a Sync Sampling packet return false; } const WirelessPacket::Payload& payload = packet.payload(); //verify the payload size if(payload.size() < 4) { //each of these packets has at least the 4 main bytes in its header return false; } //read the app id RawPacketId appId = static_cast<RawPacketId>(payload.read_uint8(PAYLOAD_OFFSET_APP_ID)); switch(appId) { case rawPacket_baseBoard: return integrityCheck_baseBoard(payload); case rawPacket_strainBoard: //TODO: parse the strain board packet return false; case rawPacket_inertialBoard: //TODO: parse the inertial board packet return false; default: //invalid app id return false; } }
WirelessPacket buildAutoCalCompletionResponse(int nodeAddress) { ByteStream payload; payload.append_uint16(0x0064); //cmd id payload.append_uint8(0x00); //completion flag payload.append_uint8(0x00); //ch1 error flag payload.append_float(0.0f); //ch1 offset payload.append_uint8(0x00); //ch2 error flag payload.append_float(0.0f); //ch2 offset payload.append_uint8(0x00); //ch3 error flag payload.append_float(0.0f); //ch3 offset payload.append_float(20.5f);//temperature //build the correct packet response first WirelessPacket packet; packet.deliveryStopFlags(DeliveryStopFlags::fromByte(0x07)); packet.type(WirelessPacket::packetType_reply); packet.nodeAddress(nodeAddress); packet.payload(payload.data()); return packet; }
bool AutoCal::Response::match_shmLink(const WirelessPacket& packet) { WirelessPacket::Payload payload = packet.payload(); std::size_t payloadLen = payload.size(); //check the main bytes of the packet if(packet.deliveryStopFlags().toByte() != 0x07 || //delivery stop flag packet.type() != WirelessPacket::packetType_reply || //app data type packet.nodeAddress() != m_nodeAddress || //node address payloadLen != 22 //payload length ) { //failed to match some of the bytes return false; } //Command ID if(payload.read_uint16(0) != 0x0064) { return false; } //Pass/Fail Flag m_completionFlag = static_cast<WirelessTypes::AutoCalCompletionFlag>(payload.read_uint8(2)); //Info Bytes for(std::size_t i = 3; i < payloadLen; ++i) { //add all of the payload info bytes to m_infoBytes m_infoBytes.push_back(payload.read_uint8(i)); } //setting success to true if it got this packet, even if the cals applied might be bad m_success = true; return true; }
bool NodeDiscoveryPacket_v2::integrityCheck(const WirelessPacket& packet) { const uint8 RADIO_CHANNEL_MIN = 11; const uint8 RADIO_CHANNEL_MAX = 26; //verify the payload size is correct if(packet.payload().size() != 15) { return false; } //read what should be the radio channel byte uint8 radioChannel = packet.payload().read_uint8(PAYLOAD_OFFSET_RADIO_CHANNEL); //verify that the radio channel byte is valid if(radioChannel < RADIO_CHANNEL_MIN || radioChannel > RADIO_CHANNEL_MAX) { return false; } //verify the delivery stop flags are what we expected if(packet.deliveryStopFlags() != stopFlags_nodeDiscovery) { //packet not intended for the PC return false; } //verify the packet type is correct if(packet.type() != packetType_nodeDiscovery_v2) { //packet is not a node discovery packet return false; } //packet looks valid return true; }
bool WirelessParser::isDuplicate(const WirelessPacket& packet) { uint16 uniqueId; uint32 packetsNode = packet.nodeAddress(); //check the packet type switch(packet.type()) { //get the unique id depending on the type of packet case WirelessPacket::packetType_LDC: uniqueId = LdcPacket::getUniqueId(packet); break; case WirelessPacket::packetType_SyncSampling: uniqueId = SyncSamplingPacket::getUniqueId(packet); break; case WirelessPacket::packetType_BufferedLDC: uniqueId = BufferedLdcPacket::getUniqueId(packet); break; case WirelessPacket::packetType_AsyncDigital: uniqueId = AsyncDigitalPacket::getUniqueId(packet); break; case WirelessPacket::packetType_AsyncDigitalAnalog: uniqueId = AsyncDigitalAnalogPacket::getUniqueId(packet); break; case WirelessPacket::packetType_diagnostic: uniqueId = DiagnosticPacket::getUniqueId(packet); break; case WirelessPacket::packetType_LDC_16ch: uniqueId = LdcPacket_16ch::getUniqueId(packet); break; case WirelessPacket::packetType_SyncSampling_16ch: uniqueId = SyncSamplingPacket_16ch::getUniqueId(packet); break; case WirelessPacket::packetType_BufferedLDC_16ch: uniqueId = BufferedLdcPacket_16ch::getUniqueId(packet); break; case WirelessPacket::packetType_HclSmartBearing_Calibrated: uniqueId = HclSmartBearing_CalPacket::getUniqueId(packet); break; case WirelessPacket::packetType_HclSmartBearing_Raw: uniqueId = HclSmartBearing_RawPacket::getUniqueId(packet); break; case WirelessPacket::packetType_rawAngleStrain: uniqueId = RawAngleStrainPacket::getUniqueId(packet); break; case WirelessPacket::packetType_roller: uniqueId = RollerPacket::getUniqueId(packet); break; //isn't a valid data packet that has a unique id, so we can't check for duplicates case WirelessPacket::packetType_nodeCommand: case WirelessPacket::packetType_nodeErrorReply: case WirelessPacket::packetType_nodeDiscovery: case WirelessPacket::packetType_TCLinkLDC: case WirelessPacket::packetType_beaconEcho: case WirelessPacket::packetType_nodeDiscovery_v2: case WirelessPacket::packetType_nodeDiscovery_v3: case WirelessPacket::packetType_nodeDiscovery_v4: case WirelessPacket::packetType_nodeReceived: case WirelessPacket::packetType_nodeSuccessReply: case WirelessPacket::packetType_baseCommand: case WirelessPacket::packetType_baseSuccessReply: case WirelessPacket::packetType_baseErrorReply: case WirelessPacket::packetType_rfScanSweep: case WirelessPacket::packetType_SHM: return false; default: assert(false); //unhandled packet type, need to add a case for it return false; } //if we found the packet's node address in the lastPacketMap if(m_lastPacketMap.find(packetsNode) != m_lastPacketMap.end()) { //if the unique id in the lastPacketMap matches the uniqueId from this packet if(m_lastPacketMap[packetsNode] == uniqueId) { //it is a duplicate packet return true; } } //update or set m_lastPacketMap's uniqueId for this node m_lastPacketMap[packetsNode] = uniqueId; //it is not a duplicate packet return false; }
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; }
bool DiagnosticPacket::integrityCheck(const WirelessPacket& packet) { WirelessPacket::Payload payload = packet.payload(); //verify the minimum payload size if(payload.size() < 7) { return false; } //verify the delivery stop flags are what we expected if(!packet.deliveryStopFlags().pc) { //packet not intended for the PC return false; } //verify the packet type is correct if(packet.type() != packetType_diagnostic) { //packet is not an LDC packet return false; } //verify packet interval if(payload.read_uint16(2) == 0) { //packet interval should never be 0 (invalid payload) return false; } DataBuffer bytes(payload); //skip the sample rate and tick bytes bytes.skipBytes(4); //verify the payload is correct while(bytes.moreToRead()) { uint8 infoLen = bytes.read_uint8(); if(infoLen == 0) { //no info length should be 0 (invalid payload) return false; } //verify we can read all the bytes in the length described if(bytes.bytesRemaining() < infoLen) { //can't read all the bytes (invalid payload) return false; } //skip passed the bytes being asked to read bytes.skipBytes(infoLen); } //packet looks valid return true; }
void WirelessPacketCollector::addDataPacket(const WirelessPacket& packet) { //create a boost_lock for thread safety mutex_lock_guard lock(m_packetMutex); try { //add a different packet depending on its type switch(packet.type()) { //Low Duty Cycle packet case WirelessPacket::packetType_LDC: m_dataPackets.push_back(LdcPacket(packet)); break; //Sync Sampling Packet case WirelessPacket::packetType_SyncSampling: m_dataPackets.push_back(SyncSamplingPacket(packet)); break; //Buffered LDC packet case WirelessPacket::packetType_BufferedLDC: m_dataPackets.push_back(BufferedLdcPacket(packet)); break; //Low Duty Cycle with 16 channels packet case WirelessPacket::packetType_LDC_16ch: m_dataPackets.push_back(LdcPacket_16ch(packet)); break; //Sync Sampling with 16 channels packet case WirelessPacket::packetType_SyncSampling_16ch: m_dataPackets.push_back(SyncSamplingPacket_16ch(packet)); break; //Buffered LDC with 16 channels packet case WirelessPacket::packetType_BufferedLDC_16ch: m_dataPackets.push_back(BufferedLdcPacket_16ch(packet)); break; //Asynchronous Digital packet case WirelessPacket::packetType_AsyncDigital: m_dataPackets.push_back(AsyncDigitalPacket(packet)); break; //Asynchronous Digital/Analog packet case WirelessPacket::packetType_AsyncDigitalAnalog: m_dataPackets.push_back(AsyncDigitalAnalogPacket(packet)); break; //Structural Health Monitor packet case WirelessPacket::packetType_SHM: m_dataPackets.push_back(ShmPacket(packet)); break; //HclSmartBearing Raw packet case WirelessPacket::packetType_HclSmartBearing_Raw: m_dataPackets.push_back(HclSmartBearing_RawPacket(packet)); break; //bad packet type, this function shouldn't have been called default: throw Error("Unknown Packet Type"); } //notify the read thread, if it is waiting for data to be put into the buffer m_emptyBufferCondition.notify_one(); } catch(Error&) { //there was an error building one of the packets, just return } }
bool BufferedLdcPacket::integrityCheck(const WirelessPacket& packet) { WirelessPacket::Payload payload = packet.payload(); //verify the payload size if(payload.size() < PAYLOAD_OFFSET_CHANNEL_DATA) { //payload must be at least a certain length return false; } //verify the app id if(payload.read_uint8(PAYLOAD_OFFSET_APP_ID) != APP_ID_VAL) { //application id is incorrect return false; } //verify the delivery stop flags are what we expected if(!packet.deliveryStopFlags().pc) { //packet not intended for the PC return false; } //read the data type uint8 dataType = payload.read_uint8(PAYLOAD_OFFSET_DATA_TYPE); //verify the data type if(dataType < WirelessTypes::dataType_first || dataType > WirelessTypes::dataType_last) { //the data type is invalid return false; } //verify the packet type is correct if(packet.type() != packetType_BufferedLDC) { //packet is not a Buffered LDC packet return false; } //calculate the number of active channels uint32 channels = ChannelMask(payload.read_uint8(PAYLOAD_OFFSET_CHANNEL_MASK)).count(); //calculate the size of a single data point uint32 dataSize = WirelessTypes::dataTypeSize(static_cast<WirelessTypes::DataType>(dataType)); uint32 recordSize = channels * dataSize; //if record size is zero, something is wrong. Bail now before divide by zero if(recordSize <= 0) { return false; } //the number of channel data bytes size_t numChannelBytes = payload.size() - PAYLOAD_OFFSET_CHANNEL_DATA; //verify that there are actually channel data bytes if(numChannelBytes == 0) { return false; } //verify the payload contains a correct number of bytes if(numChannelBytes % recordSize != 0) { return false; } //packet looks valid return true; }
void WirelessPacketCollector::addDataPacket(const WirelessPacket& packet) { //create a boost_lock for thread safety mutex_lock_guard lock(m_packetMutex); //update the last communication time NodeCommTimes::updateCommTime(packet.nodeAddress()); try { //add a different packet depending on its type switch(packet.type()) { //Low Duty Cycle packet case WirelessPacket::packetType_LDC: m_dataPackets.push_back(LdcPacket(packet)); break; //Sync Sampling Packet case WirelessPacket::packetType_SyncSampling: m_dataPackets.push_back(SyncSamplingPacket(packet)); break; //Buffered LDC packet case WirelessPacket::packetType_BufferedLDC: m_dataPackets.push_back(BufferedLdcPacket(packet)); break; //Low Duty Cycle with 16 channels packet case WirelessPacket::packetType_LDC_16ch: m_dataPackets.push_back(LdcPacket_16ch(packet)); break; //Sync Sampling with 16 channels packet case WirelessPacket::packetType_SyncSampling_16ch: m_dataPackets.push_back(SyncSamplingPacket_16ch(packet)); break; //Buffered LDC with 16 channels packet case WirelessPacket::packetType_BufferedLDC_16ch: m_dataPackets.push_back(BufferedLdcPacket_16ch(packet)); break; //Asynchronous Digital packet case WirelessPacket::packetType_AsyncDigital: m_dataPackets.push_back(AsyncDigitalPacket(packet)); break; //Asynchronous Digital/Analog packet case WirelessPacket::packetType_AsyncDigitalAnalog: m_dataPackets.push_back(AsyncDigitalAnalogPacket(packet)); break; //Structural Health Monitor packet case WirelessPacket::packetType_SHM: m_dataPackets.push_back(ShmPacket(packet)); break; //HclSmartBearing Raw packet case WirelessPacket::packetType_HclSmartBearing_Raw: m_dataPackets.push_back(HclSmartBearing_RawPacket(packet)); break; //Raw Angle Strain packet case WirelessPacket::packetType_rawAngleStrain: m_dataPackets.push_back(RawAngleStrainPacket(packet)); break; //Beacon Echo packet case WirelessPacket::packetType_beaconEcho: m_dataPackets.push_back(BeaconEchoPacket(packet)); break; //RF Sweep packet case WirelessPacket::packetType_rfScanSweep: m_dataPackets.push_back(RfSweepPacket(packet)); break; //Diagnostic packet case WirelessPacket::packetType_diagnostic: m_dataPackets.push_back(DiagnosticPacket(packet)); break; //Roller packet case WirelessPacket::packetType_roller: m_dataPackets.push_back(RollerPacket(packet)); break; //bad packet type, this function shouldn't have been called default: throw Error("Unknown Packet Type"); } //notify the read thread, if it is waiting for data to be put into the buffer m_emptyBufferCondition.notify_one(); } catch(std::exception&) { //there was an error building one of the packets, just return } }