← Back to branch summary

~cyphylab/cyphy/bonsai

« back to all changes in this revision

Viewing changes to cyphy_Drivers/cyphy_kinect/freenect/libfreenect/src/cameras.c

  • Committer: enddl22
  • Date: 2012-01-16 05:03:16 UTC
  • Revision ID: enddl22@gmail.com-20120116050316-xgzak3dfbvzvztlj
change name

Show diffs side-by-side

added added

removed removed

Lines of Context:
cyphy_Drivers/cyphy_kinect/freenect/libfreenect/src/cameras.c
 
1
/*
 
2
 * This file is part of the OpenKinect Project. http://www.openkinect.org
 
3
 *
 
4
 * Copyright (c) 2010 individual OpenKinect contributors. See the CONTRIB file
 
5
 * for details.
 
6
 *
 
7
 * This code is licensed to you under the terms of the Apache License, version
 
8
 * 2.0, or, at your option, the terms of the GNU General Public License,
 
9
 * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses,
 
10
 * or the following URLs:
 
11
 * http://www.apache.org/licenses/LICENSE-2.0
 
12
 * http://www.gnu.org/licenses/gpl-2.0.txt
 
13
 *
 
14
 * If you redistribute this file in source form, modified or unmodified, you
 
15
 * may:
 
16
 *   1) Leave this header intact and distribute it under the same terms,
 
17
 *      accompanying it with the APACHE20 and GPL20 files, or
 
18
 *   2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or
 
19
 *   3) Delete the GPL v2 clause and accompany it with the APACHE20 file
 
20
 * In all cases you must keep the copyright notice intact and include a copy
 
21
 * of the CONTRIB file.
 
22
 *
 
23
 * Binary distributions must follow the binary distribution requirements of
 
24
 * either License.
 
25
 */
 
26
 
 
27
#include <stdio.h>
 
28
#include <stdlib.h>
 
29
#include <string.h>
 
30
#include <unistd.h>
 
31
 
 
32
#include "freenect_internal.h"
 
33
 
 
34
struct pkt_hdr
 
35
{
 
36
  uint8_t magic[2];
 
37
  uint8_t pad;
 
38
  uint8_t flag;
 
39
  uint8_t unk1;
 
40
  uint8_t seq;
 
41
  uint8_t unk2;
 
42
  uint8_t unk3;
 
43
  uint32_t timestamp;
 
44
};
 
45
 
 
46
extern const struct caminit inits[];
 
47
extern const int num_inits;
 
48
 
 
49
static int
 
50
stream_process (freenect_context * ctx, packet_stream * strm, uint8_t * pkt, int len)
 
