dx10_v0.77.c


1
#include <usb.h>

2
#include <stdio.h>

3
#include <getopt.h>      // needed for getopt

4
5
extern usb_dev_handle *g_Dev;

6
extern int g_iFlags;

7
extern void abort(const char *fmt, ...);

8
9
#define WARMINGUP             0x01

10
#define NO_BEAM               0x02

11
#define INTERRUPTED_BEAM      0x04

12
#define KEYBOARD              0x08

13
14
typedef struct transdata {            // data

15
    int bAlign;    // 0: distance, 1: intensity

16
    int n;

17
    long long lldata[100];

18
} TransData;

19
20
//only this file////////////////////////////////////////////////

21
22
///////////////////////////

23
//

24
//      +----------------- unused (left) connector

25
//      |      +---------- compensator (relative humidity, air temperature, barometric pressure)

26
//      |      |      +--- used (right) connector

27
//      |      |      |

28
//      v      |      v

29
//    +----\   v   /----+

30
//    |     \-----/     |

31
//    |       AUX       |

32
//    |      TP I/O     |

33
//    |                 |

34
//    | O     ML10    O |

35
//    |       EC10      |

36
//     \               /

37
//      \      O      /

38
//       \    USB    /

39
//        +---------+

40
//             A

41
//             |

42
//             +---------- USB connector

43
//

44
//

45
// The PC sends commands down to the DX10 interface via pipe 0x01 and reads its answers back via pipe 0x81.

46
// A 5Khz data-stream will be read from DX10 interface via pipe 0x82.

47
// The dynamical measurement is a bit tricky, because the distances in the dynamic protocoll are only in the range of +-2.6 mm.

48
// Measure a static value once before gives the offset to this small +-2.6 mm range.

49
// Now you can measure a range of +-2.6 m.

50
// Even with non constant acceleration while starting the measurement, can give an offset of multiples of ~5.2 mm.

51
52
#include <math.h>

53
54
#define DYNAMIC_RANGE 0x400000          // at this value the dynamic data overflows (~5.2 mm)

55
#define NEXT_RING_INDEX(x, y) (((x+1)<y)?(x+1):(x+1-y))

56
int g_iFirstvalue;            // first static distance to shift the dynamic value

57
58
#define MAX_STREAM0x81 12

59
#define BUFFLEN0x81    0x200

60
struct {            // data for 12 asynchronous streams

61
    void* async;

62
    int nos;        // number of shorts

63
    unsigned short buff[BUFFLEN0x81];

64
} g_sCommand0x81[MAX_STREAM0x81];

65
int g_iCommand0x81_index;  // active stream for writing

66
67
#define MAX_STREAM0x82 20

68
#define BUFFLEN0x82    0x1000

69
struct {            // data for 20 asynchronous streams

70
    void* async;

71
    int nos;        // number of shorts

72
    unsigned short buff[BUFFLEN0x82];

73
} g_sData0x82[MAX_STREAM0x82];

74
int g_iData0x82_index; // active stream

75
76
77
int evaluate_static(int iAns, unsigned short *iAnswer)

