Diff: example1_LogitechC270.cpp

Revision:

0:2a9734a95d55

Child:

1:80205a2de336

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/example1_LogitechC270.cpp	Tue Dec 04 13:39:57 2012 +0000
@@ -0,0 +1,153 @@
+#if 1
+//
+// simple color tracking
+//
+#include "mbed.h"
+#include "rtos.h"
+#include "BaseUsbHost.h"
+#define DEBUG
+#include "BaseUsbHostDebug.h"
+#define TEST
+#include "BaseUsbHostTest.h"
+#include "LogitechC270.h"
+#include "BaseJpegDecode.h"
+#include "Terminal.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 BaseJpegDecode {
+public:
+    int y_center, x_center;
+    int m_x_sum, m_y_sum, m_sum;
+    uint32_t EOI_count;
+    int16_t m_buf[WIDTH/16*HEIGHT/8];
+    CalcCenter():EOI_count(0) {
+        memset(m_buf, 0, sizeof(m_buf));
+    }
+    virtual void outputDC(int mcu, int block, int value) {
+        if (mcu >= (WIDTH/16*HEIGHT/8)) {
+            return;
+        }
+        if (block == 3) { // 0-1:Y 2:Cb 3:Cr
+            value *= qt[1][0];
+            m_buf[mcu] = value; // debug
+            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;
+        }
+    };
+};
+
+CalcCenter* calc = NULL;
+void callback_motion_jpeg(uint16_t frame, uint8_t* buf, int len)
+{
+    if (calc) {
+        calc->input(buf+12, len-12);
+    }
+    if (buf[1]&1) { // FID
+        led1 = !led1;
+    }
+}
+
+BaseUsbHost *UsbHost;
+UsbHub* hub;
+ControlEp* ctlEp = NULL;
+LogitechC270* cam;
+
+void display_thread(void const *args) {
+    Timer t;
+    term.cls();
+    int fg, old_fg = 0xffffff;
+    while(1) {
+        int y;
+        for(y = 0; y < HEIGHT/8; y++) {
+            term.locate(0, y);
+            for(int x = 0; x < WIDTH/16; x++) {
+                int value = calc->m_buf[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, y);
+        term.printf("Cr:(%d,%d)[width=%d height=%d yblock=%d %u %u]\n", 
+            calc->x_center, calc->y_center, calc->width, calc->height, calc->yblock, calc->EOI_count, t.read_ms()/1000);
+        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);
+        //Thread::signal_wait(1, 2000);
+        led2 = !led2;
+    }
+}
+
+int main() {
+    term.baud(921600);
+    term.printf("%s\n", __FILE__);
+
+    calc = new CalcCenter;
+    TEST_ASSERT(calc);
+
+    UsbHost = new BaseUsbHost;
+    TEST_ASSERT_TRUE(UsbHost);
+
+    UsbHub* hub = new UsbHub();
+    TEST_ASSERT_TRUE(hub);
+
+    for(int i = 0; i < 4; i++) {
+        if (hub->PortEp[i]) {
+            if (hub->PortEp[i]->DeviceClass == 0x02) {  
+                ctlEp = hub->PortEp[i];
+                break;
+            }
+        }
+    }
+    
+    cam = new LogitechC270(C270_160x120, _15FPS, ctlEp); 
+    TEST_ASSERT_TRUE(cam);
+    cam->setOnResult(callback_motion_jpeg);
+
+    Thread thread(display_thread, NULL, osPriorityBelowNormal);
+ 
+    while(1) {
+        cam->poll();
+    }
+}
+#endif