main.cpp

00001 // BaseJpegDecode_example/main.cpp 2013/1/27
00002 // simple color tracking
00003 //
00004 #include "mbed.h"
00005 #include "rtos.h"
00006 #include "BaseUsbHost.h"
00007 #include "UvcCam.h"
00008 #include "BaseJpegDecode.h"
00009 #include "Terminal.h"
00010 #include "MyThread.h"
00011 
00012 #define WIDTH  160
00013 #define HEIGHT 120
00014 
00015 #define THRESHOLD 200
00016 
00017 DigitalOut led1(LED1), led2(LED2), led3(LED3), led4(LED4);
00018 Terminal term(USBTX, USBRX);
00019 
00020 class CalcCenter : public MyThread, public BaseJpegDecode {
00021 public:
00022     int y_center, x_center;
00023     int m_x_sum, m_y_sum, m_sum;
00024     uint32_t EOI_count;
00025     int16_t buf_Cb[WIDTH/16*HEIGHT/8]; // debug
00026     int16_t buf_Cr[WIDTH/16*HEIGHT/8]; // debug
00027     CalcCenter(BaseUvc* cam) {
00028         m_cam = cam;
00029         m_cam->setOnResult(this, &CalcCenter::callback_motion_jpeg);
00030         EOI_count = 0;
00031     }
00032 protected:
00033     void callback_motion_jpeg(uint16_t frame, uint8_t* buf, int len) {
00034         input(buf+12, len-12);
00035         if (buf[1]&1) { // FID
00036             led1 = !led1;
00037         }
00038     }
00039     
00040     virtual void outputDC(int mcu, int block, int value) {
00041         if (mcu >= (WIDTH/16*HEIGHT/8)) {
00042             return;
00043         }
00044         if (block == 2) {
00045             buf_Cb[mcu] = value * qt[1][0];
00046         } else if (block == 3) { // 0-1:Y 2:Cb 3:Cr
00047             buf_Cr[mcu] = value * qt[1][0];
00048             value *= qt[1][0];
00049             if (value >= THRESHOLD) { // red
00050                 m_x_sum += value*(mcu%(WIDTH/16));
00051                 m_y_sum += value*(mcu/(WIDTH/16));
00052                 m_sum += value;
00053             }
00054         }
00055     }
00056     virtual void outputAC(int mcu, int block, int scan, int value){};
00057     virtual void outputMARK(uint8_t c){
00058         if (c == 0xd9) { // EOI
00059             if(m_sum == 0) {
00060                 x_center = y_center = -1; // not found
00061             } else {
00062                 x_center = m_x_sum / m_sum;
00063                 y_center = m_y_sum / m_sum;
00064             }
00065             m_x_sum = m_y_sum = m_sum = 0; // reset
00066             EOI_count++;
00067             led3 = !led3;
00068         }
00069     }
00070 
00071     virtual void run() {
00072         while(true) {
00073             if (m_cam) {
00074                 m_cam->poll();
00075             }
00076         }
00077     }
00078     BaseUvc* m_cam;
00079 };
00080 
00081 BaseUvc* cam = NULL;
00082 CalcCenter* calc = NULL;
00083 
00084 class display_Thread : public MyThread {
00085     virtual void run() {
00086         term.cls();
00087         int fg, old_fg = 0xffffff;
00088         while(1) {
00089             int column = 0;
00090             for(int y = 0; y < HEIGHT/8; y++) {
00091                 term.locate(0, column++);
00092                 for(int x = 0; x < WIDTH/16; x++) {
00093                     int value = calc->buf_Cr[y*WIDTH/16+x];
00094                     if (value >= THRESHOLD) {
00095                         fg = 0xff0000; // red
00096                     } else {
00097                         fg = 0xffffff; // white
00098                     }
00099                     if (fg != old_fg) {
00100                         term.foreground(fg);
00101                         old_fg = fg;
00102                     }
00103                     term.printf("%+4d,", value);
00104                     Thread::yield();
00105                 }
00106             }
00107             term.locate(0, column++);
00108             term.printf("Cr:(%d,%d)", calc->x_center, calc->y_center);
00109 
00110             term.locate(0, column++);
00111             term.printf("width=%d height=%d yblock=%d EOI: %u]\nCC:", 
00112                          calc->width, calc->height, calc->yblock, calc->EOI_count);
00113             for(int i = 0; i < 16; i++) {
00114                 term.printf(" %u", cam->report_cc_count[i]); 
00115             }
00116             term.printf("]\nPS:"); 
00117             for(int i = 0; i < 16; i++) {
00118                 term.printf(" %u", cam->report_ps_cc_count[i]); 
00119             }
00120             term.printf("]\n");
00121             Thread::wait(150);
00122             led2 = !led2;
00123         }
00124     }
00125 };
00126 
00127 void no_memory () {
00128   error("Failed to allocate memory!\n");
00129 }
00130 
00131 int main() {
00132     term.baud(921600);
00133     term.printf("%s\n", __FILE__);
00134     set_new_handler(no_memory);
00135 
00136     BaseUsbHost* UsbHost = new BaseUsbHost;
00137     ControlEp* ctlEp = new ControlEp; // root hub
00138     if (UsbHub::check(ctlEp)) { // hub?
00139         UsbHub* hub = new UsbHub(ctlEp);
00140         ctlEp = hub->search<UvcCam>();
00141     }
00142     if (!UvcCam::check(ctlEp)) {
00143         error("UVC camera is not connected\n");
00144     }
00145     cam = new UvcCam(UVC_MJPEG, UVC_160x120, _15FPS, ctlEp); 
00146     calc = new CalcCenter(cam);
00147     calc->start();
00148 
00149     display_Thread* display_th = new display_Thread;
00150     display_th->start(osPriorityBelowNormal);
00151 
00152     Thread::wait(osWaitForever);
00153 }