serial_communication.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 "flyer_interface/serial_communication.h"
00033 
00034 namespace mav
00035 {
00036 
00037 SerialCommunication::SerialCommunication() :
00038   rx_timeout_(uart_service_), rx_timeout_occurred_(false), rx_state_(1), TX_HEADER_SIZE(6)// 3 preamble + size + type + flag
00039 {
00040   rx_packet_cnt_ = 0;
00041   rx_packet_good_cnt_ = 0;
00042   registerCallback(MAV_ACK_PKT_ID, &SerialCommunication::processPacketAck, this);
00043   tx_buffer_[0] = '>';
00044   tx_buffer_[1] = '*';
00045   tx_buffer_[2] = '>';
00046 }
00047 
00048 SerialCommunication::~SerialCommunication()
00049 {
00050   this->close();
00051 }
00052 
00053 bool SerialCommunication::connect(SerialPortPtr & serial_port, const std::string & port, uint32_t baudrate)
00054 {
00055   try
00056   {
00057     serial_port.reset(new SerialPort(uart_service_));
00058     serial_port->open(port);
00059 
00060     if(!configurePort(serial_port, &baudrate))
00061       return false;
00062 
00063     ROS_INFO_STREAM("Opened serial port " << port << " with baudrate " << baudrate);
00064     return true;
00065   }
00066   catch (boost::system::system_error::exception e)
00067   {
00068     ROS_ERROR_STREAM("Could not open serial port " << port << ", reason: " << e.what());
00069     return false;
00070   }
00071 }
00072 
00073 bool SerialCommunication::connect(const std::string & port_rx, const std::string & port_tx, uint32_t baudrate)
00074 {
00075   bool success = false;
00076   port_tx_name_ = port_tx;
00077   port_rx_name_ = port_rx;
00078 
00079   if (port_rx == port_tx)
00080   {
00081     success = connect(port_rx_, port_rx, baudrate);
00082     port_tx_ = port_rx_;
00083   }
00084   else
00085   {
00086     if (connect(port_rx_, port_rx, baudrate))
00087     {
00088       success = connect(port_tx_, port_tx, baudrate);
00089     }
00090   }
00091 
00092   if (!success)
00093     return false;
00094 
00095   // write packet that configures autobaud. LSB needs to be 1, LSB+1 needs to be 0
00096   char buf = 'a';
00097   boost::asio::write(*port_tx_, boost::asio::buffer(&buf, 1));
00098 
00099   // run uart worker threads
00100   rxReadStart(1, 1000);
00101   uart_thread_[0] = boost::thread(boost::bind(&boost::asio::io_service::run, &uart_service_));
00102   uart_thread_[1] = boost::thread(boost::bind(&boost::asio::io_service::run, &uart_service_));
00103 
00104   // this doesn't do anything on the HLP right now, just a dummy package to check successful connection
00105   MAV_DUMMY_PKT dummy;
00106   ROS_INFO("Configured serial port(s), checking connection ... ");
00107   for (int i = 0; i < 5; i++)
00108   {
00109     if (sendPacketAck(MAV_DUMMY_PKT_ID, dummy, 0.5))
00110     {
00111       ROS_INFO("Serial port connection ok.");
00112       return true;
00113     }
00114   }
00115   ROS_ERROR("Serial port connection failed!");
00116   return false;
00117 }
00118 
00119 bool SerialCommunication::configurePort(SerialPortPtr & serial_port, uint32_t * baudrate)
00120 {
00121   int32_t baudrates[] = {9600, 14400, 19200, 38400, 57600, 115200, 230400, 460800, 921600};
00122   uint32_t best_baudrate = 57600;
00123   uint32_t min_diff = 1e6;
00124 
00125   for (uint32_t i = 0; i < sizeof(baudrates) / sizeof(uint32_t); i++)
00126   {
00127     uint32_t diff = abs(baudrates[i] - *baudrate);
00128     if (diff < min_diff)
00129     {
00130       min_diff = diff;
00131       best_baudrate = baudrates[i];
00132     }
00133   }
00134 
00135   if (best_baudrate != *baudrate)
00136     ROS_WARN("Unsupported baudrate, choosing closest supported baudrate (%d)", best_baudrate);
00137 
00138   *baudrate = best_baudrate;
00139 
00140   try
00141   {
00142     serial_port->set_option(boost::asio::serial_port_base::baud_rate(best_baudrate));
00143     serial_port->set_option(boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::none));
00144     serial_port->set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none));
00145     serial_port->set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one));
00146     serial_port->set_option(boost::asio::serial_port_base::character_size(8));
00147     return true;
00148   }
00149   catch (boost::system::system_error::exception e)
00150   {
00151     ROS_ERROR_STREAM("configuring serial port failed: " << e.what());
00152     return false;
00153   }
00154 }
00155 
00156 void SerialCommunication::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 SerialCommunication::validateChecksum(uint8_t * data, uint32_t length, uint16_t checksum)
00167 {
00168   return crc16(data, length) == checksum;
00169 }
00170 
00171 void SerialCommunication::rxReadStart(uint32_t length, uint32_t timeout)
00172 {
00173   boost::asio::async_read(*port_rx_, boost::asio::buffer(rx_buffer_, length),
00174                           boost::bind(&SerialCommunication::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(&SerialCommunication::rxTimeoutCallback, this, boost::asio::placeholders::error));
00181   }
00182 }
00183 
00184 void SerialCommunication::rxTimeoutCallback(const boost::system::error_code & error)
00185 {
00186   if (!error)
00187   {
00188     port_rx_->cancel();
00189     rx_timeout_occurred_ = true;
00190     ROS_WARN("rx timeout");
00191   }
00192 }
00193 
00194 void SerialCommunication::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_BREAK();
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 SerialCommunication::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 SerialCommunication::processPacketAck(uint8_t * buf, uint32_t length)
00320 {
00321   MAV_ACK_PKT *packet_ack = (MAV_ACK_PKT *)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 SerialCommunication::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 SerialCommunication::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 SerialCommunication::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 }
00370 
00371 } // namespace asctec


flyer_interface
Author(s): Ivan Dryanovski
autogenerated on Thu Jan 2 2014 11:28:38