Syslink Bridge to Mavlink (#5479)

ROMFS/px4fmu_common/init.d/rcS

@@ -525,6 +525,7 @@ then
 		if param compare SYS_COMPANION 20
 		then
 			syslink start
+			mavlink start -d /dev/bridge0 -b 57600 -m osd -r 40000
 		fi
 		if param compare SYS_COMPANION 921600
 		then

src/drivers/boards/crazyflie/board_config.h

@@ -85,6 +85,8 @@ __BEGIN_DECLS
 #define GPIO_DRDY_MPU9250		(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTC|GPIO_PIN13)
 
 
+#define GPIO_NRF_TXEN			(GPIO_INPUT|GPIO_PULLUP|GPIO_EXTI|GPIO_PORTA|GPIO_PIN4)
+
 
 /*
  * I2C busses

src/modules/mavlink/mavlink_main.cpp

@@ -986,7 +986,7 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
 		unsigned buf_free = get_free_tx_buf();
 
 		if (buf_free < packet_len) {
-			/* no enough space in buffer to send */
+			/* not enough space in buffer to send */
 			count_txerr();
 			count_txerrbytes(packet_len);
 			pthread_mutex_unlock(&_send_mutex);

src/modules/syslink/CMakeLists.txt

@@ -36,6 +36,7 @@ px4_add_module(
 	STACK_MAIN 2000
 	SRCS
 		syslink_main.cpp
+		syslink_bridge.cpp
 		syslink_params.c
 		syslink.c
 	DEPENDS

src/modules/syslink/crtp.h

@@ -50,9 +50,12 @@
 
 #define CRTP_NULL(x) (((x).header & 0xf3) == 0xf3)
 
-#define CRTP_MAX_DATA_SIZE 29
+// 1 byte header + 31 bytes data = 32 (max ESB packet size)
+// With the NRF51, this could be increased to ~250, but the Crazyradio PA uses a NRF24 which can't do this
+#define CRTP_MAX_DATA_SIZE 31
 
 typedef struct {
+	uint8_t size; // Total size of this message, including the header (placed here to overlap with syslink length field)
 	union {
 		uint8_t header;
 		struct {

src/modules/syslink/syslink.h

@@ -56,6 +56,7 @@ extern const char *syslink_magic;
 #define SYSLINK_OW_READ	0x22
 #define SYSLINK_OW_WRITE	0x23
 
+// Limited by the CRTP packet which is limited by the ESB protocol used by the NRF
 #define SYSLINK_MAX_DATA_LEN	32
 
 #define SYSLINK_RADIO_RATE_250K 0

src/modules/syslink/syslink_bridge.cpp

@@ -0,0 +1,165 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+
+/**
+ * @file syslink_bridge.cpp
+ *
+ * Character device for talking to the radio as a plain serial port
+ */
+
+#include "syslink_main.h"
+
+
+
+SyslinkBridge::SyslinkBridge(Syslink *link) :
+	CDev("SyslinkBridge", "/dev/bridge0"),
+	_link(link),
+	_readbuffer(16, sizeof(crtp_message_t))
+{
+
+
+}
+
+SyslinkBridge::~SyslinkBridge()
+{
+
+}
+
+
+int
+SyslinkBridge::init()
+{
+	int ret = CDev::init();
+
+	/* if init failed, bail now */
+	if (ret != OK) {
+		DEVICE_DEBUG("CDev init failed");
+		return ret;
+	}
+
+
+	return ret;
+}
+
+pollevent_t
+SyslinkBridge::poll_state(struct file *filp)
+{
+	pollevent_t state = 0;
+
+	if (!_readbuffer.empty()) {
+		state |= POLLIN;
+	}
+
+	if (_link->_writebuffer.space() > 0) {
+		state |= POLLOUT;
+	}
+
+	return 0;
+}
+
+#define MIN(X, Y) (((X) < (Y)) ? (X) : (Y))
+
+ssize_t
+SyslinkBridge::read(struct file *filp, char *buffer, size_t buflen)
+{
+	int nread = 0;
+	crtp_message_t msg;
+
+	while (!_readbuffer.empty() && buflen >= sizeof(CRTP_MAX_DATA_SIZE)) {
+		_readbuffer.get(&msg, sizeof(msg));
+		int size = msg.size - sizeof(msg.header);
+		memcpy(buffer, &msg.data, size);
+
+		nread += size;
+		buffer += size;
+		buflen -= size;
+	}
+
+	return nread;
+}
+
+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;
+
+	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));
+
+		buffer += datasize;
+		remaining -= datasize;
+	}
+
+	return buflen;
+}
+
+
+
+int
+SyslinkBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	// All termios commands should be silently ignored as they are handled
+
+	switch (cmd) {
+#ifdef FIONSPACE
+
+	case FIONSPACE:
+#else
+	case FIONWRITE:
+#endif
+		*((int *) arg) = _link->_writebuffer.space() * CRTP_MAX_DATA_SIZE;
+		return 0;
+
+	default:
+		/* give it to the superclass */
+		CDev::ioctl(filp, cmd, arg);
+		return 0;
+	}
+}
+
+
+void
+SyslinkBridge::pipe_message(crtp_message_t *msg)
+{
+	_readbuffer.force(msg, sizeof(msg->size) + msg->size);
+	poll_notify(POLLIN);
+}

