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@22:973f53a0e5fd, 2017-02-13 (annotated)

Committer:
hazheng
Date:
Mon Feb 13 17:17:50 2017 +0000
Revision:
22:973f53a0e5fd
Parent:
19:c93f7fab165d
Child:
25:6f63053cee81
Improved the receive callbcak method, so it became much more stable.

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