M2D-Laser-Scanner with Ethernet-Interface
Manual
HB-M2D-Ethernet-E
Page 30 of 39
www.MElsensor.com
// M2DReadFrame, use only after M2DSync suceeded
// read count points (4 or 5 bytes) of
// scan data and put values to x, z, and intensity
// return 0 = OK, -1 = timeout, -3 = data format not known
int M2DReadFrame(M2Dinfo *inf, int count, int *x, int *z, int *intensity)
{
int i,v0,v1,v2,v3,v4;
switch (inf->sync[0]){
case 1: // data ver.1
case 4: // like 1 but number of points depends on sensor
// read 4 bytes / point
if((inf->sync[1] & 1)==1){
// linear
for(i=0;i<count;i++){
v0=ByteFromM2(inf); if(v0 == -1) return -1;
v1=ByteFromM2(inf); if(v1 == -1) return -1;
v2=ByteFromM2(inf); if(v2 == -1) return -1;
v3=ByteFromM2(inf); if(v3 == -1) return -1;
x[i]=v0+((v1 & 0x60)<<2)+((v3 & 0x07)<<9);
z[i]=v2+((v1 & 0x1F)<<7);
v3=v3 & 0xF8;
intensity[i]=(v3<=128) ? (v3 & 127) << 1 : -((v3 & 127) << 1);
}
}
else{
// not linear
for(i=0;i<count;i++){
v0=ByteFromM2(inf); if(v0 == -1) return -1;
v1=ByteFromM2(inf); if(v1 == -1) return -1;
v2=ByteFromM2(inf); if(v2 == -1) return -1;
v3=ByteFromM2(inf); if(v3 == -1) return -1;
x[i]=v0+((v1 & 0x70)<<3);
z[i]=v2+((v1 & 0x0F)<<7);
intensity[i]=(v3<=128) ? (v3 & 127) << 1 : -((v3 & 127) << 1);
}
}
break;
case 3:
// encoder data already done in sync
case 2:
// read 5 bytes / point
for(i=0;i<count;i++){
v0=ByteFromM2(inf); if(v0 == -1) return -1;
v1=ByteFromM2(inf); if(v1 == -1) return -1;
v2=ByteFromM2(inf); if(v2 == -1) return -1;
v3=ByteFromM2(inf); if(v3 == -1) return -1;
v4=ByteFromM2(inf); if(v4 == -1) return -1;
x[i]=v0+(v1<<7);
z[i]=v2+((v3<<7);
intensity[i]=v4;
}
break;
default:
return -3
}
return 0;
}
