Im trying to parse the serial data from a sensor but i (think?) im struggling to format the angle offset correctly,
Below is the format of the packet i am receiving and relevant descriptions.
my distance and start angle seem to match up (in the real world) but my angle compensation is way off.
Im pretty sure i got the compensation method correct.
so im guessing there is an error in my bit shifting and byte masking for the angle compensation bits.
Some fresh eyes would be most welcome. i suspect the issue is in the βget angle compensationβ but i have included the rest incase.
relevant code
// get start angle
float currentStartAngle = (packet[2] + ((packet[3] & 0x7F) << 8))/64.;
println("packet count = "+count+" current start angle = " + currentStartAngle + " last start angle = "+lastStartAngle + " angle difference = "+angleDiff(lastStartAngle, currentStartAngle));
//break down to 5 bit cabins starting at 4
for(int i = 4; i<= 79; i = i+5){
//get angle compensation
float angleComp1 = (packet[i+4] & 0x0F) + ((packet[i] & 0x03) << 4);
float angleComp2 = (packet[i+4] >> 4) + ((packet[i+2] & 0x03) << 4);
println(" angleComp1 = "+angleComp1+" " + " angleComp2 = " + angleComp2);
//calculate real angle
float realAngle1 = lastStartAngle + angleDiff(lastStartAngle, currentStartAngle)/32 * count - angleComp1;
float realAngle2 = lastStartAngle + angleDiff(lastStartAngle, currentStartAngle)/32 * count - angleComp2;
println(" realAngle1 = "+realAngle1+" " + " realAngle2 = " + realAngle2);
//get distances
float distance1 = (int(packet[i]>> 2) + int(packet[i+1]<< 6)); //shift and add packets
float distance2 = (int(packet[i+2]>> 2) + int(packet[i+3]<< 6));
//println(" distance1 = "+distance1+" " + " distance2 = " + distance2);
m_currScan.measurements[m_currScan.measurementCount++] = new LidarExpressMeasurement(realAngle1, distance1);
m_currScan.measurements[m_currScan.measurementCount++] = new LidarExpressMeasurement(realAngle2, distance2);
}
lastStartAngle = currentStartAngle;
}
//angle difference method
public float angleDiff(float lSAngle, float cSAngle){
if (lSAngle<=cSAngle)
return (cSAngle-lSAngle);
else if(lSAngle>cSAngle)
return (360. + cSAngle - lSAngle);
return 0;
}
And the print out from a couple of scans
packet count = 244 current start angle = 163.5625 last start angle = 126.328125 angle difference = 37.234375
angleComp1 = 62.0 angleComp2 = 46.0
realAngle1 = 348.24023 realAngle2 = 364.24023
angleComp1 = 62.0 angleComp2 = 46.0
realAngle1 = 348.24023 realAngle2 = 364.24023
angleComp1 = 61.0 angleComp2 = 45.0
realAngle1 = 349.24023 realAngle2 = 365.24023
angleComp1 = 62.0 angleComp2 = 46.0
realAngle1 = 348.24023 realAngle2 = 364.24023
angleComp1 = 62.0 angleComp2 = 45.0
realAngle1 = 348.24023 realAngle2 = 365.24023
angleComp1 = 61.0 angleComp2 = 45.0
realAngle1 = 349.24023 realAngle2 = 365.24023
angleComp1 = 62.0 angleComp2 = 45.0
realAngle1 = 348.24023 realAngle2 = 365.24023
angleComp1 = 62.0 angleComp2 = 46.0
realAngle1 = 348.24023 realAngle2 = 364.24023
angleComp1 = 62.0 angleComp2 = 46.0
realAngle1 = 348.24023 realAngle2 = 364.24023
angleComp1 = 62.0 angleComp2 = 46.0
realAngle1 = 348.24023 realAngle2 = 364.24023
angleComp1 = 62.0 angleComp2 = 46.0
realAngle1 = 348.24023 realAngle2 = 364.24023
angleComp1 = 62.0 angleComp2 = 46.0
realAngle1 = 348.24023 realAngle2 = 364.24023
angleComp1 = 62.0 angleComp2 = 46.0
realAngle1 = 348.24023 realAngle2 = 364.24023
angleComp1 = 62.0 angleComp2 = 46.0
realAngle1 = 348.24023 realAngle2 = 364.24023
angleComp1 = 62.0 angleComp2 = 46.0
realAngle1 = 348.24023 realAngle2 = 364.24023
angleComp1 = 0.0 angleComp2 = 45.0
realAngle1 = 410.24023 realAngle2 = 365.24023
packet count = 246 current start angle = 204.5 last start angle = 163.5625 angle difference = 40.9375
angleComp1 = 0.0 angleComp2 = 0.0
realAngle1 = 478.26953 realAngle2 = 478.26953
angleComp1 = 61.0 angleComp2 = 45.0
realAngle1 = 417.26953 realAngle2 = 433.26953
angleComp1 = 61.0 angleComp2 = 45.0
realAngle1 = 417.26953 realAngle2 = 433.26953
angleComp1 = 61.0 angleComp2 = 45.0
realAngle1 = 417.26953 realAngle2 = 433.26953
angleComp1 = 0.0 angleComp2 = 0.0
realAngle1 = 478.26953 realAngle2 = 478.26953
angleComp1 = 0.0 angleComp2 = 0.0
realAngle1 = 478.26953 realAngle2 = 478.26953
angleComp1 = 0.0 angleComp2 = 0.0
realAngle1 = 478.26953 realAngle2 = 478.26953
angleComp1 = 0.0 angleComp2 = 43.0
realAngle1 = 478.26953 realAngle2 = 435.26953
angleComp1 = 59.0 angleComp2 = 42.0
realAngle1 = 419.26953 realAngle2 = 436.26953
angleComp1 = 58.0 angleComp2 = 42.0
realAngle1 = 420.26953 realAngle2 = 436.26953
angleComp1 = 57.0 angleComp2 = 41.0
realAngle1 = 421.26953 realAngle2 = 437.26953
angleComp1 = 57.0 angleComp2 = 41.0
realAngle1 = 421.26953 realAngle2 = 437.26953
angleComp1 = 57.0 angleComp2 = 41.0
realAngle1 = 421.26953 realAngle2 = 437.26953
angleComp1 = 57.0 angleComp2 = 41.0
realAngle1 = 421.26953 realAngle2 = 437.26953
angleComp1 = 57.0 angleComp2 = 41.0
realAngle1 = 421.26953 realAngle2 = 437.26953
angleComp1 = 56.0 angleComp2 = 40.0
realAngle1 = 422.26953 realAngle2 = 438.26953