78
{

79
    int ret;

80
    if (iAns>28) // length

81
// iAnswer[ 0] 8010

82
    if (iAnswer[1]==0xe1) // type1 with: len .... sum FF

83
    if (iAnswer[2]==0x0e) // length from [00e1 to 00ff]

84
    if (iAnswer[3]==0xd0) { // type2: measure

85
// iAnswer[ 4] 0000 0000  s000 000?  s: sign of counter,  ?: after 82

86
// iAnswer[ 5] 0000 0000  cccc cccc  c(00-FF): counter

87
// iAnswer[ 6] 0000 0000  cccc cccc  c(00-FF): counter

88
// iAnswer[ 7] 0000 0000  cccc cccc  c(00-FF): counter

89
// iAnswer[ 8] 0000 0000  cccc cccc  c(00-FF): counter

90
// iAnswer[ 9] 0000 0000  qp01 0000  q: interrupted beam,  p: no interference

91
// iAnswer[10] 0000 0000  0r00 000b  r: red,  b: booting=>no counter, no intensity

92
// iAnswer[11] 0000 0000  000? 0000  ?:at p=0 for 4 ticks

93
// iAnswer[12] 0000 0000  000i iiii  i(00-1F): intensity

94
// iAnswer[13] 0000 0000  xxxx xxxx  sum of iAnswer[1]-iAnswer[12]

95
// iAnswer[14] 0000 0000  1111 1111  00ff

96
            ret = (iAnswer[5]<<24)|(iAnswer[6]<<16)|(iAnswer[7]<<8)|iAnswer[8];

97
            if (iAnswer[4]&0x0080)

98
                ret=-ret;

99
            if (iAnswer[9]&0x0080) // interrupted beam

100
        g_iFlags=g_iFlags|INTERRUPTED_BEAM;

101
            return ret;

102
  } else

103
    abort("\nERROR: no point");

104
  return 0;

105
}

106
107
108
109
int dx10_eval(int len, int iStep, TransData *transdata) 

