Revision 10:387c49b2fc7e, committed 2013-03-17

Comitter:

va009039

Date:

Sun Mar 17 13:22:13 2013 +0000

Parent:

9:fecabade834a

Child:

11:6a8eef89eb22

Commit message:

add readJPEG(), detach does not support.

Changed in this revision

--- a/MyThread/MyThread.h	Sat Mar 16 13:07:55 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,52 +0,0 @@
-// MyThread.h 2012/12/9
-#ifndef MY_THREAD_H
-#define MY_THREAD_H
-
-#define MAGIC_WORD 0xE25A2EA5
-static void thread_handler(void const *argument);
-
-class MyThread {
-public:
-    MyThread() {
-        m_stack_size = DEFAULT_STACK_SIZE;
-        m_stack_pointer = NULL;
-    }
-    void set_stack(uint32_t stack_size=DEFAULT_STACK_SIZE, uint8_t* stack_pointer = NULL) {
-        m_stack_size = stack_size;
-        m_stack_pointer = stack_pointer;
-    }
-    virtual void run() = 0;
-    Thread* start(osPriority priority=osPriorityNormal) {
-        if (m_stack_pointer == NULL) {
-            m_stack_pointer = reinterpret_cast<uint8_t*>(malloc(m_stack_size));
-        }
-        for(int i = 0; i < m_stack_size-64; i += 4) {
-            *reinterpret_cast<uint32_t*>(m_stack_pointer+i) = MAGIC_WORD;
-        }
-        return th = new Thread(thread_handler, this, priority, m_stack_size, m_stack_pointer);
-    }
-
-    int stack_used() {
-        int i;
-        for(i = 0; i < m_stack_size; i += 4) {
-            if(*reinterpret_cast<uint32_t*>(m_stack_pointer+i) != MAGIC_WORD) {
-                break;
-            }
-        }
-        return m_stack_size - i;       
-    }
-
-    int stack_size() { return m_stack_size; }
-protected:
-    Thread* th;
-    uint32_t m_stack_size;
-    uint8_t* m_stack_pointer;
-};
-static void thread_handler(void const *argument) {
-    MyThread* th = (MyThread*)argument;
-    if (th) {
-        th->run();
-    }    
-}
-
-#endif //MY_THREAD_H

--- a/USBHostC270/BaseUvc.cpp	Sat Mar 16 13:07:55 2013 +0000
+++ b/USBHostC270/BaseUvc.cpp	Sun Mar 17 13:22:13 2013 +0000
@@ -1,14 +1,22 @@
+// BaseUvc.cpp
+
 #include "USBHostConf.h"
 #include "USBHost.h"
 
-#include "USBHostC270.h"
 #include "BaseUvc.h"
 
+//#define ISO_DEBUG 1
+#ifdef ISO_DEBUG
+#define ISO_DBG(x, ...) std::printf("[%s:%d]"x"\r\n", __PRETTY_FUNCTION__, __LINE__, ##__VA_ARGS__);
+#else
+#define ISO_DBG(...)  while(0);
+#endif
+
 #define TEST_ASSERT(A) while(!(A)){fprintf(stderr,"\n\n%s@%d %s ASSERT!\n\n",__PRETTY_FUNCTION__,__LINE__,#A);exit(1);};
 
 void BaseUvc::poll(int millisec)
 {
-    HCITD* itd = m_isoEp->isochronousReveive(millisec);
+    HCITD* itd = m_isoEp->isochronousReceive(millisec);
     if (itd) {
         uint8_t cc = itd->ConditionCode();
         report_cc_count[cc]++;
@@ -33,6 +41,24 @@
     }
 }
 
