MEL M2-iLAN Series, Руководство пользователя

Страница: 34 / 44

M2i-LAN Laser-Scanner manual
HB-M2-iLAN-UDP-E.doc page 34 of 44
MEL Mikroelektronik GmbH, Breslauer Str. 2, 85386 Eching / Germany www.MELSensor.de
// 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;
}