00001
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include <cassert>
00019 #include <console_bridge/console.h>
00020
00021 #include <mavconn/thread_utils.h>
00022 #include <mavconn/serial.h>
00023
00024 namespace mavconn {
00025 using boost::system::error_code;
00026 using boost::asio::io_service;
00027 using boost::asio::serial_port_base;
00028 using boost::asio::buffer;
00029 typedef std::lock_guard<std::recursive_mutex> lock_guard;
00030
00031 #define PFXd "mavconn: serial%d: "
00032
00033
00034 MAVConnSerial::MAVConnSerial(uint8_t system_id, uint8_t component_id,
00035 std::string device, unsigned baudrate) :
00036 MAVConnInterface(system_id, component_id),
00037 tx_in_progress(false),
00038 io_service(),
00039 serial_dev(io_service)
00040 {
00041 logInform(PFXd "device: %s @ %d bps", channel, device.c_str(), baudrate);
00042
00043 try {
00044 serial_dev.open(device);
00045
00046
00047 serial_dev.set_option(serial_port_base::baud_rate(baudrate));
00048 serial_dev.set_option(serial_port_base::character_size(8));
00049 serial_dev.set_option(serial_port_base::parity(serial_port_base::parity::none));
00050 serial_dev.set_option(serial_port_base::stop_bits(serial_port_base::stop_bits::one));
00051 serial_dev.set_option(serial_port_base::flow_control(serial_port_base::flow_control::none));
00052 }
00053 catch (boost::system::system_error &err) {
00054 throw DeviceError("serial", err);
00055 }
00056
00057
00058 io_service.post(boost::bind(&MAVConnSerial::do_read, this));
00059
00060
00061 std::thread t(boost::bind(&io_service::run, &this->io_service));
00062 mavutils::set_thread_name(t, "MAVConnSerial%d", channel);
00063 io_thread.swap(t);
00064 }
00065
00066 MAVConnSerial::~MAVConnSerial() {
00067 close();
00068 }
00069
00070 void MAVConnSerial::close() {
00071 lock_guard lock(mutex);
00072 if (!is_open())
00073 return;
00074
00075 io_service.stop();
00076 serial_dev.close();
00077
00078
00079 for (auto &p : tx_q)
00080 delete p;
00081 tx_q.clear();
00082
00083 if (io_thread.joinable())
00084 io_thread.join();
00085
00086 port_closed();
00087 }
00088
00089 void MAVConnSerial::send_bytes(const uint8_t *bytes, size_t length)
00090 {
00091 if (!is_open()) {
00092 logError(PFXd "send: channel closed!", channel);
00093 return;
00094 }
00095
00096 MsgBuffer *buf = new MsgBuffer(bytes, length);
00097 {
00098 lock_guard lock(mutex);
00099 tx_q.push_back(buf);
00100 }
00101 io_service.post(boost::bind(&MAVConnSerial::do_write, this, true));
00102 }
00103
00104 void MAVConnSerial::send_message(const mavlink_message_t *message, uint8_t sysid, uint8_t compid)
00105 {
00106 assert(message != nullptr);
00107
00108 if (!is_open()) {
00109 logError(PFXd "send: channel closed!", channel);
00110 return;
00111 }
00112
00113 logDebug(PFXd "send: Message-Id: %d [%d bytes] Sys-Id: %d Comp-Id: %d Seq: %d",
00114 channel, message->msgid, message->len, sysid, compid, message->seq);
00115
00116 MsgBuffer *buf = new_msgbuffer(message, sysid, compid);
00117 {
00118 lock_guard lock(mutex);
00119 tx_q.push_back(buf);
00120 }
00121 io_service.post(boost::bind(&MAVConnSerial::do_write, this, true));
00122 }
00123
00124 void MAVConnSerial::do_read(void)
00125 {
00126 serial_dev.async_read_some(
00127 buffer(rx_buf, sizeof(rx_buf)),
00128 boost::bind(&MAVConnSerial::async_read_end,
00129 this,
00130 boost::asio::placeholders::error,
00131 boost::asio::placeholders::bytes_transferred));
00132 }
00133
00134 void MAVConnSerial::async_read_end(error_code error, size_t bytes_transferred)
00135 {
00136 mavlink_message_t message;
00137 mavlink_status_t status;
00138
00139 if (error) {
00140 logError(PFXd "receive: %s", channel, error.message().c_str());
00141 close();
00142 return;
00143 }
00144
00145 iostat_rx_add(bytes_transferred);
00146 for (size_t i = 0; i < bytes_transferred; i++) {
00147 if (mavlink_parse_char(channel, rx_buf[i], &message, &status)) {
00148 logDebug(PFXd "recv: Message-Id: %d [%d bytes] Sys-Id: %d Comp-Id: %d Seq: %d",
00149 channel, message.msgid, message.len, message.sysid, message.compid, message.seq);
00150
00151 message_received(&message, message.sysid, message.compid);
00152 }
00153 }
00154
00155 do_read();
00156 }
00157
00158 void MAVConnSerial::do_write(bool check_tx_state)
00159 {
00160 if (check_tx_state && tx_in_progress)
00161 return;
00162
00163 lock_guard lock(mutex);
00164 if (tx_q.empty())
00165 return;
00166
00167 tx_in_progress = true;
00168 MsgBuffer *buf = tx_q.front();
00169 serial_dev.async_write_some(
00170 buffer(buf->dpos(), buf->nbytes()),
00171 boost::bind(&MAVConnSerial::async_write_end,
00172 this,
00173 boost::asio::placeholders::error,
00174 boost::asio::placeholders::bytes_transferred));
00175 }
00176
00177 void MAVConnSerial::async_write_end(error_code error, size_t bytes_transferred)
00178 {
00179 if (error) {
00180 logError(PFXd "write: %s", channel, error.message().c_str());
00181 close();
00182 return;
00183 }
00184
00185 iostat_tx_add(bytes_transferred);
00186 lock_guard lock(mutex);
00187 if (tx_q.empty()) {
00188 tx_in_progress = false;
00189 return;
00190 }
00191
00192 MsgBuffer *buf = tx_q.front();
00193 buf->pos += bytes_transferred;
00194 if (buf->nbytes() == 0) {
00195 tx_q.pop_front();
00196 delete buf;
00197 }
00198
00199 if (!tx_q.empty())
00200 do_write(false);
00201 else
00202 tx_in_progress = false;
00203 }
00204
00205 };