51
{
 
52
  if (len < 12)
 
53
    return 0;
 
54
 
 
55
  struct pkt_hdr *hdr = (void *) pkt;
 
56
  uint8_t *data = pkt + sizeof (*hdr);
 
57
  int datalen = len - sizeof (*hdr);
 
58
 
 
59
  if (hdr->magic[0] != 'R' || hdr->magic[1] != 'B')
 
60
  {
 
61
    FN_LOG (strm->valid_frames < 2 ? LL_SPEW : LL_NOTICE,
 
62
            "[Stream %02x] Invalid magic %02x%02x\n", strm->flag, hdr->magic[0], hdr->magic[1]);
 
63
    return 0;
 
64
  }
 
65
 
 
66
  FN_FLOOD ("[Stream %02x] Packet with flag: %02x\n", strm->flag, hdr->flag);
 
67
 
 
68
  uint8_t sof = strm->flag | 1;
 
69
  uint8_t mof = strm->flag | 2;
 
70
  uint8_t eof = strm->flag | 5;
 
71
 
 
72
  // sync if required, dropping packets until SOF
 
73
  if (!strm->synced)
 
74
  {
 
75
    if (hdr->flag != sof)
 
76
    {
 
77
      FN_SPEW ("[Stream %02x] Not synced yet...\n", strm->flag);
 
78
      return 0;
 
79
    }
 
80
    strm->synced = 1;
 
81
    strm->seq = hdr->seq;
 
82
    strm->pkt_num = 0;
 
83
    strm->valid_pkts = 0;
 
84
    strm->got_pkts = 0;
 
85
  }
 
86
 
 
87
  int got_frame = 0;
 
88
 
 
89
  // handle lost packets
 
90
  if (strm->seq != hdr->seq)
 
91
  {
 
92
    uint8_t lost = strm->seq - hdr->seq;
 
93
    FN_LOG (strm->valid_frames < 2 ? LL_SPEW : LL_INFO, "[Stream %02x] Lost %d packets\n", strm->flag, lost);
 
94
    if (lost > 5)
 
95
    {
 
96
      FN_LOG (strm->valid_frames < 2 ? LL_SPEW : LL_NOTICE,
 
97
              "[Stream %02x] Lost too many packets, resyncing...\n", strm->flag);
 
98
      strm->synced = 0;
 
99
      return 0;
 
100
    }
 
101
    strm->seq = hdr->seq;
 
102
    int left = strm->pkts_per_frame - strm->pkt_num;
 
103
    if (left <= lost)
 
104
    {
 
105
      strm->pkt_num = lost - left;
 
106
      strm->valid_pkts = strm->got_pkts;
 
107
      strm->got_pkts = 0;
 
108
      got_frame = 1;
 
109
      strm->timestamp = strm->last_timestamp;
 
110
      strm->valid_frames++;
 
111
    }
 
112
    else
 
113
    {
 
114
      strm->pkt_num += lost;
 
115
    }
 
116
  }
 
117
 
 
118
  // check the header to make sure it's what we expect
 
119
  if (!(strm->pkt_num == 0 && hdr->flag == sof) &&
 
120
      !(strm->pkt_num == strm->pkts_per_frame - 1 && hdr->flag == eof) &&
 
121
      !(strm->pkt_num > 0 && strm->pkt_num < strm->pkts_per_frame - 1 && hdr->flag == mof))
 
122
  {
 
123
    FN_LOG (strm->valid_frames < 2 ? LL_SPEW : LL_NOTICE,
 
124
            "[Stream %02x] Inconsistent flag %02x with %d packets in buf (%d total), resyncing...\n",
 
125
            strm->flag, hdr->flag, strm->pkt_num, strm->pkts_per_frame);
 
126
    strm->synced = 0;
 
127
    return got_frame;
 
128
  }
 
129
 
 
130
  // copy data
 
131
  if (datalen > strm->pkt_size)
 
132
  {
 
133
    FN_LOG (strm->valid_frames < 2 ? LL_SPEW : LL_WARNING,
 
134
            "[Stream %02x] Expected %d data bytes, but got %d. Dropping...\n", strm->flag, strm->pkt_size, datalen);
 
135
    return got_frame;
 
136
  }
 
137
 
 
138
  if (datalen != strm->pkt_size && hdr->flag != eof)
 
139
    FN_LOG (strm->valid_frames < 2 ? LL_SPEW : LL_WARNING,
 
140
            "[Stream %02x] Expected %d data bytes, but got only %d\n", strm->flag, strm->pkt_size, datalen);
 
141
 
 
142
  uint8_t *dbuf = strm->buf + strm->pkt_num * strm->pkt_size;
 
143
  memcpy (dbuf, data, datalen);
 
144
 
 
145
  strm->pkt_num++;
 
146
  strm->seq++;
 
147
  strm->got_pkts++;
 
148
 
 
149
  strm->last_timestamp = hdr->timestamp;
 
150
 
 
151
  if (strm->pkt_num == strm->pkts_per_frame)
 
152
  {
 
153
    strm->pkt_num = 0;
 
154
    strm->valid_pkts = strm->got_pkts;
 
155
    strm->got_pkts = 0;
 
156
    strm->timestamp = hdr->timestamp;
 
157
    strm->valid_frames++;
 
158
    return 1;
 
159
  }
 
160
  else
 
161
  {
 
162
    return got_frame;
 
163
  }
 
164
}
 
