move all navio2 drivers to emlid navio2 board directory

boards/emlid/navio2/CMakeLists.txt

@@ -0,0 +1,36 @@
+############################################################################
+#
+#   Copyright (c) 2018 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.
+#
+############################################################################
+
+add_subdirectory(navio_adc)
+add_subdirectory(navio_rgbled)
+add_subdirectory(navio_sysfs_rc_in)

boards/emlid/navio2/cross.cmake

@@ -21,13 +21,8 @@ px4_add_board(
 		#telemetry # all available telemetry drivers
 		vmount
 
-		linux_gpio
 		linux_pwm_out
 		linux_sbus
-		navio_adc
-		navio_rgbled
-		navio_sysfs_rc_in
-		rpi_rc_in
 
 	DF_DRIVERS # NOTE: DriverFramework is migrating to intree PX4 drivers
 		hmc5883

boards/emlid/navio2/native.cmake

@@ -19,13 +19,8 @@ px4_add_board(
 		#telemetry # all available telemetry drivers
 		vmount
 
-		linux_gpio
 		linux_pwm_out
 		linux_sbus
-		navio_adc
-		navio_rgbled
-		navio_sysfs_rc_in
-		rpi_rc_in
 
 	DF_DRIVERS # NOTE: DriverFramework is migrating to intree PX4 drivers
 		hmc5883

boards/emlid/navio2/navio_adc/CMakeLists.txt

@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2015-2017 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 drivers__navio_adc
+	MAIN navio_adc
+	COMPILE_FLAGS
+	SRCS
+		navio_adc.cpp
+	DEPENDS
+	)
+

boards/emlid/navio2/navio_adc/navio_adc.cpp

@@ -0,0 +1,334 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2017 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 navio_adc.cpp
+ *
+ * Navio2 ADC Driver
+ *
+ * This driver exports the sysfs-based ADC driver on Navio2.
+ *
+ * @author Nicolae Rosia <nicolae.rosia@gmail.com>
+ */
+
+#include <px4_config.h>
+#include <px4_tasks.h>
+#include <px4_posix.h>
+#include <drivers/drv_adc.h>
+
+#include <VirtDevObj.hpp>
+
+#include <unistd.h>
+#include <stdio.h>
+#include <poll.h>
+#include <string.h>
+
+#define ADC_BASE_DEV_PATH "/dev/adc"
+#define ADC_SYSFS_PATH "/sys/kernel/rcio/adc/ch0"
+#define ADC_MAX_CHAN 6
+
+/*
+ * ADC Channels:
+ * A0 - Board voltage (5V)
+ * A1 - servo rail voltage
+ * A2 - power module voltage (ADC0, POWER port)
+ * A3 - power module current (ADC1, POWER port)
+ * A4 - ADC2 (ADC port)
+ * A5 - ADC3 (ADC port)
+ */
+
+#define NAVIO_ADC_BATTERY_VOLTAGE_CHANNEL (2)
+#define NAVIO_ADC_BATTERY_CURRENT_CHANNEL (3)
+
+__BEGIN_DECLS
+__EXPORT int navio_adc_main(int argc, char *argv[]);
+__END_DECLS
+
+class NavioADC: public DriverFramework::VirtDevObj
+{
+public:
+	NavioADC();
+	virtual ~NavioADC();
+
+	virtual int init() override;
+
+	virtual ssize_t devRead(void *buf, size_t count) override;
+	virtual int devIOCTL(unsigned long request, unsigned long arg) override;
+
+protected:
+	virtual void _measure() override;
+
+private:
+	int read_channel(px4_adc_msg_t *adc_msg, int channel);
+
+	pthread_mutex_t _samples_lock;
+	int _fd[ADC_MAX_CHAN];
+	px4_adc_msg_t _samples[ADC_MAX_CHAN];
+};
+
+NavioADC::NavioADC()
+	: DriverFramework::VirtDevObj("navio_adc", ADC0_DEVICE_PATH, ADC_BASE_DEV_PATH, 1e6 / 100)
+{
+	pthread_mutex_init(&_samples_lock, NULL);
+
+	for (int i = 0; i < ADC_MAX_CHAN; i++) {
+		_fd[i] = -1;
+	}
+}
+
+NavioADC::~NavioADC()
+{
+	pthread_mutex_destroy(&_samples_lock);
+
+	for (int i = 0; i < ADC_MAX_CHAN; i++) {
+		if (_fd[i] != -1) {
+			close(_fd[i]);
+		}
+	}
+}
+
+void NavioADC::_measure()
+{
+	px4_adc_msg_t tmp_samples[ADC_MAX_CHAN];
+
+	for (int i = 0; i < ADC_MAX_CHAN; ++i) {
+		int ret;
+
+		/* Currently we only use these channels */
+		if (i != NAVIO_ADC_BATTERY_VOLTAGE_CHANNEL &&
+		    i != NAVIO_ADC_BATTERY_CURRENT_CHANNEL) {
+			tmp_samples[i].am_channel = i;
+			tmp_samples[i].am_data = 0;
+
+			continue;
+		}
+
+		ret = read_channel(&tmp_samples[i], i);
+
+		if (ret < 0) {
+			PX4_ERR("read_channel(%d): %d", i, ret);
+			tmp_samples[i].am_channel = i;
+			tmp_samples[i].am_data = 0;
+		}
+	}
+
+	tmp_samples[NAVIO_ADC_BATTERY_VOLTAGE_CHANNEL].am_channel = ADC_BATTERY_VOLTAGE_CHANNEL;
+	tmp_samples[NAVIO_ADC_BATTERY_CURRENT_CHANNEL].am_channel = ADC_BATTERY_CURRENT_CHANNEL;
+
+	pthread_mutex_lock(&_samples_lock);
+	memcpy(&_samples, &tmp_samples, sizeof(tmp_samples));
+	pthread_mutex_unlock(&_samples_lock);
+}
+
+int NavioADC::init()
+{
+	int ret;
+
+	ret = DriverFramework::VirtDevObj::init();
+
+	if (ret != PX4_OK) {
+		PX4_ERR("init failed");
+		return ret;
+	}
+
+	for (int i = 0; i < ADC_MAX_CHAN; i++) {
+		char channel_path[sizeof(ADC_SYSFS_PATH)];
+		strncpy(channel_path, ADC_SYSFS_PATH, sizeof(ADC_SYSFS_PATH));
+		channel_path[sizeof(ADC_SYSFS_PATH) - 2] += i;
+
+		_fd[i] = ::open(channel_path, O_RDONLY);
+
+		if (_fd[i] == -1) {
+			int err = errno;
+			ret = -1;
+			PX4_ERR("init: open: %s (%d)", strerror(err), err);
+			goto cleanup;
+		}
+	}
+
+	return PX4_OK;
+
+cleanup:
+
+	for (int i = 0; i < ADC_MAX_CHAN; i++) {
+		if (_fd[i] != -1) {
+			close(_fd[i]);
+		}
+	}
+
+	return ret;
+}
+
+int NavioADC::devIOCTL(unsigned long request, unsigned long arg)
+{
+	return -ENOTTY;
+}
+
+ssize_t NavioADC::devRead(void *buf, size_t count)
+{
+	const size_t maxsize = sizeof(_samples);
+	int ret;
+
+	if (count > maxsize) {
+		count = maxsize;
+	}
+
+	ret = pthread_mutex_trylock(&_samples_lock);
+
+	if (ret != 0) {
+		return 0;
+	}
+
+	memcpy(buf, &_samples, count);
+	pthread_mutex_unlock(&_samples_lock);
+
+	return count;
+}
+
+int NavioADC::read_channel(px4_adc_msg_t *adc_msg, int channel)
+{
+	char buffer[11]; /* 32bit max INT has maximum 10 chars */
+	int ret;
+
+	if (channel < 0 || channel >= ADC_MAX_CHAN) {
+		return -EINVAL;
+	}
+
+	ret = lseek(_fd[channel], 0, SEEK_SET);
+
+	if (ret == -1) {
+		ret = errno;
+		PX4_ERR("read_channel %d: lseek: %s (%d)", channel, strerror(ret), ret);
+		return  ret;
+	}
+
+	ret = ::read(_fd[channel], buffer, sizeof(buffer) - 1);
+
+	if (ret == -1) {
+		ret = errno;
+		PX4_ERR("read_channel %d: read: %s (%d)", channel, strerror(ret), ret);
+		return  ret;
+
+	} else if (ret == 0) {
+		PX4_ERR("read_channel %d: read empty", channel);
+		ret = -EINVAL;
+		return ret;
+	}
+
+	buffer[ret] = 0;
+
+	adc_msg->am_channel = channel;
+	adc_msg->am_data = strtol(buffer, NULL, 10);
+	ret = 0;
+
+	return ret;
+}
+
+static NavioADC *instance = nullptr;
+
+int navio_adc_main(int argc, char *argv[])
+{
+	int ret;
+
+	if (argc < 2) {
+		PX4_WARN("usage: <start/stop>");
+		return PX4_ERROR;
+	}
+
+	if (!strcmp(argv[1], "start")) {
+		if (instance) {
+			PX4_WARN("already started");
+			return PX4_OK;
+		}
+
+		instance = new NavioADC;
+
+		if (!instance) {
+			PX4_WARN("not enough memory");
+			return PX4_ERROR;
+		}
+
+		if (instance->init() != PX4_OK) {
+			delete instance;
+			instance = nullptr;
+			PX4_WARN("init failed");
+			return PX4_ERROR;
+		}
+
+		return PX4_OK;
+
+	} else if (!strcmp(argv[1], "stop")) {
+		if (!instance) {
+			PX4_WARN("already stopped");
+			return PX4_OK;
+		}
+
+		delete instance;
+		instance = nullptr;
+		return PX4_OK;
+
+	} else if (!strcmp(argv[1], "test")) {
+		if (!instance) {
+			PX4_ERR("start first");
+			return PX4_ERROR;
+		}
+
+		px4_adc_msg_t adc_msgs[ADC_MAX_CHAN];
+
+		ret = instance->devRead((char *)&adc_msgs, sizeof(adc_msgs));
+
+		if (ret < 0) {
+			PX4_ERR("ret: %s (%d)\n", strerror(ret), ret);
+			return ret;
+
+		} else if (ret != sizeof(adc_msgs)) {
+			PX4_ERR("incomplete read: %d expected %d", ret, sizeof(adc_msgs));
+			return ret;
+		}
+
+		for (int i = 0; i < ADC_MAX_CHAN; ++i) {
+			PX4_INFO("chan: %d; value: %d", (int)adc_msgs[i].am_channel,
+				 adc_msgs[i].am_data);
+		}
+
+		return PX4_OK;
+
+	} else {
+		PX4_WARN("action (%s) not supported", argv[1]);
+
+		return PX4_ERROR;
+	}
+
+	return PX4_OK;
+
+}

boards/emlid/navio2/navio_rgbled/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 drivers__navio_rgbled
+	MAIN navio_rgbled
+	SRCS
+		navio_rgbled.cpp
+	DEPENDS
+		led
+		linux_gpio
+	)
+
+#add_subdirectory(test)

