LIBUSB驱动USB摄像头

#include <stdio.h> #include <stdlib.h> #include <string.h> #include <usb.h> #include "libfreenect.h" #include "cameras.h" // ------- theo code starts ------ // //as per last post on - http://libusb.6.n5.nabble.com/isochronous-transfer-td3250228.html#a3250228 //always have 64 transfers going? #define NUM_XFERS 64 //each transfer is made up of 32 Packets each 1920 #define PKTS_PER_XFER 32 #define PKT_SIZE 1920 #define XFER_SIZE (PKTS_PER_XFER*PKT_SIZE) //ignore these for know #define DEPTH_LEN XFER_SIZE #define RGB_LEN XFER_SIZE static void *rgb_bufs[NUM_XFERS]; static void *depth_bufs[NUM_XFERS]; static depthcb depth_cb; static rgbcb rgb_cb; struct frame_hdr { uint8_t magic[2]; uint8_t pad; uint8_t flag; uint8_t unk1; uint8_t seq; uint8_t unk2; uint8_t unk3; uint32_t timestamp; }; uint8_t depth_buf[2*422400]; uint16_t depth_frame[640*480]; int depth_pos = 0; //why is rgb buf 2*307200 = 2*640*480 ?????? uint8_t rgb_buf[2*307200]; uint8_t rgb_frame[640*480*4]; int rgb_pos = 0; extern const struct caminit inits[]; extern const int num_inits; static void depth_process(uint8_t *buf, size_t len) { int i; if (len == 0) return; struct frame_hdr *hdr = (void*)buf; uint8_t *data = buf + sizeof(*hdr); int datalen = len - sizeof(*hdr); //printf("%02x %x\n", hdr->flag, depth_pos); switch (hdr->flag) { case 0x71: depth_pos = 0; case 0x72: case 0x75: memcpy(&depth_buf[depth_pos], data, datalen); depth_pos += datalen; break; } if (hdr->flag != 0x75) return; printf("GOT DEPTH FRAME, %d bytes\n", depth_pos); int bitshift = 0; for (i=0; i<(640*480); i++) { int idx = (i*11)/8; uint32_t word = (depth_buf[idx]<<16) | (depth_buf[idx+1]<<8) | depth_buf[idx+2]; depth_frame[i] = ((word >> (13-bitshift)) & 0x7ff); bitshift = (bitshift + 11) % 8; } depth_cb(depth_frame, 640, 480); } static void rgb_process(uint8_t *buf, size_t len) { int x,y,i; if (len == 0) return; struct frame_hdr *hdr = (void*)buf; uint8_t *data = buf + sizeof(*hdr); int datalen = len - sizeof(*hdr); //printf("%02x %x\n", hdr->flag, depth_pos); switch (hdr->flag) { case 0x81: rgb_pos = 0; case 0x82: case 0x85: memcpy(&rgb_buf[rgb_pos], data, datalen); rgb_pos += datalen; break; } // safety check? // if( rgb_pos >= 640*480){ // rgb_pos = 0; // } //printf("hdr flag is %02x\n", hdr->flag); if (hdr->flag != 0x85 ) return; printf("GOT RGB FRAME, %d bytes\n", rgb_pos); /* int j = 0; int k = 0; for(k = 0; k < 640*480; k++){ rgb_frame[j] = rgb_buf[k]; rgb_frame[j+1] = rgb_buf[k]; rgb_frame[j+2] = rgb_buf[k]; j+= 3; } */ // horrible bayer to RGB conversion, but does the job for now for (y=0; y<480; y++) { for (x=0; x<640; x++) { i = y*640+x; if (x&1) { if (y&1) { rgb_frame[3*i+1] = rgb_buf[i]; rgb_frame[3*i+4] = rgb_buf[i]; } else { rgb_frame[3*i] = rgb_buf[i]; rgb_frame[3*i+3] = rgb_buf[i]; rgb_frame[3*(i-640)] = rgb_buf[i]; rgb_frame[3*(i-640)+3] = rgb_buf[i]; } } else { if (y&1) { rgb_frame[3*i+2] = rgb_buf[i]; rgb_frame[3*i-1] = rgb_buf[i]; rgb_frame[3*(i+640)+2] = rgb_buf[i]; rgb_frame[3*(i+640)-1] = rgb_buf[i]; } else { rgb_frame[3*i+1] = rgb_buf[i]; rgb_frame[3*i-2] = rgb_buf[i]; } } } } rgb_cb(rgb_frame, 640, 480); } //NOTE - There is something off about this approach //in libusb_ we know the actual length of the packet - with usb we know the length we wanted our packets to be, the transfer size and the num packets //this doesn't allow for the logic u see in the commented out one below. static void rgb_callback(char * buf, int pktLen) { int i; for (i=0; i<PKTS_PER_XFER; i++) { rgb_process(buf, pktLen); buf += pktLen; } } //static void rgb_callback(struct libusb_transfer *xfer) //{ // int i; // if(xfer->status == LIBUSB_TRANSFER_COMPLETED) { // uint8_t *buf = (void*)xfer->buffer; // for (i=0; i<PKTS_PER_XFER; i++) { // rgb_process(buf, xfer->iso_packet_desc[i].actual_length); // buf += RGB_LEN; // } // libusb_submit_transfer(xfer); // } else { // printf("Xfer error: %d\n", xfer->status); // } //} //static void depth_callback(void * buffer, void * context, int pktsPerTransfer, int numBytes ) //{ // int i; // if(xfer->status == LIBUSB_TRANSFER_COMPLETED) { // uint8_t *buf = buffer; // for (i=0; i<PKTS_PER_XFER; i++) { // //printf("DCB %p %d\n", buf, xfer->iso_packet_desc[i].actual_length); // depth_process(buf, numBytes); // buf += DEPTH_LEN; // } // // // // } else { // printf("Xfer error\n"); // } //} static usb_dev_handle* motorHandle = NULL; static usb_dev_handle* cameraHandle = NULL; enum LIBFREENECT_RETURN_CODE InitCameraDevice(){ struct usb_bus *busses; struct usb_bus *bus; int c, i, a; usb_init(); usb_find_busses(); usb_find_devices(); busses = usb_get_busses(); if(NULL != cameraHandle) return FREENECT_DEVICE_ALREADY_OPEN; for (bus = busses; bus; bus = bus->next) { struct usb_device *dev; for (dev = bus->devices; dev; dev = dev->next) { if (dev->descriptor.idProduct == 0x02ae && dev->descriptor.idVendor == 0x045E ) { printf("max size is %i\n", dev->descriptor.bMaxPacketSize0); cameraHandle = usb_open(dev); if(NULL == cameraHandle) return FREENECT_ERROR_DEVICE_OPEN_FAILED; break; } } if(NULL == cameraHandle) break; } if(NULL == cameraHandle) return FREENECT_ERROR_DEVICE_NOT_FOUND; // TODO : do something with the return code if(usb_set_configuration(cameraHandle, 1) < 0) return FREENECT_ERROR_DEVICE_OPEN_FAILED; // TODO : do something with the return code if(usb_claim_interface(cameraHandle, 0) < 0) return FREENECT_ERROR_DEVICE_OPEN_FAILED; //if(usb_set_altinterface(cameraHandle, 0) < 0) return FREENECT_ERROR_DEVICE_OPEN_FAILED; return FREENECT_OK; } struct cam_hdr { uint8_t magic[2]; uint16_t len; uint16_t cmd; uint16_t tag; }; extern const struct caminit inits[]; extern const int num_inits; void startUpCameraDevice(){ uint8_t obuf[0x2000]; uint8_t ibuf[0x2000]; struct cam_hdr *chdr = (void*)obuf; struct cam_hdr *rhdr = (void*)ibuf; printf("INIT CAMERA\n"); int ret = usb_control_msg(cameraHandle, 0x80, 0x06, 0x3ee, 0, ibuf, 0x12, 160); printf("First xfer: %d\n", ret); chdr->magic[0] = 0x47; chdr->magic[1] = 0x4d; int i, j; for (i=0; i<num_inits; i++) { const struct caminit *ip = &inits[i]; chdr->cmd = ip->command; chdr->tag = ip->tag; chdr->len = ip->cmdlen / 2; memcpy(obuf+sizeof(*chdr), ip->cmddata, ip->cmdlen); ret = usb_control_msg(cameraHandle, 0x40, 0, 0, 0, obuf, ip->cmdlen + sizeof(*chdr