165
 
 
166
// Unpack buffer of (vw bit) data into padded 16bit buffer.
 
167
static inline void
 
168
convert_packed_to_16bit (uint8_t * raw, uint16_t * frame, int vw)
 
169
{
 
170
  int mask = (1 << vw) - 1;
 
171
  int i;
 
172
  int bitshift = 0;
 
173
  for (i = 0; i < (640 * 480); i++)
 
174
  {
 
175
    int idx = (i * vw) / 8;
 
176
    uint32_t word = (raw[idx] << (16)) | (raw[idx + 1] << 8) | raw[idx + 2];
 
177
    frame[i] = ((word >> (((3 * 8) - vw) - bitshift)) & mask);
 
178
    bitshift = (bitshift + vw) % 8;
 
179
  }
 
180
}
 
181
 
 
182
static void
 
183
depth_process (freenect_device * dev, uint8_t * pkt, int len)
 
184
{
 
185
  freenect_context *ctx = dev->parent;
 
186
 
 
187
  if (len == 0)
 
188
    return;
 
189
 
 
190
  if (!dev->depth_running)
 
191
    return;
 
192
 
 
193
  int got_frame = stream_process (ctx, &dev->depth_stream, pkt, len);
 
194
 
 
195
  if (!got_frame)
 
196
    return;
 
197
 
 
198
  FN_SPEW ("Got depth frame %d/%d packets arrived, TS %08x\n",
 
199
           dev->depth_stream.valid_pkts, dev->depth_stream.pkts_per_frame, dev->depth_stream.timestamp);
 
200
 
 
201
  switch (dev->depth_format)
 
202
  {
 
203
  case FREENECT_FORMAT_11_BIT:
 
204
    convert_packed_to_16bit (dev->depth_raw, dev->depth_frame, 11);
 
205
    if (dev->depth_cb)
 
206
      dev->depth_cb (dev, dev->depth_frame, dev->depth_stream.timestamp);
 
207
    break;
 
208
  case FREENECT_FORMAT_10_BIT:
 
209
    convert_packed_to_16bit (dev->depth_raw, dev->depth_frame, 10);
 
210
    if (dev->depth_cb)
 
211
      dev->depth_cb (dev, dev->depth_frame, dev->depth_stream.timestamp);
 
212
    break;
 
213
  case FREENECT_FORMAT_PACKED_10_BIT:
 
214
  case FREENECT_FORMAT_PACKED_11_BIT:
 
215
    if (dev->depth_cb)
 
216
      dev->depth_cb (dev, dev->depth_raw, dev->depth_stream.timestamp);
 
217
    break;
 
218
  }
 
219
}
 
220
 
 
221
static void
 
222
rgb_process (freenect_device * dev, uint8_t * pkt, int len)
 
