Smikkelbakje/J2S1-Kobuki
1
0
Fork 0
mirror of https://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79.git synced 2025-08-03 20:04:58 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

fix illegal hardware instruction

Browse Source
This commit is contained in:
Sam Hos
2024-10-08 11:26:46 +02:00
parent bd689da36e
commit 840bdafd04
1 changed files with 100 additions and 146 deletions
Download Patch File Download Diff File Expand all files Collapse all files

220
src/C++/Driver/src/KobukiParser.cpp
View File

@@ -17,97 +17,10 @@ int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
switch (dataType) { switch (dataType) {
case 0x01: case 0x01:
if (dataLength == 0x0F) { 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;
}
int KobukiParser::parseBasicData(TKobukiData &output, unsigned char *data, int &checkedValue){
output.timestamp = data[checkedValue + 1] * 256 + data[checkedValue]; output.timestamp = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; 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++; checkedValue++;
@@ -134,78 +47,118 @@ int KobukiParser::parseBasicData(TKobukiData &output, unsigned char *data, int &
checkedValue++; checkedValue++;
output.overCurrent = data[checkedValue]; output.overCurrent = data[checkedValue];
checkedValue++; checkedValue++;
} else {
std::cerr << "Invalid data length for type 0x01" << std::endl;
checkedValue += dataLength;
} }
break;
case 0x03:
int KobukiParser::parseIRSensorData(TKobukiData &output, unsigned char *data, int &checkedValue){ if (dataLength == 0x03) {
output.IRSensorRight = data[checkedValue]; output.IRSensorRight = data[checkedValue];
checkedValue++; checkedValue++;
output.IRSensorCenter = data[checkedValue]; output.IRSensorCenter = data[checkedValue];
checkedValue++; checkedValue++;
output.IRSensorLeft = data[checkedValue]; output.IRSensorLeft = data[checkedValue];
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]; output.GyroAngle = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; checkedValue += 2;
output.GyroAngleRate = data[checkedValue + 1] * 256 + data[checkedValue]; output.GyroAngleRate = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 5; // 3 unused 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){ case 0x05:
output.CliffSensorRight = data[checkedValue]; if (dataLength == 0x06) {
checkedValue++; output.CliffSensorRight = data[checkedValue + 1] * 256 + data[checkedValue];
output.CliffSensorCenter = data[checkedValue]; checkedValue += 2;
checkedValue++; output.CliffSensorCenter = data[checkedValue + 1] * 256 + data[checkedValue];
output.CliffSensorLeft = data[checkedValue]; checkedValue += 2;
checkedValue++; 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]; output.wheelCurrentLeft = data[checkedValue];
checkedValue++; checkedValue++;
output.wheelCurrentRight = data[checkedValue]; output.wheelCurrentRight = data[checkedValue];
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]; output.extraInfo.HardwareVersionPatch = data[checkedValue];
checkedValue++; checkedValue++;
output.extraInfo.HardwareVersionMinor = data[checkedValue]; output.extraInfo.HardwareVersionMinor = data[checkedValue];
checkedValue++; checkedValue++;
output.extraInfo.HardwareVersionMajor = data[checkedValue]; output.extraInfo.HardwareVersionMajor = data[checkedValue];
checkedValue += 2; 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]; output.extraInfo.FirmwareVersionPatch = data[checkedValue];
checkedValue++; checkedValue++;
output.extraInfo.FirmwareVersionMinor = data[checkedValue]; output.extraInfo.FirmwareVersionMinor = data[checkedValue];
checkedValue++; checkedValue++;
output.extraInfo.FirmwareVersionMajor = data[checkedValue]; output.extraInfo.FirmwareVersionMajor = data[checkedValue];
checkedValue += 2; 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){ case 0x0D:
output.extraInfo.UDID0 = data[checkedValue + 3] * 256 * 256 * 256 + if (dataLength % 2 == 0) {
data[checkedValue + 2] * 256 * 256 + output.frameId = data[checkedValue];
data[checkedValue + 1] * 256 + checkedValue++;
data[checkedValue]; int howmanyFrames = data[checkedValue] / 3;
checkedValue += 4; checkedValue++;
output.extraInfo.UDID1 = data[checkedValue + 3] * 256 * 256 * 256 + output.gyroData.reserve(howmanyFrames);
data[checkedValue + 2] * 256 * 256 + output.gyroData.clear();
data[checkedValue + 1] * 256 + for (int hk = 0; hk < howmanyFrames; hk++) {
data[checkedValue]; TRawGyroData temp;
checkedValue += 4; temp.x = data[checkedValue + 1] * 256 + data[checkedValue];
output.extraInfo.UDID2 = data[checkedValue + 3] * 256 * 256 * 256 + checkedValue += 2;
data[checkedValue + 2] * 256 * 256 + temp.y = data[checkedValue + 1] * 256 + data[checkedValue];
data[checkedValue + 1] * 256 + checkedValue += 2;
data[checkedValue]; temp.z = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 4; 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]; output.digitalInput = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; checkedValue += 2;
output.analogInputCh0 = data[checkedValue + 1] * 256 + data[checkedValue]; output.analogInputCh0 = data[checkedValue + 1] * 256 + data[checkedValue];
@@ -216,9 +169,14 @@ int KobukiParser::parseDigitalAnalogInputData(TKobukiData &output, unsigned char
checkedValue += 2; checkedValue += 2;
output.analogInputCh3 = data[checkedValue + 1] * 256 + data[checkedValue]; output.analogInputCh3 = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 8; // 2+6 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 + output.extraInfo.UDID0 = data[checkedValue + 3] * 256 * 256 * 256 +
data[checkedValue + 2] * 256 * 256 + data[checkedValue + 2] * 256 * 256 +
data[checkedValue + 1] * 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 + 1] * 256 +
data[checkedValue]; data[checkedValue];
checkedValue += 4; checkedValue += 4;
} else {
std::cerr << "Invalid data length for type 0x13" << std::endl;
checkedValue += dataLength;
} }
break;
int KobukiParser::parseXYZData(TKobukiData &output, unsigned char *data, int &checkedValue){ default:
output.gyroData.reserve(data[checkedValue]); std::cerr << "Unknown data type: " << std::hex << static_cast<int>(dataType) << std::dec << std::endl;
output.gyroData.clear(); checkedValue += dataLength;
for (int hk = 0; hk < data[checkedValue]; hk++) { break;
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);
} }
} }
return 0;
}
int KobukiParser::checkChecksum(unsigned char *data) { int KobukiParser::checkChecksum(unsigned char *data) {
unsigned char chckSum = 0; unsigned char chckSum = 0;
for (int i = 0; i < data[0] + 2; i++) { for (int i = 0; i < data[0] + 2; i++) {