110
{

111
    static unsigned short iPacket=0;    // packet-counter

112
    static int iMode=0;      // counter within the 5 shorts

113
    static int iUsb=0;       // result = distance counter of wavelength/512

114
    static int iLastusb=0;   // last result

115
    static int iIntensity=0; // intensity

116
    int no=0;

117
    int j;

118
    

119
    if (g_sData0x82[g_iData0x82_index].buff[0] == iPacket) {

120
//

121
// Structure of datastream from 0x82:

122
// - First short of every packet is the packet-counter.

123
// - Then always 5 shorts for 1 distance (every 0,0002 sec = 5000 Hz).

124
// - Only the fifth short has bit 7 set.

125
//

126
// mode| bit. ..98  7654 3210

127
// ----+---------------------

128
//   0 | 0000 0000  0?cc cccc      0: always 0

129
//   1 | 0000 0000  0ccc cccc      1: always 1

130
//   2 | 0000 0000  0ccc cccc      c: counter (total 22 bit) for laser

131
//   3 | 0000 0000  0p0i iiii      i: intensity: 00-1F (00=0%, 1A=100%)

132
//   4 | 0000 0000  1000 qcc?      ?: unknown meaning (bit flickers)

133
//                                 q: interrupted beam

134
//                   p: no interference

135
//

136
// +----+----+ +----+----+ +----+----+ 

137
// |00cc|cccc| |cccc|cccc| |cccc|cccc| counter (total 22 bit) for laser

138
// |  26|5432| |1016|5432| |1054|3210| bit no

139
// |  42|2222| |2241|1111| |1100|0000| mode-byte

140
// +----+----+ +----+----+ +----+----+ 

141
//

142
        for(j=1; j<len; j++) {

143
            switch(iMode++) {

144
                case 0:

145
                    iUsb+=(g_sData0x82[g_iData0x82_index].buff[j]&0x3F) << 0; // bit 0-5

146
                    if   (g_sData0x82[g_iData0x82_index].buff[j]&~0x7F)

147
                        abort("\nmode 0: not zero: %04X", g_sData0x82[g_iData0x82_index].buff[j]&~0x7F);

148
                    break;

149
                case 1:

150
                    iUsb+=(g_sData0x82[g_iData0x82_index].buff[j]&0x7F) << 6;  // bit 6-12

151
                    if   (g_sData0x82[g_iData0x82_index].buff[j]&~0x7F)

152
                        abort("\nmode 1: not zero: %04X", g_sData0x82[g_iData0x82_index].buff[j]&~0x7F);

153
                    break;

154
                case 2:

155
                    iUsb+=(g_sData0x82[g_iData0x82_index].buff[j]&0x7F) << 14;   // bit 14-20

156
                    if   (g_sData0x82[g_iData0x82_index].buff[j]&~0x7F)

157
                        abort("\nmode 2: not zero: %04X", g_sData0x82[g_iData0x82_index].buff[j]&~0x7F);

158
                    break;

159
                case 3:

160
                    if   (g_sData0x82[g_iData0x82_index].buff[j]&~0x7F)

161
                        abort("\nmode 3: not zero: %04X", g_sData0x82[g_iData0x82_index].buff[j]&~0x7F);

162
          iIntensity=g_sData0x82[g_iData0x82_index].buff[j]&0x1F;

163
          // check errors

164
          if  (g_sData0x82[g_iData0x82_index].buff[j]&0x40)  // interrupted beam

165
            g_iFlags=g_iFlags|INTERRUPTED_BEAM;

166
                   if (iIntensity < 4) // low < 15%

167
            g_iFlags=g_iFlags|NO_BEAM;

168
                    break;

169
                case 4:

170
                    iUsb+=(g_sData0x82[g_iData0x82_index].buff[j]&0x02) << 13-1; // bit 13

171
                    iUsb+=(g_sData0x82[g_iData0x82_index].buff[j]&0x04) << 21-2; // bit 21

172
                    if  ((g_sData0x82[g_iData0x82_index].buff[j]&~0x0F) != 0x80)

173
                        abort("\nmode 4: not 0x0080: %04X", g_sData0x82[g_iData0x82_index].buff[j]&~0x0F);

174
           if (!((j==5)&&(iPacket==0))) {    // for the very first value not output anything (only store lastusb)

175
            if ((j==10)&&(iPacket==0)) {  // for the second value check direction and offset

176
              // For the very first time, offset is the 32bit value from static measurement.

177
              // After the following calculation, offset is the 32bit value without the last 22bit (which comes from dynamic measurement)

178
              // The calculation takes the first two dynamic values (time-distance 1*0,0002) and computes the value which was 500 time-steps before.

179
              // The difference of offset and this calculated value is the start value of offset.

180
              int off=0;

181
              if (iUsb-iLastusb >  DYNAMIC_RANGE/2) off=-DYNAMIC_RANGE;

182
              if (iUsb-iLastusb < -DYNAMIC_RANGE/2) off= DYNAMIC_RANGE;

183
#if 1

184
#define WAVELENGTH_IN_MM  0.000632818840000 // ~633nm

185
              printf( "static %.8f\n", 1.0*g_iFirstvalue*WAVELENGTH_IN_MM/512);

186
              printf( "last static value / %5.3fmm: %10.6f\n", DYNAMIC_RANGE*WAVELENGTH_IN_MM/512, 1.0*g_iFirstvalue/DYNAMIC_RANGE);

187
              printf( "calc dyn value:              %10.6f\n", 1.0*((iLastusb-off)-(iUsb-(iLastusb-off))*500)/DYNAMIC_RANGE);

188
              printf( "first offset in double:      %10.6f\n", 1.0*(g_iFirstvalue-((iLastusb-off)-(iUsb-(iLastusb-off))*500))/DYNAMIC_RANGE);

189
              off=(int)floor(                                  1.0*(g_iFirstvalue-((iLastusb-off)-(iUsb-(iLastusb-off))*500)+DYNAMIC_RANGE/2)/DYNAMIC_RANGE);

190
              printf( "first offset:                %3d\n", off);

191
#endif

192
              g_iFirstvalue=off * DYNAMIC_RANGE;

193
            }

194
            // check dynamic ranges to correct distance

195
            if (iUsb-iLastusb >  DYNAMIC_RANGE/2) g_iFirstvalue-=DYNAMIC_RANGE;

196
            if (iUsb-iLastusb < -DYNAMIC_RANGE/2) g_iFirstvalue+=DYNAMIC_RANGE;

197
            

198
            // check errors

199
            if  (g_sData0x82[g_iData0x82_index].buff[j]&0x08)  // interrupted beam

200
              g_iFlags=g_iFlags|INTERRUPTED_BEAM;

201
202
            // output, if necessary

203
            if (++transdata->n >= iStep) {   

204
              transdata->n=0;

205
              if (transdata->bAlign)

206
                transdata->lldata[no++]=(long long)(iIntensity*100.0/26.0);// intensity

207
              else

208
                transdata->lldata[no++]=((long long)(iUsb+g_iFirstvalue))<<6;// distance

209
              if (no<0)

210
                abort("\nERROR: underflow in transdata\n");

211
              if (no>=sizeof(transdata->lldata)/8)

212
                abort("\nERROR: overflow in transdata\n");

213
            }

214
          }

215
                    iLastusb=iUsb;

216
                    iMode=0;

217
                    iUsb=0;

218
                    break;

219
                default:

220
                    abort("\nERROR: impossible mode");

221
            }

222
        }

223
        iPacket++;

224
    } else

225
        abort("\nwrong packet number in dyn. data: expect:%04X, found:%04X (packet skipped)",

226
      iPacket, g_sData0x82[g_iData0x82_index].buff[0]);

227
    fflush(stdout);

228
  return no;

229
}

