fix illegal hardware instruction and bitwise operations

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

@@ -17,18 +17,105 @@ 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<int>(dataType) << std::dec << std::endl;
+                checkedValue += dataLength;
+                break;
+        }
+    }
+    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;
+    output.BumperCenter = (data[checkedValue] & 0x02) >> 1;
-                    std::cout << "BumperCenter: " << output.BumperCenter << std::endl;
+    output.BumperLeft = (data[checkedValue] & 0x04) >> 2;
-                    output.BumperLeft = data[checkedValue] & 0x04;
     output.BumperRight = data[checkedValue] & 0x01;
     checkedValue++;
-                    output.WheelDropLeft = data[checkedValue] & 0x02;
+    output.WheelDropLeft = (data[checkedValue] & 0x02) >> 1;
     output.WheelDropRight = data[checkedValue] & 0x01;
     checkedValue++;
-                    output.CliffCenter = data[checkedValue] & 0x02;
+    output.CliffCenter = (data[checkedValue] & 0x02) >> 1;
-                    output.CliffLeft = data[checkedValue] & 0x04;
+    output.CliffLeft = (data[checkedValue] & 0x04) >> 2;
     output.CliffRight = data[checkedValue] & 0x01;
     checkedValue++;
     output.EncoderLeft = data[checkedValue + 1] * 256 + data[checkedValue];
@@ -47,101 +134,62 @@ 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;
 }
-                break;
 
-            case 0x03:
+void KobukiParser::parseIRSensorData(TKobukiData &output, unsigned char *data, int &checkedValue){
-                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:
+void KobukiParser::parseGyroData(TKobukiData &output, unsigned char *data, int &checkedValue){
-                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:
+void KobukiParser::parseCliffSensorData(TKobukiData &output, unsigned char *data, int &checkedValue){
-                if (dataLength == 0x06) {
+    output.CliffSensorRight = data[checkedValue];
-                    output.CliffSensorRight = data[checkedValue + 1] * 256 + data[checkedValue];
+    checkedValue++;
-                    checkedValue += 2;
+    output.CliffSensorCenter = data[checkedValue];
-                    output.CliffSensorCenter = data[checkedValue + 1] * 256 + data[checkedValue];
+    checkedValue++;
-                    checkedValue += 2;
+    output.CliffSensorLeft = data[checkedValue];
-                    output.CliffSensorLeft = data[checkedValue + 1] * 256 + data[checkedValue];
+    checkedValue++;
-                    checkedValue += 2;
-                } else {
-                    std::cerr << "Invalid data length for type 0x05" << std::endl;
-                    checkedValue += dataLength;
 }
-                break;
 
-            case 0x06:
+void KobukiParser::parseWheelCurrentData(TKobukiData &output, unsigned char *data, int &checkedValue){
-                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:
+void KobukiParser::parseHardwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue){
-                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:
+void KobukiParser::parseFirmwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue){
-                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:
+void KobukiParser::parseXYZData(TKobukiData &output, unsigned char *data, int &checkedValue){
-                if (dataLength % 2 == 0) {
+    output.gyroData.reserve(data[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 < data[checkedValue]; hk++) {
         TRawGyroData temp;
         temp.x = data[checkedValue + 1] * 256 + data[checkedValue];
         checkedValue += 2;
@@ -151,14 +199,9 @@ 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;
 }
-                break;
 
-            case 0x10:
+void KobukiParser::parseDigitalAnalogInputData(TKobukiData &output, unsigned char *data, int &checkedValue){
-                if (dataLength == 0x10) {
     output.digitalInput = data[checkedValue + 1] * 256 + data[checkedValue];
     checkedValue += 2;
     output.analogInputCh0 = data[checkedValue + 1] * 256 + data[checkedValue];
@@ -169,14 +212,9 @@ 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;
 }
-                break;
 
-            case 0x13:
+void KobukiParser::parseUDIDData(TKobukiData &output, unsigned char *data, int &checkedValue){
-                if (dataLength == 0x0C) {
     output.extraInfo.UDID0 = data[checkedValue + 3] * 256 * 256 * 256 +
                              data[checkedValue + 2] * 256 * 256 +
                              data[checkedValue + 1] * 256 +
@@ -192,20 +230,8 @@ 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;
 }
-                break;
 
-            default:
-                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++) {

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);
+    void parseBasicData(TKobukiData &output, unsigned char *data, int &checkedValue);
-    int parseIRSensorData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    void parseIRSensorData(TKobukiData &output, unsigned char *data, int &checkedValue);
-    int parseGyroData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    void parseGyroData(TKobukiData &output, unsigned char *data, int &checkedValue);
-    int parseCliffSensorData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    void parseCliffSensorData(TKobukiData &output, unsigned char *data, int &checkedValue);
-    int parseWheelCurrentData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    void parseWheelCurrentData(TKobukiData &output, unsigned char *data, int &checkedValue);
-    int parseHardwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    void parseHardwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue);
-    int parseFirmwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    void parseFirmwareVersionData(TKobukiData &output, unsigned char *data, int &checkedValue);
-    int parseXYZData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    void parseXYZData(TKobukiData &output, unsigned char *data, int &checkedValue);
-    int parseDigitalAnalogInputData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    void parseDigitalAnalogInputData(TKobukiData &output, unsigned char *data, int &checkedValue);
-    int parseUDIDData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    void parseUDIDData(TKobukiData &output, unsigned char *data, int &checkedValue);
-    int parseExtraData(TKobukiData &output, unsigned char *data, int &checkedValue);
+    void parseExtraData(TKobukiData &output, unsigned char *data, int &checkedValue);
 
 };