freenect: cameras.c Source File

00001 /*
00002  * This file is part of the OpenKinect Project. http://www.openkinect.org
00003  *
00004  * Copyright (c) 2010 individual OpenKinect contributors. See the CONTRIB file
00005  * for details.
00006  *
00007  * This code is licensed to you under the terms of the Apache License, version
00008  * 2.0, or, at your option, the terms of the GNU General Public License,
00009  * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses,
00010  * or the following URLs:
00011  * http://www.apache.org/licenses/LICENSE-2.0
00012  * http://www.gnu.org/licenses/gpl-2.0.txt
00013  *
00014  * If you redistribute this file in source form, modified or unmodified, you
00015  * may:
00016  *   1) Leave this header intact and distribute it under the same terms,
00017  *      accompanying it with the APACHE20 and GPL20 files, or
00018  *   2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or
00019  *   3) Delete the GPL v2 clause and accompany it with the APACHE20 file
00020  * In all cases you must keep the copyright notice intact and include a copy
00021  * of the CONTRIB file.
00022  *
00023  * Binary distributions must follow the binary distribution requirements of
00024  * either License.
00025  */
00026 
00027 #include <stdio.h>
00028 #include <stdlib.h>
00029 #include <string.h>
00030 #include <unistd.h>
00031 
00032 #include "freenect_internal.h"
00033 
00034 struct pkt_hdr
00035 {
00036   uint8_t magic[2];
00037   uint8_t pad;
00038   uint8_t flag;
00039   uint8_t unk1;
00040   uint8_t seq;
00041   uint8_t unk2;
00042   uint8_t unk3;
00043   uint32_t timestamp;
00044 };
00045 
00046 extern const struct caminit inits[];
00047 extern const int num_inits;
00048 
00049 static int
00050 stream_process (freenect_context * ctx, packet_stream * strm, uint8_t * pkt, int len)
00051 {
00052   if (len < 12)
00053     return 0;
00054 
00055   struct pkt_hdr *hdr = (void *) pkt;
00056   uint8_t *data = pkt + sizeof (*hdr);
00057   int datalen = len - sizeof (*hdr);
00058 
00059   if (hdr->magic[0] != 'R' || hdr->magic[1] != 'B')
00060   {
00061     FN_LOG (strm->valid_frames < 2 ? LL_SPEW : LL_NOTICE,
00062             "[Stream %02x] Invalid magic %02x%02x\n", strm->flag, hdr->magic[0], hdr->magic[1]);
00063     return 0;
00064   }
00065 
00066   FN_FLOOD ("[Stream %02x] Packet with flag: %02x\n", strm->flag, hdr->flag);
00067 
00068   uint8_t sof = strm->flag | 1;
00069   uint8_t mof = strm->flag | 2;
00070   uint8_t eof = strm->flag | 5;
00071 
00072   // sync if required, dropping packets until SOF
00073   if (!strm->synced)
00074   {
00075     if (hdr->flag != sof)
00076     {
00077       FN_SPEW ("[Stream %02x] Not synced yet...\n", strm->flag);
00078       return 0;
00079     }
00080     strm->synced = 1;
00081     strm->seq = hdr->seq;
00082     strm->pkt_num = 0;
00083     strm->valid_pkts = 0;
00084     strm->got_pkts = 0;
00085   }
00086 
00087   int got_frame = 0;
00088 
00089   // handle lost packets
00090   if (strm->seq != hdr->seq)
00091   {
00092     uint8_t lost = strm->seq - hdr->seq;
00093     FN_LOG (strm->valid_frames < 2 ? LL_SPEW : LL_INFO, "[Stream %02x] Lost %d packets\n", strm->flag, lost);
00094     if (lost > 5)
00095     {
00096       FN_LOG (strm->valid_frames < 2 ? LL_SPEW : LL_NOTICE,
00097               "[Stream %02x] Lost too many packets, resyncing...\n", strm->flag);
00098       strm->synced = 0;
00099       return 0;
00100     }
00101     strm->seq = hdr->seq;
00102     int left = strm->pkts_per_frame - strm->pkt_num;
00103     if (left <= lost)
00104     {
00105       strm->pkt_num = lost - left;
00106       strm->valid_pkts = strm->got_pkts;
00107       strm->got_pkts = 0;
00108       got_frame = 1;
00109       strm->timestamp = strm->last_timestamp;
00110       strm->valid_frames++;
00111     }
00112     else
00113     {
00114       strm->pkt_num += lost;
00115     }
00116   }
00117 
00118   // check the header to make sure it's what we expect
00119   if (!(strm->pkt_num == 0 && hdr->flag == sof) &&
00120       !(strm->pkt_num == strm->pkts_per_frame - 1 && hdr->flag == eof) &&
00121       !(strm->pkt_num > 0 && strm->pkt_num < strm->pkts_per_frame - 1 && hdr->flag == mof))
00122   {
00123     FN_LOG (strm->valid_frames < 2 ? LL_SPEW : LL_NOTICE,
00124             "[Stream %02x] Inconsistent flag %02x with %d packets in buf (%d total), resyncing...\n",
00125             strm->flag, hdr->flag, strm->pkt_num, strm->pkts_per_frame);
00126     strm->synced = 0;
00127     return got_frame;
00128   }
00129 
00130   // copy data
00131   if (datalen > strm->pkt_size)
00132   {
00133     FN_LOG (strm->valid_frames < 2 ? LL_SPEW : LL_WARNING,
00134             "[Stream %02x] Expected %d data bytes, but got %d. Dropping...\n", strm->flag, strm->pkt_size, datalen);
00135     return got_frame;
00136   }
00137 
00138   if (datalen != strm->pkt_size && hdr->flag != eof)
00139     FN_LOG (strm->valid_frames < 2 ? LL_SPEW : LL_WARNING,
00140             "[Stream %02x] Expected %d data bytes, but got only %d\n", strm->flag, strm->pkt_size, datalen);
00141 
00142   uint8_t *dbuf = strm->buf + strm->pkt_num * strm->pkt_size;
00143   memcpy (dbuf, data, datalen);
00144 
00145   strm->pkt_num++;
00146   strm->seq++;
00147   strm->got_pkts++;
00148 
00149   strm->last_timestamp = hdr->timestamp;
00150 
00151   if (strm->pkt_num == strm->pkts_per_frame)
00152   {
00153     strm->pkt_num = 0;
00154     strm->valid_pkts = strm->got_pkts;
00155     strm->got_pkts = 0;
00156     strm->timestamp = hdr->timestamp;
00157     strm->valid_frames++;
00158     return 1;
00159   }
00160   else
00161   {
00162     return got_frame;
00163   }
00164 }
00165 
00166 // Unpack buffer of (vw bit) data into padded 16bit buffer.
00167 static inline void
00168 convert_packed_to_16bit (uint8_t * raw, uint16_t * frame, int vw)
00169 {
00170   int mask = (1 << vw) - 1;
00171   int i;
00172   int bitshift = 0;
00173   for (i = 0; i < (640 * 480); i++)
00174   {
00175     int idx = (i * vw) / 8;
00176     uint32_t word = (raw[idx] << (16)) | (raw[idx + 1] << 8) | raw[idx + 2];
00177     frame[i] = ((word >> (((3 * 8) - vw) - bitshift)) & mask);
00178     bitshift = (bitshift + vw) % 8;
00179   }
00180 }
00181 
00182 static void
00183 depth_process (freenect_device * dev, uint8_t * pkt, int len)
00184 {
00185   freenect_context *ctx = dev->parent;
00186 
00187   if (len == 0)
00188     return;
00189 
00190   if (!dev->depth_running)
00191     return;
00192 
00193   int got_frame = stream_process (ctx, &dev->depth_stream, pkt, len);
00194 
00195   if (!got_frame)
00196     return;
00197 
00198   FN_SPEW ("Got depth frame %d/%d packets arrived, TS %08x\n",
00199            dev->depth_stream.valid_pkts, dev->depth_stream.pkts_per_frame, dev->depth_stream.timestamp);
00200 
00201   switch (dev->depth_format)
00202   {
00203   case FREENECT_FORMAT_11_BIT:
00204     convert_packed_to_16bit (dev->depth_raw, dev->depth_frame, 11);
00205     if (dev->depth_cb)
00206       dev->depth_cb (dev, dev->depth_frame, dev->depth_stream.timestamp);
00207     break;
00208   case FREENECT_FORMAT_10_BIT:
00209     convert_packed_to_16bit (dev->depth_raw, dev->depth_frame, 10);
00210     if (dev->depth_cb)
00211       dev->depth_cb (dev, dev->depth_frame, dev->depth_stream.timestamp);
00212     break;
00213   case FREENECT_FORMAT_PACKED_10_BIT:
00214   case FREENECT_FORMAT_PACKED_11_BIT:
00215     if (dev->depth_cb)
00216       dev->depth_cb (dev, dev->depth_raw, dev->depth_stream.timestamp);
00217     break;
00218   }
00219 }
00220 
00221 static void
00222 rgb_process (freenect_device * dev, uint8_t * pkt, int len)
00223 {
00224   freenect_context *ctx = dev->parent;
00225   int x, y, i;
00226 
00227   if (len == 0)
00228     return;
00229 
00230   if (!dev->rgb_running && !dev->ir_running)
00231     return;
00232 
00233   int got_frame = stream_process (ctx, &dev->rgb_stream, pkt, len);
00234 
00235   if (!got_frame)
00236     return;
00237 
00238   FN_SPEW ("Got RGB frame %d/%d packets arrived, TS %08x\n", dev->rgb_stream.valid_pkts,
00239            dev->rgb_stream.pkts_per_frame, dev->rgb_stream.timestamp);
00240 
00241   freenect_pixel *rgb_frame = NULL;
00242   freenect_pixel_ir *ir_frame = NULL;
00243 
00244   if (dev->rgb_format == FREENECT_FORMAT_BAYER)
00245   {
00246     if (!dev->rgb_running)
00247       return;
00248     rgb_frame = dev->rgb_raw;
00249   }
00250   else if (dev->rgb_format == FREENECT_FORMAT_IR)
00251   {
00252     if (!dev->ir_running)
00253       return;
00254     ir_frame = (freenect_pixel_ir *) dev->ir_frame;
00255     convert_packed_to_16bit (dev->ir_raw, ir_frame, 10);
00256   }
00257   else
00258   {
00259     if (!dev->rgb_running)
00260       return;
00261     rgb_frame = dev->rgb_frame;
00262     /* Pixel arrangement:
00263      * G R G R G R G R
00264      * B G B G B G B G
00265      * G R G R G R G R
00266      * B G B G B G B G
00267      * G R G R G R G R
00268      * B G B G B G B G
00269      */
00270     for (y = 0; y < 480; y++)
00271     {
00272       for (x = 0; x < 640; x++)
00273       {
00274         i = (y * 640 + x);
00275         if ((y & 1) == 0)
00276         {
00277           if ((x & 1) == 0)
00278           {
00279             // topleft G pixel
00280             uint8_t rr = dev->rgb_raw[i + 1];
00281             uint8_t rl = x == 0 ? rr : dev->rgb_raw[i - 1];
00282             uint8_t bb = dev->rgb_raw[i + 640];
00283             uint8_t bt = y == 0 ? bb : dev->rgb_raw[i - 640];
00284             rgb_frame[3 * i + 0] = (rl + rr) >> 1;
00285             rgb_frame[3 * i + 1] = dev->rgb_raw[i];
00286             rgb_frame[3 * i + 2] = (bt + bb) >> 1;
00287           }
00288           else
00289           {
00290             // R pixel
00291             uint8_t gl = dev->rgb_raw[i - 1];
00292             uint8_t gr = x == 639 ? gl : dev->rgb_raw[i + 1];
00293             uint8_t gb = dev->rgb_raw[i + 640];
00294             uint8_t gt = y == 0 ? gb : dev->rgb_raw[i - 640];
00295             uint8_t bbl = dev->rgb_raw[i + 639];
00296             uint8_t btl = y == 0 ? bbl : dev->rgb_raw[i - 641];
00297             uint8_t bbr = x == 639 ? bbl : dev->rgb_raw[i + 641];
00298             uint8_t btr = x == 639 ? btl : y == 0 ? bbr : dev->rgb_raw[i - 639];
00299             rgb_frame[3 * i + 0] = dev->rgb_raw[i];
00300             rgb_frame[3 * i + 1] = (gl + gr + gb + gt) >> 2;
00301             rgb_frame[3 * i + 2] = (bbl + btl + bbr + btr) >> 2;
00302           }
00303         }
00304         else
00305         {
00306           if ((x & 1) == 0)
00307           {
00308             // B pixel
00309             uint8_t gr = dev->rgb_raw[i + 1];
00310             uint8_t gl = x == 0 ? gr : dev->rgb_raw[i - 1];
00311             uint8_t gt = dev->rgb_raw[i - 640];
00312             uint8_t gb = y == 479 ? gt : dev->rgb_raw[i + 640];
00313             uint8_t rtr = dev->rgb_raw[i - 639];
00314             uint8_t rbr = y == 479 ? rtr : dev->rgb_raw[i - 641];
00315             uint8_t rtl = x == 0 ? rtr : dev->rgb_raw[i - 641];
00316             uint8_t rbl = x == 0 ? rbr : y == 479 ? rtl : dev->rgb_raw[i + 639];
00317             rgb_frame[3 * i + 0] = (rbl + rtl + rbr + rtr) >> 2;
00318             rgb_frame[3 * i + 1] = (gl + gr + gb + gt) >> 2;
00319             rgb_frame[3 * i + 2] = dev->rgb_raw[i];
00320           }
00321           else
00322           {
00323             // botright G pixel
00324             uint8_t bl = dev->rgb_raw[i - 1];
00325             uint8_t br = x == 639 ? bl : dev->rgb_raw[i + 1];
00326             uint8_t rt = dev->rgb_raw[i - 640];
00327             uint8_t rb = y == 479 ? rt : dev->rgb_raw[i + 640];
00328             rgb_frame[3 * i + 0] = (rt + rb) >> 1;
00329             rgb_frame[3 * i + 1] = dev->rgb_raw[i];
00330             rgb_frame[3 * i + 2] = (bl + br) >> 1;
00331           }
00332         }
00333       }
00334     }
00335   }
00336 
00337   if (dev->rgb_cb && dev->rgb_running)
00338     dev->rgb_cb (dev, rgb_frame, dev->rgb_stream.timestamp);
00339   if (dev->ir_cb && dev->ir_running)
00340     dev->ir_cb (dev, ir_frame, dev->rgb_stream.timestamp);
00341 }
00342 
00343 typedef struct
00344 {
00345   uint8_t magic[2];
00346   uint16_t len;
00347   uint16_t cmd;
00348   uint16_t tag;
00349 } cam_hdr;
00350 
00351 static int
00352 send_cmd (freenect_device * dev, uint16_t cmd, void *cmdbuf, unsigned int cmd_len, void *replybuf,
00353           unsigned int reply_len)
00354 {
00355   freenect_context *ctx = dev->parent;
00356   int res, actual_len;
00357   uint8_t obuf[0x400];
00358   uint8_t ibuf[0x200];
00359   cam_hdr *chdr = (void *) obuf;
00360   cam_hdr *rhdr = (void *) ibuf;
00361 
00362   if (cmd_len & 1 || cmd_len > (0x400 - sizeof (*chdr)))
00363   {
00364     FN_ERROR ("send_cmd: Invalid command length (0x%x)\n", cmd_len);
00365     return -1;
00366   }
00367 
00368   chdr->magic[0] = 0x47;
00369   chdr->magic[1] = 0x4d;
00370   chdr->cmd = cmd;
00371   chdr->tag = dev->cam_tag;
00372   chdr->len = cmd_len / 2;
00373 
00374   memcpy (obuf + sizeof (*chdr), cmdbuf, cmd_len);
00375 
00376   res = fnusb_control (&dev->usb_cam, 0x40, 0, 0, 0, obuf, cmd_len + sizeof (*chdr));
00377   FN_SPEW ("Control cmd=%04x tag=%04x len=%04x: %d\n", cmd, dev->cam_tag, cmd_len, res);
00378   if (res < 0)
00379   {
00380     FN_ERROR ("send_cmd: Output control transfer failed (%d)\n", res);
00381     return res;
00382   }
00383 
00384   do
00385   {
00386     actual_len = fnusb_control (&dev->usb_cam, 0xc0, 0, 0, 0, ibuf, 0x200);
00387   }
00388   while (actual_len == 0);
00389   FN_SPEW ("Control reply: %d\n", res);
00390   if (actual_len < sizeof (*rhdr))
00391   {
00392     FN_ERROR ("send_cmd: Input control transfer failed (%d)\n", res);
00393     return res;
00394   }
00395   actual_len -= sizeof (*rhdr);
00396 
00397   if (rhdr->magic[0] != 0x52 || rhdr->magic[1] != 0x42)
00398   {
00399     FN_ERROR ("send_cmd: Bad magic %02x %02x\n", rhdr->magic[0], rhdr->magic[1]);
00400     return -1;
00401   }
00402   if (rhdr->cmd != chdr->cmd)
00403   {
00404     FN_ERROR ("send_cmd: Bad cmd %02x != %02x\n", rhdr->cmd, chdr->cmd);
00405     return -1;
00406   }
00407   if (rhdr->tag != chdr->tag)
00408   {
00409     FN_ERROR ("send_cmd: Bad tag %04x != %04x\n", rhdr->tag, chdr->tag);
00410     return -1;
00411   }
00412   if (rhdr->len != (actual_len / 2))
00413   {
00414     FN_ERROR ("send_cmd: Bad len %04x != %04x\n", rhdr->len, (int) (actual_len / 2));
00415     return -1;
00416   }
00417 
00418   if (actual_len > reply_len)
00419   {
00420     FN_WARNING ("send_cmd: Data buffer is %d bytes long, but got %d bytes\n", reply_len, actual_len);
00421     memcpy (replybuf, ibuf + sizeof (*rhdr), reply_len);
00422   }
00423   else
00424   {
00425     memcpy (replybuf, ibuf + sizeof (*rhdr), actual_len);
00426   }
00427 
00428   dev->cam_tag++;
00429 
00430   return actual_len;
00431 }
00432 
00433 static int
00434 write_register (freenect_device * dev, uint16_t reg, uint16_t data)
00435 {
00436   freenect_context *ctx = dev->parent;
00437   uint16_t reply[2];
00438   uint16_t cmd[2];
00439   int res;
00440 
00441   cmd[0] = reg;
00442   cmd[1] = data;
00443 
00444   FN_DEBUG ("Write Reg 0x%04x <= 0x%02x\n", reg, data);
00445   res = send_cmd (dev, 0x03, cmd, 4, reply, 4);
00446   if (res < 0)
00447     return res;
00448   if (res != 2)
00449   {
00450     FN_WARNING ("send_cmd returned %d [%04x %04x], 0000 expected\n", res, reply[0], reply[1]);
00451   }
00452   return 0;
00453 }
00454 
00456 int
00457 freenect_start_depth (freenect_device * dev)
00458 {
00459   freenect_context *ctx = dev->parent;
00460   int res;
00461 
00462   if (dev->depth_running)
00463     return (-1);
00464 
00465   dev->depth_stream.buf = dev->depth_raw;
00466   dev->depth_stream.pkts_per_frame =
00467     dev->depth_format == FREENECT_FORMAT_11_BIT ? DEPTH_PKTS_11_BIT_PER_FRAME : DEPTH_PKTS_10_BIT_PER_FRAME;
00468   dev->depth_stream.pkt_size = DEPTH_PKTDSIZE;
00469   dev->depth_stream.synced = 0;
00470   dev->depth_stream.flag = 0x70;
00471   dev->depth_stream.valid_frames = 0;
00472 
00473   res = fnusb_start_iso (&dev->usb_cam, &dev->depth_isoc, depth_process, 0x82, NUM_XFERS, PKTS_PER_XFER, DEPTH_PKTBUF);
00474   if (res < 0)
00475     return (res);
00476 
00477   write_register (dev, 0x06, 0x00);     // reset depth stream
00478   switch (dev->depth_format)
00479   {
00480   case FREENECT_FORMAT_11_BIT:
00481   case FREENECT_FORMAT_PACKED_11_BIT:
00482     write_register (dev, 0x12, 0x03);
00483     break;
00484   case FREENECT_FORMAT_10_BIT:
00485   case FREENECT_FORMAT_PACKED_10_BIT:
00486     write_register (dev, 0x12, 0x02);
00487     break;
00488   default:
00489     FN_ERROR ("Invalid depth format %d\n", dev->depth_format);
00490     break;
00491   }
00492   write_register (dev, 0x13, 0x01);
00493   write_register (dev, 0x14, 0x1e);
00494   write_register (dev, 0x06, 0x02);     // start depth stream
00495 
00496   dev->depth_running = 1;
00497   return (0);
00498 }
00499 
00501 int
00502 freenect_start_rgb (freenect_device * dev)
00503 {
00504   int res;
00505 
00506   if (dev->rgb_running)
00507     return (-1);
00508 
00509   if (dev->ir_running)
00510   {
00511     freenect_context *ctx = dev->parent;
00512     FN_WARNING ("Stopping IR stream...");
00513     freenect_stop_ir (dev);
00514     FN_WARNING ("done\n");
00515   }
00516 
00517   dev->rgb_stream.buf = dev->rgb_raw;
00518   dev->rgb_stream.pkts_per_frame = RGB_PKTS_PER_FRAME_RGB;
00519   dev->rgb_stream.pkt_size = RGB_PKTDSIZE;
00520   dev->rgb_stream.synced = 0;
00521   dev->rgb_stream.flag = 0x80;
00522   dev->rgb_stream.valid_frames = 0;
00523 
00524   res = fnusb_start_iso (&dev->usb_cam, &dev->rgb_isoc, rgb_process, 0x81, NUM_XFERS, PKTS_PER_XFER, RGB_PKTBUF);
00525   if (res < 0)
00526     return (res);
00527 
00528   write_register (dev, 0x05, 0x00);     // reset rgb stream
00529   write_register (dev, 0x0c, 0x00);
00530   write_register (dev, 0x0d, 0x01);
00531   write_register (dev, 0x0e, 0x1e);     // 30Hz bayer
00532   write_register (dev, 0x05, 0x01);     // start rgb stream
00533   write_register (dev, 0x47, 0x00);     // disable Hflip
00534 
00535   dev->rgb_running = 1;
00536   return (0);
00537 }
00538 
00540 int
00541 freenect_start_ir (freenect_device * dev)
00542 {
00543   int res;
00544 
00545   if (dev->ir_running)
00546     return (-1);
00547 
00548   if (dev->rgb_running)
00549   {
00550     freenect_context *ctx = dev->parent;
00551     FN_WARNING ("Stopping RGB stream...");
00552     freenect_stop_rgb (dev);
00553     FN_WARNING ("done\n");
00554   }
00555 
00556   dev->rgb_stream.buf = dev->ir_raw;
00557   dev->rgb_stream.pkts_per_frame = RGB_PKTS_PER_FRAME_IR;
00558   dev->rgb_stream.pkt_size = RGB_PKTDSIZE;
00559   dev->rgb_stream.synced = 0;
00560   dev->rgb_stream.flag = 0x80;
00561   dev->rgb_stream.valid_frames = 0;
00562 
00563   res = fnusb_start_iso (&dev->usb_cam, &dev->rgb_isoc, rgb_process, 0x81, NUM_XFERS, PKTS_PER_XFER, RGB_PKTBUF);
00564   if (res < 0)
00565     return (res);
00566 
00567   write_register (dev, 0x05, 0x00);     // reset rgb stream
00568   write_register (dev, 0x0c, 0x00);
00569   write_register (dev, 0x0d, 0x01);
00570   write_register (dev, 0x0e, 0x1e);     // 30Hz bayer
00571   write_register (dev, 0x05, 0x03);     // start IR stream
00572   write_register (dev, 0x47, 0x00);     // disable Hflip
00573 
00574   dev->ir_running = 1;
00575   return (0);
00576 }
00577 
00579 int
00580 freenect_stop_depth (freenect_device * dev)
00581 {
00582   freenect_context *ctx = dev->parent;
00583   int res;
00584 
00585   if (!dev->depth_running)
00586     return (-1);
00587 
00588   dev->depth_running = 0;
00589   write_register (dev, 0x06, 0x00);     // stop depth stream
00590 
00591   res = fnusb_stop_iso (&dev->usb_cam, &dev->depth_isoc);
00592   if (res < 0)
00593   {
00594     FN_ERROR ("Failed to stop depth isochronous stream: %d\n", res);
00595     return (res);
00596   }
00597 
00598   return (0);
00599 }
00600 
00602 int
00603 freenect_stop_rgb (freenect_device * dev)
00604 {
00605   freenect_context *ctx = dev->parent;
00606   int res;
00607 
00608   if (!dev->rgb_running)
00609     return (-1);
00610 
00611   dev->rgb_running = 0;
00612   write_register (dev, 0x05, 0x00);     // stop rgb stream
00613 
00614   res = fnusb_stop_iso (&dev->usb_cam, &dev->rgb_isoc);
00615   if (res < 0)
00616   {
00617     FN_ERROR ("Failed to stop RGB isochronous stream: %d\n", res);
00618     return (res);
00619   }
00620 
00621   return (0);
00622 }
00623 
00625 int
00626 freenect_stop_ir (freenect_device * dev)
00627 {
00628   freenect_context *ctx = dev->parent;
00629   int res;
00630 
00631   if (!dev->ir_running)
00632     return (-1);
00633 
00634   dev->ir_running = 0;
00635   write_register (dev, 0x05, 0x00);     // stop IR stream
00636 
00637   res = fnusb_stop_iso (&dev->usb_cam, &dev->rgb_isoc);
00638   if (res < 0)
00639   {
00640     FN_ERROR ("Failed to stop IR isochronous stream: %d\n", res);
00641     return (res);
00642   }
00643 
00644   return (0);
00645 }
00646 
00647 void
00648 freenect_set_depth_callback (freenect_device * dev, freenect_depth_cb cb)
00649 {
00650   dev->depth_cb = cb;
00651 }
00652 
00653 void
00654 freenect_set_rgb_callback (freenect_device * dev, freenect_rgb_cb cb)
00655 {
00656   dev->rgb_cb = cb;
00657 }
00658 
00659 void
00660 freenect_set_ir_callback (freenect_device * dev, freenect_ir_cb cb)
00661 {
00662   dev->ir_cb = cb;
00663 }
00664 
00665 int
00666 freenect_set_rgb_format (freenect_device * dev, freenect_rgb_format fmt)
00667 {
00668   dev->rgb_format = fmt;
00669   return (0);
00670 }
00671 
00672 int
00673 freenect_set_depth_format (freenect_device * dev, freenect_depth_format fmt)
00674 {
00675   dev->depth_format = fmt;
00676   return (0);
00677 }