230
231
// read one async response -or- read till response ends with 0x00ff

232
int read_async(int flag)

233
{

234
    int i, ret, end;

235
    char condition=1;

236
    do {

237
        g_iCommand0x81_index = NEXT_RING_INDEX(g_iCommand0x81_index, MAX_STREAM0x81);

238
        ret=usb_reap_async(g_sCommand0x81[g_iCommand0x81_index].async, 5000);

239
#if 1

240
        printf("(%d): ", ret);

241
        for(i=0; i<ret/2; i++)

242
            printf("%04x ", *(unsigned short*)&g_sCommand0x81[g_iCommand0x81_index].buff[i]);

243
        printf("\n");

244
        fflush(stdout);

245
#endif

246
        end=*((unsigned short*)(&g_sCommand0x81[g_iCommand0x81_index].buff[ret/2-1]));

247
        usb_submit_async(g_sCommand0x81[g_iCommand0x81_index].async, (char*)&g_sCommand0x81[g_iCommand0x81_index].buff[0], BUFFLEN0x81);

248
        

249
        if (flag) condition=0;

250
        if (end==0x00ff) condition=0;

251
    } while (condition);

252
    return ret;

253
}

254
255
void dx10_start_seq1()

256
{

257
  int i;

258
  g_iData0x82_index=-1;

259
  g_iCommand0x81_index=-1;

260
  if (usb_clear_halt(g_Dev, 0x81) < 0) //-- URB_FUNCTION_RESET_PIPE

261
    abort("\nERROR: reset%s: ", usb_strerror());

262
263
  // do once for setup => load command pipe

264
    for(i=0; i<MAX_STREAM0x81; i++)

265
        if (usb_bulk_setup_async(g_Dev, &g_sCommand0x81[i].async, 0x81) < 0)

266
            abort("\nERROR: no setup before interrupt:\n%s", usb_strerror());

267
    // submit this transfer and returns immediately

268
    for(i=0; i<MAX_STREAM0x81; i++) {

269
        if (usb_submit_async(g_sCommand0x81[i].async, (char*)&g_sCommand0x81[i].buff[0], BUFFLEN0x81) < 0)

270
            abort("\nERROR: no submit before interrupt:\n%s", usb_strerror());

271
    }

272
}

273
274
void dx10_zero_seq234()

275
{

276
    char DOWN_Zerosta[]={0x10,0x80,0xe2,0x01,0x05,0x00,0xd3,0x00,0xba,0x00,0xff,0x00};

277
    if(usb_bulk_write(g_Dev, 0x01, DOWN_Zerosta, sizeof(DOWN_Zerosta), 1000)< 0)

278
        abort("\nERROR: write: ");

279
  // BACK_Commit

280
    read_async(0);

281
282
    char DOWN_Zerodyn[]={0x10,0x80,0xe2,0x01,0x05,0x00,0xcc,0x00,0xb3,0x00,0xff,0x00};

283
    if(usb_bulk_write(g_Dev, 0x01, DOWN_Zerodyn, sizeof(DOWN_Zerodyn), 1000)< 0)

284
        abort("\nERROR: write: ");

285
  // BACK_Commit

286
    read_async(0);

287
}

