Improve the Crazyflie MAVLink tunnel to increase efficiency

This change fragments MAVLink packets more efficiently and therefore increases the net throughput. This in turn makes the connection significantly more stable and the Crazyflie experience overall more usable.

src/modules/syslink/syslink_bridge.cpp

@@ -48,8 +48,10 @@ SyslinkBridge::SyslinkBridge(Syslink *link) :
 	_link(link),
 	_readbuffer(16, sizeof(crtp_message_t))
 {
-
-
+	_msg_to_send.header = 0;
+	_msg_to_send.size = sizeof(_msg_to_send.header);
+	_msg_to_send.port = CRTP_PORT_MAVLINK;
+	_msg_to_send_size_remaining = CRTP_MAX_DATA_SIZE - 1;
 }
 
 SyslinkBridge::~SyslinkBridge()
@@ -111,21 +113,28 @@ SyslinkBridge::read(struct file *filp, char *buffer, size_t buflen)
 ssize_t
 SyslinkBridge::write(struct file *filp, const char *buffer, size_t buflen)
 {
-	crtp_message_t msg;
+	int buflen_rem = buflen;
 
-	// Queue and send next time we get a RAW radio packet
-	int remaining = buflen;
+	while (buflen_rem > 0) {
 
-	while (remaining > 0) {
-		int datasize = MIN(remaining, CRTP_MAX_DATA_SIZE);
-		msg.size = datasize + sizeof(msg.header);
-		msg.port = CRTP_PORT_MAVLINK;
-		memcpy(&msg.data, buffer, datasize);
-
-		_link->_writebuffer.force(&msg, sizeof(crtp_message_t));
+		int datasize = MIN(_msg_to_send_size_remaining, buflen_rem);
+		_msg_to_send.size += datasize;
+		memcpy(&_msg_to_send.data[CRTP_MAX_DATA_SIZE - 1 - _msg_to_send_size_remaining], buffer, datasize);
 
 		buffer += datasize;
-		remaining -= datasize;
+		_msg_to_send_size_remaining -= datasize;
+		buflen_rem -= datasize;
+
+
+		if (_msg_to_send_size_remaining == 0) {
+
+			if (_link->_writebuffer.force(&_msg_to_send, sizeof(crtp_message_t))) {
+				PX4_WARN("write buffer overflow");
+			}
+
+			_msg_to_send.size = sizeof(_msg_to_send.header);
+			_msg_to_send_size_remaining = CRTP_MAX_DATA_SIZE - 1;
+		}
 	}
 
 	return buflen;

src/modules/syslink/syslink_main.cpp

@@ -47,7 +47,6 @@
 #include <string.h>
 #include <fcntl.h>
 #include <sys/ioctl.h>
-#include <poll.h>
 #include <termios.h>
 #include <sys/types.h>
 #include <sys/stat.h>
@@ -70,6 +69,7 @@
 #include "drv_deck.h"
 
 
+
 __BEGIN_DECLS
 extern void led_init(void);
 extern void led_on(int led);
@@ -254,7 +254,7 @@ Syslink::open_serial(const char *dev)
 	tcgetattr(fd, &config);
 
 	// clear ONLCR flag (which appends a CR for every LF)
-	config.c_oflag &= 0;
+	config.c_oflag = 0;
 	config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
 
 	// Disable hardware flow control
@@ -336,15 +336,16 @@ Syslink::task_main()
 
 	syslink_parse_init(&state);
 
-	// setup initial parameters
+	//setup initial parameters
 	update_params(true);
 
 	while (_task_running) {
-		int poll_ret = px4_poll(fds, 2, 1000);
+		int poll_ret = px4_poll(fds, 2, 500);
 
 		/* handle the poll result */
 		if (poll_ret == 0) {
-			/* this means none of our providers is giving us data */
+			/* timeout: this means none of our providers is giving us data */
+
 		} else if (poll_ret < 0) {
 			/* this is seriously bad - should be an emergency */
 			if (error_counter < 10 || error_counter % 50 == 0) {
@@ -374,6 +375,7 @@ Syslink::task_main()
 				update_params(false);
 			}
 		}
+
 	}
 
 	close(_fd);
@@ -460,7 +462,7 @@ Syslink::handle_message(syslink_message_t *msg)
 		PX4_INFO("GOT %d", msg->type);
 	}
 
-	// Send queued messages
+	//Send queued messages
 	if (!_queue.empty()) {
 		_queue.get(msg, sizeof(syslink_message_t));
 		send_message(msg);
@@ -706,7 +708,6 @@ Syslink::send_bytes(const void *data, size_t len)
 {
 	// TODO: This could be way more efficient
 	//       Using interrupts/DMA/polling would be much better
-
 	for (size_t i = 0; i < len; i++) {
 		// Block until we can send a byte
 		while (px4_arch_gpioread(GPIO_NRF_TXEN)) ;

src/modules/syslink/syslink_main.h

@@ -181,6 +181,8 @@ private:
 	// Stores data that was received from syslink but not yet read by another driver
 	ringbuffer::RingBuffer _readbuffer;
 
+	crtp_message_t _msg_to_send;
+	int _msg_to_send_size_remaining;
 
 };