From 7ac3c97aad041bd38b601db33d0f905a91878014 Mon Sep 17 00:00:00 2001
From: barzanisar <nisarb@student.ethz.ch>
Date: Thu, 22 Feb 2018 17:41:02 +0100
Subject: [PATCH] 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.
---
.../nuttx-configs/crazyflie/nsh/defconfig | 2 +-
src/drivers/boards/crazyflie/board_config.h | 2 +-
src/modules/syslink/syslink_bridge.cpp | 35 ++++++++++++-------
src/modules/syslink/syslink_main.cpp | 15 ++++----
src/modules/syslink/syslink_main.h | 2 ++
5 files changed, 34 insertions(+), 22 deletions(-)
diff --git a/platforms/nuttx/nuttx-configs/crazyflie/nsh/defconfig b/platforms/nuttx/nuttx-configs/crazyflie/nsh/defconfig
index 4a3ac530c5..c30fa72cf7 100644
--- a/platforms/nuttx/nuttx-configs/crazyflie/nsh/defconfig
+++ b/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
diff --git a/src/drivers/boards/crazyflie/board_config.h b/src/drivers/boards/crazyflie/board_config.h
index 671e420af4..2000b7e81a 100644
--- a/src/drivers/boards/crazyflie/board_config.h
+++ b/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
diff --git a/src/modules/syslink/syslink_bridge.cpp b/src/modules/syslink/syslink_bridge.cpp
index 2455e2d241..d3300a7355 100644
--- a/src/modules/syslink/syslink_bridge.cpp
+++ b/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;
-
- // Queue and send next time we get a RAW radio packet
- int remaining = buflen;
+ int buflen_rem = buflen;
- 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);
+ while (buflen_rem > 0) {
- _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;
diff --git a/src/modules/syslink/syslink_main.cpp b/src/modules/syslink/syslink_main.cpp
index 4f859cbc91..69b6a41fa5 100644
--- a/src/modules/syslink/syslink_main.cpp
+++ b/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)) ;
diff --git a/src/modules/syslink/syslink_main.h b/src/modules/syslink/syslink_main.h
index d78fa2e1e8..b335396dd5 100644
--- a/src/modules/syslink/syslink_main.h
+++ b/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;
};
--
GitLab