+USB_TYPE BaseUvc::Control(int req, int cs, int index, uint8_t* buf, int size)
+{
+    if (req == SET_CUR) {    
+        return host->controlWrite(dev,
+                    USB_HOST_TO_DEVICE | USB_REQUEST_TYPE_CLASS | USB_RECIPIENT_INTERFACE, 
+                    req, cs<<8, index, buf, size);
+    }
+    return host->controlRead(dev,
+                USB_DEVICE_TO_HOST | USB_REQUEST_TYPE_CLASS | USB_RECIPIENT_INTERFACE, 
+                req, cs<<8, index, buf, size);
+}
+
+USB_TYPE BaseUvc::setInterfaceAlternate(uint8_t intf, uint8_t alt)
+{
+    return host->controlWrite(dev, USB_HOST_TO_DEVICE | USB_RECIPIENT_INTERFACE,
+                                   SET_INTERFACE, alt, intf, NULL, 0);
+}
+
 void BaseUvc::onResult(uint16_t frame, uint8_t* buf, int len)
 {
   if(m_pCbItem && m_pCbMeth)
@@ -78,7 +104,7 @@
 
 IsochronousEp::IsochronousEp(int addr, uint8_t ep, uint16_t size):BaseEp(addr, ep, size)
 {
-    C270_DBG("%p FA:%d EP:%02X MPS:%d\n", this, addr, ep, size);
+    ISO_DBG("%p FA:%d EP:%02X MPS:%d\n", this, addr, ep, size);
     TEST_ASSERT(m_pED);
     
     m_pED->Control |= (1 << 15); // F Format ITD
@@ -101,18 +127,7 @@
     if (hcca == NULL) {
         return;
     }
-    for(int i = 0; i < 32; i++) {
-        if (hcca->InterruptTable[i] == NULL) {
-            hcca->InterruptTable[i] = m_pED;
-        } else {
-            _HCED* nextEd = hcca->InterruptTable[i];
-            while(nextEd->Next && nextEd->Next != m_pED) {
-                nextEd = nextEd->Next;
-            }
-            nextEd->Next = m_pED;
-        }
-    }
-    //DBG_ED(m_pED);
+    hcca->enqueue(m_pED);
 }
 
 void IsochronousEp::reset(int delay_ms)
@@ -130,7 +145,7 @@
     return itd;
 }
 
-HCITD* IsochronousEp::isochronousReveive(int millisec)
+HCITD* IsochronousEp::isochronousReceive(int millisec)
 {
     TEST_ASSERT(m_itd_queue_count >= 0);
     while(m_itd_queue_count < 3 && m_itd_queue_count < HCTD_QUEUE_SIZE) {
@@ -171,7 +186,7 @@
         } else if (evt.status == osEventTimeout) {
             return NULL;
         } else {
-            //DBG("evt.status: %02x\n", evt.status);
+            ISO_DBG("evt.status: %02x\n", evt.status);
             TEST_ASSERT(evt.status == osEventMessage);
             return NULL;
         }
@@ -184,12 +199,50 @@
     LPC_USB->HcControl |= OR_CONTROL_PLE;
 }
 
+void IsochronousEp::disconnect()
+{
+    m_pED->setSkip();
+    ISO_DBG("m_itd_queue_count: %d", m_itd_queue_count);
+    Timer t;
+    t.reset();
+    t.start();
+    while(m_itd_queue_count > 0 && t.read_ms() <= (8*3)) {
+        HCITD* itd = get_queue_HCITD(0);
+        if (itd) {
+            ISO_DBG("ITD: %p", itd);
+            delete itd;
+            m_itd_queue_count--;
+            t.reset();
+        }
+    }
+    ISO_DBG("m_itd_queue_count: %d, t_ms: %d", m_itd_queue_count, t.read_ms());
+    HCITD* head = reinterpret_cast<HCITD*>(reinterpret_cast<uint32_t>(m_pED->HeadTd)&~3); // delete Halted and Toggle Carry bit
+    TEST_ASSERT(head);
+    HCITD* tail = reinterpret_cast<HCITD*>(m_pED->TailTd);
+    TEST_ASSERT(tail);
+    while(head != tail) {
+        HCITD* next = head->Next;
+        TEST_ASSERT(next);
+        ISO_DBG("ED ITD:%p next:%p", head, next);        
+        delete head;
+        TEST_ASSERT(m_itd_queue_count > 0);
+        m_itd_queue_count--;
+        head = next;
+    }
+    TEST_ASSERT(m_itd_queue_count == 0);
+    delete head;
+    
+    _HCCA* hcca = reinterpret_cast<_HCCA*>(LPC_USB->HcHCCA);
+    TEST_ASSERT(hcca);
+    hcca->dequeue(m_pED);
+    delete m_pED;
+}
+
 BaseEp::BaseEp(int addr, uint8_t ep, uint16_t size, int lowSpeed):m_td_queue_count(0)
 {
-    C270_DBG("%p FA=%d EN=%02x MPS=%d S=%d\n", this, addr, ep, size, lowSpeed);
+    ISO_DBG("%p FA=%d EN=%02x MPS=%d S=%d\n", this, addr, ep, size, lowSpeed);
     TEST_ASSERT(size >= 8 && size <= 1023);    
     TEST_ASSERT(lowSpeed == 0 || lowSpeed == 1);
     m_pED  = new _HCED(addr, ep, size, lowSpeed);
     TEST_ASSERT(m_pED);
 }
