parser finaly works

src/C++/Driver/src/KobukiParser.cpp

@@ -3,34 +3,33 @@
 
 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]) {
-    while (checkedValue < data[0])
+        unsigned char dataType = data[checkedValue];
-    {
+        unsigned char dataLength = data[checkedValue + 1];
-        // basic data subload
+        checkedValue += 2;
-        if (data[checkedValue] == 0x01)
+
-        {
+        switch (dataType) {
-            checkedValue++;
+            case 0x01:
-            if (data[checkedValue] != 0x0F)
+                if (dataLength == 0x0F) {
-                return -1;
-            checkedValue++;
                     output.timestamp = data[checkedValue + 1] * 256 + data[checkedValue];
                     checkedValue += 2;
-            output.BumperCenter = data[checkedValue] && 0x02;
+                    output.BumperCenter = data[checkedValue] & 0x02;
                     std::cout << "BumperCenter: " << output.BumperCenter << std::endl;
-            output.BumperLeft = data[checkedValue] && 0x04;
+                    output.BumperLeft = data[checkedValue] & 0x04;
-            output.BumperRight = data[checkedValue] && 0x01;
+                    output.BumperRight = data[checkedValue] & 0x01;
                     checkedValue++;
-            output.WheelDropLeft = data[checkedValue] && 0x02;
+                    output.WheelDropLeft = data[checkedValue] & 0x02;
-            output.WheelDropRight = data[checkedValue] && 0x01;
+                    output.WheelDropRight = data[checkedValue] & 0x01;
                     checkedValue++;
-            output.CliffCenter = data[checkedValue] && 0x02;
+                    output.CliffCenter = data[checkedValue] & 0x02;
-            output.CliffLeft = data[checkedValue] && 0x04;
+                    output.CliffLeft = data[checkedValue] & 0x04;
-            output.CliffRight = data[checkedValue] && 0x01;
+                    output.CliffRight = data[checkedValue] & 0x01;
                     checkedValue++;
                     output.EncoderLeft = data[checkedValue + 1] * 256 + data[checkedValue];
                     checkedValue += 2;
@@ -48,98 +47,101 @@ int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
                     checkedValue++;
                     output.overCurrent = data[checkedValue];
                     checkedValue++;
+                } else {
+                    std::cerr << "Invalid data length for type 0x01" << std::endl;
+                    checkedValue += dataLength;
                 }
-        else if (data[checkedValue] == 0x03)
+                break;
-        {
+
-            checkedValue++;
+            case 0x03:
-            if (data[checkedValue] != 0x03)
+                if (dataLength == 0x03) {
-                return -3;
-            checkedValue++;
                     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;
                 }
-        else if (data[checkedValue] == 0x04)
+                break;
-        {
+
-            checkedValue++;
+            case 0x04:
-            if (data[checkedValue] != 0x07)
+                if (dataLength == 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
+                    checkedValue += 5; // 3 unused
+                } else {
+                    std::cerr << "Invalid data length for type 0x04" << std::endl;
+                    checkedValue += dataLength;
                 }
-        else if (data[checkedValue] == 0x05)
+                break;
-        {
+
-            checkedValue++;
+            case 0x05:
-            if (data[checkedValue] != 0x06)
+                if (dataLength == 0x06) {
-                return -5;
+                    output.CliffSensorRight = data[checkedValue + 1] * 256 + data[checkedValue];
-            checkedValue++;
-            output.CliffSensorRight =
-                data[checkedValue + 1] * 256 + data[checkedValue];
                     checkedValue += 2;
-            output.CliffSensorCenter =
+                    output.CliffSensorCenter = data[checkedValue + 1] * 256 + data[checkedValue];
-                data[checkedValue + 1] * 256 + data[checkedValue];
                     checkedValue += 2;
-            output.CliffSensorLeft =
+                    output.CliffSensorLeft = data[checkedValue + 1] * 256 + data[checkedValue];
-                data[checkedValue + 1] * 256 + data[checkedValue];
                     checkedValue += 2;
+                } else {
+                    std::cerr << "Invalid data length for type 0x05" << std::endl;
+                    checkedValue += dataLength;
                 }
-        else if (data[checkedValue] == 0x06)
+                break;
-        {
+
-            checkedValue++;
+            case 0x06:
-            if (data[checkedValue] != 0x02)
+                if (dataLength == 0x02) {
-                return -6;
-            checkedValue++;
                     output.wheelCurrentLeft = data[checkedValue];
                     checkedValue++;
                     output.wheelCurrentRight = data[checkedValue];
                     checkedValue++;
+                } else {
+                    std::cerr << "Invalid data length for type 0x06" << std::endl;
+                    checkedValue += dataLength;
                 }
-        else if (data[checkedValue] == 0x0A)
+                break;
-        {
+
-            checkedValue++;
+            case 0x0A:
-            if (data[checkedValue] != 0x04)
+                if (dataLength == 0x04) {
-                return -7;
-            checkedValue++;
                     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;
                 }
-        else if (data[checkedValue] == 0x0B)
+                break;
-        {
+
-            checkedValue++;
+            case 0x0B:
-            if (data[checkedValue] != 0x04)
+                if (dataLength == 0x04) {
-                return -8;
-            checkedValue++;
                     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;
                 }
-        else if (data[checkedValue] == 0x0D)
+                break;
-        {
+
-            checkedValue++;
+            case 0x0D:
-            if (data[checkedValue] % 2 != 0)
+                if (dataLength % 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++)
+                    for (int hk = 0; hk < howmanyFrames; hk++) {
-            {
                         TRawGyroData temp;
                         temp.x = data[checkedValue + 1] * 256 + data[checkedValue];
                         checkedValue += 2;
@@ -149,13 +151,14 @@ int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
                         checkedValue += 2;
                         output.gyroData.push_back(temp);
                     }
+                } else {
+                    std::cerr << "Invalid data length for type 0x0D" << std::endl;
+                    checkedValue += dataLength;
                 }
-        else if (data[checkedValue] == 0x10)
+                break;
-        {
+
-            checkedValue++;
+            case 0x10:
-            if (data[checkedValue] != 0x10)
+                if (dataLength == 0x10) {
-                return -10;
-            checkedValue++;
                     output.digitalInput = data[checkedValue + 1] * 256 + data[checkedValue];
                     checkedValue += 2;
                     output.analogInputCh0 = data[checkedValue + 1] * 256 + data[checkedValue];
@@ -166,13 +169,14 @@ int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
                     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;
                 }
-        else if (data[checkedValue] == 0x13)
+                break;
-        {
+
-            checkedValue++;
+            case 0x13:
-            if (data[checkedValue] != 0x0C)
+                if (dataLength == 0x0C) {
-                return -11;
-            checkedValue++;
                     output.extraInfo.UDID0 = data[checkedValue + 3] * 256 * 256 * 256 +
                                              data[checkedValue + 2] * 256 * 256 +
                                              data[checkedValue + 1] * 256 +
@@ -188,16 +192,20 @@ int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
                                              data[checkedValue + 1] * 256 +
                                              data[checkedValue];
                     checkedValue += 4;
+                } else {
+                    std::cerr << "Invalid data length for type 0x13" << std::endl;
+                    checkedValue += dataLength;
                 }
-        else
+                break;
-        {
+
-            checkedValue++;
+            default:
-            checkedValue += data[checkedValue] + 1;
+                std::cerr << "Unknown data type: " << std::hex << static_cast<int>(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++) {