288
289
void dx10_pre_loop_seq2()

290
{

291
  int i, ret;

292
  // DOWN_Measure (=Position)

293
    char DOWN_Measure[]={0x10,0x80,0xe2,0x01,0x05,0x00,0xd0,0x00,0xb7,0x00,0xff,0x00};

294
    if(usb_bulk_write(g_Dev, 0x01, DOWN_Measure, sizeof(DOWN_Measure), 1000)< 0)

295
        abort("\nERROR: write: ");

296
  // BACK_Commit

297
    ret=read_async(0);

298
    g_iFirstvalue=evaluate_static(ret, &g_sCommand0x81[g_iCommand0x81_index].buff[0]);

299
300
  if (usb_clear_halt(g_Dev, 0x82) < 0)

301
    abort("\nERROR: reset 82%s", usb_strerror());

302
  for(i=0; i<MAX_STREAM0x82; i++) // do once for setup

303
    if (usb_bulk_setup_async(g_Dev, &g_sData0x82[i].async, 0x82) < 0)

304
      abort("\nERROR: usb_bulk_setup_async: %s", usb_strerror());

305
  for(i=0; i<MAX_STREAM0x82; i++) // submit this transfer and returns immediately

306
    if (usb_submit_async(g_sData0x82[i].async, (char*)&g_sData0x82[i].buff[0], BUFFLEN0x82) < 0)

307
      abort("\nERROR: usb_submit_async: %s", usb_strerror());

308
  

309
  // DOWN_MSta8030 (=Start)

310
    char DOWN_MSta8030[]={0x30,0x80};

311
    if(usb_bulk_write(g_Dev, 0x01, DOWN_MSta8030, sizeof(DOWN_MSta8030), 1000)< 0)

312
        abort("\nERROR: write: ");

313
  // BACK_Commit

314
    read_async(1);

315
}

316
317
int  dx10_read_loop_seq3() {

318
    g_iData0x82_index = NEXT_RING_INDEX(g_iData0x82_index, MAX_STREAM0x82);

319
  if ((g_sData0x82[g_iData0x82_index].nos=usb_reap_async(g_sData0x82[g_iData0x82_index].async, 5000)/2) < 0)

320
    abort("\nERROR: reap02 %d\n", g_iData0x82_index);

321
  if (usb_submit_async(g_sData0x82[g_iData0x82_index].async, (char*)&g_sData0x82[g_iData0x82_index].buff[0], BUFFLEN0x82) < 0)

322
    abort("\nERROR: usb_submit_async:\n%s", usb_strerror());

323
  return g_sData0x82[g_iData0x82_index].nos;

324
}

325
326
void dx10_post_loop_seq4()

327
{

328
  // DOWN_MEnd8040 (=Stop)

329
    char DOWN_MEnd8040[]={0x40,0x80};

330
    if(usb_bulk_write(g_Dev, 0x01, DOWN_MEnd8040, sizeof(DOWN_MEnd8040), 1000)< 0)

331
        abort("\nERROR: write: ");

332
  // BACK_Commit

333
    read_async(1);

334
335
  if (usb_resetep(g_Dev, 0x82) < 0)

336
    abort("\nERROR: abort/reset 82%s", usb_strerror());

337
}

338
339
void dx10_end_seq5()

340
{

341
  if (usb_resetep(g_Dev, 0x81) < 0)

342
    abort("\nERROR: abort/reset 81%s", usb_strerror());

343
  if (usb_resetep(g_Dev, 0x01) < 0)

344
    abort("\nERROR: abort/reset 82%s", usb_strerror());

345
}