#include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "sensor.h" #include "ov9650_regs.h" #define USB_VID (0x0483) /* vendor id */ #define USB_PID (0x5740) /* product id */ #define EP_IN (0x81) /* IN endpoint */ #define EP_OUT (0x01) /* OUT endpoint */ #define TIMEOUT (1000) /* I/O transfer timeout */ #define DEBUG_LEVEL (1) /* libusb debug level */ #define LOCK_SURFACE(surface) \ if (SDL_LockSurface(surface) < 0 ) { \ fprintf(stderr, "failed to lock surface - %s\n", SDL_GetError());\ return -1;\ }\ const char *optstring = "f:r:s:c:p:h"; const char *usage = "usage: openmv \n" \ "-f set pixel format yuv422/rgb565/grayscale\n"\ "-r set frame rate 2/8/15/30/60\n"\ "-s set frame size QQVGA\n"\ "-c send command to camera SNAPSHOT/COLOR_TRACK\n"\ "-p probe register, pass reg addr in hex and use -/+ to change value\n"\ "-h print this message and exit\n"; libusb_device_handle *dev = NULL; SDL_Surface *surface, *screen; int stop = 0; static void sig_hdlr(int signum, siginfo_t *siginfo, void *user) { switch (signum) { case SIGINT: stop =1; } } const char *err_str(int err) { switch (err) { case 0: return "Success"; case LIBUSB_ERROR_TIMEOUT: return "transfer timed out"; case LIBUSB_ERROR_PIPE: return "endpoint halted"; case LIBUSB_ERROR_OVERFLOW: return "overflow"; case LIBUSB_ERROR_NO_DEVICE: return "device has been disconnected"; case LIBUSB_ERROR_NOT_FOUND: return "the requested interface does not exist"; default: return "other"; }; } void cleanup(void) { libusb_close(dev); libusb_exit(NULL); SDL_FreeSurface(screen); SDL_FreeSurface(surface); SDL_Quit(); } int bulk_xfr(int ep, uint8_t *buf, int len) { int ret; if ((ret = libusb_bulk_transfer(dev, ep, buf, len, &len, TIMEOUT)) != 0) { fprintf(stderr, "I/O error: %s offset: %d\n", err_str(ret), len); return ret; } return len; } long get_time_ms() { static struct timeval timeval; gettimeofday(&timeval, NULL); return timeval.tv_sec * 1000 + (long) (timeval.tv_usec * 0.001f); } int write_image(char *path, struct frame_buffer *image) { int i; FILE *fp = fopen(path, "w"); if (fp == NULL) { printf("Unable to open file %s\n", path); return -1; } /* write header */ fprintf(fp, "P5\n%d %d\n%d\n", image->width, image->height, 255); /* write pixels */ for (i = 0; i < (image->width * image->height); i++) { fprintf(fp, "%c", image->pixels[i]); } fclose(fp); return 0; } void send_command(enum sensor_command cmd, int n, ...) { int i; va_list args; uint8_t cmd_buf[64]; /* first byte is the command */ cmd_buf[0] = cmd; /* process var args */ va_start(args, n); for (i=0; i 0) { switch (opt) { case 'c': if (strcmp(optarg, "SNAPSHOT")==0) { sensor_cmd = CMD_SNAPSHOT; } else if (strcmp(optarg, "COLOR_TRACK")==0) { sensor_cmd = CMD_COLOR_TRACK; } else if (strcmp(optarg, "FACE_DETECTION")==0) { sensor_cmd = CMD_FACE_DETECTION; } else { fprintf(stderr, "unsupported command <%s>\n%s\n", optarg, usage); exit(1); } break; case 'f': if (strcmp(optarg, "yuv422")==0) { fb->bpp =2; sensor.pixformat = PIXFORMAT_YUV422; } else if (strcmp(optarg, "rgb565")==0) { fb->bpp =2; sensor.pixformat = PIXFORMAT_RGB565; } else if (strcmp(optarg, "grayscale")==0) { fb->bpp =1; sensor.pixformat = PIXFORMAT_GRAYSCALE; } else { fprintf(stderr, "unsupported pixformat <%s>\n%s\n", optarg, usage); exit(1); } break; case 's': if (strcmp(optarg, "QQCIF")==0) { fb->width = 88; fb->height = 72; sensor.framesize = FRAMESIZE_QQCIF; } else if (strcmp(optarg, "QQVGA")==0) { fb->width = 160; fb->height = 120; sensor.framesize = FRAMESIZE_QQVGA; } else if (strcmp(optarg, "QCIF")==0) { fb->width = 176; fb->height = 144; sensor.framesize = FRAMESIZE_QCIF; } else { fprintf(stderr, "unsupported framesize <%s>\n%s\n", optarg, usage); exit(1); } break; case 'r': switch (atoi(optarg)) { case 2: sensor.framerate=FRAMERATE_2FPS; break; case 8: sensor.framerate=FRAMERATE_8FPS; break; case 15: sensor.framerate=FRAMERATE_15FPS; break; case 30: sensor.framerate=FRAMERATE_30FPS; break; case 60: sensor.framerate=FRAMERATE_60FPS; break; default: fprintf(stderr, "unsupported framerate <%s>\n%s\n", optarg, usage); exit(1); } break; case 'p': enable_probe = 1; reg_addr = (uint8_t) strtol(optarg, NULL, 16); break; case '?': case 'h': fprintf(stderr, "%s\n", usage); exit(1); } } /*init libusb*/ libusb_init(NULL); /*set cleanup to be called at exit*/ atexit(cleanup); /*install signal handlers*/ sigaction(SIGINT, &act, NULL); /*set debugging level*/ libusb_set_debug(NULL, DEBUG_LEVEL); if ((dev = libusb_open_device_with_vid_pid(NULL, USB_VID, USB_PID)) == NULL){ fprintf(stderr, "Device could not be found.\n"); exit(1); } /* reset device */ libusb_reset_device(dev); /* claim interace zero */ if (libusb_claim_interface(dev, 0) != 0) { fprintf(stderr, "Failed to claim interface\n"); exit(1); } Uint32 rmask, gmask, bmask, amask; #if (SDL_BYTEORDER == SDL_BIG_ENDIAN) rmask = 0xFF000000; gmask = 0x00FF0000; bmask = 0x0000FF00; amask = 0x00000000; #else rmask = 0x000000FF; gmask = 0x0000FF00; bmask = 0x00FF0000; amask = 0x00000000; #endif /* Init SDL library */ if (SDL_Init(SDL_INIT_VIDEO)){ fprintf(stderr, "failed to initialize SDL_VIDEO - %s\n", SDL_GetError()); exit(1); } /* Init SDL TTF library */ if(TTF_Init()==-1) { fprintf(stderr, "TTF_Init: %s\n", TTF_GetError()); exit(1); } if ((screen = SDL_SetVideoMode(fb->width, fb->height, 32, SDL_SWSURFACE)) == NULL) { fprintf(stderr, "failed to set video mode - %s\n", SDL_GetError()); exit(1); } if ((surface = SDL_CreateRGBSurface(SDL_SWSURFACE, fb->width, fb->height, 32, rmask, gmask, bmask, amask)) == NULL) { fprintf(stderr, "failed to create rgb surface - %s\n", SDL_GetError()); exit(1); } // load font.ttf at size 16 into font TTF_Font *font; font=TTF_OpenFont("font.ttf", 12); if(!font) { fprintf(stderr, "TTF_OpenFontRW: %s\n", TTF_GetError()); exit(1); } /* reset all registers to their default values */ send_command(CMD_RESET_SENSOR, 0); /* set pixelformat */ send_command(CMD_SET_PIXFORMAT, 1, sensor.pixformat); /* set framesize */ send_command(CMD_SET_FRAMESIZE, 1, sensor.framesize); /* set framerate */ send_command(CMD_SET_FRAMERATE, 1, sensor.framerate); /* set gain ceiling */ send_command(CMD_SET_GAINCEILING, 1, GAINCEILING_8X); /* set brightness */ send_command(CMD_SET_BRIGHTNESS, 1, 3); sleep(1); /* Controlled by AWB */ /*Blue/Red Channels amplifiers*/ // write_reg(REG_RED, 0x80); // write_reg(REG_BLUE, 0x80); SDL_Event event; long t_start, t_elapsed=0, t_total=0, frames=0; /* allocate frame buffer */ fb->pixels = malloc(fb->width*fb->height*fb->bpp); char text_buf[64]; while(1) { t_start = get_time_ms(); /* request frame */ cmd_buf[0] = sensor_cmd; bulk_xfr(EP_OUT, cmd_buf, 1); int ret, len = 0; int frame_size = (fb->width*fb->height*fb->bpp); while (len < frame_size) { ret = bulk_xfr(EP_IN, fb->pixels + len, frame_size-len); if (stop || ret == LIBUSB_ERROR_PIPE) { exit(0); } len += ret; } int i; uint8_t *pixels = surface->pixels; for (i=0; ibpp, pixels+=4) { if (fb->bpp == 1) { /* map grayscale to RGB888 */ pixels[0]=fb->pixels[i]; pixels[1]=fb->pixels[i]; pixels[2]=fb->pixels[i]; } else { uint8_t p0 = fb->pixels[i]; uint8_t p1 = fb->pixels[i+1]; /* map RGB565 to RGB888 */ #if (SDL_BYTEORDER == SDL_BIG_ENDIAN) pixels[0] = (uint8_t) (p1&0x1F) * 255/31; pixels[1] = (uint8_t) (((p0&0x07)<<3) | (p1>>5)) * 255/63; pixels[2] = (uint8_t) (p0>>3) * 255/31; #else pixels[0] = (uint8_t) (p0>>3) * 255/31; pixels[1] = (uint8_t) (((p0&0x07)<<3) | (p1>>5)) * 255/63; pixels[2] = (uint8_t) (p1&0x1F) * 255/31; #endif } pixels[3]=0; } ++frames; t_elapsed = get_time_ms() - t_start; t_total += t_elapsed; if (enable_probe) { char buf[9]={0}; for (i=0; i<8; i++) { buf[i]=(((reg_val<>7)+48; } send_command(CMD_WRITE_REGISTER, 2, reg_addr, reg_val); sprintf(text_buf, "REG 0x%.2X 0x%.2X %s", reg_addr, reg_val, buf); } else { sprintf(text_buf, "FPS %.2f", 1000/(float)(t_total / frames)); } SDL_PollEvent(&event); switch (event.type) { case SDL_QUIT: exit(0); case SDL_KEYDOWN: switch (event.key.keysym.sym) { case SDLK_0: reg_val=0; break; case SDLK_KP_PLUS: reg_val++; break; case SDLK_KP_MINUS: reg_val--; break; case SDLK_LEFT: reg_val = reg_val<<1 | reg_val>>7; break; case SDLK_RIGHT: reg_val = reg_val>>1 | reg_val<<7; break; case SDLK_s: write_image("snapshot.pgm", fb); printf("snapshot saved...\n"); break; case SDLK_ESCAPE: exit(0); default: break; } break; } SDL_Color fg = {255, 0, 0, 0}; SDL_Surface *text = TTF_RenderText_Solid(font, text_buf, fg); SDL_Rect rect = {0, 0, 0, 0}; SDL_BlitSurface(text, NULL, surface, &rect); SDL_BlitSurface(surface, NULL, screen, NULL); SDL_FreeSurface(text); /*flush*/ SDL_UpdateRect(screen, 0, 0, fb->width, fb->height); } return 0; }