mavconn: Reuse tx buffer (resize by extents)

Part of #38.
mavlink · Jul 11, 2014 · 7652b4c · 7652b4c
1 parent d17346a
commit 7652b4c
Show file tree

Hide file tree

Showing 4 changed files with 55 additions and 13 deletions.
diff --git a/include/mavros/mavconn_serial.h b/include/mavros/mavconn_serial.h
@@ -62,8 +62,12 @@ class MAVConnSerial : public MAVConnInterface {
 	static constexpr size_t RX_BUFSIZE = MAVLINK_MAX_PACKET_LEN;
 	uint8_t rx_buf[RX_BUFSIZE];
 	std::vector<uint8_t> tx_q;
+	static constexpr size_t TX_EXTENT = 256;	//!< extent size for tx buffer
+	static constexpr size_t TX_DELSIZE = 4096;	//!< buffer delete condition
 	boost::shared_array<uint8_t> tx_buf;
-	size_t tx_buf_size;
+	size_t tx_buf_size;				//!< size of current buffer()
+	size_t tx_buf_max_size;				//!< allocated buffer size
+	bool tx_in_process;				//!< tx status
 	boost::recursive_mutex mutex;
 
 	void do_read(void);

diff --git a/include/mavros/mavconn_udp.h b/include/mavros/mavconn_udp.h
@@ -67,8 +67,12 @@ class MAVConnUDP : public MAVConnInterface {
 	static constexpr size_t RX_BUFSIZE = MAVLINK_MAX_PACKET_LEN;
 	uint8_t rx_buf[RX_BUFSIZE];
 	std::vector<uint8_t> tx_q;
+	static constexpr size_t TX_EXTENT = 256;	//!< extent size for tx buffer
+	static constexpr size_t TX_DELSIZE = 4096;	//!< buffer delete condition
 	boost::shared_array<uint8_t> tx_buf;
-	size_t tx_buf_size;
+	size_t tx_buf_size;				//!< size of current buffer()
+	size_t tx_buf_max_size;				//!< allocated buffer size
+	bool tx_in_process;				//!< tx status
 	boost::recursive_mutex mutex;
 	bool sender_exists;
 

diff --git a/src/mavconn/mavconn_serial.cpp b/src/mavconn/mavconn_serial.cpp
@@ -32,7 +32,10 @@ MAVConnSerial::MAVConnSerial(uint8_t system_id, uint8_t component_id,
 		std::string device, unsigned baudrate) :
 	MAVConnInterface(system_id, component_id),
 	io_service(),
-	serial_dev(io_service, device)
+	serial_dev(io_service, device),
+	tx_buf_size(0),
+	tx_buf_max_size(0),
+	tx_in_process(false)
 {
 	serial_dev.set_option(asio::serial_port_base::baud_rate(baudrate));
 	serial_dev.set_option(asio::serial_port_base::character_size(8));
@@ -43,7 +46,7 @@ MAVConnSerial::MAVConnSerial(uint8_t system_id, uint8_t component_id,
 	ROS_INFO_STREAM_NAMED("mavconn", "serial: device: " << device << " @ " << baudrate << " bps");
 
 	// reserve some space in tx queue
-	tx_q.reserve(RX_BUFSIZE);
+	tx_q.reserve(TX_EXTENT * 2);
 
 	// give some work to io_service before start
 	io_service.post(boost::bind(&MAVConnSerial::do_read, this));
@@ -132,8 +135,22 @@ void MAVConnSerial::async_read_end(boost::system::error_code error, size_t bytes
 
 void MAVConnSerial::copy_and_async_write(void)
 {
+	// should called with locked mutex from io_service thread
+
 	tx_buf_size = tx_q.size();
-	tx_buf.reset(new uint8_t[tx_buf_size]);
+	// mark transmission in progress
+	tx_in_process = true;
+
+	if (tx_buf_max_size > TX_DELSIZE ||
+			tx_buf_size >= tx_buf_max_size) {
+
+		// Set buff eq. or gt than tx_buf_size
+		tx_buf_max_size = (tx_buf_size % TX_EXTENT == 0)? tx_buf_size :
+			(tx_buf_size / TX_EXTENT + 1) * TX_EXTENT;
+
+		tx_buf.reset(new uint8_t[tx_buf_max_size]);
+	}
+
 	std::copy(tx_q.begin(), tx_q.end(), tx_buf.get());
 	tx_q.clear();
 
@@ -147,7 +164,7 @@ void MAVConnSerial::copy_and_async_write(void)
 void MAVConnSerial::do_write(void)
 {
 	// if write not in progress
-	if (tx_buf == 0) {
+	if (!tx_in_process) {
 		boost::recursive_mutex::scoped_lock lock(mutex);
 		copy_and_async_write();
 	}
@@ -159,7 +176,7 @@ void MAVConnSerial::async_write_end(boost::system::error_code error)
 		boost::recursive_mutex::scoped_lock lock(mutex);
 
 		if (tx_q.empty()) {
-			tx_buf.reset();
+			tx_in_process = false;
 			tx_buf_size = 0;
 			return;
 		}

diff --git a/src/mavconn/mavconn_udp.cpp b/src/mavconn/mavconn_udp.cpp
@@ -36,7 +36,10 @@ MAVConnUDP::MAVConnUDP(uint8_t system_id, uint8_t component_id,
 	io_service(),
 	io_work(new asio::io_service::work(io_service)),
 	socket(io_service),
-	sender_exists(false)
+	sender_exists(false),
+	tx_buf_size(0),
+	tx_buf_max_size(0),
+	tx_in_process(false)
 {
 	udp::resolver resolver(io_service);
 
@@ -68,7 +71,7 @@ MAVConnUDP::MAVConnUDP(uint8_t system_id, uint8_t component_id,
 	socket.bind(server_endpoint);
 
 	// reserve some space in tx queue
-	tx_q.reserve(RX_BUFSIZE);
+	tx_q.reserve(TX_EXTENT * 2);
 
 	// give some work to io_service before start
 	io_service.post(boost::bind(&MAVConnUDP::do_read, this));
@@ -165,8 +168,22 @@ void MAVConnUDP::async_read_end(boost::system::error_code error, size_t bytes_tr
 
 void MAVConnUDP::copy_and_async_write(void)
 {
+	// should called with locked mutex from io_service thread
+
 	tx_buf_size = tx_q.size();
-	tx_buf.reset(new uint8_t[tx_buf_size]);
+	// mark transmission in progress
+	tx_in_process = true;
+
+	if (tx_buf_max_size > TX_DELSIZE ||
+			tx_buf_size >= tx_buf_max_size) {
+
+		// Set buff eq. or gt than tx_buf_size
+		tx_buf_max_size = (tx_buf_size % TX_EXTENT == 0)? tx_buf_size :
+			(tx_buf_size / TX_EXTENT + 1) * TX_EXTENT;
+
+		tx_buf.reset(new uint8_t[tx_buf_max_size]);
+	}
+
 	std::copy(tx_q.begin(), tx_q.end(), tx_buf.get());
 	tx_q.clear();
 
@@ -181,12 +198,12 @@ void MAVConnUDP::copy_and_async_write(void)
 void MAVConnUDP::do_write(void)
 {
 	if (!sender_exists) {
-		ROS_DEBUG_THROTTLE_NAMED(10, "mavconn", "udp::do_write: sender do not exists!");
+		ROS_DEBUG_THROTTLE_NAMED(30, "mavconn", "udp::do_write: sender do not exists!");
 		return;
 	}
 
 	// if write not in progress
-	if (tx_buf == 0) {
+	if (!tx_in_process) {
 		boost::recursive_mutex::scoped_lock lock(mutex);
 		copy_and_async_write();
 	}
@@ -198,7 +215,7 @@ void MAVConnUDP::async_write_end(boost::system::error_code error)
 		boost::recursive_mutex::scoped_lock lock(mutex);
 
 		if (tx_q.empty()) {
-			tx_buf.reset();
+			tx_in_process = false;
 			tx_buf_size = 0;
 			return;
 		}