diff --git a/src/C++/Driver/src/KobukiParser.cpp b/src/C++/Driver/src/KobukiParser.cpp index 629368b..b43c487 100644 --- a/src/C++/Driver/src/KobukiParser.cpp +++ b/src/C++/Driver/src/KobukiParser.cpp @@ -17,97 +17,10 @@ int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) { switch (dataType) { case 0x01: if (dataLength == 0x0F) { - parseBasicData(output, data, checkedValue); - } else { - checkedValue += dataLength; - } - break; - - case 0x03: - if (dataLength == 0x03) { - parseIRSensorData(output, data, checkedValue); - } else { - checkedValue += dataLength; - } - break; - - case 0x04: - if (dataLength == 0x07) { - parseGyroData(output, data, checkedValue); - } else { - checkedValue += dataLength; - } - break; - - case 0x05: - if (dataLength == 0x06) { - parseCliffSensorData(output, data, checkedValue); - } else { - checkedValue += dataLength; - } - break; - - case 0x06: - if (dataLength == 0x02) { - parseWheelCurrentData(output, data, checkedValue); - } else { - checkedValue += dataLength; - } - break; - - case 0x0A: - if (dataLength == 0x04) { - parseHardwareVersionData(output, data, checkedValue); - } else { - checkedValue += dataLength; - } - break; - - case 0x0B: - if (dataLength == 0x04) { - parseFirmwareVersionData(output, data, checkedValue); - } else { - checkedValue += dataLength; - } - break; - - case 0x0D: - if (dataLength % 2 == 0) { - parseXYZData(output, data, checkedValue); - } else { - checkedValue += dataLength; - } - break; - - case 0x10: - if (dataLength == 0x10) { - parseDigitalAnalogInputData(output, data, checkedValue); - } else { - checkedValue += dataLength; - } - break; - - case 0x13: - if (dataLength == 0x0C) { - parseUDIDData(output, data, checkedValue); - } else { - checkedValue += dataLength; - } - break; - - default: - std::cerr << "Unknown data type: " << std::hex << static_cast(dataType) << std::dec << std::endl; - checkedValue += dataLength; - break; - } - } - return 0; -} - -int KobukiParser::parseBasicData(TKobukiData &output, unsigned char *data, int &checkedValue){ output.timestamp = data[checkedValue + 1] * 256 + data[checkedValue]; checkedValue += 2; output.BumperCenter = data[checkedValue] & 0x02; + std::cout << "BumperCenter: " << output.BumperCenter << std::endl; output.BumperLeft = data[checkedValue] & 0x04; output.BumperRight = data[checkedValue] & 0x01; checkedValue++; @@ -134,78 +47,118 @@ int KobukiParser::parseBasicData(TKobukiData &output, unsigned char *data, int & checkedValue++; output.overCurrent = data[checkedValue]; checkedValue++; -} + } else { + std::cerr << "Invalid data length for type 0x01" << std::endl; + checkedValue += dataLength; + } + break; - -int KobukiParser::parseIRSensorData(TKobukiData &output, unsigned char *data, int &checkedValue){ - output.IRSensorRight = data[checkedValue]; + case 0x03: + if (dataLength == 0x03) { + output.IRSensorRight = data[checkedValue]; checkedValue++; output.IRSensorCenter = data[checkedValue]; checkedValue++; output.IRSensorLeft = data[checkedValue]; checkedValue++; -} + } else { + std::cerr << "Invalid data length for type 0x03" << std::endl; + checkedValue += dataLength; + } + break; -int KobukiParser::parseGyroData(TKobukiData &output, unsigned char *data, int &checkedValue){ + case 0x04: + if (dataLength == 0x07) { output.GyroAngle = data[checkedValue + 1] * 256 + data[checkedValue]; checkedValue += 2; output.GyroAngleRate = data[checkedValue + 1] * 256 + data[checkedValue]; checkedValue += 5; // 3 unused -} + } else { + std::cerr << "Invalid data length for type 0x04" << std::endl; + checkedValue += dataLength; + } + break; -int KobukiParser::parseCliffSensorData(TKobukiData &output, unsigned char *data, int &checkedValue){ - output.CliffSensorRight = data[checkedValue]; - checkedValue++; - output.CliffSensorCenter = data[checkedValue]; - checkedValue++; - output.CliffSensorLeft = data[checkedValue]; - checkedValue++; -} + case 0x05: + if (dataLength == 0x06) { + output.CliffSensorRight = data[checkedValue + 1] * 256 + data[checkedValue]; + checkedValue += 2; + output.CliffSensorCenter = data[checkedValue + 1] * 256 + data[checkedValue]; + checkedValue += 2; + output.CliffSensorLeft = data[checkedValue + 1] * 256 + data[checkedValue]; + checkedValue += 2; + } else { + std::cerr << "Invalid data length for type 0x05" << std::endl; + checkedValue += dataLength; + } + break; -int KobukiParser::parseWheelCurrentData(TKobukiData &output, unsigned char *data, int &checkedValue){ + case 0x06: + if (dataLength == 0x02) { output.wheelCurrentLeft = data[checkedValue]; checkedValue++; output.wheelCurrentRight = data[checkedValue]; checkedValue++; -} + } else { + std::cerr << "Invalid data length for type 0x06" << std::endl; + checkedValue += dataLength; + } + break; -int KobukiParser::parseHardwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue){ + case 0x0A: + if (dataLength == 0x04) { output.extraInfo.HardwareVersionPatch = data[checkedValue]; checkedValue++; output.extraInfo.HardwareVersionMinor = data[checkedValue]; checkedValue++; output.extraInfo.HardwareVersionMajor = data[checkedValue]; checkedValue += 2; -} + } else { + std::cerr << "Invalid data length for type 0x0A" << std::endl; + checkedValue += dataLength; + } + break; -int KobukiParser::parseFirmwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue){ + case 0x0B: + if (dataLength == 0x04) { output.extraInfo.FirmwareVersionPatch = data[checkedValue]; checkedValue++; output.extraInfo.FirmwareVersionMinor = data[checkedValue]; checkedValue++; output.extraInfo.FirmwareVersionMajor = data[checkedValue]; checkedValue += 2; -} + } else { + std::cerr << "Invalid data length for type 0x0B" << std::endl; + checkedValue += dataLength; + } + break; -int KobukiParser::parseExtraData(TKobukiData &output, unsigned char *data, int &checkedValue){ - output.extraInfo.UDID0 = data[checkedValue + 3] * 256 * 256 * 256 + - data[checkedValue + 2] * 256 * 256 + - data[checkedValue + 1] * 256 + - data[checkedValue]; - checkedValue += 4; - output.extraInfo.UDID1 = data[checkedValue + 3] * 256 * 256 * 256 + - data[checkedValue + 2] * 256 * 256 + - data[checkedValue + 1] * 256 + - data[checkedValue]; - checkedValue += 4; - output.extraInfo.UDID2 = data[checkedValue + 3] * 256 * 256 * 256 + - data[checkedValue + 2] * 256 * 256 + - data[checkedValue + 1] * 256 + - data[checkedValue]; - checkedValue += 4; -} + case 0x0D: + if (dataLength % 2 == 0) { + output.frameId = data[checkedValue]; + checkedValue++; + int howmanyFrames = data[checkedValue] / 3; + checkedValue++; + output.gyroData.reserve(howmanyFrames); + output.gyroData.clear(); + for (int hk = 0; hk < howmanyFrames; hk++) { + TRawGyroData temp; + temp.x = data[checkedValue + 1] * 256 + data[checkedValue]; + checkedValue += 2; + temp.y = data[checkedValue + 1] * 256 + data[checkedValue]; + checkedValue += 2; + temp.z = data[checkedValue + 1] * 256 + data[checkedValue]; + checkedValue += 2; + output.gyroData.push_back(temp); + } + } else { + std::cerr << "Invalid data length for type 0x0D" << std::endl; + checkedValue += dataLength; + } + break; -int KobukiParser::parseDigitalAnalogInputData(TKobukiData &output, unsigned char *data, int &checkedValue){ + case 0x10: + if (dataLength == 0x10) { output.digitalInput = data[checkedValue + 1] * 256 + data[checkedValue]; checkedValue += 2; output.analogInputCh0 = data[checkedValue + 1] * 256 + data[checkedValue]; @@ -216,9 +169,14 @@ int KobukiParser::parseDigitalAnalogInputData(TKobukiData &output, unsigned char checkedValue += 2; output.analogInputCh3 = data[checkedValue + 1] * 256 + data[checkedValue]; checkedValue += 8; // 2+6 -} + } else { + std::cerr << "Invalid data length for type 0x10" << std::endl; + checkedValue += dataLength; + } + break; -int KobukiParser::parseUDIDData(TKobukiData &output, unsigned char *data, int &checkedValue){ + case 0x13: + if (dataLength == 0x0C) { output.extraInfo.UDID0 = data[checkedValue + 3] * 256 * 256 * 256 + data[checkedValue + 2] * 256 * 256 + data[checkedValue + 1] * 256 + @@ -234,24 +192,20 @@ int KobukiParser::parseUDIDData(TKobukiData &output, unsigned char *data, int &c data[checkedValue + 1] * 256 + data[checkedValue]; checkedValue += 4; + } else { + std::cerr << "Invalid data length for type 0x13" << std::endl; + checkedValue += dataLength; + } + break; + + default: + std::cerr << "Unknown data type: " << std::hex << static_cast(dataType) << std::dec << std::endl; + checkedValue += dataLength; + break; + } + } + return 0; } - -int KobukiParser::parseXYZData(TKobukiData &output, unsigned char *data, int &checkedValue){ - output.gyroData.reserve(data[checkedValue]); - output.gyroData.clear(); - for (int hk = 0; hk < data[checkedValue]; hk++) { - TRawGyroData temp; - temp.x = data[checkedValue + 1] * 256 + data[checkedValue]; - checkedValue += 2; - temp.y = data[checkedValue + 1] * 256 + data[checkedValue]; - checkedValue += 2; - temp.z = data[checkedValue + 1] * 256 + data[checkedValue]; - checkedValue += 2; - output.gyroData.push_back(temp); - } -} - - int KobukiParser::checkChecksum(unsigned char *data) { unsigned char chckSum = 0; for (int i = 0; i < data[0] + 2; i++) {