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

parser finaly works

Browse Source
This commit is contained in:
Sam Hos
2024-10-07 15:53:46 +02:00
parent 76c088bd50
commit 65b5a3c417
1 changed files with 196 additions and 188 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -3,201 +3,209 @@
int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) { int KobukiParser::parseKobukiMessage(TKobukiData &output, unsigned char *data) {
int rtrnvalue = checkChecksum(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; return -2;
}
int checkedValue = 1; 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; output.timestamp = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue++; checkedValue += 2;
output.timestamp = data[checkedValue + 1] * 256 + data[checkedValue]; output.BumperCenter = data[checkedValue] & 0x02;
checkedValue += 2; std::cout << "BumperCenter: " << output.BumperCenter << std::endl;
output.BumperCenter = data[checkedValue] && 0x02; output.BumperLeft = data[checkedValue] & 0x04;
std::cout << "BumperCenter: " << output.BumperCenter << std::endl; output.BumperRight = data[checkedValue] & 0x01;
output.BumperLeft = data[checkedValue] && 0x04; checkedValue++;
output.BumperRight = data[checkedValue] && 0x01; output.WheelDropLeft = data[checkedValue] & 0x02;
checkedValue++; output.WheelDropRight = data[checkedValue] & 0x01;
output.WheelDropLeft = data[checkedValue] && 0x02; checkedValue++;
output.WheelDropRight = data[checkedValue] && 0x01; output.CliffCenter = data[checkedValue] & 0x02;
checkedValue++; output.CliffLeft = data[checkedValue] & 0x04;
output.CliffCenter = data[checkedValue] && 0x02; output.CliffRight = data[checkedValue] & 0x01;
output.CliffLeft = data[checkedValue] && 0x04; checkedValue++;
output.CliffRight = data[checkedValue] && 0x01; output.EncoderLeft = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue++; checkedValue += 2;
output.EncoderLeft = data[checkedValue + 1] * 256 + data[checkedValue]; output.EncoderRight = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; checkedValue += 2;
output.EncoderRight = data[checkedValue + 1] * 256 + data[checkedValue]; output.PWMleft = data[checkedValue];
checkedValue += 2; checkedValue++;
output.PWMleft = data[checkedValue]; output.PWMright = data[checkedValue];
checkedValue++; checkedValue++;
output.PWMright = data[checkedValue]; output.ButtonPress = data[checkedValue];
checkedValue++; checkedValue++;
output.ButtonPress = data[checkedValue]; output.Charger = data[checkedValue];
checkedValue++; checkedValue++;
output.Charger = data[checkedValue]; output.Battery = data[checkedValue];
checkedValue++; checkedValue++;
output.Battery = data[checkedValue]; output.overCurrent = data[checkedValue];
checkedValue++; checkedValue++;
output.overCurrent = data[checkedValue]; } else {
checkedValue++; std::cerr << "Invalid data length for type 0x01" << std::endl;
} checkedValue += dataLength;
else if (data[checkedValue] == 0x03) }
{ break;
checkedValue++;
if (data[checkedValue] != 0x03) case 0x03:
return -3; if (dataLength == 0x03) {
checkedValue++; 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;
else if (data[checkedValue] == 0x04) checkedValue += dataLength;
{ }
checkedValue++; break;
if (data[checkedValue] != 0x07)
return -4; case 0x04:
checkedValue++; 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 unsued checkedValue += 5; // 3 unused
} } else {
else if (data[checkedValue] == 0x05) std::cerr << "Invalid data length for type 0x04" << std::endl;
{ checkedValue += dataLength;
checkedValue++; }
if (data[checkedValue] != 0x06) break;
return -5;
checkedValue++; case 0x05:
output.CliffSensorRight = if (dataLength == 0x06) {
data[checkedValue + 1] * 256 + data[checkedValue]; output.CliffSensorRight = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; checkedValue += 2;
output.CliffSensorCenter = output.CliffSensorCenter = data[checkedValue + 1] * 256 + data[checkedValue];
data[checkedValue + 1] * 256 + data[checkedValue]; checkedValue += 2;
checkedValue += 2; output.CliffSensorLeft = data[checkedValue + 1] * 256 + data[checkedValue];
output.CliffSensorLeft = checkedValue += 2;
data[checkedValue + 1] * 256 + data[checkedValue]; } else {
checkedValue += 2; std::cerr << "Invalid data length for type 0x05" << std::endl;
} checkedValue += dataLength;
else if (data[checkedValue] == 0x06) }
{ break;
checkedValue++;
if (data[checkedValue] != 0x02) case 0x06:
return -6; if (dataLength == 0x02) {
checkedValue++; 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;
else if (data[checkedValue] == 0x0A) checkedValue += dataLength;
{ }
checkedValue++; break;
if (data[checkedValue] != 0x04)
return -7; case 0x0A:
checkedValue++; 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 {
else if (data[checkedValue] == 0x0B) std::cerr << "Invalid data length for type 0x0A" << std::endl;
{ checkedValue += dataLength;
checkedValue++; }
if (data[checkedValue] != 0x04) break;
return -8;
checkedValue++; case 0x0B:
output.extraInfo.FirmwareVersionPatch = data[checkedValue]; if (dataLength == 0x04) {
checkedValue++; output.extraInfo.FirmwareVersionPatch = data[checkedValue];
output.extraInfo.FirmwareVersionMinor = data[checkedValue]; checkedValue++;
checkedValue++; output.extraInfo.FirmwareVersionMinor = data[checkedValue];
output.extraInfo.FirmwareVersionMajor = data[checkedValue]; checkedValue++;
checkedValue += 2; output.extraInfo.FirmwareVersionMajor = data[checkedValue];
} checkedValue += 2;
else if (data[checkedValue] == 0x0D) } else {
{ std::cerr << "Invalid data length for type 0x0B" << std::endl;
checkedValue++; checkedValue += dataLength;
if (data[checkedValue] % 2 != 0) }
return -9; break;
checkedValue++;
output.frameId = data[checkedValue]; case 0x0D:
checkedValue++; if (dataLength % 2 == 0) {
int howmanyFrames = data[checkedValue] / 3; output.frameId = data[checkedValue];
checkedValue++; checkedValue++;
output.gyroData.reserve(howmanyFrames); int howmanyFrames = data[checkedValue] / 3;
output.gyroData.clear(); checkedValue++;
for (int hk = 0; hk < howmanyFrames; hk++) output.gyroData.reserve(howmanyFrames);
{ output.gyroData.clear();
TRawGyroData temp; for (int hk = 0; hk < howmanyFrames; hk++) {
temp.x = data[checkedValue + 1] * 256 + data[checkedValue]; TRawGyroData temp;
checkedValue += 2; temp.x = data[checkedValue + 1] * 256 + data[checkedValue];
temp.y = data[checkedValue + 1] * 256 + data[checkedValue]; checkedValue += 2;
checkedValue += 2; temp.y = data[checkedValue + 1] * 256 + data[checkedValue];
temp.z = data[checkedValue + 1] * 256 + data[checkedValue]; checkedValue += 2;
checkedValue += 2; temp.z = data[checkedValue + 1] * 256 + data[checkedValue];
output.gyroData.push_back(temp); checkedValue += 2;
} output.gyroData.push_back(temp);
} }
else if (data[checkedValue] == 0x10) } else {
{ std::cerr << "Invalid data length for type 0x0D" << std::endl;
checkedValue++; checkedValue += dataLength;
if (data[checkedValue] != 0x10) }
return -10; break;
checkedValue++;
output.digitalInput = data[checkedValue + 1] * 256 + data[checkedValue]; case 0x10:
checkedValue += 2; if (dataLength == 0x10) {
output.analogInputCh0 = data[checkedValue + 1] * 256 + data[checkedValue]; output.digitalInput = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; checkedValue += 2;
output.analogInputCh1 = data[checkedValue + 1] * 256 + data[checkedValue]; output.analogInputCh0 = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; checkedValue += 2;
output.analogInputCh2 = data[checkedValue + 1] * 256 + data[checkedValue]; output.analogInputCh1 = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 2; checkedValue += 2;
output.analogInputCh3 = data[checkedValue + 1] * 256 + data[checkedValue]; output.analogInputCh2 = data[checkedValue + 1] * 256 + data[checkedValue];
checkedValue += 8; // 2+6 checkedValue += 2;
} output.analogInputCh3 = data[checkedValue + 1] * 256 + data[checkedValue];
else if (data[checkedValue] == 0x13) checkedValue += 8; // 2+6
{ } else {
checkedValue++; std::cerr << "Invalid data length for type 0x10" << std::endl;
if (data[checkedValue] != 0x0C) checkedValue += dataLength;
return -11; }
checkedValue++; break;
output.extraInfo.UDID0 = data[checkedValue + 3] * 256 * 256 * 256 +
data[checkedValue + 2] * 256 * 256 + case 0x13:
data[checkedValue + 1] * 256 + if (dataLength == 0x0C) {
data[checkedValue]; output.extraInfo.UDID0 = data[checkedValue + 3] * 256 * 256 * 256 +
checkedValue += 4; data[checkedValue + 2] * 256 * 256 +
output.extraInfo.UDID1 = data[checkedValue + 3] * 256 * 256 * 256 + data[checkedValue + 1] * 256 +
data[checkedValue + 2] * 256 * 256 + data[checkedValue];
data[checkedValue + 1] * 256 + checkedValue += 4;
data[checkedValue]; output.extraInfo.UDID1 = data[checkedValue + 3] * 256 * 256 * 256 +
checkedValue += 4; data[checkedValue + 2] * 256 * 256 +
output.extraInfo.UDID2 = data[checkedValue + 3] * 256 * 256 * 256 + data[checkedValue + 1] * 256 +
data[checkedValue + 2] * 256 * 256 + data[checkedValue];
data[checkedValue + 1] * 256 + checkedValue += 4;
data[checkedValue]; output.extraInfo.UDID2 = data[checkedValue + 3] * 256 * 256 * 256 +
checkedValue += 4; data[checkedValue + 2] * 256 * 256 +
} data[checkedValue + 1] * 256 +
else data[checkedValue];
{ checkedValue += 4;
checkedValue++; } else {
checkedValue += data[checkedValue] + 1; 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; 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++) {