CanUsb.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2015-2018, Dataspeed Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Dataspeed Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include <dataspeed_can_usb/CanUsb.h>
00036 #include <lusb/UsbDevice.h>
00037 #include <string.h>     // memcpy() and memset()
00038 #include <sstream>
00039 
00040 // Protocol definition
00041 #include "UsbCanMessages.h"
00042 
00043 #include <queue>
00044 class TxQueue
00045 {
00046 public:
00047   TxQueue(size_t max_size) : max_size_(max_size) {}
00048   ~TxQueue() {}
00049   inline bool empty() const { return queue_.empty(); }
00050   inline size_t size() const { return queue_.size(); }
00051   inline MessageBuffer& front() { return queue_.front(); }
00052   inline const MessageBuffer& front() const { return queue_.front(); }
00053   inline MessageBuffer& back() { return queue_.back(); }
00054   inline const MessageBuffer& back() const { return queue_.back(); }
00055   inline bool push(const MessageBuffer& __x) {
00056     if (queue_.size() < max_size_) {
00057       queue_.push(__x);
00058       return true;
00059     }
00060     return false;
00061   }
00062   inline void pop() { queue_.pop(); }
00063   inline size_t maxSize() const { return max_size_; }
00064 private:
00065   size_t max_size_;
00066   std::queue<MessageBuffer> queue_;
00067 };
00068 
00069 namespace dataspeed_can_usb
00070 {
00071 
00072 CanUsb::CanUsb(lusb::UsbDevice *dev) : ready_(false), heap_dev_(false), dev_(dev), recv_callback_(NULL), version_major_(0), version_minor_(0), version_build_(0), version_comms_(0), serial_number_(0), num_channels_(0)
00073 {
00074   if (!dev_) {
00075     dev_ = new lusb::UsbDevice(USB_VID, USB_PID, USB_MI);
00076     heap_dev_ = true;
00077   }
00078   queue_ = new TxQueue(100);
00079 }
00080 CanUsb::~CanUsb()
00081 {
00082   if (dev_) {
00083     if (dev_->isOpen()) {
00084       dev_->stopBulkReadThread(STREAM_ENDPOINT);
00085       dev_->close();
00086     }
00087     if (heap_dev_) {
00088       delete dev_;
00089     }
00090     dev_ = NULL;
00091   }
00092   if (queue_) {
00093     delete queue_;
00094     queue_ = NULL;
00095   }
00096 }
00097 
00098 bool CanUsb::configure(const std::string &mac)
00099 {
00100   if (readVersion()) {
00101     if (getNumChannels()) {
00102       if (mac.empty() || mac_addr_.match(mac)) {
00103         dev_->startBulkReadThread(boost::bind(&CanUsb::recvStream, this, _1, _2), STREAM_ENDPOINT);
00104         ready_ = true;
00105         return true;
00106       }
00107     }
00108   }
00109   return false;
00110 }
00111 bool CanUsb::open(const std::string &mac)
00112 {
00113   if (!isOpen()) {
00114     if (!dev_->isOpen()) {
00115       if (mac.empty()) {
00116         if (dev_->open()) {
00117           if (configure()) {
00118             return true;
00119           }
00120         }
00121         dev_->close();
00122       } else {
00123         std::vector<lusb::UsbDevice::Location> list;
00124         dev_->listDevices(list);
00125         for (size_t i = 0; i < list.size(); i++) {
00126           if (dev_->open(list[i])) {
00127             if (configure(mac)) {
00128               return true;
00129             }
00130           }
00131           dev_->close();
00132         }
00133       }
00134     } else {
00135       if (configure()) {
00136         return true;
00137       }
00138     }
00139   }
00140   return isOpen();
00141 }
00142 bool CanUsb::isOpen()
00143 {
00144   if (ready_) {
00145     if (dev_->isOpen()) {
00146       return true;
00147     } else {
00148       ready_ = false;
00149     }
00150   }
00151   return false;
00152 }
00153 void CanUsb::closeDevice()
00154 {
00155   dev_->stopBulkReadThread(STREAM_ENDPOINT);
00156   dev_->close();
00157   ready_ = false;
00158 }
00159 
00160 bool CanUsb::readVersion()
00161 {
00162   ConfigPacket packet;
00163   packet.msg_id = USB_ID_VERSION;
00164   if (writeConfig(&packet, sizeof(packet.msg_id))) {
00165     int len = readConfig(&packet, sizeof(packet));
00166     if ((len >= (int)sizeof(packet.version) - (int)sizeof(packet.version.mac_addr)) && (packet.msg_id == USB_ID_VERSION)) {
00167       version_major_ = packet.version.firmware.major;
00168       version_minor_ = packet.version.firmware.minor;
00169       version_build_ = packet.version.firmware.build;
00170       version_comms_ = packet.version.comms;
00171       serial_number_ = packet.version.serial_number;
00172       mac_addr_ = (len >= (int)sizeof(packet.version)) ? MacAddr(packet.version.mac_addr) : MacAddr();
00173       return true;
00174     }
00175   }
00176   return false;
00177 }
00178 
00179 bool CanUsb::getNumChannels()
00180 {
00181   ConfigPacket packet;
00182   packet.msg_id = USB_ID_NUM_CHANNELS;
00183   if (writeConfig(&packet, sizeof(packet.msg_id))) {
00184     int len = readConfig(&packet, sizeof(packet));
00185     if ((len >= (int)sizeof(packet.num_channels)) && (packet.msg_id == USB_ID_NUM_CHANNELS)) {
00186       num_channels_ = packet.num_channels.num_channels;
00187       return true;
00188     }
00189   }
00190   return false;
00191 }
00192 
00193 void CanUsb::recvStream(const void *data, int size)
00194 {
00195   if (recv_callback_) {
00196     const MessageBuffer *ptr = ((StreamPacket*)data)->msg;
00197     while (size >= (int)sizeof(*ptr)) {
00198         size -= sizeof(*ptr);
00199         recv_callback_(ptr->channel, ptr->id, ptr->extended, ptr->dlc, ptr->data);
00200         ptr++;
00201     }
00202   }
00203 }
00204 
00205 bool CanUsb::reboot()
00206 {
00207   ConfigPacket packet;
00208   packet.msg_id = USB_ID_REBOOT;
00209   if (writeConfig(&packet, sizeof(packet.msg_id))) {
00210     closeDevice();
00211     return true;
00212   }
00213   return false;
00214 }
00215 
00216 bool CanUsb::reset()
00217 {
00218   ConfigPacket packet;
00219   packet.msg_id = USB_ID_RESET;
00220   if (writeConfig(&packet, sizeof(packet.msg_id))) {
00221     int len = readConfig(&packet, sizeof(packet), USB_DEFAULT_TIMEOUT);
00222     if ((len >= (int)sizeof(packet.msg_id)) && (packet.msg_id == USB_ID_RESET)) {
00223       return true;
00224     }
00225   }
00226   return false;
00227 }
00228 
00229 bool CanUsb::setBitrate(unsigned int channel, uint32_t bitrate, uint8_t mode)
00230 {
00231   ConfigPacket packet;
00232   packet.msg_id = USB_ID_SET_BUS_CFG;
00233   packet.bus_cfg.channel = channel;
00234   packet.bus_cfg.bitrate = bitrate;
00235   packet.bus_cfg.mode = mode;
00236   if (writeConfig(&packet, sizeof(packet.bus_cfg))) {
00237     int len = readConfig(&packet, sizeof(packet), USB_DEFAULT_TIMEOUT);
00238     if ((len >= (int)sizeof(packet.bus_cfg)) && (packet.msg_id == USB_ID_GET_BUS_CFG)) {
00239       return true;
00240     }
00241   }
00242   return false;
00243 }
00244 
00245 bool CanUsb::addFilter(unsigned int channel, uint32_t mask, uint32_t match)
00246 {
00247   ConfigPacket packet;
00248   packet.msg_id = USB_ID_SET_FILTER;
00249   packet.filter.channel = channel;
00250   packet.filter.mask = mask;
00251   packet.filter.match = match;
00252   if (writeConfig(&packet, sizeof(packet.filter))) {
00253     int len = readConfig(&packet, sizeof(packet), USB_DEFAULT_TIMEOUT);
00254     if ((len >= (int)sizeof(packet.filter)) && (packet.msg_id == USB_ID_SET_FILTER)) {
00255       if (packet.filter.success) {
00256         return true;
00257       }
00258     }
00259   }
00260   return false;
00261 }
00262 
00263 bool CanUsb::getStats(std::vector<uint32_t> &rx_drops, std::vector<uint32_t> &tx_drops,
00264                       std::vector<uint8_t> &rx_errors, std::vector<uint8_t> &tx_errors, bool)
00265 {
00266   // TODO: implement clear functionality (last param)
00267   ConfigPacket packet;
00268   packet.msg_id = USB_ID_GET_STATS;
00269   if (writeConfig(&packet, sizeof(packet.bus_cfg))) {
00270     int len = readConfig(&packet, sizeof(packet), USB_DEFAULT_TIMEOUT);
00271     if ((len >= (int)sizeof(packet.stats)) && (packet.msg_id == USB_ID_GET_STATS)) {
00272       unsigned int size = std::min(num_channels_, (unsigned int)MAX_CHANNELS);
00273       rx_drops.resize(size);
00274       tx_drops.resize(size);
00275       rx_errors.resize(size);
00276       tx_errors.resize(size);
00277       for (unsigned int i = 0; i < size; i++) {
00278         rx_drops[i] = packet.stats.rx_drops[i];
00279         tx_drops[i] = packet.stats.tx_drops[i];
00280         rx_errors[i] = packet.stats.rx_errors[i];
00281         tx_errors[i] = packet.stats.tx_errors[i];
00282       }
00283       return true;
00284     }
00285   }
00286   return false;
00287 }
00288 
00289 bool CanUsb::getTimeStamp(uint32_t &timestamp)
00290 {
00291   ConfigPacket packet;
00292   packet.msg_id = USB_ID_GET_TIME;
00293   if (writeConfig(&packet, sizeof(packet.msg_id))) {
00294     int len = readConfig(&packet, sizeof(packet));
00295     if ((len >= (int)sizeof(packet.time)) && (packet.msg_id == USB_ID_GET_TIME)) {
00296       timestamp = packet.time.stamp;
00297       return true;
00298     }
00299   }
00300   return false;
00301 }
00302 
00303 void CanUsb::sendMessage(unsigned int channel, uint32_t id, bool extended, uint8_t dlc, const uint8_t data[8], bool flush)
00304 {
00305   MessageBuffer msg;
00306   msg.headerWord[0] = 0;
00307   msg.headerWord[1] = 0;
00308   msg.id = id;
00309   msg.extended = extended ? 1 : 0;
00310   msg.channel = channel;
00311   msg.dlc = dlc;
00312   memcpy(msg.data, data, 8);
00313   queue_->push(msg);
00314   if (flush || (queue_->size() >= sizeof(StreamPacket) / sizeof(MessageBuffer))) {
00315     flushMessages();
00316   }
00317 }
00318 void CanUsb::flushMessages()
00319 {
00320   if (!queue_->empty()) {
00321     unsigned int num = std::min(queue_->size(), sizeof(StreamPacket) / sizeof(MessageBuffer));
00322     if (writeStream(&queue_->front(), num * sizeof(MessageBuffer))) {
00323       while (num--) {
00324         queue_->pop();
00325       }
00326     }
00327   }
00328 }
00329 
00330 bool CanUsb::writeConfig(const void * data, int size)
00331 {
00332   return writeConfig(data, size, USB_DEFAULT_TIMEOUT);
00333 }
00334 int CanUsb::readConfig(void * data, int size)
00335 {
00336   return readConfig(data, size, USB_DEFAULT_TIMEOUT);
00337 }
00338 bool CanUsb::writeConfig(const void * data, int size, int timeout)
00339 {
00340   if (!dev_->bulkWrite(data, size, CONFIGURATION_ENDPOINT, timeout)) {
00341     return false;
00342   }
00343   return true;
00344 }
00345 int CanUsb::readConfig(void * data, int size, int timeout)
00346 {
00347   int len = dev_->bulkRead(data, size, CONFIGURATION_ENDPOINT, timeout);
00348   if (len < 0) {
00349     return -1;
00350   }
00351   return len;
00352 }
00353 
00354 bool CanUsb::writeStream(const void * data, int size)
00355 {
00356   if (!dev_->bulkWrite(data, size, STREAM_ENDPOINT, USB_DEFAULT_TIMEOUT)) {
00357     return false;
00358   }
00359   return true;
00360 }
00361 int CanUsb::readStream(void * data, int size)
00362 {
00363   int len = dev_->bulkRead(data, size, STREAM_ENDPOINT, USB_DEFAULT_TIMEOUT);
00364   if (len < 0) {
00365     return -1;
00366   }
00367   return len;
00368 }
00369 
00370 std::string CanUsb::version() const
00371 {
00372   std::stringstream s;
00373   s << version_major_ << "." << version_minor_ << "." << version_build_ << "-" << version_comms_;
00374   return s.str();
00375 }
00376 
00377 } // namespace dataspeed_can_usb
00378 


dataspeed_can_usb
Author(s): Kevin Hallenbeck
autogenerated on Thu Jun 6 2019 21:16:43