Diff: example2_LogitechC270.cpp

Revision:

2:c10029b87439

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/example2_LogitechC270.cpp	Tue Dec 11 15:28:00 2012 +0000
@@ -0,0 +1,178 @@
+#if 1
+// example2_LogitechC270.cpp
+//
+// simple color tracking
+//
+#include "mbed.h"
+#include "rtos.h"
+#include "BaseUsbHost.h"
+#include "LogitechC270.h"
+#include "UvcCam.h"
+#include "BaseJpegDecode.h"
+#include "Terminal.h"
+#include "MyThread.h"
+
+// Logitech C270
+#define WIDTH  160
+#define HEIGHT 120
+
+#define THRESHOLD 200
+
+DigitalOut led1(LED1), led2(LED2), led3(LED3), led4(LED4);
+Terminal term(USBTX, USBRX);
+
+class CalcCenter : public MyThread, public BaseJpegDecode {
+public:
+    int y_center, x_center;
+    int m_x_sum, m_y_sum, m_sum;
+    uint32_t EOI_count;
+    int16_t buf_Cb[WIDTH/16*HEIGHT/8]; // debug
+    int16_t buf_Cr[WIDTH/16*HEIGHT/8]; // debug
+    CalcCenter(BaseUvc* cam) {
+        m_cam = cam;
+        m_cam->setOnResult(this, &CalcCenter::callback_motion_jpeg);
+        EOI_count = 0;
+    }
+protected:
+    void callback_motion_jpeg(uint16_t frame, uint8_t* buf, int len) {
+        input(buf+12, len-12);
+        if (buf[1]&1) { // FID
+            led1 = !led1;
+        }
+    }
+    
+    virtual void outputDC(int mcu, int block, int value) {
+        if (mcu >= (WIDTH/16*HEIGHT/8)) {
+            return;
+        }
+        if (block == 2) {
+            buf_Cb[mcu] = value * qt[1][0];
+        } else if (block == 3) { // 0-1:Y 2:Cb 3:Cr
+            buf_Cr[mcu] = value * qt[1][0];
+            value *= qt[1][0];
+            if (value >= THRESHOLD) { // red
+                m_x_sum += value*(mcu%(WIDTH/16));
+                m_y_sum += value*(mcu/(WIDTH/16));
+                m_sum += value;
+            }
+        }
+    }
+    virtual void outputAC(int mcu, int block, int scan, int value){};
+    virtual void outputMARK(uint8_t c){
+        if (c == 0xd9) { // EOI
+            if(m_sum == 0) {
+                x_center = y_center = -1; // not found
+            } else {
+                x_center = m_x_sum / m_sum;
+                y_center = m_y_sum / m_sum;
+            }
+            m_x_sum = m_y_sum = m_sum = 0; // reset
+            EOI_count++;
+            led3 = !led3;
+        }
+    }
+
+    virtual void run() {
+        while(true) {
+            if (m_cam) {
+                m_cam->poll();
+            }
+        }
+    }
+    BaseUvc* m_cam;
+};
+
+BaseUvc* cam = NULL;
+CalcCenter* calc = NULL;
+
+#define PI 3.14159265
+
+class display_Thread : public MyThread {
+    virtual void run() {
+        term.cls();
+        int fg, old_fg = 0xffffff;
+        while(1) {
+            int column = 0;
+            for(int y = 0; y < HEIGHT/8; y++) {
+                term.locate(0, column++);
+                for(int x = 0; x < WIDTH/16; x++) {
+                    int value = calc->buf_Cr[y*WIDTH/16+x];
+                    if (value >= THRESHOLD) {
+                        fg = 0xff0000; // red
+                    } else {
+                        fg = 0xffffff; // white
+                    }
+                    if (fg != old_fg) {
+                        term.foreground(fg);
+                        old_fg = fg;
+                    }
+                    term.printf("%+4d,", value);
+                }
+            }
+            term.locate(0, column++);
+            term.printf("Cr:(%d,%d)", calc->x_center, calc->y_center);
+#if 0            
+            for(int y = 0; y < HEIGHT/8; y++) {
+                term.locate(0, column++);
+                for(int x = 0; x < WIDTH/16; x++) {
+                    float Cb = calc->buf_Cb[y*WIDTH/16+x];
+                    float Cr = calc->buf_Cr[y*WIDTH/16+x];
+                    float value;
+                    if (Cb == 0.0) {
+                        value = 0.0;
+                    } else {
+                        value = (atan(Cr / Cb) + PI/2.0)* 360.0 / PI;
+                    }
+                    term.printf("%3.0f,", value);
+                }
+            }
+#endif
+            term.locate(0, column++);
+            term.printf("width=%d height=%d yblock=%d EOI: %u]\n", 
+                         calc->width, calc->height, calc->yblock, calc->EOI_count);
+            term.printf("CC:"); 
+            for(int i = 0; i < 16; i++) {
+                term.printf(" %u", cam->report_cc_count[i]); 
+            }
+            term.printf("]\n"); 
+            term.printf("PS:"); 
+            for(int i = 0; i < 16; i++) {
+                term.printf(" %u", cam->report_ps_cc_count[i]); 
+            }
+            term.printf("]\n"); 
+            Thread::wait(200);
+            led2 = !led2;
+        }
+    }
+};
+
+int main() {
+    term.baud(921600);
+    term.printf("%s\n", __FILE__);
+
+    BaseUsbHost* UsbHost = new BaseUsbHost;
+    UsbHub* hub = new UsbHub();
+
+    for(int port = 1; port <= MAX_HUB_PORT; port++) {
+        ControlEp* ctlEp = hub->GetPortEp(port);
+        if (LogitechC270::check(ctlEp)) {
+            cam = new LogitechC270(C270_160x120, _10FPS, ctlEp); 
+            break;
+        }
+        if (UvcCam::check(ctlEp)) {
+            cam = new UvcCam(UVC_MJPEG, UVC_160x120, _10FPS, ctlEp); 
+            break;
+        }
+    }
+    if (cam == NULL) {
+        error("USB camera not found\n");
+    }
+    calc = new CalcCenter(cam);
+    calc->start();
+
+    display_Thread* th = new display_Thread;
+    th->start(osPriorityBelowNormal);
+
+    Thread::wait(osWaitForever);
+}
+#endif