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.

platforms/nuttx/nuttx-configs/crazyflie/nsh/defconfig

@@ -931,7 +931,7 @@ CONFIG_DEV_FIFO_SIZE=1024
 # CONFIG_POWER is not set
 # CONFIG_SENSORS is not set
 CONFIG_SERIAL=y
-# CONFIG_DEV_LOWCONSOLE is not set
+CONFIG_DEV_LOWCONSOLE=y
 CONFIG_SERIAL_REMOVABLE=y
 # CONFIG_SERIAL_CONSOLE is not set
 # CONFIG_16550_UART is not set

src/drivers/boards/crazyflie/board_config.h

@@ -168,7 +168,7 @@
 #define PX4_PWM_ALTERNATE_RANGES
 #define PWM_LOWEST_MIN 0
 #define PWM_MOTOR_OFF	0
-#define PWM_DEFAULT_MIN 0
+#define PWM_DEFAULT_MIN 20
 #define PWM_HIGHEST_MIN 0
 #define PWM_HIGHEST_MAX 255
 #define PWM_DEFAULT_MAX 255

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
+	while (buflen_rem > 0) {
-	int remaining = buflen;
 
-	while (remaining > 0) {
+		int datasize = MIN(_msg_to_send_size_remaining, buflen_rem);
-		int datasize = MIN(remaining, CRTP_MAX_DATA_SIZE);
+		_msg_to_send.size += datasize;
-		msg.size = datasize + sizeof(msg.header);
+		memcpy(&_msg_to_send.data[CRTP_MAX_DATA_SIZE - 1 - _msg_to_send_size_remaining], buffer, datasize);
-		msg.port = CRTP_PORT_MAVLINK;
-		memcpy(&msg.data, buffer, datasize);
-
-		_link->_writebuffer.force(&msg, sizeof(crtp_message_t));
 
 		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;
 
 };