src/modules/syslink/syslink_main.cpp

@@ -70,6 +70,7 @@ Syslink::Syslink() :
 	_syslink_task(-1),
 	_task_running(false),
 	_fd(0),
+	_writebuffer(16, sizeof(crtp_message_t)),
 	_battery_pub(nullptr),
 	_rc_pub(nullptr),
 	_cmd_pub(nullptr),
@@ -87,7 +88,7 @@ Syslink::start()
 				"syslink",
 				SCHED_DEFAULT,
 				SCHED_PRIORITY_DEFAULT,
-				3600,
+				1500,
 				Syslink::task_main_trampoline,
 				NULL
 			);
@@ -127,6 +128,27 @@ Syslink::set_address(uint64_t addr)
 }
 
 
+int count_out = 0;
+
+int
+Syslink::send_queued_raw_message()
+{
+	if (_writebuffer.empty()) {
+		return 0;
+	}
+
+	syslink_message_t msg;
+	msg.type = SYSLINK_RADIO_RAW;
+
+	count_out++;
+
+	_writebuffer.get(&msg.length, sizeof(crtp_message_t));
+
+	return send_message(&msg);
+}
+
+
+
 // 1M 8N1 serial connection to NRF51
 int
 Syslink::open_serial(const char *dev)
@@ -154,7 +176,6 @@ Syslink::open_serial(const char *dev)
 	config.c_oflag &= ~ONLCR;
 
 	// Disable hardware flow control
-	// TODO: CF2 uses a TX EN as output flow control
 	config.c_cflag &= ~CRTSCTS;
 
 
@@ -199,7 +220,8 @@ Syslink::task_main()
 	param_get(_param_radio_addr1, &addr + 4);
 	param_get(_param_radio_addr2, &addr);
 
-
+	_bridge = new SyslinkBridge(this);
+	_bridge->init();
 
 	_battery.reset(&_battery_status);
 
@@ -217,9 +239,12 @@ Syslink::task_main()
 
 
 	/* Set non-blocking */
+	/*
 	int flags = fcntl(_fd, F_GETFL, 0);
 	fcntl(_fd, F_SETFL, flags | O_NONBLOCK);
+	*/
 
+	px4_arch_configgpio(GPIO_NRF_TXEN);
 
 	set_datarate(rate);
 	usleep(1000);
@@ -283,6 +308,8 @@ Syslink::task_main()
 
 static int count = 0;
 static int null_count = 0;
+static int count_in = 0;
+//static int count_out = 0;
 static hrt_abstime lasttime = 0;
 
 
@@ -292,10 +319,12 @@ Syslink::handle_message(syslink_message_t *msg)
 	hrt_abstime t = hrt_absolute_time();
 
 	if (t - lasttime > 1000000) {
-		PX4_INFO("%d packets per second (%d null)", count, null_count);
+		PX4_INFO("%d p/s (%d null) (%d in / %d out raw)", count, null_count, count_in, count_out);
 		lasttime = t;
 		count = 0;
 		null_count = 0;
+		count_in = 0;
+		count_out = 0;
 	}
 
 	count++;
