Syslink for battery, RSSI, and initial radio control

ROMFS/px4fmu_common/init.d/rcS

@@ -518,6 +518,10 @@ then
 		then
 			frsky_telemetry start -d /dev/ttyS2
 		fi
+		if param compare SYS_COMPANION 20
+		then
+			syslink start
+		fi
 		if param compare SYS_COMPANION 921600
 		then
 			mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 80000 -x

cmake/configs/nuttx_crazyflie_default.cmake

@@ -44,6 +44,7 @@ set(config_module_list
 	modules/land_detector
 
 	modules/dummy
+	modules/syslink
 
 	#
 	# Estimation modules (EKF/ SO3 / other filters)

nuttx-configs/crazyflie/nsh/defconfig

@@ -386,7 +386,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=42
+CONFIG_NFILE_DESCRIPTORS=51
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4

src/modules/syslink/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+px4_add_module(
+	MODULE modules__syslink
+	MAIN syslink
+	STACK_MAIN 2000
+	SRCS
+		syslink_main.cpp
+		syslink_params.c
+		syslink.c
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :

src/modules/syslink/crtp.h

@@ -0,0 +1,74 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+
+#include <stdint.h>
+
+#define CRTP_PORT_CONSOLE 0x00
+#define CRTP_PORT_PARAM	0x02
+#define CRTP_PORT_COMMANDER	0x03
+#define CRTP_PORT_MEM 0x04
+#define CRTP_PORT_LOG 0x05
+
+#define CRTP_PORT_MAVLINK 0x08 // Non-standard port for transmitting mavlink messages
+
+#define CRTP_PORT_PLATFORM 0x0D
+#define CRTP_PORT_DEBUG 0x0E
+#define CRTP_PORT_LINK 0x0F
+
+#define CRTP_NULL(x) (((x).header & 0xf3) == 0xf3)
+
+#define CRTP_MAX_DATA_SIZE 29
+
+typedef struct {
+	union {
+		uint8_t header;
+		struct {
+			uint8_t channel : 2;
+			uint8_t link : 2;
+			uint8_t port : 4;
+		};
+	};
+
+	uint8_t data[CRTP_MAX_DATA_SIZE];
+} __attribute__((packed)) crtp_message_t;
+
+
+typedef struct {
+	float roll; // -20 to 20
+	float pitch;
+	float yaw; // -150 to 150
+	uint16_t thrust;
+} crtp_commander;

src/modules/syslink/syslink.c

@@ -0,0 +1,153 @@
+/****************************************************************************
+ *
+ *   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.c
+ *
+ * Crazyflie Syslink protocol implementation
+ *
+ * @author Dennis Shtatnov <densht@gmail.com>
+ */
+
+
+#include <px4_defines.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <sys/ioctl.h>
+#include <systemlib/err.h>
+#include <poll.h>
+#include <termios.h>
+
+
+#include "syslink.h"
+
+
+const char *syslink_magic = "\xbc\xcf";
+
+void syslink_parse_init(syslink_parse_state *state)
+{
+	state->state = SYSLINK_STATE_START;
+	state->index = 0;
+}
+
+
+int syslink_parse_char(syslink_parse_state *state, char c, syslink_message_t *msg)
+{
+
+	switch (state->state) {
+	case SYSLINK_STATE_START:
+		if (c == syslink_magic[state->index]) {
+			state->index++;
+
+		} else {
+			state->index = 0;
+		}
+
+		if (syslink_magic[state->index] == '\x00') {
+			state->state = SYSLINK_STATE_TYPE;
+		}
+
+		break;
+
+	case SYSLINK_STATE_TYPE:
+		msg->type = c;
+		state->state = SYSLINK_STATE_LENGTH;
+		break;
+
+	case SYSLINK_STATE_LENGTH:
+		msg->length = c;
+
+		if (c > SYSLINK_MAX_DATA_LEN) { // Too long
+			state->state = SYSLINK_STATE_START;
+
+		} else {
+			state->state = c > 0 ? SYSLINK_STATE_DATA : SYSLINK_STATE_CKSUM;
+		}
+
+		state->index = 0;
+		break;
+
+	case SYSLINK_STATE_DATA:
+		msg->data[state->index++] = c;
+
+		if (state->index >= msg->length) {
+			state->state = SYSLINK_STATE_CKSUM;
+			state->index = 0;
+			syslink_compute_cksum(msg);
+		}
+
+		break;
+
+	case SYSLINK_STATE_CKSUM:
+		if (c != msg->cksum[state->index]) {
+			PX4_INFO("Bad checksum");
+			state->state = SYSLINK_STATE_START;
+			state->index = 0;
+			break;
+		}
+
+		state->index++;
+
+		if (state->index >= sizeof(msg->cksum)) {
+			state->state = SYSLINK_STATE_START;
+			state->index = 0;
+			return 1;
+		}
+
+
+		break;
+	}
+
+	return 0;
+
+}
+
+/*
+	Computes Fletcher 8bit checksum per RFC1146
+	A := A + D[i]
+	B := B + A
+*/
+void syslink_compute_cksum(syslink_message_t *msg)
+{
+	uint8_t a = 0, b = 0;
+	uint8_t *Di = (uint8_t *)msg, *end = Di + (2 + msg->length) * sizeof(uint8_t);
+
+	while (Di < end) {
+		a = a + *Di;
+		b = b + a;
+		++Di;
+	}
+
+	msg->cksum[0] = a;
+	msg->cksum[1] = b;
+}

src/modules/syslink/syslink.h

@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+
+#include <stdint.h>
+
+
+extern const char *syslink_magic;
+
+
+#define SYSLINK_RADIO_RAW	0x00
+#define SYSLINK_RADIO_CHANNEL	0x01
+#define SYSLINK_RADIO_DATARATE	0x02
+#define SYSLINK_RADIO_CONTWAVE	0x03
+#define SYSLINK_RADIO_RSSI	0x04
+#define SYSLINK_RADIO_ADDRESS	0x05
+
+#define SYSLINK_PM_SOURCE	0x10
+#define SYSLINK_PM_ONOFF_SWITCHOFF	0x11
+#define SYSLINK_PM_BATTERY_VOLTAGE	0x12
+#define SYSLINK_PM_BATTERY_STATE	0x13
+#define SYSLINK_PM_BATTERY_AUTOUPDATE	0x14
+
+#define SYSLINK_OW_SCAN	0x20
+#define SYSLINK_OW_GETINFO	0x21
+#define SYSLINK_OW_READ	0x22
+#define SYSLINK_OW_WRITE	0x23
+
+#define SYSLINK_MAX_DATA_LEN	32
+
+#define SYSLINK_RADIO_RATE_250K 0
+#define SYSLINK_RADIO_RATE_1M 1
+#define SYSLINK_RADIO_RATE_2M 2
+
+
+typedef struct {
+	uint8_t type;
+	uint8_t length;
+	uint8_t data[SYSLINK_MAX_DATA_LEN];
+	uint8_t cksum[2];
+} syslink_message_t;
+
+typedef enum {
+	SYSLINK_STATE_START = 0,
+	SYSLINK_STATE_TYPE,
+	SYSLINK_STATE_LENGTH,
+	SYSLINK_STATE_DATA,
+	SYSLINK_STATE_CKSUM
+} SYSLINK_STATE;
+
+typedef struct  {
+	SYSLINK_STATE state;
+	int index;
+
+} syslink_parse_state;
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+extern void syslink_parse_init(syslink_parse_state *state);
+
+// Returns true if a full message was parsed
+extern int syslink_parse_char(syslink_parse_state *state, char c, syslink_message_t *msg);
+
+extern void syslink_compute_cksum(syslink_message_t *msg);
+
+#ifdef __cplusplus
+}
+#endif

src/modules/syslink/syslink_main.cpp

@@ -0,0 +1,411 @@
+/****************************************************************************
+ *
+ *   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_main.cpp
+ * Entry point for syslink module used to communicate with the NRF module on a Crazyflie
+ */
+
+#include <px4_config.h>
+#include <px4_tasks.h>
+#include <px4_posix.h>
+#include <px4_defines.h>
+
+#include <unistd.h>
+#include <stdio.h>
+#include <poll.h>
+#include <string.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <poll.h>
+#include <termios.h>
+
+#include <drivers/drv_rc_input.h>
+
+#include <systemlib/err.h>
+
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/input_rc.h>
+
+#include "crtp.h"
+#include "syslink_main.h"
+
+extern "C" { __EXPORT int syslink_main(int argc, char *argv[]); }
+
+
+Syslink *g_syslink;
+
+Syslink::Syslink() :
+	_syslink_task(-1),
+	_task_running(false),
+	_fd(0),
+	_battery_pub(nullptr),
+	_rc_pub(nullptr),
+	_cmd_pub(nullptr),
+	_rssi(RC_INPUT_RSSI_MAX)
+{
+
+}
+
+
+int
+Syslink::start()
+{
+	_task_running = true;
+	_syslink_task = px4_task_spawn_cmd(
+				"syslink",
+				SCHED_DEFAULT,
+				SCHED_PRIORITY_DEFAULT,
+				3600,
+				Syslink::task_main_trampoline,
+				NULL
+			);
+
+	return 0;
+}
+
+
+int
+Syslink::set_datarate(uint8_t datarate)
+{
+	syslink_message_t msg;
+	msg.type = SYSLINK_RADIO_DATARATE;
+	msg.length = 1;
+	msg.data[0] = datarate;
+	return send_message(&msg);
+}
+
+int
+Syslink::set_channel(uint8_t channel)
+{
+	syslink_message_t msg;
+	msg.type = SYSLINK_RADIO_CHANNEL;
+	msg.length = 1;
+	msg.data[0] = channel;
+	return send_message(&msg);
+}
+
+int
+Syslink::set_address(uint64_t addr)
+{
+	syslink_message_t msg;
+	msg.type = SYSLINK_RADIO_ADDRESS;
+	msg.length = 5;
+	memcpy(&msg.data, &addr, 5);
+	return send_message(&msg);
+}
+
+
+// 1M 8N1 serial connection to NRF51
+int
+Syslink::open_serial(const char *dev)
+{
+	int rate = B1000000;
+
+	// open uart
+	int fd = px4_open(dev, O_RDWR | O_NOCTTY);
+	int termios_state = -1;
+
+	if (fd < 0) {
+		PX4_ERR("failed to open uart device!");
+		return -1;
+	}
+
+	// set baud rate
+	struct termios config;
+	tcgetattr(fd, &config);
+
+	// clear ONLCR flag (which appends a CR for every LF)
+	config.c_oflag &= ~ONLCR;
+
+	// Disable hardware flow control
+	// TODO: CF2 uses a TX EN as output flow control
+	config.c_cflag &= ~CRTSCTS;
+
+
+	/* Set baud rate */
+	if (cfsetispeed(&config, rate) < 0 || cfsetospeed(&config, rate) < 0) {
+		warnx("ERR SET BAUD %s: %d\n", dev, termios_state);
+		px4_close(fd);
+		return -1;
+	}
+
+	if ((termios_state = tcsetattr(fd, TCSANOW, &config)) < 0) {
+		PX4_WARN("ERR SET CONF %s\n", dev);
+		px4_close(fd);
+		return -1;
+	}
+
+	return fd;
+}
+
+
+
+int
+Syslink::task_main_trampoline(int argc, char *argv[])
+{
+	g_syslink->task_main();
+	return 0;
+}
+
+void
+Syslink::task_main()
+{
+	param_t _param_radio_channel = param_find("SLNK_RADIO_CHAN");
+	param_t _param_radio_rate = param_find("SLNK_RADIO_RATE");
+	param_t _param_radio_addr1 = param_find("SLNK_RADIO_ADDR1");
+	param_t _param_radio_addr2 = param_find("SLNK_RADIO_ADDR2");
+
+	uint32_t channel, rate;
+	uint64_t addr = 0;
+
+	param_get(_param_radio_channel, &channel);
+	param_get(_param_radio_rate, &rate);
+	param_get(_param_radio_addr1, &addr + 4);
+	param_get(_param_radio_addr2, &addr);
+
+
+
+	_battery.reset(&_battery_status);
+
+
+	//	int ret;
+
+	/* Open serial port */
+	const char *dev = "/dev/ttyS2";
+	_fd = open_serial(dev);
+
+	if (_fd < 0) {
+		err(1, "can't open %s", dev);
+		return;
+	}
+
+
+	/* Set non-blocking */
+	int flags = fcntl(_fd, F_GETFL, 0);
+	fcntl(_fd, F_SETFL, flags | O_NONBLOCK);
+
+
+	set_datarate(rate);
+	usleep(1000);
+	set_channel(channel);
+	usleep(1000);
+	set_address(addr);
+
+
+	px4_pollfd_struct_t fds[1];
+	fds[0].fd = _fd;
+	fds[0].events = POLLIN;
+
+	int error_counter = 0;
+
+	char buf[64];
+	int nread;
+
+	syslink_parse_state state;
+	syslink_message_t msg;
+
+	syslink_parse_init(&state);
+
+
+	while (_task_running) {
+		/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
+		int poll_ret = px4_poll(fds, 1, 1000);
+
+		/* handle the poll result */
+		if (poll_ret == 0) {
+			/* 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) {
+				/* use a counter to prevent flooding (and slowing us down) */
+				PX4_ERR("[syslink] ERROR return value from poll(): %d"
+					, poll_ret);
+			}
+
+			error_counter++;
+
+		} else {
+			if (fds[0].revents & POLLIN) {
+				if ((nread = read(_fd, buf, sizeof(buf))) < 0) {
+					continue;
+				}
+
+				for (int i = 0; i < nread; i++) {
+					if (syslink_parse_char(&state, buf[i], &msg)) {
+						handle_message(&msg);
+					}
+				}
+			}
+		}
+	}
+
+	close(_fd);
+
+}
+
+
+
+static int count = 0;
+static int null_count = 0;
+static hrt_abstime lasttime = 0;
+
+
+void
+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);
+		lasttime = t;
+		count = 0;
+		null_count = 0;
+	}
+
+	count++;
+
+	if (msg->type == SYSLINK_PM_ONOFF_SWITCHOFF) {
+		// When the power button is hit
+	} else if (msg->type == SYSLINK_PM_BATTERY_STATE) {
+
+		if (msg->length != 9) {
+			return;
+		}
+
+		//uint8_t flags = msg->data[0];
+		//float iset = *((float *)&msg->data[5]);
+		float vbat;
+		memcpy(&vbat, &msg->data[1], sizeof(float));
+
+		_battery.updateBatteryStatus(t, vbat, 0, 0, false, &_battery_status);
+
+		// announce the battery status if needed, just publish else
+		if (_battery_pub != nullptr) {
+			orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
+
+		} else {
+			_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
+		}
+
+	} else if (msg->type == SYSLINK_RADIO_RSSI) {
+		uint8_t rssi = msg->data[0]; // Between 40 and 100 meaning -40 dBm to -100 dBm
+		_rssi = 140 - rssi * 100 / (100 - 40);
+
+	} else if (msg->type == SYSLINK_RADIO_CHANNEL) {
+		PX4_INFO("Channel ACK %d", msg->data[0]);
+
+	} else if (msg->type == SYSLINK_RADIO_DATARATE) {
+		PX4_INFO("Datarate ACK %d", msg->data[0]);
+
+	} else if (msg->type == SYSLINK_RADIO_RAW) {
+		handle_raw(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->data;
+
+	if (CRTP_NULL(*c)) {
+		null_count++;
+		return;
+	}
+
+
+	if (c->port == CRTP_PORT_COMMANDER) {
+
+		crtp_commander *cmd = (crtp_commander *) &c->data[0];
+
+		struct rc_input_values rc = {};
+
+		rc.timestamp_publication = hrt_absolute_time();
+		rc.timestamp_last_signal = rc.timestamp_publication;
+		rc.channel_count = 5;
+		rc.rc_failsafe = false;
+		rc.rc_lost = false;
+		rc.rc_lost_frame_count = 0;
+		rc.rc_total_frame_count = 1;
+		rc.rc_ppm_frame_length = 0;
+		rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK;
+		rc.rssi = _rssi;
+
+
+		double pitch = cmd->pitch, roll = cmd->roll, yaw = cmd->yaw;
+
+		/* channels (scaled to rc limits) */
+		rc.values[0] = pitch * 500 / 20 + 1500;
+		rc.values[1] = roll * 500 / 20 + 1500;
+		rc.values[2] = yaw * 500 / 150 + 1500;
+		rc.values[3] = cmd->thrust * 1000 / USHRT_MAX + 1000;
+		rc.values[4] = 1000; // Dummy channel as px4 needs at least 5
+
+		if (_rc_pub == nullptr) {
+			_rc_pub = orb_advertise(ORB_ID(input_rc), &rc);
+
+		} else {
+			orb_publish(ORB_ID(input_rc), _rc_pub, &rc);
+		}
+
+	} else if (c->port == CRTP_PORT_MAVLINK) {
+		/* Pipe to Mavlink bridge */
+	}
+
+}
+
+int
+Syslink::send_message(syslink_message_t *msg)
+{
+	syslink_compute_cksum(msg);
+	write(_fd, syslink_magic, 2);
+	write(_fd, &msg->type, sizeof(msg->type));
+	write(_fd, &msg->length, sizeof(msg->length));
+	write(_fd, &msg->data, msg->length);
+	write(_fd, &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();
+
+	return 0;
+}

src/modules/syslink/syslink_main.h

@@ -0,0 +1,80 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include <stdint.h>
+#include <systemlib/battery.h>
+#include <uORB/uORB.h>
+#include "syslink.h"
+
+class Syslink
+{
+public:
+	Syslink();
+
+	int start();
+
+	// TODO: These should wait for an ACK
+	int set_datarate(uint8_t datarate);
+	int set_channel(uint8_t channel);
+	int set_address(uint64_t addr);
+
+private:
+
+	int open_serial(const char *dev);
+
+	void handle_message(syslink_message_t *msg);
+	void handle_raw(syslink_message_t *sys);
+
+	// Checksums and transmits a syslink message
+	int send_message(syslink_message_t *msg);
+
+	int _syslink_task;
+	bool _task_running;
+
+	int _fd;
+
+	orb_advert_t _battery_pub;
+	orb_advert_t _rc_pub;
+	orb_advert_t _cmd_pub;
+
+	struct battery_status_s _battery_status;
+
+	Battery _battery;
+
+	int32_t _rssi;
+
+	static int task_main_trampoline(int argc, char *argv[]);
+
+	void task_main();
+
+};

src/modules/syslink/syslink_params.c

@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ *   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_params.c
+ *
+ * Parameters defined by the syslink module and the exposed NRF51 radio
+ *
+ * @author Dennis Shtatnov <densht@gmail.com>
+ */
+
+#include <nuttx/config.h>
+#include <systemlib/param/param.h>
+
+
+/**
+ * Operating channel of the NRF51
+ *
+ * @min 0
+ * @max 125
+ * @group Syslink
+ */
+PARAM_DEFINE_INT32(SLNK_RADIO_CHAN, 80);
+
+/**
+ * Operating datarate of the NRF51
+ *
+ * @min 0
+ * @max 2
+ * @group Syslink
+ */
+PARAM_DEFINE_INT32(SLNK_RADIO_RATE, 0);
+
+/**
+ * Operating address of the NRF51 (most significant byte)
+ *
+ * @group Syslink
+ */
+PARAM_DEFINE_INT32(SLNK_RADIO_ADDR1, (uint32_t) 0xE7);
+
+/**
+ * Operating address of the NRF51 (least significant 4 bytes)
+ *
+ * @group Syslink
+ */
+PARAM_DEFINE_INT32(SLNK_RADIO_ADDR2, (uint32_t) 0xE7E7E7E7);