-

--- a/USBHostC270/BaseUvc.h	Sat Mar 16 13:07:55 2013 +0000
+++ b/USBHostC270/BaseUvc.h	Sun Mar 17 13:22:13 2013 +0000
@@ -1,5 +1,28 @@
+// BaseUvc.h
+
 #pragma once
 
+// --- UVC --------------------------------------------------
+#define _30FPS  333333
+#define _25FPS  400000
+#define _20FPS  500000
+#define _15FPS  666666
+#define _10FPS 1000000
+#define _5FPS  2000000
+#define _1FPS 10000000
+
+#define SET_CUR  0x01
+#define GET_CUR  0x81
+#define GET_MIN  0x82
+#define GET_MAX  0x83
+#define GET_RES  0x84
+#define GET_LEN  0x85
+#define GET_INFO 0x86
+#define GET_DEF  0x87
+
+#define VS_PROBE_CONTROL  0x01
+#define VS_COMMIT_CONTROL 0x02
+
 class BaseEp;
 struct HCITD {    // HostController Isochronous Transfer Descriptor
     __IO uint32_t Control;      // +0 Transfer descriptor control
@@ -80,6 +103,10 @@
         Control &= ~0xffff0000;
         Control |= size<<16;
     }
+
+    inline void setSkip() {
+        Control |= 1<<14;
+    }
 };
 
 struct _HCCA {    // Host Controller Communication Area
@@ -101,6 +128,37 @@
     inline void operator delete(void* p) {
         free(p);
     }
+    
+    inline void enqueue(_HCED* ed) {
+        for(int i = 0; i < 32; i++) {
+            if (InterruptTable[i] == NULL) {
+                InterruptTable[i] = ed;
+            } else {
+                _HCED* nextEd = InterruptTable[i];
+                while(nextEd->Next && nextEd->Next != ed) {
+                    nextEd = nextEd->Next;
+                }
+                nextEd->Next = ed;
+            }
+        }
+    }
+    
+    inline void dequeue(_HCED* ed) {
+         for(int i = 0; i < 32; i++) {
+            if (InterruptTable[i] == ed) {
+                InterruptTable[i] = ed->Next;
+            } else if (InterruptTable[i]) {
+                _HCED* nextEd = InterruptTable[i];
+                while(nextEd) {
+                    if (nextEd->Next == ed) {
+                        nextEd->Next = ed->Next;
+                        break;
+                    }
+                    nextEd = nextEd->Next;
+                }
+            }
+         }
+    }
 };
 
 #define HCTD_QUEUE_SIZE 3
@@ -133,10 +191,11 @@
 public:
     IsochronousEp(int addr, uint8_t ep, uint16_t size);
     void reset(int delay_ms = 100);
-    HCITD* isochronousReveive(int millisec=osWaitForever);
+    HCITD* isochronousReceive(int millisec=osWaitForever);
     int isochronousSend(uint8_t* buf, int len, int millisec=osWaitForever);
     HCITD* get_queue_HCITD(int millisec);
     uint16_t m_PacketSize;