@@ -313,7 +342,7 @@ Syslink::handle_message(syslink_message_t *msg)
 		float vbat;
 		memcpy(&vbat, &msg->data[1], sizeof(float));
 
-		_battery.updateBatteryStatus(t, vbat, 0, 0, false, &_battery_status);
+		_battery.updateBatteryStatus(t, vbat, -1, 0, false, &_battery_status);
 
 		// announce the battery status if needed, just publish else
 		if (_battery_pub != nullptr) {
@@ -335,6 +364,9 @@ Syslink::handle_message(syslink_message_t *msg)
 
 	} else if (msg->type == SYSLINK_RADIO_RAW) {
 		handle_raw(msg);
+
+	} else {
+		PX4_INFO("GOT %d", msg->type);
 	}
 
 
@@ -343,17 +375,14 @@ Syslink::handle_message(syslink_message_t *msg)
 void
 Syslink::handle_raw(syslink_message_t *sys)
 {
-	// TODO: Now allow one raw message to be sent from the queue
+	crtp_message_t *c = (crtp_message_t *) &sys->length;
-
-	crtp_message_t *c = (crtp_message_t *) &sys->data;
 
 	if (CRTP_NULL(*c)) {
+		// TODO: Handle bootloader messages if possible
+
 		null_count++;
-		return;
-	}
 
-
+	} else if (c->port == CRTP_PORT_COMMANDER) {
-	if (c->port == CRTP_PORT_COMMANDER) {
 
 		crtp_commander *cmd = (crtp_commander *) &c->data[0];
 
@@ -388,28 +417,136 @@ Syslink::handle_raw(syslink_message_t *sys)
 		}
 
 	} else if (c->port == CRTP_PORT_MAVLINK) {
+		count_in++;
 		/* Pipe to Mavlink bridge */
+		_bridge->pipe_message(c);
+
+	} else {
+		handle_raw_other(sys);
 	}
 
+	// Allow one raw message to be sent from the queue
+	send_queued_raw_message();
+}
+
+
+void
+Syslink::handle_raw_other(syslink_message_t *sys)
+{
+	// This function doesn't actually do anything
+	// It is just here to return null responses to most standard messages
+
+	crtp_message_t *c = (crtp_message_t *) &sys->length;
+
+	if (c->port == CRTP_PORT_LOG) {
+
+		PX4_INFO("Log: %d %d", c->channel, c->data[0]);
+
+		if (c->channel == 0) { // Table of Contents Access
+
+			uint8_t cmd = c->data[0];
+
+			if (cmd == 0) { // GET_ITEM
+				//int id = c->data[1];
+				memset(&c->data[2], 0, 3);
+				c->data[2] = 1; // type
+				c->size = 1 + 5;
+				send_message(sys);
+
+			} else if (cmd == 1) { // GET_INFO
+				memset(&c->data[1], 0, 7);
+				c->size = 1 + 8;
+				send_message(sys);
+			}
+
+		} else if (c->channel == 1) { // Log control
+
+			uint8_t cmd = c->data[0];
+
+			PX4_INFO("Responding to cmd: %d", cmd);
+			c->data[2] = 0; // Success
+			c->size = 3 + 1;
+
+			// resend message
+			send_message(sys);
+
+		} else if (c->channel == 2) { // Log data
+
+		}
+	} else if (c->port == CRTP_PORT_MEM) {
+		if (c->channel == 0) { // Info
+			int cmd = c->data[0];
+
+			if (cmd == 1) { // GET_NBR_OF_MEMS
+				c->data[1] = 0;
+				c->size = 2 + 1;
+
+				// resend message
+				send_message(sys);
+			}
+		}
+
+	} else if (c->port == CRTP_PORT_PARAM) {
+		if (c->channel == 0) { // TOC Access
+			//	uint8_t msgId = c->data[0];
+
+			c->data[1] = 0; // Last parameter (id = 0)
+			memset(&c->data[2], 0, 10);
+			c->size = 1 + 8;
+			send_message(sys);
+		}
+
+		else if (c->channel == 1) { // Param read
+			// 0 is ok
+			c->data[1] = 0; // value
+			c->size = 1 + 3;
+			send_message(sys);
+		}
+
+	} else {
+		PX4_INFO("Got raw: %d", c->port);
+	}
+}
+
+int
+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 (int i = 0; i < len; i++) {
+		// Block until we can send a byte
+		while (px4_arch_gpioread(GPIO_NRF_TXEN)) ;
+
+		write(_fd, ((const char *)data) + i, 1);
+	}
+
+	return 0;
 }
 
 int
 Syslink::send_message(syslink_message_t *msg)
 {
 	syslink_compute_cksum(msg);
-	write(_fd, syslink_magic, 2);
+	send_bytes(syslink_magic, 2);
-	write(_fd, &msg->type, sizeof(msg->type));
+	send_bytes(&msg->type, sizeof(msg->type));
-	write(_fd, &msg->length, sizeof(msg->length));
+	send_bytes(&msg->length, sizeof(msg->length));
-	write(_fd, &msg->data, msg->length);
+	send_bytes(&msg->data, msg->length);
-	write(_fd, &msg->cksum, sizeof(msg->cksum));
+	send_bytes(&msg->cksum, sizeof(msg->cksum));
 	return 0;
 }
 
