frame_timestamp = sub->Data(pdata++);
}
else if (dversion == 0x2){
- v2 = 1;
data = sub->Data(pdata);
frame_id = (data & 0x000000ff);
frame_id = (source << 16) | frame_id;
#endif
if (sub->Data(pdata) != 0x55555555) {
- //printf("ERROR: Sensor Header (0x55555555) Not Found! (got: %08x)\n",sub->Data(pdata));
+ printf("ERROR: Sensor Header (0x55555555) Not Found! (got: %08x)\n",sub->Data(pdata));
return -1;
}
sensorIsValid = 1;
//statistics[mySensor][FRVALID]++;
}
- else if(frame_status == 0x0f) { // New Format, DVERSION 0x2+
+ else if(frame_status == 0xff) { // New Format, DVERSION 0x2+
sensorIsValid = 1;
//statistics[mySensor][FRVALID]++;
}
statistics[mySensor][FRBROKEN]++;
}
- if(sensorIsValid && frame_length>0x0){
+ if(sensorIsValid){
//Hey Sensor, tell me who you are!
pdata++;
unsigned sensorNumber = sub->Data(pdata++);