comm.cpp
Go to the documentation of this file.
00001 /*
00002 
00003 Copyright (c) 2011, Markus Achtelik, ASL, ETH Zurich, Switzerland
00004 You can contact the author at <markus dot achtelik at mavt dot ethz dot ch>
00005 
00006 All rights reserved.
00007 
00008 Redistribution and use in source and binary forms, with or without
00009 modification, are permitted provided that the following conditions are met:
00010 * Redistributions of source code must retain the above copyright
00011 notice, this list of conditions and the following disclaimer.
00012 * Redistributions in binary form must reproduce the above copyright
00013 notice, this list of conditions and the following disclaimer in the
00014 documentation and/or other materials provided with the distribution.
00015 * Neither the name of ETHZ-ASL nor the
00016 names of its contributors may be used to endorse or promote products
00017 derived from this software without specific prior written permission.
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00020 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022 DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY
00023 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 
00030 */
00031 
00032 #include "comm.h"
00033 #include <ros/ros.h>
00034 
00035 Comm::Comm() :
00036   rx_timeout_(uart_service_), rx_timeout_occurred_(false), rx_state_(1), TX_HEADER_SIZE(6)// 3 preamble + size + type + flag
00037 {
00038   rx_packet_cnt_ = 0;
00039   rx_packet_good_cnt_ = 0;
00040   registerCallback(HLI_PACKET_ID_ACK, &Comm::processPacketAck, this);
00041   tx_buffer_[0] = 'a';
00042   tx_buffer_[1] = '*';
00043   tx_buffer_[2] = '>';
00044 }
00045 
00046 Comm::~Comm()
00047 {
00048   this->close();
00049 }
00050 
00051 bool Comm::connect(SerialPortPtr & serial_port, const std::string & port, uint32_t baudrate)
00052 {
00053   try
00054   {
00055     serial_port.reset(new SerialPort(uart_service_));
00056     serial_port->open(port);
00057 
00058     if(!configurePort(serial_port, &baudrate))
00059       return false;
00060 
00061     ROS_INFO_STREAM("INFO: opened serial port " << port << " with baudrate " << baudrate);
00062     return true;
00063   }
00064   catch (boost::system::system_error::exception &e)
00065   {
00066     ROS_ERROR_STREAM("ERROR: could not open serial port " << port << " reason: " << e.what());
00067     return false;
00068   }
00069   return false;
00070 }
00071 
00072 bool Comm::connect(const std::string & port_rx, const std::string & port_tx, uint32_t baudrate)
00073 {
00074   bool success = false;
00075   port_tx_name_ = port_tx;
00076   port_rx_name_ = port_rx;
00077 
00078   if (port_rx == port_tx)
00079   {
00080     success = connect(port_rx_, port_rx, baudrate);
00081     port_tx_ = port_rx_;
00082   }
00083   else
00084   {
00085     if (connect(port_rx_, port_rx, baudrate))
00086     {
00087       success = connect(port_tx_, port_tx, baudrate);
00088     }
00089   }
00090 
00091   if (!success)
00092     return false;
00093 
00094   // write packet that configures autobaud. LSB needs to be 1, LSB+1 needs to be 0
00095   char buf = 'a';
00096   boost::asio::write(*port_tx_, boost::asio::buffer(&buf, 1));
00097 
00098   // run uart worker threads
00099   rxReadStart(1, 1000);
00100   uart_thread_[0] = boost::thread(boost::bind(&boost::asio::io_service::run, &uart_service_));
00101   uart_thread_[1] = boost::thread(boost::bind(&boost::asio::io_service::run, &uart_service_));
00102 
00103   // this doesn't do anything on the HLP right now, just a dummy package to check successful connection
00104   HLI_BAUDRATE dummy;
00105   ROS_INFO("configured serial port(s), checking connection ... ");
00106   for (int i = 0; i < 5; i++)
00107   {
00108     if (sendPacketAck(HLI_PACKET_ID_BAUDRATE, dummy, 0.5))
00109     {
00110       ROS_INFO("ok");
00111       return true;
00112     }
00113   }
00114   ROS_ERROR("failed");
00115   return false;
00116 }
00117 
00118 bool Comm::configurePort(SerialPortPtr & serial_port, uint32_t * baudrate)
00119 {
00120   int32_t baudrates[] = {9600, 14400, 19200, 38400, 57600, 115200, 230400, 460800, 921600};
00121   uint32_t best_baudrate = 57600;
00122   uint32_t min_diff = 1e6;
00123 
00124   for (uint32_t i = 0; i < sizeof(baudrates) / sizeof(uint32_t); i++)
00125   {
00126     uint32_t diff = abs(baudrates[i] - *baudrate);
00127     if (diff < min_diff)
00128     {
00129       min_diff = diff;
00130       best_baudrate = baudrates[i];
00131     }
00132   }
00133 
00134   if (best_baudrate != *baudrate)
00135     ROS_WARN("Unsupported baudrate, choosing closest supported baudrate (%d)", best_baudrate);
00136 
00137   *baudrate = best_baudrate;
00138 
00139   try
00140   {
00141     serial_port->set_option(boost::asio::serial_port_base::baud_rate(best_baudrate));
00142     serial_port->set_option(boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::none));
00143     serial_port->set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none));
00144     serial_port->set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one));
00145     serial_port->set_option(boost::asio::serial_port_base::character_size(8));
00146     return true;
00147   }
00148   catch (boost::system::system_error::exception &e)
00149   {
00150     ROS_ERROR_STREAM("configuring serial port failed: " << e.what());
00151     return false;
00152   }
00153   return false;
00154 }
00155 
00156 void Comm::close(){
00157   uart_service_.post(boost::bind(&boost::asio::deadline_timer::cancel, &rx_timeout_));
00158   uart_service_.post(boost::bind(&boost::asio::serial_port::close, port_rx_));
00159   if (port_rx_name_ != port_tx_name_)
00160     uart_service_.post(boost::bind(&boost::asio::serial_port::close, port_tx_));
00161 
00162   uart_thread_[0].join();
00163   uart_thread_[1].join();
00164 }
00165 
00166 bool Comm::validateChecksum(uint8_t * data, uint32_t length, uint16_t checksum)
00167 {
00168   return crc16(data, length) == checksum;
00169 }
00170 
00171 void Comm::rxReadStart(uint32_t length, uint32_t timeout)
00172 {
00173   boost::asio::async_read(*port_rx_, boost::asio::buffer(rx_buffer_, length),
00174                           boost::bind(&Comm::rxReadCallback, this, boost::asio::placeholders::error,
00175                                       boost::asio::placeholders::bytes_transferred));
00176 
00177   if (timeout != 0)
00178   {
00179     rx_timeout_.expires_from_now(boost::posix_time::milliseconds(timeout));
00180     rx_timeout_.async_wait(boost::bind(&Comm::rxTimeoutCallback, this, boost::asio::placeholders::error));
00181   }
00182 }
00183 
00184 void Comm::rxTimeoutCallback(const boost::system::error_code & error)
00185 {
00186   if (!error)
00187   {
00188     port_rx_->cancel();
00189     rx_timeout_occurred_ = true;
00190     printf("rx timeout\n");
00191   }
00192 }
00193 
00194 void Comm::rxReadCallback(const boost::system::error_code& error, size_t bytes_transferred)
00195 {
00196   static uint32_t bytes_to_read = 1;
00197   const int CHECKSUM_SIZE = 2;
00198 
00199   static uint8_t packet_type = 0;
00200   static uint8_t packet_size = 0;
00201 
00202   static uint32_t err_cnt = 0;
00203 
00204   if (error)
00205   {
00206     if (error == boost::asio::error::operation_aborted)
00207     {
00208       // cancel came from timer? -> try again
00209       if (rx_timeout_occurred_)
00210       {
00211         rx_timeout_occurred_ = false;
00212         rx_state_ = 1;
00213         rxReadStart(1, 1000);
00214       }
00215       return;
00216     }
00217 
00218     std::cerr << "read error: " << error.message() << std::endl;
00219     if(err_cnt < 10){
00220       rx_state_ = 1;
00221       usleep(1e5);
00222       rxReadStart(1, 1000);
00223     }
00224     else{
00225       ROS_ERROR("Too many read errors");
00226       ros::shutdown();
00227     }
00228     err_cnt ++;
00229     return;
00230   }
00231 
00232   rx_timeout_.cancel();
00233 
00234   if (rx_state_ == 1)
00235   {
00236     packet_type = 0;
00237     packet_size = 0;
00238     if (rx_buffer_[0] == 0xff)
00239       rx_state_++;
00240   }
00241   else if (rx_state_ == 2)
00242   {
00243     if (rx_buffer_[0] == 0x09/*recvPreamble_[1]*/)
00244     {
00245       rx_state_ = 3;
00246       bytes_to_read = 2;
00247     }
00248     else
00249     {
00250       rx_state_ = 1;
00251       bytes_to_read = 1;
00252     }
00253   }
00254   else if (rx_state_ == 3)
00255   {
00256     // read packet size
00257     packet_size = rx_buffer_[0];
00258     // determine packet type
00259     packet_type = rx_buffer_[1];
00260     bytes_to_read = packet_size + CHECKSUM_SIZE; // read packet and checksum at once
00261     rx_state_ = 4;
00262   }
00263   else if (rx_state_ == 4)
00264   {
00265     // message body is read - validate checksum now
00266     uint16_t checksum_received = 0;
00267     uint16_t checksum_computed = 0;
00268     rx_packet_cnt_++;
00269     checksum_received = *((uint16_t*)(&rx_buffer_[packet_size]));
00270     checksum_computed = crc16(&packet_type, sizeof(packet_type));
00271     checksum_computed = crc16(rx_buffer_, packet_size, checksum_computed);
00272     if (checksum_received != checksum_computed)
00273     {
00274       std::cerr << "checksum error for packet " << (int)packet_type << " ,resyncing " << std::endl;
00275       bytes_to_read = 1;
00276       rx_state_ = 1;
00277     }
00278     else
00279     {
00280       rx_packet_good_cnt_++;
00281       // look for registered callback and call it if available
00282       CommCallbackMap::iterator callback_it;
00283       callback_it = callbacks_.find(packet_type);
00284       if (callback_it != callbacks_.end())
00285         callback_it->second(rx_buffer_, packet_size);
00286 
00287       // everything is successfully finished and synchronized, read header in one step now
00288       bytes_to_read = 4;
00289       rx_state_ = 5;
00290     }
00291   }
00292   else if (rx_state_ == 5)
00293   {
00294     // read packet size
00295     packet_size = rx_buffer_[2];
00296     // determine packet type
00297     packet_type = rx_buffer_[3];
00298     bytes_to_read = packet_size + CHECKSUM_SIZE; // read packet and checksum at once
00299     rx_state_ = 4;
00300   }
00301 
00302   rxReadStart(bytes_to_read, 1000);
00303 }
00304 
00305 void Comm::serializePacket(uint8_t packet_id, const void * data, const int32_t & packet_size, uint8_t flag)
00306 {
00307   assert((packet_size + TX_HEADER_SIZE) <= 256);
00308   tx_bytes2send_ = TX_HEADER_SIZE + packet_size + sizeof(uint16_t);
00309   tx_buffer_[3] = packet_size;
00310   tx_buffer_[4] = packet_id;
00311   tx_buffer_[5] = flag;
00312 
00313   memcpy(&tx_buffer_[6], data, packet_size);
00314   uint16_t chk = crc16(&tx_buffer_[4], packet_size + 2); // datasize + descriptor + flag
00315   tx_buffer_[packet_size + TX_HEADER_SIZE] = (uint8_t)(chk & 0xff);
00316   tx_buffer_[packet_size + TX_HEADER_SIZE + 1] = (uint8_t)(chk >> 8);
00317 }
00318 
00319 void Comm::processPacketAck(uint8_t * buf, uint32_t length)
00320 {
00321   HLI_ACK *packet_ack = (HLI_ACK*)buf;
00322   boost::mutex::scoped_lock lock(tx_mutex_);
00323   packet_acks_.push_back(packet_ack->ack_packet);
00324 //  ROS_INFO("packet ack %d", packet_ack->ack_packet);
00325 }
00326 
00327 bool Comm::waitForPacketAck(uint8_t ack_id, const double & timeout)
00328 {
00329   int sleeptime = 1e5; // 100ms
00330   int wait_count = static_cast<int> (timeout * 1e6 / sleeptime);
00331 
00332   for (int i = 0; i < wait_count; i++)
00333   {
00334     boost::mutex::scoped_lock lock(tx_mutex_);
00335     for (std::vector<uint8_t>::iterator it = packet_acks_.begin(); it < packet_acks_.end(); it++)
00336     {
00337       if (*it == ack_id)
00338       {
00339         packet_acks_.erase(it);
00340         return true;
00341       }
00342     }
00343     lock.unlock();
00344     usleep(sleeptime);
00345   }
00346   std::cerr << "waiting for acknowledged packet timed out" << std::endl;
00347   return false;
00348 }
00349 
00350 uint16_t Comm::crc_update(uint16_t crc, uint8_t data)
00351 {
00352   data ^= (crc & 0xff);
00353   data ^= data << 4;
00354 
00355   return ((((uint16_t)data << 8) | ((crc >> 8) & 0xff)) ^ (uint8_t)(data >> 4) ^ ((uint16_t)data << 3));
00356 }
00357 
00358 uint16_t Comm::crc16(void* data, uint16_t cnt, uint16_t crc)
00359 {
00360   uint8_t * ptr = (uint8_t *)data;
00361   int i;
00362 
00363   for (i = 0; i < cnt; i++)
00364   {
00365     crc = crc_update(crc, *ptr);
00366     ptr++;
00367   }
00368   return crc;
00369 }


asctec_hl_interface
Author(s): Markus Achtelik, Michael Achtelik, Stephan Weiss, Laurent Kneip
autogenerated on Thu Aug 27 2015 12:26:52