+
+
 int syslink_main(int argc, char *argv[])
 {
-	PX4_INFO("Starting daemon...");
 	g_syslink = new Syslink();
 	g_syslink->start();
 
+	// Wait for task and bridge to start
+	usleep(5000);
+
+	PX4_INFO("Started syslink on /dev/ttyS2");
+
 	return 0;
 }

src/modules/syslink/syslink_main.h

@@ -32,9 +32,18 @@
  ****************************************************************************/
 
 #include <stdint.h>
+
 #include <systemlib/battery.h>
+
+#include <drivers/device/device.h>
+#include <drivers/device/ringbuffer.h>
+
 #include <uORB/uORB.h>
+
 #include "syslink.h"
+#include "crtp.h"
+
+class SyslinkBridge;
 
 class Syslink
 {
@@ -50,19 +59,35 @@ public:
 
 private:
 
+	friend class SyslinkBridge;
+
 	int open_serial(const char *dev);
 
 	void handle_message(syslink_message_t *msg);
 	void handle_raw(syslink_message_t *sys);
 
+	// Handles other types of messages that we don't really care about, but
+	// will be maintained with the bare minimum implementation for supporting
+	// other crazyflie libraries
+	void handle_raw_other(syslink_message_t *sys);
+
+	int send_bytes(const void *data, size_t len);
+
 	// Checksums and transmits a syslink message
 	int send_message(syslink_message_t *msg);
 
+	int send_queued_raw_message();
+
+
 	int _syslink_task;
 	bool _task_running;
 
 	int _fd;
 
+	// Stores data that was needs to be written as a raw message
+	ringbuffer::RingBuffer _writebuffer;
+	SyslinkBridge *_bridge;
+
 	orb_advert_t _battery_pub;
 	orb_advert_t _rc_pub;
 	orb_advert_t _cmd_pub;
@@ -78,3 +103,34 @@ private:
 	void task_main();
 
 };
+
+
+class SyslinkBridge : public device::CDev
+{
+
+public:
+	SyslinkBridge(Syslink *link);
+	virtual ~SyslinkBridge();
+
+	virtual int	init();
+
+	virtual ssize_t	read(struct file *filp, char *buffer, size_t buflen);
+	virtual ssize_t	write(struct file *filp, const char *buffer, size_t buflen);
+	virtual int	ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	// Makes the message available for reading to processes reading from the bridge
+	void pipe_message(crtp_message_t *msg);
+
+protected:
+
+	virtual pollevent_t poll_state(struct file *filp);
+
+private:
+
+	Syslink *_link;
+
+	// Stores data that was received from syslink but not yet read by another driver
+	ringbuffer::RingBuffer _readbuffer;
+
+
+};

src/modules/syslink/syslink_params.c

@@ -59,7 +59,7 @@ PARAM_DEFINE_INT32(SLNK_RADIO_CHAN, 80);
  * @max 2
  * @group Syslink
  */
-PARAM_DEFINE_INT32(SLNK_RADIO_RATE, 0);
+PARAM_DEFINE_INT32(SLNK_RADIO_RATE, 2);
 
 /**
  * Operating address of the NRF51 (most significant byte)