Junichi Katsu / Mbed 2 deprecated JBB_BTLE_Test

JBBoardに接続したモーター２つをRCBControllerでコントロールするテストです。

Dependencies:   FatFileSystem TB6612FNG2 mbed

Fork of JBB_BTLE_Test by Jksoft Blue mbed Board Developer

uvc/uvc.cpp@0:1ed23ab1345f, 2012-06-26 (annotated)

Committer:
va009039
Date:
Tue Jun 26 14:27:45 2012 +0000
Revision:
0:1ed23ab1345f
fix overflow spp_service_buffer

Who changed what in which revision?

UserRevisionLine numberNew contents of line
va009039 0:1ed23ab1345f 1 #include "mbed.h"
va009039 0:1ed23ab1345f 2 #include "uvc.h"
va009039 0:1ed23ab1345f 3
va009039 0:1ed23ab1345f 4 //#define __DEBUG
va009039 0:1ed23ab1345f 5 //#define __DEBUG3
va009039 0:1ed23ab1345f 6 #include "mydbg.h"
va009039 0:1ed23ab1345f 7 #include "usb_itd.h"
va009039 0:1ed23ab1345f 8
va009039 0:1ed23ab1345f 9 uvc::uvc(int cam)
va009039 0:1ed23ab1345f 10 {
va009039 0:1ed23ab1345f 11 DBG("cam=%d\n", cam);
va009039 0:1ed23ab1345f 12 DBG_ASSERT(cam >= 0);
va009039 0:1ed23ab1345f 13 m_cam = cam;
va009039 0:1ed23ab1345f 14 m_init = false;
va009039 0:1ed23ab1345f 15 m_connect = false;
va009039 0:1ed23ab1345f 16 m_int_seq = 0;
va009039 0:1ed23ab1345f 17 m_iso_seq = 0;
va009039 0:1ed23ab1345f 18 m_width = 0;
va009039 0:1ed23ab1345f 19 m_height = 0;
va009039 0:1ed23ab1345f 20 m_payload = PAYLOAD_MJPEG;
va009039 0:1ed23ab1345f 21 m_FormatIndex = 0;
va009039 0:1ed23ab1345f 22 m_FrameIndex = 0;
va009039 0:1ed23ab1345f 23 m_FrameInterval = 0;
va009039 0:1ed23ab1345f 24 m_PacketSize = 0;
va009039 0:1ed23ab1345f 25 m_stream = NULL;
va009039 0:1ed23ab1345f 26 for(int i = 0; i <= 15; i++) {
va009039 0:1ed23ab1345f 27 ReportConditionCode[i] = 0;
va009039 0:1ed23ab1345f 28 }
va009039 0:1ed23ab1345f 29 clearOnResult();
va009039 0:1ed23ab1345f 30 }
va009039 0:1ed23ab1345f 31
va009039 0:1ed23ab1345f 32 uvc::~uvc()
va009039 0:1ed23ab1345f 33 {
va009039 0:1ed23ab1345f 34 clearOnResult();
va009039 0:1ed23ab1345f 35 }
va009039 0:1ed23ab1345f 36
va009039 0:1ed23ab1345f 37 int uvc::setup()
va009039 0:1ed23ab1345f 38 {
va009039 0:1ed23ab1345f 39 if (!m_init) {
va009039 0:1ed23ab1345f 40 return _init();
va009039 0:1ed23ab1345f 41 }
va009039 0:1ed23ab1345f 42 return 0;
va009039 0:1ed23ab1345f 43 }
va009039 0:1ed23ab1345f 44
va009039 0:1ed23ab1345f 45 int uvc::get_jpeg(const char* path)
va009039 0:1ed23ab1345f 46 {
va009039 0:1ed23ab1345f 47 const int size = 4800;
va009039 0:1ed23ab1345f 48 const int timeout = 5000;
va009039 0:1ed23ab1345f 49 uint8_t* buf = new uint8_t[size];
va009039 0:1ed23ab1345f 50 DBG_ASSERT(buf);
va009039 0:1ed23ab1345f 51 if (buf == NULL) {
va009039 0:1ed23ab1345f 52 return -1;
va009039 0:1ed23ab1345f 53 }
va009039 0:1ed23ab1345f 54 usb_mjpeg mjpeg(buf, size);
va009039 0:1ed23ab1345f 55 attach(&mjpeg);
va009039 0:1ed23ab1345f 56 Timer t;
va009039 0:1ed23ab1345f 57 t.reset();
va009039 0:1ed23ab1345f 58 t.start();
va009039 0:1ed23ab1345f 59 while(t.read_ms() < timeout) {
va009039 0:1ed23ab1345f 60 int stat = isochronous();
va009039 0:1ed23ab1345f 61 if (mjpeg.status() >= 0) {
va009039 0:1ed23ab1345f 62 break;
va009039 0:1ed23ab1345f 63 }
va009039 0:1ed23ab1345f 64 }
va009039 0:1ed23ab1345f 65 detach();
va009039 0:1ed23ab1345f 66 int len = mjpeg.status();
va009039 0:1ed23ab1345f 67 if (len >= 0) {
va009039 0:1ed23ab1345f 68 if (path != NULL) {
va009039 0:1ed23ab1345f 69 FILE *fp = fopen(path, "wb");
va009039 0:1ed23ab1345f 70 if (fp != NULL) {
va009039 0:1ed23ab1345f 71 for(int i = 0; i < len; i++) {
va009039 0:1ed23ab1345f 72 fputc(buf[i], fp);
va009039 0:1ed23ab1345f 73 }
va009039 0:1ed23ab1345f 74 fclose(fp);
va009039 0:1ed23ab1345f 75 }
va009039 0:1ed23ab1345f 76 }
va009039 0:1ed23ab1345f 77 }
va009039 0:1ed23ab1345f 78 delete[] buf;
va009039 0:1ed23ab1345f 79 return len;
va009039 0:1ed23ab1345f 80 }
va009039 0:1ed23ab1345f 81
va009039 0:1ed23ab1345f 82 int uvc::get_jpeg(uint8_t* buf, int size)
va009039 0:1ed23ab1345f 83 {
va009039 0:1ed23ab1345f 84 //DBG("buf=%p size=%d\n", buf, size);
va009039 0:1ed23ab1345f 85 const int timeout = 5000;
va009039 0:1ed23ab1345f 86 usb_mjpeg mjpeg(buf, size);
va009039 0:1ed23ab1345f 87 attach(&mjpeg);
va009039 0:1ed23ab1345f 88 Timer t;
va009039 0:1ed23ab1345f 89 t.reset();
va009039 0:1ed23ab1345f 90 t.start();
va009039 0:1ed23ab1345f 91 while(t.read_ms() < timeout) {
va009039 0:1ed23ab1345f 92 int stat = isochronous();
va009039 0:1ed23ab1345f 93 if (mjpeg.status() >= 0) {
va009039 0:1ed23ab1345f 94 break;
va009039 0:1ed23ab1345f 95 }
va009039 0:1ed23ab1345f 96 }
va009039 0:1ed23ab1345f 97 detach();
va009039 0:1ed23ab1345f 98 int len = mjpeg.status();
va009039 0:1ed23ab1345f 99 return len;
va009039 0:1ed23ab1345f 100 }
va009039 0:1ed23ab1345f 101
va009039 0:1ed23ab1345f 102 bool uvc::interrupt()
va009039 0:1ed23ab1345f 103 {
va009039 0:1ed23ab1345f 104 if (!m_init) {
va009039 0:1ed23ab1345f 105 _init();
va009039 0:1ed23ab1345f 106 }
va009039 0:1ed23ab1345f 107 if (m_int_seq == 0) {
va009039 0:1ed23ab1345f 108 int rc = m_pEpIntIn->transfer(m_int_buf, sizeof(m_int_buf));
va009039 0:1ed23ab1345f 109 if (rc != USBERR_PROCESSING) {
va009039 0:1ed23ab1345f 110 return false;
va009039 0:1ed23ab1345f 111 }
va009039 0:1ed23ab1345f 112 m_int_seq++;
va009039 0:1ed23ab1345f 113 }
va009039 0:1ed23ab1345f 114 int len = m_pEpIntIn->status();
va009039 0:1ed23ab1345f 115 if (len > 0) {
va009039 0:1ed23ab1345f 116 m_int_seq = 0;
va009039 0:1ed23ab1345f 117 DBG_BYTES("interrupt", m_int_buf, len);
va009039 0:1ed23ab1345f 118 return true;
va009039 0:1ed23ab1345f 119 }
va009039 0:1ed23ab1345f 120 return false;
va009039 0:1ed23ab1345f 121 }
va009039 0:1ed23ab1345f 122
va009039 0:1ed23ab1345f 123 #define CC_NOERROR 0x0
va009039 0:1ed23ab1345f 124 #define CC_DATAOVERRUN 0x8
va009039 0:1ed23ab1345f 125 #define CC_DATAUNDERRUN 0x9
va009039 0:1ed23ab1345f 126
va009039 0:1ed23ab1345f 127 inline void DI()
va009039 0:1ed23ab1345f 128 {
va009039 0:1ed23ab1345f 129 NVIC_DisableIRQ(USB_IRQn);
va009039 0:1ed23ab1345f 130 }
va009039 0:1ed23ab1345f 131
va009039 0:1ed23ab1345f 132 inline void EI()
va009039 0:1ed23ab1345f 133 {
va009039 0:1ed23ab1345f 134 NVIC_EnableIRQ(USB_IRQn);
va009039 0:1ed23ab1345f 135 }
va009039 0:1ed23ab1345f 136
va009039 0:1ed23ab1345f 137 int uvc::isochronous()
va009039 0:1ed23ab1345f 138 {
va009039 0:1ed23ab1345f 139 if (m_iso_seq == 0) {
va009039 0:1ed23ab1345f 140 uint16_t frame = LPC_USB->HcFmNumber;
va009039 0:1ed23ab1345f 141 m_iso_frame = frame + 10; // 10msec
va009039 0:1ed23ab1345f 142 DBG_ASSERT(m_pEpIsoIn->m_itdActive == 0);
va009039 0:1ed23ab1345f 143 m_iso_seq++;
va009039 0:1ed23ab1345f 144 }
va009039 0:1ed23ab1345f 145 if (m_iso_seq == 1) {
va009039 0:1ed23ab1345f 146 DBG_ASSERT(m_itdCount > 0 && m_itdCount <= 8);
va009039 0:1ed23ab1345f 147 while(m_pEpIsoIn->m_itdActive < m_itdCount) {
va009039 0:1ed23ab1345f 148 int len = m_PacketSize * m_FrameCount;
va009039 0:1ed23ab1345f 149 uint8_t* buf = (uint8_t*)usb_get_bp(len);
va009039 0:1ed23ab1345f 150 if (buf == NULL) {
va009039 0:1ed23ab1345f 151 DBG("len=%d\n", len);
va009039 0:1ed23ab1345f 152 DBG("m_itdCount=%d\n", m_itdCount);
va009039 0:1ed23ab1345f 153 }
va009039 0:1ed23ab1345f 154 DBG_ASSERT(buf);
va009039 0:1ed23ab1345f 155 int rc = m_pEpIsoIn->transfer(m_iso_frame, m_FrameCount, buf, len);
va009039 0:1ed23ab1345f 156 m_iso_frame += m_FrameCount;
va009039 0:1ed23ab1345f 157 DBG_ASSERT(rc == USBERR_PROCESSING);
va009039 0:1ed23ab1345f 158 }
va009039 0:1ed23ab1345f 159 m_iso_seq++;
va009039 0:1ed23ab1345f 160 }
va009039 0:1ed23ab1345f 161 if (m_iso_seq == 2) {
va009039 0:1ed23ab1345f 162 //DBG("frame:%04X\n", LPC_USB->HcFmNumber);
va009039 0:1ed23ab1345f 163 while(1) {
va009039 0:1ed23ab1345f 164 DI();
va009039 0:1ed23ab1345f 165 bool empty = m_pEpIsoIn->queue_done_itd.empty();
va009039 0:1ed23ab1345f 166 EI();
va009039 0:1ed23ab1345f 167
va009039 0:1ed23ab1345f 168 if (empty) {
va009039 0:1ed23ab1345f 169 break;
va009039 0:1ed23ab1345f 170 }
va009039 0:1ed23ab1345f 171
va009039 0:1ed23ab1345f 172 DI();
va009039 0:1ed23ab1345f 173 HCITD* itd = m_pEpIsoIn->queue_done_itd.front();
va009039 0:1ed23ab1345f 174 m_pEpIsoIn->queue_done_itd.pop();
va009039 0:1ed23ab1345f 175 EI();
va009039 0:1ed23ab1345f 176
va009039 0:1ed23ab1345f 177 m_pEpIsoIn->m_itdActive--;
va009039 0:1ed23ab1345f 178 usb_itd iso_td(itd);
va009039 0:1ed23ab1345f 179 //DBG("frame:%04X\n", LPC_USB->HcFmNumber);
va009039 0:1ed23ab1345f 180 //DBG("itd->Control=%08X\n", itd->Control);
va009039 0:1ed23ab1345f 181 int cc = iso_td.ConditionCode();
va009039 0:1ed23ab1345f 182 DBG_ASSERT(cc >= 0 && cc <= 15);
va009039 0:1ed23ab1345f 183 ReportConditionCode[cc]++;
va009039 0:1ed23ab1345f 184 if (cc != CC_NOERROR) {
va009039 0:1ed23ab1345f 185 DBG3("%04X ERR:%X\n", LPC_USB->HcFmNumber, cc);
va009039 0:1ed23ab1345f 186 iso_td.free();
va009039 0:1ed23ab1345f 187 m_iso_seq = 3;
va009039 0:1ed23ab1345f 188 return -1;
va009039 0:1ed23ab1345f 189 }
va009039 0:1ed23ab1345f 190 uint16_t frame = iso_td.StartingFrame();
va009039 0:1ed23ab1345f 191 int fc = iso_td.FrameCount();
va009039 0:1ed23ab1345f 192 for(int i = 0; i < fc; i++) {
va009039 0:1ed23ab1345f 193 int len = iso_td.Length(i);
va009039 0:1ed23ab1345f 194 if (len > 0) {
va009039 0:1ed23ab1345f 195 if (m_stream) {
va009039 0:1ed23ab1345f 196 m_stream->input(frame+i, iso_td.BufferPage(i, m_PacketSize), len);
va009039 0:1ed23ab1345f 197 }
va009039 0:1ed23ab1345f 198 onResult(frame+i, iso_td.BufferPage(i, m_PacketSize), len);
va009039 0:1ed23ab1345f 199 }
va009039 0:1ed23ab1345f 200 }
va009039 0:1ed23ab1345f 201 iso_td.free();
va009039 0:1ed23ab1345f 202 }
va009039 0:1ed23ab1345f 203 //DBG("frame:%04X\n", LPC_USB->HcFmNumber);
va009039 0:1ed23ab1345f 204 m_iso_seq = 1;
va009039 0:1ed23ab1345f 205 return m_pEpIsoIn->m_itdActive;
va009039 0:1ed23ab1345f 206 }
va009039 0:1ed23ab1345f 207 if (m_iso_seq == 3) { // cleanup
va009039 0:1ed23ab1345f 208 DBG("m_pEpIsoIn->queue_done_itd.size() :%d\n", m_pEpIsoIn->queue_done_itd.size());
va009039 0:1ed23ab1345f 209 while(1) {
va009039 0:1ed23ab1345f 210 DI();
va009039 0:1ed23ab1345f 211 bool empty = m_pEpIsoIn->queue_done_itd.empty();
va009039 0:1ed23ab1345f 212 EI();
va009039 0:1ed23ab1345f 213
va009039 0:1ed23ab1345f 214 if (empty) {
va009039 0:1ed23ab1345f 215 break;
va009039 0:1ed23ab1345f 216 }
va009039 0:1ed23ab1345f 217
va009039 0:1ed23ab1345f 218 DI();
va009039 0:1ed23ab1345f 219 HCITD* itd = m_pEpIsoIn->queue_done_itd.front();
va009039 0:1ed23ab1345f 220 m_pEpIsoIn->queue_done_itd.pop();
va009039 0:1ed23ab1345f 221 EI();
va009039 0:1ed23ab1345f 222
va009039 0:1ed23ab1345f 223 m_pEpIsoIn->m_itdActive--;
va009039 0:1ed23ab1345f 224 usb_itd iso_td(itd);
va009039 0:1ed23ab1345f 225 iso_td.free();
va009039 0:1ed23ab1345f 226 }
va009039 0:1ed23ab1345f 227 if (m_pEpIsoIn->m_itdActive == 0) {
va009039 0:1ed23ab1345f 228 m_iso_seq = 0;
va009039 0:1ed23ab1345f 229 }
va009039 0:1ed23ab1345f 230 }
va009039 0:1ed23ab1345f 231 return m_pEpIsoIn->m_itdActive;
va009039 0:1ed23ab1345f 232 }
va009039 0:1ed23ab1345f 233
va009039 0:1ed23ab1345f 234 void uvc::attach(usb_stream* stream)
va009039 0:1ed23ab1345f 235 {
va009039 0:1ed23ab1345f 236 m_stream = stream;
va009039 0:1ed23ab1345f 237 }
va009039 0:1ed23ab1345f 238
va009039 0:1ed23ab1345f 239 void uvc::detach()
va009039 0:1ed23ab1345f 240 {
va009039 0:1ed23ab1345f 241 m_stream = NULL;
va009039 0:1ed23ab1345f 242 }
va009039 0:1ed23ab1345f 243
va009039 0:1ed23ab1345f 244 void uvc::onResult(uint16_t frame, uint8_t* buf, int len)
va009039 0:1ed23ab1345f 245 {
va009039 0:1ed23ab1345f 246 if(m_pCbItem && m_pCbMeth)
va009039 0:1ed23ab1345f 247 (m_pCbItem->*m_pCbMeth)(frame, buf, len);
va009039 0:1ed23ab1345f 248 else if(m_pCb)
va009039 0:1ed23ab1345f 249 m_pCb(frame, buf, len);
va009039 0:1ed23ab1345f 250 }
va009039 0:1ed23ab1345f 251
va009039 0:1ed23ab1345f 252 void uvc::setOnResult( void (*pMethod)(uint16_t, uint8_t*, int) )
va009039 0:1ed23ab1345f 253 {
va009039 0:1ed23ab1345f 254 m_pCb = pMethod;
va009039 0:1ed23ab1345f 255 m_pCbItem = NULL;
va009039 0:1ed23ab1345f 256 m_pCbMeth = NULL;
va009039 0:1ed23ab1345f 257 }
va009039 0:1ed23ab1345f 258
va009039 0:1ed23ab1345f 259 void uvc::clearOnResult()
va009039 0:1ed23ab1345f 260 {
va009039 0:1ed23ab1345f 261 m_pCb = NULL;
va009039 0:1ed23ab1345f 262 m_pCbItem = NULL;
va009039 0:1ed23ab1345f 263 m_pCbMeth = NULL;
va009039 0:1ed23ab1345f 264 }