From 65b5a3c41711a7b8909b5de74c373cb778d4ced3 Mon Sep 17 00:00:00 2001 From: Sam Hos Date: Mon, 7 Oct 2024 15:53:46 +0200 Subject: [PATCH] parser finaly works --- src/C++/Driver/src/KobukiParser.cpp | 384 ++++++++++++++-------------- 1 file changed, 196 insertions(+), 188 deletions(-) diff --git a/src/C++/Driver/src/KobukiParser.cpp b/src/C++/Driver/src/KobukiParser.cpp index 2dcd1f0..b43c487 100644 --- a/src/C++/Driver/src/KobukiParser.cpp +++ b/src/C++/Driver/src/KobukiParser.cpp @@ -3,201 +3,209 @@ int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) { int rtrnvalue = checkChecksum(data); - // ak je zly checksum,tak kaslat na to - if (rtrnvalue != 0) + if (rtrnvalue != 0) { + std::cerr << "Invalid checksum" << std::endl; return -2; + } int checkedValue = 1; - // kym neprejdeme celu dlzku - while (checkedValue < data[0]) - { - // basic data subload - if (data[checkedValue] == 0x01) - { - checkedValue++; - if (data[checkedValue] != 0x0F) - return -1; - 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++; - 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++; - } - else if (data[checkedValue] == 0x03) - { - checkedValue++; - if (data[checkedValue] != 0x03) - return -3; - checkedValue++; - output.IRSensorRight = data[checkedValue]; - checkedValue++; - output.IRSensorCenter = data[checkedValue]; - checkedValue++; - output.IRSensorLeft = data[checkedValue]; - checkedValue++; - } - else if (data[checkedValue] == 0x04) - { - checkedValue++; - if (data[checkedValue] != 0x07) - return -4; - checkedValue++; - output.GyroAngle = data[checkedValue + 1] * 256 + data[checkedValue]; - checkedValue += 2; - output.GyroAngleRate = data[checkedValue + 1] * 256 + data[checkedValue]; - checkedValue += 5; // 3 unsued - } - else if (data[checkedValue] == 0x05) - { - checkedValue++; - if (data[checkedValue] != 0x06) - return -5; - checkedValue++; - 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 if (data[checkedValue] == 0x06) - { - checkedValue++; - if (data[checkedValue] != 0x02) - return -6; - checkedValue++; - output.wheelCurrentLeft = data[checkedValue]; - checkedValue++; - output.wheelCurrentRight = data[checkedValue]; - checkedValue++; - } - else if (data[checkedValue] == 0x0A) - { - checkedValue++; - if (data[checkedValue] != 0x04) - return -7; - checkedValue++; - output.extraInfo.HardwareVersionPatch = data[checkedValue]; - checkedValue++; - output.extraInfo.HardwareVersionMinor = data[checkedValue]; - checkedValue++; - output.extraInfo.HardwareVersionMajor = data[checkedValue]; - checkedValue += 2; - } - else if (data[checkedValue] == 0x0B) - { - checkedValue++; - if (data[checkedValue] != 0x04) - return -8; - checkedValue++; - output.extraInfo.FirmwareVersionPatch = data[checkedValue]; - checkedValue++; - output.extraInfo.FirmwareVersionMinor = data[checkedValue]; - checkedValue++; - output.extraInfo.FirmwareVersionMajor = data[checkedValue]; - checkedValue += 2; - } - else if (data[checkedValue] == 0x0D) - { - checkedValue++; - if (data[checkedValue] % 2 != 0) - return -9; - checkedValue++; - 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 if (data[checkedValue] == 0x10) - { - checkedValue++; - if (data[checkedValue] != 0x10) - return -10; - 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 - } - else if (data[checkedValue] == 0x13) - { - checkedValue++; - if (data[checkedValue] != 0x0C) - return -11; - 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; - } - else - { - checkedValue++; - checkedValue += data[checkedValue] + 1; + while (checkedValue < data[0]) { + unsigned char dataType = data[checkedValue]; + unsigned char dataLength = data[checkedValue + 1]; + checkedValue += 2; + + 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++; + } 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++; + } 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 + } 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; + } 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++; + } 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; + } 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; + } 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); + } + } 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 + } 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; + } 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::checkChecksum(unsigned char *data) { unsigned char chckSum = 0; for (int i = 0; i < data[0] + 2; i++) {