223
{
 
224
  freenect_context *ctx = dev->parent;
 
225
  int x, y, i;
 
226
 
 
227
  if (len == 0)
 
228
    return;
 
229
 
 
230
  if (!dev->rgb_running && !dev->ir_running)
 
231
    return;
 
232
 
 
233
  int got_frame = stream_process (ctx, &dev->rgb_stream, pkt, len);
 
234
 
 
235
  if (!got_frame)
 
236
    return;
 
237
 
 
238
  FN_SPEW ("Got RGB frame %d/%d packets arrived, TS %08x\n", dev->rgb_stream.valid_pkts,
 
239
           dev->rgb_stream.pkts_per_frame, dev->rgb_stream.timestamp);
 
240
 
 
241
  freenect_pixel *rgb_frame = NULL;
 
242
  freenect_pixel_ir *ir_frame = NULL;
 
243
 
 
244
  if (dev->rgb_format == FREENECT_FORMAT_BAYER)
 
245
  {
 
246
    if (!dev->rgb_running)
 
247
      return;
 
248
    rgb_frame = dev->rgb_raw;
 
249
  }
 
250
  else if (dev->rgb_format == FREENECT_FORMAT_IR)
 
251
  {
 
252
    if (!dev->ir_running)
 
253
      return;
 
254
    ir_frame = (freenect_pixel_ir *) dev->ir_frame;
 
255
    convert_packed_to_16bit (dev->ir_raw, ir_frame, 10);
 
256
  }
 
257
  else
 
258
  {
 
259
    if (!dev->rgb_running)
 
260
      return;
 
261
    rgb_frame = dev->rgb_frame;
 
262
    /* Pixel arrangement:
 
263
     * G R G R G R G R
 
264
     * B G B G B G B G
 
265
     * G R G R G R G R
 
266
     * B G B G B G B G
 
267
     * G R G R G R G R
 
268
     * B G B G B G B G
 
269
     */
 
270
    for (y = 0; y < 480; y++)
 
271
    {
 
272
      for (x = 0; x < 640; x++)
 
273
      {
 
274
        i = (y * 640 + x);
 
275
        if ((y & 1) == 0)
 
276
        {
 
277
          if ((x & 1) == 0)
 
278
          {
 
279
            // topleft G pixel
 
280
            uint8_t rr = dev->rgb_raw[i + 1];
 
281
            uint8_t rl = x == 0 ? rr : dev->rgb_raw[i - 1];
 
282
            uint8_t bb = dev->rgb_raw[i + 640];
 
283
            uint8_t bt = y == 0 ? bb : dev->rgb_raw[i - 640];
 
284
            rgb_frame[3 * i + 0] = (rl + rr) >> 1;
 
285
            rgb_frame[3 * i + 1] = dev->rgb_raw[i];
 
286
            rgb_frame[3 * i + 2] = (bt + bb) >> 1;
 
287
          }
 
288
          else
 
289
          {
 
290
            // R pixel
 
291
            uint8_t gl = dev->rgb_raw[i - 1];
 
292
            uint8_t gr = x == 639 ? gl : dev->rgb_raw[i + 1];
 
293
            uint8_t gb = dev->rgb_raw[i + 640];
 
294
            uint8_t gt = y == 0 ? gb : dev->rgb_raw[i - 640];
 
295
            uint8_t bbl = dev->rgb_raw[i + 639];
 
296
            uint8_t btl = y == 0 ? bbl : dev->rgb_raw[i - 641];
 
297
            uint8_t bbr = x == 639 ? bbl : dev->rgb_raw[i + 641];
 
298
            uint8_t btr = x == 639 ? btl : y == 0 ? bbr : dev->rgb_raw[i - 639];
 
299
            rgb_frame[3 * i + 0] = dev->rgb_raw[i];
 
300
            rgb_frame[3 * i + 1] = (gl + gr + gb + gt) >> 2;
 
301
            rgb_frame[3 * i + 2] = (bbl + btl + bbr + btr) >> 2;
 
302
          }
 
303
        }
 
304
        else
 
305
        {
 
306
          if ((x & 1) == 0)
 
307
          {
 
308
            // B pixel
 
309
            uint8_t gr = dev->rgb_raw[i + 1];
 
310
            uint8_t gl = x == 0 ? gr : dev->rgb_raw[i - 1];
 
311
            uint8_t gt = dev->rgb_raw[i - 640];
 
312
            uint8_t gb = y == 479 ? gt : dev->rgb_raw[i + 640];
 
313
            uint8_t rtr = dev->rgb_raw[i - 639];
 
314
            uint8_t rbr = y == 479 ? rtr : dev->rgb_raw[i - 641];
 
315
            uint8_t rtl = x == 0 ? rtr : dev->rgb_raw[i - 641];
 
316
            uint8_t rbl = x == 0 ? rbr : y == 479 ? rtl : dev->rgb_raw[i + 639];
 
317
            rgb_frame[3 * i + 0] = (rbl + rtl + rbr + rtr) >> 2;
 
318
            rgb_frame[3 * i + 1] = (gl + gr + gb + gt) >> 2;
 
319
            rgb_frame[3 * i + 2] = dev->rgb_raw[i];
 
320
          }
 
321
          else
 
322
          {
 
323
            // botright G pixel
 
324
            uint8_t bl = dev->rgb_raw[i - 1];
 
325
            uint8_t br = x == 639 ? bl : dev->rgb_raw[i + 1];
 
326
            uint8_t rt = dev->rgb_raw[i - 640];
 
327
            uint8_t rb = y == 479 ? rt : dev->rgb_raw[i + 640];
 
328
            rgb_frame[3 * i + 0] = (rt + rb) >> 1;
 
329
            rgb_frame[3 * i + 1] = dev->rgb_raw[i];
 
330
            rgb_frame[3 * i + 2] = (bl + br) >> 1;
 
331
          }
 
332
        }
 
333
      }
 
334
    }
 
335
  }
 
336
 
 
337
  if (dev->rgb_cb && dev->rgb_running)
 
338
    dev->rgb_cb (dev, rgb_frame, dev->rgb_stream.timestamp);
 
339
  if (dev->ir_cb && dev->ir_running)
 
340
    dev->ir_cb (dev, ir_frame, dev->rgb_stream.timestamp);
 
341
}
 
