00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
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)
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
00096 char buf = 'a';
00097 boost::asio::write(*port_tx_, boost::asio::buffer(&buf, 1));
00098
00099
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
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
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)
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
00257 packet_size = rx_buffer_[0];
00258
00259 packet_type = rx_buffer_[1];
00260 bytes_to_read = packet_size + CHECKSUM_SIZE;
00261 rx_state_ = 4;
00262 }
00263 else if (rx_state_ == 4)
00264 {
00265
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
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
00288 bytes_to_read = 4;
00289 rx_state_ = 5;
00290 }
00291 }
00292 else if (rx_state_ == 5)
00293 {
00294
00295 packet_size = rx_buffer_[2];
00296
00297 packet_type = rx_buffer_[3];
00298 bytes_to_read = packet_size + CHECKSUM_SIZE;
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);
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
00325 }
00326
00327 bool SerialCommunication::waitForPacketAck(uint8_t ack_id, const double & timeout)
00328 {
00329 int sleeptime = 1e5;
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 }