From 5cb6534a4eb6143ffceb6f2c9e4cc631b30f152f Mon Sep 17 00:00:00 2001 From: Sam Hos Date: Tue, 8 Oct 2024 11:50:27 +0200 Subject: [PATCH] fix illegal hardware instruction and bitwise operations --- src/C++/Driver/src/KobukiParser.cpp | 252 +++++++++++++++------------- src/C++/Driver/src/KobukiParser.h | 22 +-- 2 files changed, 150 insertions(+), 124 deletions(-) diff --git a/src/C++/Driver/src/KobukiParser.cpp b/src/C++/Driver/src/KobukiParser.cpp index b43c487..8a62c73 100644 --- a/src/C++/Driver/src/KobukiParser.cpp +++ b/src/C++/Driver/src/KobukiParser.cpp @@ -17,183 +17,80 @@ int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) { switch (dataType) { case 0x01: if (dataLength == 0x0F) { - 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++; - output.WheelDropLeft = data[checkedValue] & 0x02; - output.WheelDropRight = data[checkedValue] & 0x01; - checkedValue++; - output.CliffCenter = data[checkedValue] & 0x02; - output.CliffLeft = data[checkedValue] & 0x04; - output.CliffRight = data[checkedValue] & 0x01; - checkedValue++; - output.EncoderLeft = data[checkedValue + 1] * 256 + data[checkedValue]; - checkedValue += 2; - output.EncoderRight = data[checkedValue + 1] * 256 + data[checkedValue]; - checkedValue += 2; - output.PWMleft = data[checkedValue]; - checkedValue++; - output.PWMright = data[checkedValue]; - checkedValue++; - output.ButtonPress = data[checkedValue]; - checkedValue++; - output.Charger = data[checkedValue]; - checkedValue++; - output.Battery = data[checkedValue]; - checkedValue++; - output.overCurrent = data[checkedValue]; - checkedValue++; + parseBasicData(output, data, checkedValue); } else { - std::cerr << "Invalid data length for type 0x01" << std::endl; checkedValue += dataLength; } break; case 0x03: if (dataLength == 0x03) { - output.IRSensorRight = data[checkedValue]; - checkedValue++; - output.IRSensorCenter = data[checkedValue]; - checkedValue++; - output.IRSensorLeft = data[checkedValue]; - checkedValue++; + parseIRSensorData(output, data, checkedValue); } else { - std::cerr << "Invalid data length for type 0x03" << std::endl; checkedValue += dataLength; } break; 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 + parseGyroData(output, data, checkedValue); } else { - std::cerr << "Invalid data length for type 0x04" << std::endl; checkedValue += dataLength; } break; 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; + parseCliffSensorData(output, data, checkedValue); } else { - std::cerr << "Invalid data length for type 0x05" << std::endl; checkedValue += dataLength; } break; case 0x06: if (dataLength == 0x02) { - output.wheelCurrentLeft = data[checkedValue]; - checkedValue++; - output.wheelCurrentRight = data[checkedValue]; - checkedValue++; + parseWheelCurrentData(output, data, checkedValue); } else { - std::cerr << "Invalid data length for type 0x06" << std::endl; checkedValue += dataLength; } break; case 0x0A: if (dataLength == 0x04) { - output.extraInfo.HardwareVersionPatch = data[checkedValue]; - checkedValue++; - output.extraInfo.HardwareVersionMinor = data[checkedValue]; - checkedValue++; - output.extraInfo.HardwareVersionMajor = data[checkedValue]; - checkedValue += 2; + parseHardwareVersionData(output, data, checkedValue); } else { - std::cerr << "Invalid data length for type 0x0A" << std::endl; checkedValue += dataLength; } break; case 0x0B: if (dataLength == 0x04) { - output.extraInfo.FirmwareVersionPatch = data[checkedValue]; - checkedValue++; - output.extraInfo.FirmwareVersionMinor = data[checkedValue]; - checkedValue++; - output.extraInfo.FirmwareVersionMajor = data[checkedValue]; - checkedValue += 2; + parseFirmwareVersionData(output, data, checkedValue); } else { - std::cerr << "Invalid data length for type 0x0B" << std::endl; checkedValue += dataLength; } break; 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); - } + parseXYZData(output, data, checkedValue); } else { - std::cerr << "Invalid data length for type 0x0D" << std::endl; checkedValue += dataLength; } break; case 0x10: if (dataLength == 0x10) { - output.digitalInput = data[checkedValue + 1] * 256 + data[checkedValue]; - checkedValue += 2; - output.analogInputCh0 = data[checkedValue + 1] * 256 + data[checkedValue]; - checkedValue += 2; - output.analogInputCh1 = data[checkedValue + 1] * 256 + data[checkedValue]; - checkedValue += 2; - output.analogInputCh2 = data[checkedValue + 1] * 256 + data[checkedValue]; - checkedValue += 2; - output.analogInputCh3 = data[checkedValue + 1] * 256 + data[checkedValue]; - checkedValue += 8; // 2+6 + parseDigitalAnalogInputData(output, data, checkedValue); } else { - std::cerr << "Invalid data length for type 0x10" << std::endl; checkedValue += dataLength; } break; case 0x13: if (dataLength == 0x0C) { - 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; + parseUDIDData(output, data, checkedValue); } else { - std::cerr << "Invalid data length for type 0x13" << std::endl; checkedValue += dataLength; } break; @@ -206,6 +103,135 @@ int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) { } return 0; } + +void KobukiParser::parseBasicData(TKobukiData &output, unsigned char *data, int &checkedValue){ + output.timestamp = data[checkedValue + 1] * 256 + data[checkedValue]; + checkedValue += 2; + output.BumperCenter = (data[checkedValue] & 0x02) >> 1; + output.BumperLeft = (data[checkedValue] & 0x04) >> 2; + output.BumperRight = data[checkedValue] & 0x01; + checkedValue++; + output.WheelDropLeft = (data[checkedValue] & 0x02) >> 1; + output.WheelDropRight = data[checkedValue] & 0x01; + checkedValue++; + output.CliffCenter = (data[checkedValue] & 0x02) >> 1; + output.CliffLeft = (data[checkedValue] & 0x04) >> 2; + output.CliffRight = data[checkedValue] & 0x01; + checkedValue++; + output.EncoderLeft = data[checkedValue + 1] * 256 + data[checkedValue]; + checkedValue += 2; + output.EncoderRight = data[checkedValue + 1] * 256 + data[checkedValue]; + checkedValue += 2; + output.PWMleft = data[checkedValue]; + checkedValue++; + output.PWMright = data[checkedValue]; + checkedValue++; + output.ButtonPress = data[checkedValue]; + checkedValue++; + output.Charger = data[checkedValue]; + checkedValue++; + output.Battery = data[checkedValue]; + checkedValue++; + output.overCurrent = data[checkedValue]; + checkedValue++; +} + +void KobukiParser::parseIRSensorData(TKobukiData &output, unsigned char *data, int &checkedValue){ + output.IRSensorRight = data[checkedValue]; + checkedValue++; + output.IRSensorCenter = data[checkedValue]; + checkedValue++; + output.IRSensorLeft = data[checkedValue]; + checkedValue++; +} + +void KobukiParser::parseGyroData(TKobukiData &output, unsigned char *data, int &checkedValue){ + output.GyroAngle = data[checkedValue + 1] * 256 + data[checkedValue]; + checkedValue += 2; + output.GyroAngleRate = data[checkedValue + 1] * 256 + data[checkedValue]; + checkedValue += 5; // 3 unused +} + +void KobukiParser::parseCliffSensorData(TKobukiData &output, unsigned char *data, int &checkedValue){ + output.CliffSensorRight = data[checkedValue]; + checkedValue++; + output.CliffSensorCenter = data[checkedValue]; + checkedValue++; + output.CliffSensorLeft = data[checkedValue]; + checkedValue++; +} + +void KobukiParser::parseWheelCurrentData(TKobukiData &output, unsigned char *data, int &checkedValue){ + output.wheelCurrentLeft = data[checkedValue]; + checkedValue++; + output.wheelCurrentRight = data[checkedValue]; + checkedValue++; +} + +void KobukiParser::parseHardwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue){ + output.extraInfo.HardwareVersionPatch = data[checkedValue]; + checkedValue++; + output.extraInfo.HardwareVersionMinor = data[checkedValue]; + checkedValue++; + output.extraInfo.HardwareVersionMajor = data[checkedValue]; + checkedValue += 2; +} + +void KobukiParser::parseFirmwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue){ + output.extraInfo.FirmwareVersionPatch = data[checkedValue]; + checkedValue++; + output.extraInfo.FirmwareVersionMinor = data[checkedValue]; + checkedValue++; + output.extraInfo.FirmwareVersionMajor = data[checkedValue]; + checkedValue += 2; +} + +void 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); + } +} + +void KobukiParser::parseDigitalAnalogInputData(TKobukiData &output, unsigned char *data, int &checkedValue){ + output.digitalInput = data[checkedValue + 1] * 256 + data[checkedValue]; + checkedValue += 2; + output.analogInputCh0 = data[checkedValue + 1] * 256 + data[checkedValue]; + checkedValue += 2; + output.analogInputCh1 = data[checkedValue + 1] * 256 + data[checkedValue]; + checkedValue += 2; + output.analogInputCh2 = data[checkedValue + 1] * 256 + data[checkedValue]; + checkedValue += 2; + output.analogInputCh3 = data[checkedValue + 1] * 256 + data[checkedValue]; + checkedValue += 8; // 2+6 +} + +void KobukiParser::parseUDIDData(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; +} + int KobukiParser::checkChecksum(unsigned char *data) { unsigned char chckSum = 0; for (int i = 0; i < data[0] + 2; i++) { diff --git a/src/C++/Driver/src/KobukiParser.h b/src/C++/Driver/src/KobukiParser.h index 5bdb216..aca3eb0 100644 --- a/src/C++/Driver/src/KobukiParser.h +++ b/src/C++/Driver/src/KobukiParser.h @@ -38,17 +38,17 @@ public: private: int checkChecksum(unsigned char *data); - int parseBasicData(TKobukiData &output, unsigned char *data, int &checkedValue); - int parseIRSensorData(TKobukiData &output, unsigned char *data, int &checkedValue); - int parseGyroData(TKobukiData &output, unsigned char *data, int &checkedValue); - int parseCliffSensorData(TKobukiData &output, unsigned char *data, int &checkedValue); - int parseWheelCurrentData(TKobukiData &output, unsigned char *data, int &checkedValue); - int parseHardwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue); - int parseFirmwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue); - int parseXYZData(TKobukiData &output, unsigned char *data, int &checkedValue); - int parseDigitalAnalogInputData(TKobukiData &output, unsigned char *data, int &checkedValue); - int parseUDIDData(TKobukiData &output, unsigned char *data, int &checkedValue); - int parseExtraData(TKobukiData &output, unsigned char *data, int &checkedValue); + void parseBasicData(TKobukiData &output, unsigned char *data, int &checkedValue); + void parseIRSensorData(TKobukiData &output, unsigned char *data, int &checkedValue); + void parseGyroData(TKobukiData &output, unsigned char *data, int &checkedValue); + void parseCliffSensorData(TKobukiData &output, unsigned char *data, int &checkedValue); + void parseWheelCurrentData(TKobukiData &output, unsigned char *data, int &checkedValue); + void parseHardwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue); + void parseFirmwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue); + void parseXYZData(TKobukiData &output, unsigned char *data, int &checkedValue); + void parseDigitalAnalogInputData(TKobukiData &output, unsigned char *data, int &checkedValue); + void parseUDIDData(TKobukiData &output, unsigned char *data, int &checkedValue); + void parseExtraData(TKobukiData &output, unsigned char *data, int &checkedValue); };