342
 
 
343
typedef struct
 
344
{
 
345
  uint8_t magic[2];
 
346
  uint16_t len;
 
347
  uint16_t cmd;
 
348
  uint16_t tag;
 
349
} cam_hdr;
 
350
 
 
351
static int
 
352
send_cmd (freenect_device * dev, uint16_t cmd, void *cmdbuf, unsigned int cmd_len, void *replybuf,
 
353
          unsigned int reply_len)
 
354
{
 
355
  freenect_context *ctx = dev->parent;
 
356
  int res, actual_len;
 
357
  uint8_t obuf[0x400];
 
358
  uint8_t ibuf[0x200];
 
359
  cam_hdr *chdr = (void *) obuf;
 
360
  cam_hdr *rhdr = (void *) ibuf;
 
361
 
 
362
  if (cmd_len & 1 || cmd_len > (0x400 - sizeof (*chdr)))
 
363
  {
 
364
    FN_ERROR ("send_cmd: Invalid command length (0x%x)\n", cmd_len);
 
365
    return -1;
 
366
  }
 
367
 
 
368
  chdr->magic[0] = 0x47;
 
369
  chdr->magic[1] = 0x4d;
 
370
  chdr->cmd = cmd;
 
371
  chdr->tag = dev->cam_tag;
 
372
  chdr->len = cmd_len / 2;
 
373
 
 
374
  memcpy (obuf + sizeof (*chdr), cmdbuf, cmd_len);
 
375
 
 
376
  res = fnusb_control (&dev->usb_cam, 0x40, 0, 0, 0, obuf, cmd_len + sizeof (*chdr));
 
377
  FN_SPEW ("Control cmd=%04x tag=%04x len=%04x: %d\n", cmd, dev->cam_tag, cmd_len, res);
 
378
  if (res < 0)
 
379
  {
 
380
    FN_ERROR ("send_cmd: Output control transfer failed (%d)\n", res);
 
381
    return res;
 
382
  }
 
383
 
 
384
  do
 
385
  {
 
386
    actual_len = fnusb_control (&dev->usb_cam, 0xc0, 0, 0, 0, ibuf, 0x200);
 
387
  }
 
388
  while (actual_len == 0);
 
389
  FN_SPEW ("Control reply: %d\n", res);
 
390
  if (actual_len < sizeof (*rhdr))
 
391
  {
 
392
    FN_ERROR ("send_cmd: Input control transfer failed (%d)\n", res);
 
393
    return res;
 
394
  }
 
395
  actual_len -= sizeof (*rhdr);
 
396
 
 
397
  if (rhdr->magic[0] != 0x52 || rhdr->magic[1] != 0x42)
 
398
  {
 
399
    FN_ERROR ("send_cmd: Bad magic %02x %02x\n", rhdr->magic[0], rhdr->magic[1]);
 
400
    return -1;
 
401
  }
 
402
  if (rhdr->cmd != chdr->cmd)
 
403
  {
 
404
    FN_ERROR ("send_cmd: Bad cmd %02x != %02x\n", rhdr->cmd, chdr->cmd);
 
405
    return -1;
 
406
  }
 
407
  if (rhdr->tag != chdr->tag)
 
408
  {
 
409
    FN_ERROR ("send_cmd: Bad tag %04x != %04x\n", rhdr->tag, chdr->tag);
 
410
    return -1;
 
411
  }
 
412
  if (rhdr->len != (actual_len / 2))
 
413
  {
 
414
    FN_ERROR ("send_cmd: Bad len %04x != %04x\n", rhdr->len, (int) (actual_len / 2));
 
415
    return -1;
 
416
  }
 
417
 
 
418
  if (actual_len > reply_len)
 
419
  {
 
420
    FN_WARNING ("send_cmd: Data buffer is %d bytes long, but got %d bytes\n", reply_len, actual_len);
 
421
    memcpy (replybuf, ibuf + sizeof (*rhdr), reply_len);
 
422
  }
 
423
  else
 
424
  {
 
425
    memcpy (replybuf, ibuf + sizeof (*rhdr), actual_len);
 
426
  }
 
427
 
 
428
  dev->cam_tag++;
 
429
 
 
430
  return actual_len;
 
431
}
 
