Jordan Brack / SmartWheelsPublic

SmartWheels self-driving race car. Designed for NXP Cup. Uses FRDM-KL25Z, area-scan camera, and simple image processing to detect and navigate any NXP spec track.

Dependencies:   TSI USBDevice mbed-dev

Fork of SmartWheels by haofan Zheng

PCConnector/SWUSBServer.cpp@26:5814404856e2, 2017-02-21 (annotated)

Committer:
hazheng
Date:
Tue Feb 21 20:36:38 2017 +0000
Revision:
26:5814404856e2
Parent:
25:6f63053cee81 
Finished camera initialization, and some test code for camera.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Bobymicjohn 11:676ea42afd56 1 #include "SWUSBServer.h"
Bobymicjohn 11:676ea42afd56 2
Bobymicjohn 11:676ea42afd56 3 #define THREAD_SIGNAL_QUEUE 0xa
Bobymicjohn 11:676ea42afd56 4 #define UNRELIABLE_QUEUE_MAX_SIZE 5
Bobymicjohn 11:676ea42afd56 5
hazheng 19:c93f7fab165d 6 #define RECEIVE_BUF_SIZE 70
hazheng 19:c93f7fab165d 7
Bobymicjohn 11:676ea42afd56 8 #include "USBSerial.h"
Bobymicjohn 11:676ea42afd56 9
hazheng 18:bf6c5f8281eb 10
Bobymicjohn 11:676ea42afd56 11 namespace SW{
Bobymicjohn 11:676ea42afd56 12
Bobymicjohn 11:676ea42afd56 13 USBServer::USBServer(uint16_t vendor_id, uint16_t product_id) :
hazheng 18:bf6c5f8281eb 14 Serial(USBTX, USBRX),
Bobymicjohn 11:676ea42afd56 15 //m_hid(HID_REPORT_LENGTH, HID_REPORT_LENGTH, vendor_id, product_id),
Bobymicjohn 11:676ea42afd56 16 m_shouldTerminate(false),
hazheng 13:7dcb1642ef99 17 m_stat(SER_STAT_STOPPED),
Bobymicjohn 11:676ea42afd56 18 m_usbThread(NULL),
hazheng 19:c93f7fab165d 19 m_recvBuf(new char[RECEIVE_BUF_SIZE]),
hazheng 19:c93f7fab165d 20 m_bufPos(0),
hazheng 19:c93f7fab165d 21 m_hasMsgIn(false),
hazheng 19:c93f7fab165d 22 m_isReading(false)
Bobymicjohn 11:676ea42afd56 23 {
hazheng 18:bf6c5f8281eb 24 this->attach(callback(this, &USBServer::RxCallback), RxIrq);
Bobymicjohn 11:676ea42afd56 25 }
Bobymicjohn 11:676ea42afd56 26
Bobymicjohn 11:676ea42afd56 27 USBServer::~USBServer()
Bobymicjohn 11:676ea42afd56 28 {
hazheng 25:6f63053cee81 29 Serial::~Serial();
Bobymicjohn 11:676ea42afd56 30 Terminate();
Bobymicjohn 11:676ea42afd56 31 }
Bobymicjohn 11:676ea42afd56 32
Bobymicjohn 11:676ea42afd56 33 void USBServer::Terminate()
Bobymicjohn 11:676ea42afd56 34 {
Bobymicjohn 11:676ea42afd56 35 m_shouldTerminate = true;
Bobymicjohn 11:676ea42afd56 36 m_usbThread->signal_set(THREAD_SIGNAL_QUEUE);
Bobymicjohn 11:676ea42afd56 37 m_usbThread->signal_set(0xffff);
Bobymicjohn 11:676ea42afd56 38 m_usbThread->terminate();
hazheng 19:c93f7fab165d 39 this->attach(NULL, RxIrq);
Bobymicjohn 11:676ea42afd56 40 delete m_usbThread;
hazheng 19:c93f7fab165d 41 delete m_recvBuf;
Bobymicjohn 11:676ea42afd56 42 }
Bobymicjohn 11:676ea42afd56 43
hazheng 18:bf6c5f8281eb 44
hazheng 18:bf6c5f8281eb 45
Bobymicjohn 11:676ea42afd56 46 void USBServer::Update(float deltaTime)
Bobymicjohn 11:676ea42afd56 47 {
hazheng 13:7dcb1642ef99 48 if(!m_shouldTerminate && m_stat == SER_STAT_STOPPED)
Bobymicjohn 11:676ea42afd56 49 {
Bobymicjohn 11:676ea42afd56 50 if(m_usbThread)
Bobymicjohn 11:676ea42afd56 51 {
hazheng 18:bf6c5f8281eb 52 m_usbThread->terminate();
Bobymicjohn 11:676ea42afd56 53 delete m_usbThread;
Bobymicjohn 11:676ea42afd56 54 }
Bobymicjohn 11:676ea42afd56 55 m_usbThread = new Thread(callback(this, &USBServer::ConnectingThread));
Bobymicjohn 11:676ea42afd56 56 //m_hidThread.start(callback(this, &USBServer::HIDConnectingThread));
Bobymicjohn 11:676ea42afd56 57 }
Bobymicjohn 11:676ea42afd56 58
hazheng 13:7dcb1642ef99 59 if(!m_shouldTerminate && m_stat == SER_STAT_CONNECTED)
Bobymicjohn 11:676ea42afd56 60 {
hazheng 19:c93f7fab165d 61
Bobymicjohn 11:676ea42afd56 62 if(m_usbThread)
Bobymicjohn 11:676ea42afd56 63 {
hazheng 18:bf6c5f8281eb 64 m_usbThread->terminate();
Bobymicjohn 11:676ea42afd56 65 delete m_usbThread;
Bobymicjohn 11:676ea42afd56 66 }
Bobymicjohn 11:676ea42afd56 67 m_usbThread = new Thread(callback(this, &USBServer::RunningThread));
Bobymicjohn 11:676ea42afd56 68 //m_hidThread.start(callback(this, &USBServer::HIDThread));
Bobymicjohn 11:676ea42afd56 69 }
Bobymicjohn 11:676ea42afd56 70 }
Bobymicjohn 11:676ea42afd56 71
Bobymicjohn 11:676ea42afd56 72 bool USBServer::PushReliableMsg(const char type, const std::string & msg)
Bobymicjohn 11:676ea42afd56 73 {
Bobymicjohn 11:676ea42afd56 74 if(msg.length() <= HID_REPORT_LENGTH)
Bobymicjohn 11:676ea42afd56 75 {
Bobymicjohn 11:676ea42afd56 76 m_qlocker.lock();
Bobymicjohn 11:676ea42afd56 77 m_msgQueue.push_back(type + msg);
hazheng 13:7dcb1642ef99 78 if(m_stat == SER_STAT_RUNNING && m_usbThread)
Bobymicjohn 11:676ea42afd56 79 m_usbThread->signal_set(THREAD_SIGNAL_QUEUE);
Bobymicjohn 11:676ea42afd56 80 m_qlocker.unlock();
Bobymicjohn 11:676ea42afd56 81 return true;
Bobymicjohn 11:676ea42afd56 82 }
Bobymicjohn 11:676ea42afd56 83 else
Bobymicjohn 11:676ea42afd56 84 {
Bobymicjohn 11:676ea42afd56 85 return false;
Bobymicjohn 11:676ea42afd56 86 }
Bobymicjohn 11:676ea42afd56 87 }
Bobymicjohn 11:676ea42afd56 88
Bobymicjohn 11:676ea42afd56 89 bool USBServer::PushUnreliableMsg(const char type, const std::string & msg)
Bobymicjohn 11:676ea42afd56 90 {
hazheng 13:7dcb1642ef99 91 if(m_stat != SER_STAT_RUNNING || m_msgQueue.size() >= UNRELIABLE_QUEUE_MAX_SIZE)
Bobymicjohn 11:676ea42afd56 92 return false;
Bobymicjohn 11:676ea42afd56 93
Bobymicjohn 11:676ea42afd56 94 if(msg.length() <= HID_REPORT_LENGTH)
Bobymicjohn 11:676ea42afd56 95 {
Bobymicjohn 11:676ea42afd56 96 m_qlocker.lock();
Bobymicjohn 11:676ea42afd56 97 m_msgQueue.push_back(type + msg);
hazheng 13:7dcb1642ef99 98 if(m_stat == SER_STAT_RUNNING && m_usbThread)
Bobymicjohn 11:676ea42afd56 99 m_usbThread->signal_set(THREAD_SIGNAL_QUEUE);
Bobymicjohn 11:676ea42afd56 100 m_qlocker.unlock();
Bobymicjohn 11:676ea42afd56 101 return true;
Bobymicjohn 11:676ea42afd56 102 }
Bobymicjohn 11:676ea42afd56 103 else
Bobymicjohn 11:676ea42afd56 104 {
Bobymicjohn 11:676ea42afd56 105 return false;
Bobymicjohn 11:676ea42afd56 106 }
Bobymicjohn 11:676ea42afd56 107 }
Bobymicjohn 11:676ea42afd56 108
Bobymicjohn 11:676ea42afd56 109 void USBServer::RunningThread()
Bobymicjohn 11:676ea42afd56 110 {
hazheng 13:7dcb1642ef99 111 m_stat = SER_STAT_RUNNING;
hazheng 19:c93f7fab165d 112 Timer tickTimer;
hazheng 19:c93f7fab165d 113 tickTimer.start();
Bobymicjohn 11:676ea42afd56 114 while(!m_shouldTerminate)
Bobymicjohn 11:676ea42afd56 115 {
Bobymicjohn 11:676ea42afd56 116 //m_qlocker.lock(); m_uqlocker.lock();
Bobymicjohn 11:676ea42afd56 117 if(m_msgQueue.size() <= 0)
Bobymicjohn 11:676ea42afd56 118 {
Bobymicjohn 11:676ea42afd56 119 //m_qlocker.unlock(); m_uqlocker.unlock();
hazheng 19:c93f7fab165d 120 Thread::signal_wait(THREAD_SIGNAL_QUEUE, 2000);
Bobymicjohn 11:676ea42afd56 121 }
Bobymicjohn 11:676ea42afd56 122 //m_qlocker.unlock(); m_uqlocker.unlock();
Bobymicjohn 11:676ea42afd56 123 std::string msg = "";
Bobymicjohn 11:676ea42afd56 124 if(m_msgQueue.size() > 0)
Bobymicjohn 11:676ea42afd56 125 {
Bobymicjohn 11:676ea42afd56 126 m_qlocker.lock();
Bobymicjohn 11:676ea42afd56 127 msg = m_msgQueue.front();
Bobymicjohn 11:676ea42afd56 128 m_msgQueue.pop_front();
Bobymicjohn 11:676ea42afd56 129 m_qlocker.unlock();
hazheng 19:c93f7fab165d 130
hazheng 19:c93f7fab165d 131 uint16_t size = static_cast<uint16_t>(msg.size());
hazheng 19:c93f7fab165d 132 this->putc('\x02');
hazheng 19:c93f7fab165d 133 this->write(&size, 2);
hazheng 19:c93f7fab165d 134 this->write(&(msg[0]), msg.size());
hazheng 19:c93f7fab165d 135 this->putc('\x03');
hazheng 19:c93f7fab165d 136 //this->printf("\x02%s\x03", msg.c_str());
Bobymicjohn 11:676ea42afd56 137 }
Bobymicjohn 11:676ea42afd56 138
hazheng 19:c93f7fab165d 139
hazheng 19:c93f7fab165d 140
hazheng 19:c93f7fab165d 141 std::string tempStr;
hazheng 19:c93f7fab165d 142 if(m_hasMsgIn)
hazheng 19:c93f7fab165d 143 {
hazheng 19:c93f7fab165d 144 this->attach(NULL, RxIrq);
hazheng 19:c93f7fab165d 145 m_recvBuf[m_bufPos] = '\0';
hazheng 19:c93f7fab165d 146 tempStr = m_recvBuf;
hazheng 19:c93f7fab165d 147 m_bufPos = 0;
hazheng 19:c93f7fab165d 148 m_hasMsgIn = false;
hazheng 19:c93f7fab165d 149 this->attach(callback(this, &USBServer::RxCallback), RxIrq);
hazheng 19:c93f7fab165d 150
hazheng 19:c93f7fab165d 151 if(tempStr.compare(TICK_MSG_PC) == 0)
hazheng 19:c93f7fab165d 152 {
hazheng 26:5814404856e2 153 //PushReliableMsg('D', "Got Tick!");
hazheng 19:c93f7fab165d 154 tickTimer.reset();
hazheng 19:c93f7fab165d 155 }
hazheng 19:c93f7fab165d 156 }
hazheng 19:c93f7fab165d 157
hazheng 19:c93f7fab165d 158 if(tickTimer.read_ms() > 5000)
hazheng 19:c93f7fab165d 159 {
hazheng 19:c93f7fab165d 160 PushReliableMsg('D', "Time out!!");
hazheng 19:c93f7fab165d 161 m_stat = SER_STAT_STOPPED;
hazheng 19:c93f7fab165d 162 return;
hazheng 19:c93f7fab165d 163 }
Bobymicjohn 11:676ea42afd56 164 }
hazheng 16:66c7a09e71ee 165
Bobymicjohn 11:676ea42afd56 166 }
hazheng 18:bf6c5f8281eb 167
hazheng 18:bf6c5f8281eb 168
hazheng 18:bf6c5f8281eb 169 void USBServer::RxCallback()
hazheng 18:bf6c5f8281eb 170 {
hazheng 22:973f53a0e5fd 171 char ch = this->_getc();
hazheng 22:973f53a0e5fd 172 if(m_bufPos >= RECEIVE_BUF_SIZE)
hazheng 22:973f53a0e5fd 173 {
hazheng 22:973f53a0e5fd 174 return;
hazheng 22:973f53a0e5fd 175 }
hazheng 18:bf6c5f8281eb 176
hazheng 22:973f53a0e5fd 177 if(m_hasMsgIn)
hazheng 18:bf6c5f8281eb 178 {
hazheng 22:973f53a0e5fd 179 return;
hazheng 22:973f53a0e5fd 180 }
hazheng 22:973f53a0e5fd 181
hazheng 22:973f53a0e5fd 182 if(!m_isReading)
hazheng 22:973f53a0e5fd 183 {
hazheng 22:973f53a0e5fd 184 //char ch = this->_getc();
hazheng 18:bf6c5f8281eb 185 if(ch == '\x02')
hazheng 18:bf6c5f8281eb 186 {
hazheng 19:c93f7fab165d 187 m_isReading = true;
hazheng 18:bf6c5f8281eb 188 }
hazheng 18:bf6c5f8281eb 189 }
hazheng 22:973f53a0e5fd 190 else
hazheng 18:bf6c5f8281eb 191 {
hazheng 22:973f53a0e5fd 192 //char ch = this->_getc();
hazheng 18:bf6c5f8281eb 193 if(ch == '\x03')
hazheng 18:bf6c5f8281eb 194 {
hazheng 19:c93f7fab165d 195 m_isReading = false;
hazheng 19:c93f7fab165d 196 m_hasMsgIn = true;
hazheng 18:bf6c5f8281eb 197 }
hazheng 18:bf6c5f8281eb 198 else
hazheng 18:bf6c5f8281eb 199 {
hazheng 19:c93f7fab165d 200 m_recvBuf[m_bufPos++] = ch;
hazheng 18:bf6c5f8281eb 201 }
hazheng 18:bf6c5f8281eb 202 }
hazheng 22:973f53a0e5fd 203
hazheng 22:973f53a0e5fd 204 //fflush(_file);
hazheng 18:bf6c5f8281eb 205 }
Bobymicjohn 11:676ea42afd56 206
Bobymicjohn 11:676ea42afd56 207 void USBServer::ConnectingThread()
Bobymicjohn 11:676ea42afd56 208 {
hazheng 13:7dcb1642ef99 209 m_stat = SER_STAT_CONNECTING;
Bobymicjohn 11:676ea42afd56 210
hazheng 17:84acc292b4c2 211 while(m_stat != SER_STAT_CONNECTED)
hazheng 17:84acc292b4c2 212 {
hazheng 17:84acc292b4c2 213
hazheng 19:c93f7fab165d 214 while(!m_hasMsgIn)
hazheng 17:84acc292b4c2 215 {
hazheng 19:c93f7fab165d 216 std::string strBuf = HANDSHAKE_MSG_TER;
hazheng 19:c93f7fab165d 217 uint16_t size = static_cast<uint16_t>(strBuf.size());
hazheng 19:c93f7fab165d 218
hazheng 19:c93f7fab165d 219 this->putc('\x02');
hazheng 19:c93f7fab165d 220 this->write(&size, 2);
hazheng 19:c93f7fab165d 221 this->write(&(strBuf[0]), strBuf.size());
hazheng 19:c93f7fab165d 222 this->putc('\x03');
hazheng 19:c93f7fab165d 223
hazheng 19:c93f7fab165d 224 //this->printf("\x02%s\x03", HANDSHAKE_MSG_TER);
hazheng 17:84acc292b4c2 225 wait(1.0);
hazheng 17:84acc292b4c2 226 }
hazheng 17:84acc292b4c2 227
hazheng 18:bf6c5f8281eb 228 std::string tempStr;
hazheng 19:c93f7fab165d 229 if(m_hasMsgIn)
hazheng 17:84acc292b4c2 230 {
hazheng 18:bf6c5f8281eb 231 this->attach(NULL, RxIrq);
hazheng 19:c93f7fab165d 232 m_recvBuf[m_bufPos] = '\0';
hazheng 19:c93f7fab165d 233 tempStr = m_recvBuf;
hazheng 19:c93f7fab165d 234 m_bufPos = 0;
hazheng 19:c93f7fab165d 235 m_hasMsgIn = false;
hazheng 18:bf6c5f8281eb 236 this->attach(callback(this, &USBServer::RxCallback), RxIrq);
hazheng 17:84acc292b4c2 237 }
hazheng 18:bf6c5f8281eb 238
hazheng 18:bf6c5f8281eb 239 if(tempStr.compare(HANDSHAKE_MSG_PC) == 0)
hazheng 18:bf6c5f8281eb 240 {
hazheng 19:c93f7fab165d 241 PushReliableMsg('D', "Terminal Connected!");
hazheng 18:bf6c5f8281eb 242 m_stat = SER_STAT_CONNECTED;
hazheng 18:bf6c5f8281eb 243 }
hazheng 18:bf6c5f8281eb 244
hazheng 17:84acc292b4c2 245 }
Bobymicjohn 11:676ea42afd56 246
hazheng 18:bf6c5f8281eb 247
Bobymicjohn 11:676ea42afd56 248 }
Bobymicjohn 11:676ea42afd56 249
hazheng 13:7dcb1642ef99 250
hazheng 13:7dcb1642ef99 251 uint8_t USBServer::GetStatus() const
hazheng 13:7dcb1642ef99 252 {
hazheng 13:7dcb1642ef99 253 return m_stat;
hazheng 13:7dcb1642ef99 254 }
hazheng 13:7dcb1642ef99 255
Bobymicjohn 11:676ea42afd56 256 }