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@16:66c7a09e71ee, 2017-02-10 (annotated)

Committer:
hazheng
Date:
Fri Feb 10 03:23:20 2017 +0000
Revision:
16:66c7a09e71ee
Parent:
13:7dcb1642ef99
Child:
17:84acc292b4c2
Added delta time.

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
Bobymicjohn 11:676ea42afd56 6 #include "USBSerial.h"
Bobymicjohn 11:676ea42afd56 7
Bobymicjohn 11:676ea42afd56 8 namespace SW{
Bobymicjohn 11:676ea42afd56 9
Bobymicjohn 11:676ea42afd56 10 USBServer::USBServer(uint16_t vendor_id, uint16_t product_id) :
Bobymicjohn 11:676ea42afd56 11 //m_hid(HID_REPORT_LENGTH, HID_REPORT_LENGTH, vendor_id, product_id),
Bobymicjohn 11:676ea42afd56 12 m_shouldTerminate(false),
hazheng 13:7dcb1642ef99 13 m_stat(SER_STAT_STOPPED),
Bobymicjohn 11:676ea42afd56 14 m_usbThread(NULL),
hazheng 16:66c7a09e71ee 15 m_usb(NULL),
hazheng 16:66c7a09e71ee 16 m_tickMsgTimer(0.0f)
Bobymicjohn 11:676ea42afd56 17 {
Bobymicjohn 11:676ea42afd56 18
Bobymicjohn 11:676ea42afd56 19 }
Bobymicjohn 11:676ea42afd56 20
Bobymicjohn 11:676ea42afd56 21 USBServer::~USBServer()
Bobymicjohn 11:676ea42afd56 22 {
Bobymicjohn 11:676ea42afd56 23 Terminate();
Bobymicjohn 11:676ea42afd56 24 }
Bobymicjohn 11:676ea42afd56 25
Bobymicjohn 11:676ea42afd56 26 void USBServer::Terminate()
Bobymicjohn 11:676ea42afd56 27 {
Bobymicjohn 11:676ea42afd56 28 m_shouldTerminate = true;
Bobymicjohn 11:676ea42afd56 29 m_usbThread->signal_set(THREAD_SIGNAL_QUEUE);
Bobymicjohn 11:676ea42afd56 30 m_usbThread->signal_set(0xffff);
Bobymicjohn 11:676ea42afd56 31 //m_usbThread->join();
Bobymicjohn 11:676ea42afd56 32 m_usbThread->terminate();
Bobymicjohn 11:676ea42afd56 33 delete m_usbThread;
Bobymicjohn 11:676ea42afd56 34 delete m_usb;
Bobymicjohn 11:676ea42afd56 35 }
Bobymicjohn 11:676ea42afd56 36
Bobymicjohn 11:676ea42afd56 37 void USBServer::Update(float deltaTime)
Bobymicjohn 11:676ea42afd56 38 {
hazheng 16:66c7a09e71ee 39 m_tickMsgTimer += deltaTime;
hazheng 13:7dcb1642ef99 40 if(!m_shouldTerminate && m_stat == SER_STAT_STOPPED)
Bobymicjohn 11:676ea42afd56 41 {
Bobymicjohn 11:676ea42afd56 42 if(m_usbThread)
Bobymicjohn 11:676ea42afd56 43 {
Bobymicjohn 11:676ea42afd56 44 delete m_usbThread;
Bobymicjohn 11:676ea42afd56 45 }
Bobymicjohn 11:676ea42afd56 46 m_usbThread = new Thread(callback(this, &USBServer::ConnectingThread));
Bobymicjohn 11:676ea42afd56 47 //m_hidThread.start(callback(this, &USBServer::HIDConnectingThread));
Bobymicjohn 11:676ea42afd56 48 }
Bobymicjohn 11:676ea42afd56 49
hazheng 13:7dcb1642ef99 50 if(!m_shouldTerminate && m_stat == SER_STAT_CONNECTED)
Bobymicjohn 11:676ea42afd56 51 {
Bobymicjohn 11:676ea42afd56 52 if(m_usbThread)
Bobymicjohn 11:676ea42afd56 53 {
Bobymicjohn 11:676ea42afd56 54 delete m_usbThread;
Bobymicjohn 11:676ea42afd56 55 }
Bobymicjohn 11:676ea42afd56 56 m_usbThread = new Thread(callback(this, &USBServer::RunningThread));
Bobymicjohn 11:676ea42afd56 57 //m_hidThread.start(callback(this, &USBServer::HIDThread));
Bobymicjohn 11:676ea42afd56 58 }
hazheng 16:66c7a09e71ee 59
hazheng 16:66c7a09e71ee 60 if(m_tickMsgTimer >= 2.0f)
hazheng 16:66c7a09e71ee 61 {
hazheng 16:66c7a09e71ee 62 PushReliableMsg('T', TICKING_MSG);
hazheng 16:66c7a09e71ee 63 m_tickMsgTimer = 0.0f;
hazheng 16:66c7a09e71ee 64 }
Bobymicjohn 11:676ea42afd56 65 }
Bobymicjohn 11:676ea42afd56 66
Bobymicjohn 11:676ea42afd56 67 bool USBServer::PushReliableMsg(const char type, const std::string & msg)
Bobymicjohn 11:676ea42afd56 68 {
Bobymicjohn 11:676ea42afd56 69 if(msg.length() <= HID_REPORT_LENGTH)
Bobymicjohn 11:676ea42afd56 70 {
Bobymicjohn 11:676ea42afd56 71 m_qlocker.lock();
Bobymicjohn 11:676ea42afd56 72 m_msgQueue.push_back(type + msg);
hazheng 13:7dcb1642ef99 73 if(m_stat == SER_STAT_RUNNING && m_usbThread)
Bobymicjohn 11:676ea42afd56 74 m_usbThread->signal_set(THREAD_SIGNAL_QUEUE);
Bobymicjohn 11:676ea42afd56 75 m_qlocker.unlock();
Bobymicjohn 11:676ea42afd56 76 return true;
Bobymicjohn 11:676ea42afd56 77 }
Bobymicjohn 11:676ea42afd56 78 else
Bobymicjohn 11:676ea42afd56 79 {
Bobymicjohn 11:676ea42afd56 80 return false;
Bobymicjohn 11:676ea42afd56 81 }
Bobymicjohn 11:676ea42afd56 82 }
Bobymicjohn 11:676ea42afd56 83
Bobymicjohn 11:676ea42afd56 84 bool USBServer::PushUnreliableMsg(const char type, const std::string & msg)
Bobymicjohn 11:676ea42afd56 85 {
hazheng 13:7dcb1642ef99 86 if(m_stat != SER_STAT_RUNNING || m_msgQueue.size() >= UNRELIABLE_QUEUE_MAX_SIZE)
Bobymicjohn 11:676ea42afd56 87 return false;
Bobymicjohn 11:676ea42afd56 88
Bobymicjohn 11:676ea42afd56 89 if(msg.length() <= HID_REPORT_LENGTH)
Bobymicjohn 11:676ea42afd56 90 {
Bobymicjohn 11:676ea42afd56 91 m_qlocker.lock();
Bobymicjohn 11:676ea42afd56 92 m_msgQueue.push_back(type + msg);
hazheng 13:7dcb1642ef99 93 if(m_stat == SER_STAT_RUNNING && m_usbThread)
Bobymicjohn 11:676ea42afd56 94 m_usbThread->signal_set(THREAD_SIGNAL_QUEUE);
Bobymicjohn 11:676ea42afd56 95 m_qlocker.unlock();
Bobymicjohn 11:676ea42afd56 96 return true;
Bobymicjohn 11:676ea42afd56 97 }
Bobymicjohn 11:676ea42afd56 98 else
Bobymicjohn 11:676ea42afd56 99 {
Bobymicjohn 11:676ea42afd56 100 return false;
Bobymicjohn 11:676ea42afd56 101 }
Bobymicjohn 11:676ea42afd56 102 }
Bobymicjohn 11:676ea42afd56 103
Bobymicjohn 11:676ea42afd56 104 void USBServer::RunningThread()
Bobymicjohn 11:676ea42afd56 105 {
hazheng 13:7dcb1642ef99 106 m_stat = SER_STAT_RUNNING;
Bobymicjohn 11:676ea42afd56 107
Bobymicjohn 11:676ea42afd56 108 while(!m_shouldTerminate)
Bobymicjohn 11:676ea42afd56 109 {
Bobymicjohn 11:676ea42afd56 110 //m_qlocker.lock(); m_uqlocker.lock();
Bobymicjohn 11:676ea42afd56 111 if(m_msgQueue.size() <= 0)
Bobymicjohn 11:676ea42afd56 112 {
Bobymicjohn 11:676ea42afd56 113 //m_qlocker.unlock(); m_uqlocker.unlock();
Bobymicjohn 11:676ea42afd56 114 Thread::signal_wait(THREAD_SIGNAL_QUEUE);
Bobymicjohn 11:676ea42afd56 115 }
Bobymicjohn 11:676ea42afd56 116 //m_qlocker.unlock(); m_uqlocker.unlock();
Bobymicjohn 11:676ea42afd56 117 std::string msg = "";
Bobymicjohn 11:676ea42afd56 118 if(m_msgQueue.size() > 0)
Bobymicjohn 11:676ea42afd56 119 {
Bobymicjohn 11:676ea42afd56 120 m_qlocker.lock();
Bobymicjohn 11:676ea42afd56 121 msg = m_msgQueue.front();
Bobymicjohn 11:676ea42afd56 122 m_msgQueue.pop_front();
Bobymicjohn 11:676ea42afd56 123 m_qlocker.unlock();
Bobymicjohn 11:676ea42afd56 124 }
Bobymicjohn 11:676ea42afd56 125
Bobymicjohn 11:676ea42afd56 126 m_usb->printf("%s\n", msg.c_str());
Bobymicjohn 11:676ea42afd56 127 }
hazheng 16:66c7a09e71ee 128
Bobymicjohn 11:676ea42afd56 129 }
Bobymicjohn 11:676ea42afd56 130
Bobymicjohn 11:676ea42afd56 131 void USBServer::ConnectingThread()
Bobymicjohn 11:676ea42afd56 132 {
hazheng 13:7dcb1642ef99 133 m_stat = SER_STAT_CONNECTING;
Bobymicjohn 11:676ea42afd56 134
Bobymicjohn 11:676ea42afd56 135 m_usb = new Serial(USBTX, USBRX, "SmartWheels");
Bobymicjohn 11:676ea42afd56 136
hazheng 16:66c7a09e71ee 137 m_usb->printf("\x02%s\x03", HANDSHAKE_MSG_TER);
Bobymicjohn 11:676ea42afd56 138
hazheng 13:7dcb1642ef99 139 m_stat = SER_STAT_CONNECTED;
Bobymicjohn 11:676ea42afd56 140
Bobymicjohn 11:676ea42afd56 141 }
Bobymicjohn 11:676ea42afd56 142
hazheng 13:7dcb1642ef99 143
hazheng 13:7dcb1642ef99 144 uint8_t USBServer::GetStatus() const
hazheng 13:7dcb1642ef99 145 {
hazheng 13:7dcb1642ef99 146 return m_stat;
hazheng 13:7dcb1642ef99 147 }
hazheng 13:7dcb1642ef99 148
Bobymicjohn 11:676ea42afd56 149 }