+    void disconnect();
 private:
     HCITD* new_HCITD(BaseEp* obj);
     int m_itd_queue_count;
@@ -148,7 +207,8 @@
 class BaseUvc {
 public:
     void poll(int millisec=osWaitForever);
-    int Control(int req, int cs, int index, uint8_t* buf, int size);
+    USB_TYPE Control(int req, int cs, int index, uint8_t* buf, int size);
+    USB_TYPE setInterfaceAlternate(uint8_t intf, uint8_t alt);
     //ControlEp* m_ctlEp;
     IsochronousEp* m_isoEp;
     uint32_t report_cc_count[16];  // ConditionCode
@@ -168,4 +228,7 @@
     CDummy* m_pCbItem;
     void (CDummy::*m_pCbMeth)(uint16_t, uint8_t*, int);
     void (*m_pCb)(uint16_t, uint8_t*, int);
+protected:
+    USBHost * host;
+    USBDeviceConnected * dev;
 };

--- a/USBHostC270/USBHostC270.cpp	Sat Mar 16 13:07:55 2013 +0000
+++ b/USBHostC270/USBHostC270.cpp	Sun Mar 17 13:22:13 2013 +0000
@@ -1,14 +1,24 @@
 #include "USBHostC270.h"
 #include "dbg.h"
 
+//#define C270_DEBUG 1
+#ifdef C270_DEBUG
+#define C270_DBG(x, ...) std::printf("[%s:%d]"x"\r\n", __PRETTY_FUNCTION__, __LINE__, ##__VA_ARGS__);
+#else
+#define C270_DBG(...)  while(0);
+#endif
+
 // ------------------ HcControl Register ---------------------
 #define  OR_CONTROL_IE                  0x00000008
 
 USBHostC270::USBHostC270(int formatIndex, int frameIndex, uint32_t interval)
 {
+    C270_DBG("formatIndex: %d, frameIndex: %d, interval: %d", formatIndex, frameIndex, interval);
     _formatIndex = formatIndex;
     _frameIndex = frameIndex;
     _interval = interval;
+    m_isoEp = NULL;
+    clearOnResult();
     host = USBHost::getHostInst();
     init();
 }
@@ -21,18 +31,17 @@
     c270_intf = -1;
     c270_device_found = false;
     c270_vid_pid_found = false;