432
 
 
433
static int
 
434
write_register (freenect_device * dev, uint16_t reg, uint16_t data)
 
435
{
 
436
  freenect_context *ctx = dev->parent;
 
437
  uint16_t reply[2];
 
438
  uint16_t cmd[2];
 
439
  int res;
 
440
 
 
441
  cmd[0] = reg;
 
442
  cmd[1] = data;
 
443
 
 
444
  FN_DEBUG ("Write Reg 0x%04x <= 0x%02x\n", reg, data);
 
445
  res = send_cmd (dev, 0x03, cmd, 4, reply, 4);
 
446
  if (res < 0)
 
447
    return res;
 
448
  if (res != 2)
 
449
  {
 
450
    FN_WARNING ("send_cmd returned %d [%04x %04x], 0000 expected\n", res, reply[0], reply[1]);
 
451
  }
 
452
  return 0;
 
453
}
 
454
 
 
455
/** \brief Start the RGB stream acquisition for the given Freenect device. */
 
456
int
 
457
freenect_start_depth (freenect_device * dev)
 
458
{
 
459
  freenect_context *ctx = dev->parent;
 
460
  int res;
 
461
 
 
462
  if (dev->depth_running)
 
463
    return (-1);
 
464
 
 
465
  dev->depth_stream.buf = dev->depth_raw;
 
466
  dev->depth_stream.pkts_per_frame =
 
467
    dev->depth_format == FREENECT_FORMAT_11_BIT ? DEPTH_PKTS_11_BIT_PER_FRAME : DEPTH_PKTS_10_BIT_PER_FRAME;
 
468
  dev->depth_stream.pkt_size = DEPTH_PKTDSIZE;
 
469
  dev->depth_stream.synced = 0;
 
470
  dev->depth_stream.flag = 0x70;
 
471
  dev->depth_stream.valid_frames = 0;
 
472
 
 
473
  res = fnusb_start_iso (&dev->usb_cam, &dev->depth_isoc, depth_process, 0x82, NUM_XFERS, PKTS_PER_XFER, DEPTH_PKTBUF);
 
474
  if (res < 0)
 
475
    return (res);
 
476
 
 
477
  write_register (dev, 0x06, 0x00);     // reset depth stream
 
478
  switch (dev->depth_format)
 
479
  {
 
480
  case FREENECT_FORMAT_11_BIT:
 
481
  case FREENECT_FORMAT_PACKED_11_BIT:
 
482
    write_register (dev, 0x12, 0x03);
 
483
    break;
 
484
  case FREENECT_FORMAT_10_BIT:
 
485
  case FREENECT_FORMAT_PACKED_10_BIT:
 
486
    write_register (dev, 0x12, 0x02);
 
487
    break;
 
488
  default:
 
489
    FN_ERROR ("Invalid depth format %d\n", dev->depth_format);
 
490
    break;
 
491
  }
 
492
  write_register (dev, 0x13, 0x01);
 
493
  write_register (dev, 0x14, 0x1e);
 
494
  write_register (dev, 0x06, 0x02);     // start depth stream
 
495
 
 
496
  dev->depth_running = 1;
 
497
  return (0);
 
498
}
 
