Go to the documentation of this file.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 #ifndef FLYER_INTERFACE_SERIAL_COMMUNICATION_H
00033 #define FLYER_INTERFACE_SERIAL_COMMUNICATION_H
00034
00035 #include <iostream>
00036 #include <map>
00037 #include <vector>
00038 #include <boost/bind.hpp>
00039 #include <boost/function.hpp>
00040 #include <boost/shared_ptr.hpp>
00041 #include <boost/thread.hpp>
00042 #include <boost/asio.hpp>
00043 #include <ros/ros.h>
00044 #include <mav_common/comm.h>
00045 #include <mav_common/comm_packets.h>
00046
00047 namespace mav
00048 {
00049
00050 typedef boost::function<void(uint8_t*, int32_t)> CommCallbackType;
00051 typedef std::map<uint8_t, CommCallbackType> CommCallbackMap;
00052
00054
00060 class SerialCommunication
00061 {
00062 typedef boost::asio::serial_port SerialPort;
00063 typedef boost::shared_ptr<SerialPort> SerialPortPtr;
00064
00065 private:
00066
00067 SerialPortPtr port_rx_;
00068 SerialPortPtr port_tx_;
00069 boost::asio::io_service uart_service_;
00070 boost::asio::deadline_timer rx_timeout_;
00071 bool rx_timeout_occurred_;
00072 boost::thread uart_thread_[2];
00073
00074 std::string port_rx_name_;
00075 std::string port_tx_name_;
00076
00077 uint32_t rx_packet_cnt_;
00078 uint32_t rx_packet_good_cnt_;
00079
00080 uint8_t tx_buffer_[256];
00081 uint8_t rx_buffer_[256];
00082 uint32_t tx_bytes2send_;
00083 int rx_state_;
00084 std::vector<uint8_t> packet_acks_;
00085 boost::mutex tx_mutex_;
00086 const int TX_HEADER_SIZE;
00087
00088 bool baudrate_change_successful_;
00089 CommCallbackMap callbacks_;
00090
00091 void rxReadStart(uint32_t length, uint32_t timeout = 0);
00092 void rxReadCallback(const boost::system::error_code& error, size_t bytes_transferred);
00093 void rxTimeoutCallback(const boost::system::error_code & error);
00094 bool configurePort(SerialPortPtr & serial_port, uint32_t *baudrate);
00095 bool connect(SerialPortPtr & serial_port, const std::string & port, uint32_t baudrate);
00096
00097 void processPacketAck(uint8_t * buf, uint32_t length);
00098 bool validateChecksum(uint8_t * buf, uint32_t length, uint16_t checksum);
00099
00100 bool waitForPacketAck(uint8_t ack_id, const double & timeout = 1.0);
00101
00102 inline uint16_t crc_update(uint16_t crc, uint8_t data);
00103 uint16_t crc16(void* data, uint16_t cnt, uint16_t crc = 0xff);
00104
00105 void serializePacket(uint8_t packet_id, const void * data, const int32_t & packet_size, uint8_t flag = 0);
00106
00107 public:
00108
00109 SerialCommunication();
00110 ~SerialCommunication();
00111
00113
00124 bool connect(const std::string & port_rx, const std::string & port_tx, uint32_t baudrate);
00125
00127 void close();
00128
00129 uint32_t getRxPackets()
00130 {
00131 return rx_packet_cnt_;
00132 }
00133
00134 uint32_t getRxPacketsGood()
00135 {
00136 return rx_packet_good_cnt_;
00137 }
00138
00140 template<typename T>
00141 bool sendPacket(uint8_t packet_id, const T & data)
00142 {
00143 serializePacket(packet_id, &data, sizeof(T));
00144
00145 return tx_bytes2send_ == boost::asio::write(*port_tx_, boost::asio::buffer(tx_buffer_, tx_bytes2send_));
00146 }
00147
00149 template<typename T>
00150 bool sendPacketAck(uint8_t packet_id, const T & data, const double & timeout = 1.0)
00151 {
00152
00153 ros::Time time_start = ros::Time::now();
00154 uint8_t ack_id = (uint8_t)(time_start.toNSec() & 0xff) | MAV_COMM_ACK;
00155
00156
00157
00158
00159 serializePacket(packet_id, &data, sizeof(T), ack_id);
00160 if (tx_bytes2send_ != boost::asio::write(*port_tx_, boost::asio::buffer(tx_buffer_, tx_bytes2send_)))
00161 return false;
00162
00163 return waitForPacketAck(ack_id, timeout);
00164 }
00165
00167
00174 template<class T>
00175 void registerCallback(uint8_t id, void(T::*cb_func)(uint8_t*, uint32_t), T* p_obj)
00176 {
00177 if (callbacks_.find(id) != callbacks_.end())
00178 std::cerr<<"message "<< id << "is already registered"<<std::endl;
00179 else
00180 callbacks_[id] = boost::bind(cb_func, p_obj, _1, _2);
00181 }
00182
00183 void loopOnce(){uart_service_.run_one();};
00184 };
00185
00186 }
00187
00188 #endif // FLYER_INTERFACE_SERIAL_COMMUNICATION_H