-    clearOnResult();
 }
 
 bool USBHostC270::connected()
 {
-    C270_DBG("");
+    C270_DBG("dev_connected: %d", dev_connected);
     return dev_connected;
 }
 
 bool USBHostC270::connect()
 {
-    C270_DBG("");
+    C270_DBG("dev_connected: %d", dev_connected);
     if (dev_connected) {
         return true;
     }
@@ -48,7 +57,7 @@
             if (c270_device_found) {
                 USB_INFO("New C270 device: VID:%04x PID:%04x [dev: %p - intf: %d]", dev->getVid(), dev->getPid(), dev, c270_intf);
                 dev->setName("C270", c270_intf);
-                host->registerDriver(dev, c270_intf, this, &USBHostC270::init);
+                host->registerDriver(dev, c270_intf, this, &USBHostC270::onDisconnect);
                 int addr = dev->getAddress();
                 m_isoEp = new IsochronousEp(addr, C270_EN, C270_MPS);
                 uint8_t buf[26];
@@ -80,6 +89,16 @@
     return false;
 }
 
+void USBHostC270::onDisconnect()
+{
+    C270_DBG("dev_connected: %d", dev_connected);
+    // TODO
+    if (m_isoEp) {
+       m_isoEp->disconnect();
+    }
+    init();
+}
+
 /*virtual*/ void USBHostC270::setVidPid(uint16_t vid, uint16_t pid)
 {
     C270_DBG("vid:%04x,pid:%04x", vid, pid);
@@ -95,6 +114,7 @@
     C270_DBG("intf_nb=%d,intf_class=%02X,intf_subclass=%d,intf_protocol=%d", intf_nb, intf_class, intf_subclass, intf_protocol);
     if ((c270_intf == -1) && c270_vid_pid_found) {
         c270_intf = intf_nb;
+        c270_vid_pid_found = false;
         c270_device_found = true;
         return true;
     }
@@ -107,23 +127,42 @@
     return false;
 }
 
-USB_TYPE USBHostC270::setInterfaceAlternate(uint8_t intf, uint8_t alt)
-{
-    C270_DBG("intf:%d, alt:%d", intf, alt);
-    return host->controlWrite(dev, USB_HOST_TO_DEVICE | USB_RECIPIENT_INTERFACE,
-                                   SET_INTERFACE, alt, intf, NULL, 0);
+#define SEQ_READ_IDOL 0
+#define SEQ_READ_EXEC 1
+#define SEQ_READ_DONE 2
+
+int USBHostC270::readJPEG(uint8_t* buf, int size, int timeout_ms) {
+    _buf = buf;
+    _pos = 0;
+    _size = size;
+    _seq = SEQ_READ_IDOL;
+    setOnResult(this, &USBHostC270::callback_motion_jpeg);
+    Timer timeout_t;
+    timeout_t.reset();
+    timeout_t.start();
+    while(timeout_t.read_ms() < timeout_ms && _seq != SEQ_READ_DONE) {
+        poll(timeout_ms);
+    } 
+    return _pos;
 }
 
-USB_TYPE USBHostC270::Control(int req, int cs, int index, uint8_t* buf, int size)
-{
-    C270_DBG("req:%d,cs:%d,index:%d", req, cs,index);
-    if (req == SET_CUR) {    
-        return host->controlWrite(dev,
-                    USB_HOST_TO_DEVICE | USB_REQUEST_TYPE_CLASS | USB_RECIPIENT_INTERFACE, 
-                    req, cs<<8, index, buf, size);
+/* virtual */ void USBHostC270::outputJPEG(uint8_t c, int status) { // from decodeMJPEG
+    if (_seq == SEQ_READ_IDOL) {
+        if (status == JPEG_START) {
+            _pos = 0;
+            _seq = SEQ_READ_EXEC;
+        }
     }
-    return host->controlRead(dev,
-                USB_DEVICE_TO_HOST | USB_REQUEST_TYPE_CLASS | USB_RECIPIENT_INTERFACE, 
-                req, cs<<8, index, buf, size);
+    if (_seq == SEQ_READ_EXEC) {
+        if (_pos < _size) {
+            _buf[_pos++] = c;  
+        }  
+        if (status == JPEG_END) {
+            _seq = SEQ_READ_DONE;
+        }
+    }
 }
 
+void USBHostC270::callback_motion_jpeg(uint16_t frame, uint8_t* buf, int len) {
+        inputPacket(buf, len);
+}

--- a/USBHostC270/USBHostC270.h	Sat Mar 16 13:07:55 2013 +0000
+++ b/USBHostC270/USBHostC270.h	Sun Mar 17 13:22:13 2013 +0000
@@ -1,7 +1,9 @@
 #include "USBHostConf.h"
 #include "USBHost.h"
 #include "BaseUvc.h"
+#include "decodeMJPEG.h"
 
+// Logitech C270
 #define C270_VID 0x046d
 #define C270_PID 0x0825
 #define C270_160x120 2
@@ -24,33 +26,6 @@
 #define C270_MPS  192
 #define C270_IF_ALT 1
 
-// --- UVC --------------------------------------------------
-#define _30FPS  333333
-#define _25FPS  400000
-#define _20FPS  500000
-#define _15FPS  666666
-#define _10FPS 1000000
-#define _5FPS  2000000
-#define _1FPS 10000000
-
-#define SET_CUR  0x01
-#define GET_CUR  0x81
-#define GET_MIN  0x82
-#define GET_MAX  0x83
-#define GET_RES  0x84
-#define GET_LEN  0x85
-#define GET_INFO 0x86
-#define GET_DEF  0x87
-
-#define VS_PROBE_CONTROL  0x01
-#define VS_COMMIT_CONTROL 0x02
-
-#define C270_DEBUG 1
-#ifdef C270_DEBUG
-#define C270_DBG(x, ...) std::printf("[%s:%d]"x"\r\n", __PRETTY_FUNCTION__, __LINE__, ##__VA_ARGS__);
-#else
-#define C270_DBG(...)  while(0);
-#endif
 #define TEST_ASSERT(A) while(!(A)){fprintf(stderr,"\n\n%s@%d %s ASSERT!\n\n",__PRETTY_FUNCTION__,__LINE__,#A);exit(1);};
 
 class IsochronousEp;
@@ -58,7 +33,7 @@
 /** 
  * A class to communicate a C270
  */
-class USBHostC270 : public IUSBEnumerator, public BaseUvc {
+class USBHostC270 : public IUSBEnumerator, public BaseUvc, public decodeMJPEG {
 public:
     /**
     * Constructor
@@ -80,6 +55,16 @@
      */
     bool connect();
 
+    /**
+     * read jpeg image
+     *
+     * @param buf read buffer 
+     * @param size buffer size 
+     * @param timeout_ms timeout default 15sec
+     * @return jpeg size if read success else -1
+     */
+    int readJPEG(uint8_t* buf, int size, int timeout_ms = 15*1000);
+
 protected:
     //From IUSBEnumerator
     virtual void setVidPid(uint16_t vid, uint16_t pid);
@@ -87,8 +72,6 @@
     virtual bool useEndpoint(uint8_t intf_nb, ENDPOINT_TYPE type, ENDPOINT_DIRECTION dir); //Must return true if the endpoint will be used
 
 private:
-    USBHost * host;
-    USBDeviceConnected * dev;
     bool dev_connected;
 
     int c270_intf;
@@ -97,9 +80,13 @@
     int _formatIndex;
     int _frameIndex;
     uint32_t _interval;
+    uint8_t _seq;
+    uint8_t* _buf;
+    int _pos;
+    int _size;
 
+    virtual void outputJPEG(uint8_t c, int status); // from decodeMJPEG
+    void callback_motion_jpeg(uint16_t frame, uint8_t* buf, int len);
     void init();
-    USB_TYPE setInterfaceAlternate(uint8_t intf, uint8_t alt);
-    USB_TYPE Control(int req, int cs, int index, uint8_t* buf, int size);
+    void onDisconnect();
 };
-

--- a/main.cpp	Sat Mar 16 13:07:55 2013 +0000
+++ b/main.cpp	Sun Mar 17 13:22:13 2013 +0000
@@ -1,135 +1,44 @@
-// USBHostC270_HelloWorld/main.cpp
-#include "mbed.h"
-#include "USBHostMSD.h"
-#include "USBHostC270.h"
-#include "decodeMJPEG.h"
-#include "MyThread.h"
-
-#define IMAGE_BUF_SIZE (1024*3)
-
-Serial pc(USBTX, USBRX);
-DigitalOut led1(LED1),led2(LED2),led3(LED3);
-
-struct ImageBuffer {
-    int pos;
-    uint8_t buf[IMAGE_BUF_SIZE];
-    void clear() { pos = 0; }
-    int size() { return pos; }
-    uint8_t get(int pos) { return buf[pos]; } 
-    void put(uint8_t c) {
-        if (pos < sizeof(buf)) {
-            buf[pos++] = c;
-        }
-    }
-};
-
-Mail<ImageBuffer, 1> mail_box;
-class captureJPEG : public MyThread, public decodeMJPEG {
-public:
-    captureJPEG(BaseUvc* cam) : m_cam(cam) {
-        m_buf = NULL;
-        m_cam->setOnResult(this, &captureJPEG::callback_motion_jpeg);
-    }
-private:    
-    virtual void outputJPEG(uint8_t c, int status) {
-        if (m_buf == NULL && status == JPEG_START) {
-            m_buf = mail_box.alloc();
-            if (m_buf) {
-                m_buf->clear();
-            }
-        }
-        if (m_buf) {
-            m_buf->put(c);
-            if (status == JPEG_END) {
-                mail_box.put(m_buf);
-                m_buf = NULL;
-                led3 = !led3;
-            }
-        }
-    }
-
-    void callback_motion_jpeg(uint16_t frame, uint8_t* buf, int len) {
-        inputPacket(buf, len);
-        led1 = buf[1]&1;    // FID
-        if (buf[1]&2) {     // EOF
-            led2 = !led2;
-        }
-    }
-
-    virtual void run() {
-        while(true) {
-            if (m_cam) {
-                m_cam->poll();
-            }
-        }
-    }
-    ImageBuffer* m_buf;
-    BaseUvc* m_cam;
-};
-
-int main() {
-    pc.baud(921600);
-    printf("%s\n", __FILE__);
-
-    USBHostMSD* msd = new USBHostMSD("usb");
-    while(!msd->connect()) {
-        Thread::wait(200);
-    }
-    
-
-    USBHostC270* cam = new USBHostC270(C270_MJPEG, C270_160x120, _5FPS);
-    while(!cam->connect()) {
-        Thread::wait(200);
-    }
-
-    captureJPEG* capture = new captureJPEG(cam);
-    capture->set_stack(512);
-    capture->start();
-
-    Timer t;
-    t.reset();
-    t.start();
-    Timer interval_t;
-    interval_t.reset();
-    interval_t.start();
-    int shot = 0;
-    while(1) {
-        osEvent evt = mail_box.get();
-        if (evt.status == osEventMail) {
-            ImageBuffer *buf = reinterpret_cast<ImageBuffer*>(evt.value.p);
-            if (interval_t.read() > 10) {
-                char path[32];
-                snprintf(path, sizeof(path), "/usb/image%02d.jpg", shot % 100);
-                printf("%d %s %d bytes\n", shot, path, buf->size());
-                if (msd->connected()) {
-                    FILE* fp = fopen(path, "wb");
-                    if (fp) {
-                        for(int i = 0; i < buf->size(); i++) {
-                            fputc(buf->get(i), fp);
-                        }
-                        fclose(fp);
-                    }
-                }
-                interval_t.reset();
-                shot++;
-            }
-            mail_box.free(buf);
-        }
-        if (t.read() > 5) {
-            printf("captureJPEG stack used: %d/%d bytes\n", capture->stack_used(), capture->stack_size());
-            printf("CC:");
-            for(int i = 0; i < 16; i++) {
-                printf(" %u", cam->report_cc_count[i]); 
-            }
-            printf("\nPS:"); 
-            for(int i = 0; i < 16; i++) {
-                printf(" %u", cam->report_ps_cc_count[i]); 
-            }
-            printf("\n");
-            t.reset();
-        }
-        if (!msd->connected()) {
-            msd->connect();
-        }
-    }
-}
+#include "USBHostMSD.h"
+#include "USBHostC270.h"
+
+Serial pc(USBTX, USBRX);
+BusOut leds(LED1, LED2, LED3);
+
+int main() {
+    pc.baud(921600);
+
+    USBHostMSD* msd = new USBHostMSD("usb"); // USB flash drive
+
+    USBHostC270* cam = new USBHostC270(C270_MJPEG, C270_160x120, _5FPS); // Logitech C270
+    while(!cam->connect()) {
+        Thread::wait(500);
+    }
+
+    uint8_t buf[1024*3];
+    Timer interval_t;
+    interval_t.reset();
+    interval_t.start();
+    int shot = 0;
+    while(1) {
+        if (interval_t.read() > 10) {
+            int r = cam->readJPEG(buf, sizeof(buf));
+            char path[32];
+            snprintf(path, sizeof(path), "/usb/image%02d.jpg", shot % 20);
+            printf("%d %s %d bytes\n", shot, path, r);
+            if (msd->connected()) {
+                FILE* fp = fopen(path, "wb");
+                if (fp) {
+                    fwrite(buf, r, 1, fp);
+                    fclose(fp);
+                }
+                shot++;
+                leds = shot % 8;
+            }
+            interval_t.reset();
+        }
+        if (!msd->connected()) {
+            msd->connect();
+        }
+        cam->poll();    
+    }
+}