499
 
 
500
/** \brief Start the RGB stream acquisition for the given Freenect device. */
 
501
int
 
502
freenect_start_rgb (freenect_device * dev)
 
503
{
 
504
  int res;
 
505
 
 
506
  if (dev->rgb_running)
 
507
    return (-1);
 
508
 
 
509
  if (dev->ir_running)
 
510
  {
 
511
    freenect_context *ctx = dev->parent;
 
512
    FN_WARNING ("Stopping IR stream...");
 
513
    freenect_stop_ir (dev);
 
514
    FN_WARNING ("done\n");
 
515
  }
 
516
 
 
517
  dev->rgb_stream.buf = dev->rgb_raw;
 
518
  dev->rgb_stream.pkts_per_frame = RGB_PKTS_PER_FRAME_RGB;
 
519
  dev->rgb_stream.pkt_size = RGB_PKTDSIZE;
 
520
  dev->rgb_stream.synced = 0;
 
521
  dev->rgb_stream.flag = 0x80;
 
522
  dev->rgb_stream.valid_frames = 0;
 
523
 
 
524
  res = fnusb_start_iso (&dev->usb_cam, &dev->rgb_isoc, rgb_process, 0x81, NUM_XFERS, PKTS_PER_XFER, RGB_PKTBUF);
 
525
  if (res < 0)
 
526
    return (res);
 
527
 
 
528
  write_register (dev, 0x05, 0x00);     // reset rgb stream
 
529
  write_register (dev, 0x0c, 0x00);
 
530
  write_register (dev, 0x0d, 0x01);
 
531
  write_register (dev, 0x0e, 0x1e);     // 30Hz bayer
 
532
  write_register (dev, 0x05, 0x01);     // start rgb stream
 
533
  write_register (dev, 0x47, 0x00);     // disable Hflip
 
534
 
 
535
  dev->rgb_running = 1;
 
536
  return (0);
 
537
}
 
538
 
 
539
/** \brief Start the IR stream acquisition for the given Freenect device. */
 
540
int
 
541
freenect_start_ir (freenect_device * dev)
 
542
{
 
543
  int res;
 
544
 
 
545
  if (dev->ir_running)
 
546
    return (-1);
 
547
 
 
548
  if (dev->rgb_running)
 
549
  {
 
550
    freenect_context *ctx = dev->parent;
 
551
    FN_WARNING ("Stopping RGB stream...");
 
552
    freenect_stop_rgb (dev);
 
553
    FN_WARNING ("done\n");
 
554
  }
 
555
 
 
556
  dev->rgb_stream.buf = dev->ir_raw;
 
557
  dev->rgb_stream.pkts_per_frame = RGB_PKTS_PER_FRAME_IR;
 
558
  dev->rgb_stream.pkt_size = RGB_PKTDSIZE;
 
559
  dev->rgb_stream.synced = 0;
 
560
  dev->rgb_stream.flag = 0x80;
 
561
  dev->rgb_stream.valid_frames = 0;
 
562
 
 
563
  res = fnusb_start_iso (&dev->usb_cam, &dev->rgb_isoc, rgb_process, 0x81, NUM_XFERS, PKTS_PER_XFER, RGB_PKTBUF);
 
564
  if (res < 0)
 
565
    return (res);
 
566
 
 
567
  write_register (dev, 0x05, 0x00);     // reset rgb stream
 
568
  write_register (dev, 0x0c, 0x00);
 
569
  write_register (dev, 0x0d, 0x01);
 
570
  write_register (dev, 0x0e, 0x1e);     // 30Hz bayer
 
571
  write_register (dev, 0x05, 0x03);     // start IR stream
 
572
  write_register (dev, 0x47, 0x00);     // disable Hflip
 
573
 
 
574
  dev->ir_running = 1;
 
575
  return (0);
 
576
}
 
