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@19:c93f7fab165d, 2017-02-12 (annotated)

Committer:
hazheng
Date:
Sun Feb 12 22:29:45 2017 +0000
Revision:
19:c93f7fab165d
Parent:
18:bf6c5f8281eb
Child:
22:973f53a0e5fd
Self-recover connection. But it is still not very 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 19:c93f7fab165d 152 tickTimer.reset();
hazheng 19:c93f7fab165d 153 }
hazheng 19:c93f7fab165d 154 }
hazheng 19:c93f7fab165d 155
hazheng 19:c93f7fab165d 156 if(tickTimer.read_ms() > 5000)
hazheng 19:c93f7fab165d 157 {
hazheng 19:c93f7fab165d 158 PushReliableMsg('D', "Time out!!");
hazheng 19:c93f7fab165d 159 m_stat = SER_STAT_STOPPED;
hazheng 19:c93f7fab165d 160 return;
hazheng 19:c93f7fab165d 161 }
Bobymicjohn 11:676ea42afd56 162 }
hazheng 16:66c7a09e71ee 163
Bobymicjohn 11:676ea42afd56 164 }
hazheng 18:bf6c5f8281eb 165
hazheng 18:bf6c5f8281eb 166
hazheng 18:bf6c5f8281eb 167 void USBServer::RxCallback()
hazheng 18:bf6c5f8281eb 168 {
hazheng 18:bf6c5f8281eb 169
hazheng 19:c93f7fab165d 170 if(!m_isReading && !m_hasMsgIn)
hazheng 18:bf6c5f8281eb 171 {
hazheng 18:bf6c5f8281eb 172 char ch = this->_getc();
hazheng 18:bf6c5f8281eb 173 if(ch == '\x02')
hazheng 18:bf6c5f8281eb 174 {
hazheng 19:c93f7fab165d 175 m_isReading = true;
hazheng 18:bf6c5f8281eb 176 }
hazheng 18:bf6c5f8281eb 177 }
hazheng 18:bf6c5f8281eb 178
hazheng 18:bf6c5f8281eb 179 while(this->readable())
hazheng 18:bf6c5f8281eb 180 {
hazheng 18:bf6c5f8281eb 181 char ch = this->_getc();
hazheng 18:bf6c5f8281eb 182 if(ch == '\x03')
hazheng 18:bf6c5f8281eb 183 {
hazheng 19:c93f7fab165d 184 m_isReading = false;
hazheng 19:c93f7fab165d 185 m_hasMsgIn = true;
hazheng 18:bf6c5f8281eb 186 }
hazheng 18:bf6c5f8281eb 187 else
hazheng 18:bf6c5f8281eb 188 {
hazheng 19:c93f7fab165d 189 m_recvBuf[m_bufPos++] = ch;
hazheng 18:bf6c5f8281eb 190 }
hazheng 18:bf6c5f8281eb 191 }
hazheng 18:bf6c5f8281eb 192 }
Bobymicjohn 11:676ea42afd56 193
Bobymicjohn 11:676ea42afd56 194 void USBServer::ConnectingThread()
Bobymicjohn 11:676ea42afd56 195 {
hazheng 13:7dcb1642ef99 196 m_stat = SER_STAT_CONNECTING;
Bobymicjohn 11:676ea42afd56 197
hazheng 17:84acc292b4c2 198 while(m_stat != SER_STAT_CONNECTED)
hazheng 17:84acc292b4c2 199 {
hazheng 17:84acc292b4c2 200
hazheng 19:c93f7fab165d 201 while(!m_hasMsgIn)
hazheng 17:84acc292b4c2 202 {
hazheng 19:c93f7fab165d 203 std::string strBuf = HANDSHAKE_MSG_TER;
hazheng 19:c93f7fab165d 204 uint16_t size = static_cast<uint16_t>(strBuf.size());
hazheng 19:c93f7fab165d 205
hazheng 19:c93f7fab165d 206 this->putc('\x02');
hazheng 19:c93f7fab165d 207 this->write(&size, 2);
hazheng 19:c93f7fab165d 208 this->write(&(strBuf[0]), strBuf.size());
hazheng 19:c93f7fab165d 209 this->putc('\x03');
hazheng 19:c93f7fab165d 210
hazheng 19:c93f7fab165d 211 //this->printf("\x02%s\x03", HANDSHAKE_MSG_TER);
hazheng 17:84acc292b4c2 212 wait(1.0);
hazheng 17:84acc292b4c2 213 }
hazheng 17:84acc292b4c2 214
hazheng 18:bf6c5f8281eb 215 std::string tempStr;
hazheng 19:c93f7fab165d 216 if(m_hasMsgIn)
hazheng 17:84acc292b4c2 217 {
hazheng 18:bf6c5f8281eb 218 this->attach(NULL, RxIrq);
hazheng 19:c93f7fab165d 219 m_recvBuf[m_bufPos] = '\0';
hazheng 19:c93f7fab165d 220 tempStr = m_recvBuf;
hazheng 19:c93f7fab165d 221 m_bufPos = 0;
hazheng 19:c93f7fab165d 222 m_hasMsgIn = false;
hazheng 18:bf6c5f8281eb 223 this->attach(callback(this, &USBServer::RxCallback), RxIrq);
hazheng 17:84acc292b4c2 224 }
hazheng 18:bf6c5f8281eb 225
hazheng 18:bf6c5f8281eb 226 if(tempStr.compare(HANDSHAKE_MSG_PC) == 0)
hazheng 18:bf6c5f8281eb 227 {
hazheng 19:c93f7fab165d 228 PushReliableMsg('D', "Terminal Connected!");
hazheng 18:bf6c5f8281eb 229 m_stat = SER_STAT_CONNECTED;
hazheng 18:bf6c5f8281eb 230 }
hazheng 18:bf6c5f8281eb 231
hazheng 17:84acc292b4c2 232 }
Bobymicjohn 11:676ea42afd56 233
hazheng 18:bf6c5f8281eb 234
Bobymicjohn 11:676ea42afd56 235 }
Bobymicjohn 11:676ea42afd56 236
hazheng 13:7dcb1642ef99 237
hazheng 13:7dcb1642ef99 238 uint8_t USBServer::GetStatus() const
hazheng 13:7dcb1642ef99 239 {
hazheng 13:7dcb1642ef99 240 return m_stat;
hazheng 13:7dcb1642ef99 241 }
hazheng 13:7dcb1642ef99 242
Bobymicjohn 11:676ea42afd56 243 }