boards/emlid/navio2/navio_rgbled/navio_rgbled.cpp

@@ -0,0 +1,288 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 - 2017 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 <px4_posix.h>
+#include <drivers/drv_led.h>
+
+#include "navio_rgbled.h"
+
+#define RGBLED_BASE_DEVICE_PATH "/dev/rgbled"
+
+using namespace DriverFramework;
+
+
+RGBLED::RGBLED(const char *name)
+	: DevObj(name,
+		 RGBLED0_DEVICE_PATH,
+		 RGBLED_BASE_DEVICE_PATH,
+		 DeviceBusType_UNKNOWN,
+		 0)
+	, _gpioR(4)
+	, _gpioG(27)
+	, _gpioB(6)
+{
+};
+
+RGBLED::~RGBLED()
+{
+	if (_led_controller.led_control_subscription() >= 0) {
+		orb_unsubscribe(_led_controller.led_control_subscription());
+	}
+};
+
+int RGBLED::start()
+{
+	int res = DevObj::init();
+
+	if (res != 0) {
+		DF_LOG_ERR("could not init DevObj (%i)", res);
+		return res;
+	}
+
+	res = _gpioR.exportPin();
+
+	if (res != 0) {
+		PX4_ERR("red led: failed to export");
+		goto cleanup;
+	}
+
+	res = _gpioR.setDirection(LinuxGPIO::Direction::OUT);
+
+	if (res != 0) {
+		PX4_ERR("red led: failed to set direction");
+		goto cleanup;
+	}
+
+	res = _gpioG.exportPin();
+
+	if (res != 0) {
+		PX4_ERR("green led: failed to export");
+		goto cleanup;
+	}
+
+	res = _gpioG.setDirection(LinuxGPIO::Direction::OUT);
+
+	if (res != 0) {
+		PX4_ERR("green led: failed to set direction");
+		goto cleanup;
+	}
+
+	res = _gpioB.exportPin();
+
+	if (res != 0) {
+		PX4_ERR("blue led: failed to export");
+		goto cleanup;
+	}
+
+	res = _gpioB.setDirection(LinuxGPIO::Direction::OUT);
+
+	if (res != 0) {
+		PX4_ERR("blue led: failed to set direction");
+		goto cleanup;
+	}
+
+	// update at fixed interval
+	DevObj::setSampleInterval(_led_controller.maximum_update_interval());
+
+	res = DevObj::start();
+
+	if (res != 0) {
+		DF_LOG_ERR("could not start DevObj (%i)", res);
+		return res;
+	}
+
+	return res;
+
+cleanup:
+	_gpioR.unexportPin();
+	_gpioG.unexportPin();
+	_gpioB.unexportPin();
+
+	return res;
+}
+
+int RGBLED::stop()
+{
+	int res;
+
+	_gpioR.unexportPin();
+	_gpioG.unexportPin();
+	_gpioB.unexportPin();
+
+	res = DevObj::stop();
+
+	if (res < 0) {
+		DF_LOG_ERR("could not stop DevObj");
+		//this may not be an error for this device
+		return res;
+	}
+
+	return 0;
+}
+
+void RGBLED::_measure()
+{
+	if (!_led_controller.is_init()) {
+		int led_control_sub = orb_subscribe(ORB_ID(led_control));
+		_led_controller.init(led_control_sub);
+	}
+
+	LedControlData led_control_data;
+
+	if (_led_controller.update(led_control_data) == 1) {
+		switch (led_control_data.leds[0].color) {
+		case led_control_s::COLOR_RED:
+			_gpioR.writeValue(LinuxGPIO::Value::LOW);
+			_gpioG.writeValue(LinuxGPIO::Value::HIGH);
+			_gpioB.writeValue(LinuxGPIO::Value::HIGH);
+			break;
+
+		case led_control_s::COLOR_GREEN:
+			_gpioR.writeValue(LinuxGPIO::Value::HIGH);
+			_gpioG.writeValue(LinuxGPIO::Value::LOW);
+			_gpioB.writeValue(LinuxGPIO::Value::HIGH);
+			break;
+
+		case led_control_s::COLOR_BLUE:
+			_gpioR.writeValue(LinuxGPIO::Value::HIGH);
+			_gpioG.writeValue(LinuxGPIO::Value::HIGH);
+			_gpioB.writeValue(LinuxGPIO::Value::LOW);
+			break;
+
+		case led_control_s::COLOR_AMBER: //make it the same as yellow
+		case led_control_s::COLOR_YELLOW:
+			_gpioR.writeValue(LinuxGPIO::Value::LOW);
+			_gpioG.writeValue(LinuxGPIO::Value::LOW);
+			_gpioB.writeValue(LinuxGPIO::Value::HIGH);
+			break;
+
+		case led_control_s::COLOR_PURPLE:
+			_gpioR.writeValue(LinuxGPIO::Value::LOW);
+			_gpioG.writeValue(LinuxGPIO::Value::HIGH);
+			_gpioB.writeValue(LinuxGPIO::Value::LOW);
+			break;
+
+		case led_control_s::COLOR_CYAN:
+			_gpioR.writeValue(LinuxGPIO::Value::HIGH);
+			_gpioG.writeValue(LinuxGPIO::Value::LOW);
+			_gpioB.writeValue(LinuxGPIO::Value::LOW);
+			break;
+
+		case led_control_s::COLOR_WHITE:
+			_gpioR.writeValue(LinuxGPIO::Value::LOW);
+			_gpioG.writeValue(LinuxGPIO::Value::LOW);
+			_gpioB.writeValue(LinuxGPIO::Value::LOW);
+			break;
+
+		default: // led_control_s::COLOR_OFF
+			_gpioR.writeValue(LinuxGPIO::Value::HIGH);
+			_gpioG.writeValue(LinuxGPIO::Value::HIGH);
+			_gpioB.writeValue(LinuxGPIO::Value::HIGH);
+			break;
+		}
+	}
+}
+
+extern "C" { __EXPORT int navio_rgbled_main(int argc, char *argv[]); }
+
+namespace navio_rgbled
+{
+int start();
+int stop();
+void usage();
+
+RGBLED *g_dev = nullptr;
+
+int start()
+{
+	g_dev = new RGBLED("navio_rgbled");
+
+	if (g_dev == nullptr) {
+		PX4_ERR("failed instantiating RGBLED");
+		return -1;
+	}
+
+	return g_dev->start();
+}
+
+int stop()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("not running");
+		return -1;
+	}
+
+	g_dev->stop();
+
+	delete g_dev;
+	g_dev = nullptr;
+
+	return 0;
+}
+
+void usage()
+{
+	PX4_INFO("Usage: navio_rgbled 'start', 'stop'");
+}
+
+} //namespace navio_rgbled
+
+int navio_rgbled_main(int argc, char *argv[])
+{
+	int ret = 0;
+	int myoptind = 1;
+
+	if (argc <= 1) {
+		navio_rgbled::usage();
+		return 1;
+	}
+
+	const char *verb = argv[myoptind];
+
+
+	if (!strcmp(verb, "start")) {
+		ret = navio_rgbled::start();
+	}
+
+	else if (!strcmp(verb, "stop")) {
+		ret = navio_rgbled::stop();
+	}
+
+	else {
+		navio_rgbled::usage();
+		return 1;
+	}
+
+	return ret;
+}
+