577
 
 
578
/** \brief Stop the depth stream for the given Freenect device. */
 
579
int
 
580
freenect_stop_depth (freenect_device * dev)
 
581
{
 
582
  freenect_context *ctx = dev->parent;
 
583
  int res;
 
584
 
 
585
  if (!dev->depth_running)
 
586
    return (-1);
 
587
 
 
588
  dev->depth_running = 0;
 
589
  write_register (dev, 0x06, 0x00);     // stop depth stream
 
590
 
 
591
  res = fnusb_stop_iso (&dev->usb_cam, &dev->depth_isoc);
 
592
  if (res < 0)
 
593
  {
 
594
    FN_ERROR ("Failed to stop depth isochronous stream: %d\n", res);
 
595
    return (res);
 
596
  }
 
597
 
 
598
  return (0);
 
599
}
 
600
 
 
601
/** \brief Stop the RGB stream for the given Freenect device. */
 
602
int
 
603
freenect_stop_rgb (freenect_device * dev)
 
604
{
 
605
  freenect_context *ctx = dev->parent;
 
606
  int res;
 
607
 
 
608
  if (!dev->rgb_running)
 
609
    return (-1);
 
610
 
 
611
  dev->rgb_running = 0;
 
612
  write_register (dev, 0x05, 0x00);     // stop rgb stream
 
613
 
 
614
  res = fnusb_stop_iso (&dev->usb_cam, &dev->rgb_isoc);
 
615
  if (res < 0)
 
616
  {
 
617
    FN_ERROR ("Failed to stop RGB isochronous stream: %d\n", res);
 
618
    return (res);
 
619
  }
 
620
 
 
621
  return (0);
 
622
}
 
623
 
 
624
/** \brief Stop the IR stream for the given Freenect device. */
 
625
int
 
626
freenect_stop_ir (freenect_device * dev)
 
627
{
 
628
  freenect_context *ctx = dev->parent;
 
629
  int res;
 
630
 
 
631
  if (!dev->ir_running)
 
632
    return (-1);
 
633
 
 
634
  dev->ir_running = 0;
 
635
  write_register (dev, 0x05, 0x00);     // stop IR stream
 
636
 
 
637
  res = fnusb_stop_iso (&dev->usb_cam, &dev->rgb_isoc);
 
638
  if (res < 0)
 
639
  {
 
640
    FN_ERROR ("Failed to stop IR isochronous stream: %d\n", res);
 
641
    return (res);
 
642
  }
 
643
 
 
644
  return (0);
 
645
}
 
646
 
 
647
void
 
648
freenect_set_depth_callback (freenect_device * dev, freenect_depth_cb cb)
 
649
{
 
650
  dev->depth_cb = cb;
 
651
}
 
652
 
 
653
void
 
654
freenect_set_rgb_callback (freenect_device * dev, freenect_rgb_cb cb)
 
655
{
 
656
  dev->rgb_cb = cb;
 
657
}
 
658
 
 
659
void
 
660
freenect_set_ir_callback (freenect_device * dev, freenect_ir_cb cb)
 
661
{
 
662
  dev->ir_cb = cb;
 
663
}
 
664
 
 
665
int
 
666
freenect_set_rgb_format (freenect_device * dev, freenect_rgb_format fmt)
 
667
{
 
668
  dev->rgb_format = fmt;
 
669
  return (0);
 
670
}
 
671
 
 
672
int
 
673
freenect_set_depth_format (freenect_device * dev, freenect_depth_format fmt)
 
674
{
 
675
  dev->depth_format = fmt;
 
676
  return (0);
 
677
}