boards/emlid/navio2/navio_rgbled/navio_rgbled.h

@@ -0,0 +1,57 @@
+/****************************************************************************
+ *
+ *   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 <lib/drivers/linux_gpio/linux_gpio.h>
+#include <DevObj.hpp>
+
+#include <lib/led/led.h>
+
+class RGBLED : public DriverFramework::DevObj
+{
+public:
+	RGBLED(const char *name);
+	virtual ~RGBLED();
+
+	int start();
+	int stop();
+
+protected:
+	void _measure();
+
+private:
+	LedController _led_controller;
+	LinuxGPIO _gpioR;
+	LinuxGPIO _gpioG;
+	LinuxGPIO _gpioB;
+};

boards/emlid/navio2/navio_rgbled/test/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.
+#
+############################################################################
+include_directories(..)
+
+add_executable(navio_rgbled_test
+	main.cpp
+	test.cpp
+	)
+
+target_link_libraries(navio_rgbled_test
+	drivers__navio_rgbled
+	drivers__navio_gpio
+	df_driver_framework
+	)

boards/emlid/navio2/navio_rgbled/test/main.cpp

@@ -0,0 +1,39 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+extern int do_test();
+
+int main()
+{
+	return do_test();
+}

boards/emlid/navio2/navio_rgbled/test/test.cpp

@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   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 <px4_defines.h>
+
+#include <stdio.h>
+#include <unistd.h>
+
+#include <DevMgr.hpp>
+
+#include <drivers/drv_rgbled.h>
+
+#include "navio_rgbled.h"
+
+using namespace DriverFramework;
+
+int do_test();
+
+int do_test()
+{
+	DevHandle h;
+	RGBLED *g_dev = nullptr;
+
+	if (Framework::initialize() < 0) {
+		printf("Framework init failed\n");
+		return -1;
+	}
+
+	g_dev = new RGBLED("navio_rgbled test");
+	g_dev->start();
+
+	DevMgr::getHandle(RGBLED0_DEVICE_PATH, h);
+
+	if (!h.isValid()) {
+		printf("No RGB LED at " RGBLED0_DEVICE_PATH);
+		return -1;
+	}
+
+	printf("off\n");
+	h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_OFF);
+	sleep(2);
+
+	printf("red\n");
+	h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_RED);
+	sleep(2);
+
+	printf("yellow\n");
+	h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_YELLOW);
+	sleep(2);
+
+	printf("purple\n");
+	h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_PURPLE);
+	sleep(2);
+
+	printf("green\n");
+	h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_GREEN);
+	sleep(2);
+
+	printf("blue\n");
+	h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_BLUE);
+	sleep(2);
+
+	printf("blue blink slow\n");
+	h.ioctl(RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_BLINK_SLOW);
+	sleep(10);
+
+	printf("green blink normal\n");
+	h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_GREEN);
+	h.ioctl(RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_BLINK_NORMAL);
+	sleep(10);
+
+	printf("red blink fast\n");
+	h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_RED);
+	h.ioctl(RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_BLINK_FAST);
+	sleep(10);
+
+	printf("blue breathe (bogus)\n");
+	h.ioctl(RGBLED_SET_COLOR, (unsigned long)RGBLED_COLOR_BLUE);
+	h.ioctl(RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_BREATHE);
+	sleep(10);
+
+	return 0;
+}

boards/emlid/navio2/navio_sysfs_rc_in/CMakeLists.txt

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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 drivers__navio_sysfs_rc_in
+	MAIN navio_sysfs_rc_in
+	STACK_MAIN 1200
+	COMPILE_FLAGS
+	SRCS
+		navio_sysfs_rc_in.cpp
+	DEPENDS
+	)
+

boards/emlid/navio2/navio_sysfs_rc_in/navio_sysfs_rc_in.cpp

@@ -0,0 +1,304 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2015 Mark Charlebois. All rights reserved.
+ *   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 <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <unistd.h>
+
+#include <px4_config.h>
+#include <px4_workqueue.h>
+#include <px4_defines.h>
+
+#include <drivers/drv_hrt.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/input_rc.h>
+
+namespace navio_sysfs_rc_in
+{
+
+extern "C" __EXPORT int navio_sysfs_rc_in_main(int argc, char *argv[]);
+
+#define RCINPUT_DEVICE_PATH_BASE "/sys/kernel/rcio/rcin"
+
+#define RCINPUT_MEASURE_INTERVAL_US 20000 // microseconds
+
+
+class RcInput
+{
+public:
+	RcInput() :
+		_shouldExit(false),
+		_isRunning(false),
+		_work{},
+		_rcinput_pub(nullptr),
+		_channels(8), //D8R-II plus
+		_data{}
+	{
+		memset(_ch_fd, 0, sizeof(_ch_fd));
+	}
+	~RcInput()
+	{
+		work_cancel(HPWORK, &_work);
+		_isRunning = false;
+	}
+
+	/* @return 0 on success, -errno on failure */
+	int start();
+
+	/* @return 0 on success, -errno on failure */
+	void stop();
+
+	/* Trampoline for the work queue. */
+	static void cycle_trampoline(void *arg);
+
+	bool isRunning() { return _isRunning; }
+
+private:
+	void _cycle();
+	void _measure();
+
+	bool _shouldExit;
+	bool _isRunning;
+	struct work_s _work;
+
+	orb_advert_t _rcinput_pub;
+
+	int _channels;
+	int _ch_fd[input_rc_s::RC_INPUT_MAX_CHANNELS];
+	struct input_rc_s _data;
+
+	int navio_rc_init();
+};
+
+int RcInput::navio_rc_init()
+{
+	int i;
+	char buf[64];
+
+	for (i = 0; i < _channels; ++i) {
+		::snprintf(buf, sizeof(buf), "%s/ch%d", RCINPUT_DEVICE_PATH_BASE, i);
+		int fd = ::open(buf, O_RDONLY);
+
+		if (fd < 0) {
+			PX4_WARN("error: open %d failed", i);
+			break;
+		}
+
+		_ch_fd[i] = fd;
+	}
+
+	for (; i < input_rc_s::RC_INPUT_MAX_CHANNELS; ++i) {
+		_data.values[i] = UINT16_MAX;
+	}
+
+	_rcinput_pub = orb_advertise(ORB_ID(input_rc), &_data);
+
+	if (_rcinput_pub == nullptr) {
+		PX4_WARN("error: advertise failed");
+		return -1;
+	}
+
+	return 0;
+}
+
+int RcInput::start()
+{
+	int result = 0;
+
+	result = navio_rc_init();
+
+	if (result != 0) {
+		PX4_WARN("error: RC initialization failed");
+		return -1;
+	}
+
+	_isRunning = true;
+	result = work_queue(HPWORK, &_work, (worker_t)&RcInput::cycle_trampoline, this, 0);
+
+	if (result == -1) {
+		_isRunning = false;
+	}
+
+	return result;
+}
+
+void RcInput::stop()
+{
+	_shouldExit = true;
+}
+
+void RcInput::cycle_trampoline(void *arg)
+{
+	RcInput *dev = reinterpret_cast<RcInput *>(arg);
+	dev->_cycle();
+}
+
+void RcInput::_cycle()
+{
+	_measure();
+
+	if (!_shouldExit) {
+		work_queue(HPWORK, &_work, (worker_t)&RcInput::cycle_trampoline, this,
+			   USEC2TICK(RCINPUT_MEASURE_INTERVAL_US));
+	}
+}
+
+void RcInput::_measure(void)
+{
+	uint64_t ts;
+	char buf[12];
+
+	for (int i = 0; i < _channels; ++i) {
+		int res;
+
+		if ((res = ::pread(_ch_fd[i], buf, sizeof(buf) - 1, 0)) < 0) {
+			_data.values[i] = UINT16_MAX;
+			continue;
+		}
+
+		buf[sizeof(buf) - 1] = '\0';
+
+		_data.values[i] = atoi(buf);
+	}
+
+	ts = hrt_absolute_time();
+	_data.timestamp = ts;
+	_data.timestamp_last_signal = ts;
+	_data.channel_count = _channels;
+	_data.rssi = 100;
+	_data.rc_lost_frame_count = 0;
+	_data.rc_total_frame_count = 1;
+	_data.rc_ppm_frame_length = 100;
+	_data.rc_failsafe = false;
+	_data.rc_lost = false;
+	_data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM;
+
+	orb_publish(ORB_ID(input_rc), _rcinput_pub, &_data);
+}
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+static void
+usage(const char *reason)
+{
+	if (reason) {
+		PX4_ERR("%s", reason);
+	}
+
+	PX4_INFO("usage: navio_sysfs_rc_in {start|stop|status}");
+}
+
+static RcInput *rc_input = nullptr;
+
+int navio_sysfs_rc_in_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		usage("missing command");
+		return 1;
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (rc_input != nullptr && rc_input->isRunning()) {
+			PX4_WARN("already running");
+			/* this is not an error */
+			return 0;
+		}
+
+		rc_input = new RcInput();
+
+		// Check if alloc worked.
+		if (rc_input == nullptr) {
+			PX4_ERR("alloc failed");
+			return -1;
+		}
+
+		int ret = rc_input->start();
+
+		if (ret != 0) {
+			PX4_ERR("start failed");
+		}
+
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+
+		if (rc_input == nullptr || !rc_input->isRunning()) {
+			PX4_WARN("not running");
+			/* this is not an error */
+			return 0;
+		}
+
+		rc_input->stop();
+
+		// Wait for task to die
+		int i = 0;
+
+		do {
+			/* wait up to 3s */
+			usleep(100000);
+
+		} while (rc_input->isRunning() && ++i < 30);
+
+		delete rc_input;
+		rc_input = nullptr;
+
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (rc_input != nullptr && rc_input->isRunning()) {
+			PX4_INFO("running");
+
+		} else {
+			PX4_INFO("not running\n");
+		}
+
+		return 0;
+	}
+
+	usage("unrecognized command");
+	return 1;
+
+}
+
+}; // namespace navio_sysfs_rc_in