Merge pull request #1 from PX4/master

From original

src/drivers/batt_smbus/batt_smbus.cpp

@@ -452,10 +452,7 @@ BATT_SMBUS::read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_
 	uint8_t pec = get_PEC(reg, true, buff, bufflen + 1);
 
 	if (pec != buff[bufflen + 1]) {
-		// debug
-		warnx("CurrPEC:%x vs MyPec:%x", (int)buff[bufflen + 1], (int)pec);
 		return 0;
-
 	}
 
 	// copy data

src/drivers/boards/px4fmu-v2/board_config.h

@@ -207,6 +207,11 @@ __BEGIN_DECLS
 #define HRT_TIMER		8	/* use timer8 for the HRT */
 #define HRT_TIMER_CHANNEL	1	/* use capture/compare channel */
 
+/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */
+#define PWMIN_TIMER		4
+#define PWMIN_TIMER_CHANNEL	2
+#define GPIO_PWM_IN		GPIO_TIM4_CH2IN_2
+
 /****************************************************************************************************
  * Public Types
  ****************************************************************************************************/

src/drivers/drv_gpio.h

@@ -159,4 +159,6 @@
 
 #define GPIO_SENSOR_RAIL_RESET	GPIOC(13)
 
+#define GPIO_PERIPHERAL_RAIL_RESET	GPIOC(14)
+
 #endif /* _DRV_GPIO_H */

src/drivers/drv_oreoled.h

@@ -0,0 +1,149 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012-2013 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 drv_oreoled.h
+ *
+ * Oreo led device API
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+/* oreoled device path */
+#define OREOLED0_DEVICE_PATH "/dev/oreoled0"
+
+/*
+ * ioctl() definitions
+ */
+
+#define _OREOLEDIOCBASE		(0x2d00)
+#define _OREOLEDIOC(_n)		(_IOC(_OREOLEDIOCBASE, _n))
+
+/** set constant RGB values */
+#define OREOLED_SET_RGB			_OREOLEDIOC(1)
+
+/** run macro */
+#define OREOLED_RUN_MACRO		_OREOLEDIOC(2)
+
+/** send bytes */
+#define OREOLED_SEND_BYTES		_OREOLEDIOC(3)
+
+/* Oreo LED driver supports up to 4 leds */
+#define OREOLED_NUM_LEDS		4
+
+/* instance of 0xff means apply to all instances */
+#define OREOLED_ALL_INSTANCES	0xff
+
+/* maximum command length that can be sent to LEDs */
+#define OREOLED_CMD_LENGTH_MAX	24
+
+/* enum passed to OREOLED_SET_MODE ioctl()
+ *	defined by hardware */
+enum oreoled_pattern {
+	OREOLED_PATTERN_OFF = 0,
+	OREOLED_PATTERN_SINE = 1,
+	OREOLED_PATTERN_SOLID = 2,
+	OREOLED_PATTERN_SIREN = 3,
+	OREOLED_PATTERN_STROBE = 4,
+	OREOLED_PATTERN_FADEIN = 5,
+	OREOLED_PATTERN_FADEOUT = 6,
+	OREOLED_PATTERN_PARAMUPDATE = 7,
+	OREOLED_PATTERN_ENUM_COUNT
+};
+
+/* enum passed to OREOLED_SET_MODE ioctl()
+ *	defined by hardware */
+enum oreoled_param {
+	OREOLED_PARAM_BIAS_RED = 0,
+	OREOLED_PARAM_BIAS_GREEN = 1,
+	OREOLED_PARAM_BIAS_BLUE = 2,
+	OREOLED_PARAM_AMPLITUDE_RED = 3,
+	OREOLED_PARAM_AMPLITUDE_GREEN = 4,
+	OREOLED_PARAM_AMPLITUDE_BLUE = 5,
+	OREOLED_PARAM_PERIOD = 6,
+	OREOLED_PARAM_REPEAT = 7,
+	OREOLED_PARAM_PHASEOFFSET = 8,
+	OREOLED_PARAM_MACRO = 9,
+	OREOLED_PARAM_ENUM_COUNT
+};
+
+/* enum of available macros
+ * 	defined by hardware */
+enum oreoled_macro {
+	OREOLED_PARAM_MACRO_RESET = 0,
+	OREOLED_PARAM_MACRO_COLOUR_CYCLE = 1,
+	OREOLED_PARAM_MACRO_BREATH = 2,
+	OREOLED_PARAM_MACRO_STROBE = 3,
+	OREOLED_PARAM_MACRO_FADEIN = 4,
+	OREOLED_PARAM_MACRO_FADEOUT = 5,
+	OREOLED_PARAM_MACRO_RED = 6,
+	OREOLED_PARAM_MACRO_GREEN = 7,
+	OREOLED_PARAM_MACRO_BLUE = 8,
+	OREOLED_PARAM_MACRO_YELLOW = 9,
+	OREOLED_PARAM_MACRO_WHITE = 10,
+	OREOLED_PARAM_MACRO_ENUM_COUNT
+};
+
+/* 
+  structure passed to OREOLED_SET_RGB ioctl()
+  Note that the driver scales the brightness to 0 to 255, regardless
+  of the hardware scaling
+ */
+typedef struct {
+	uint8_t instance;
+	oreoled_pattern pattern;
+	uint8_t red;
+	uint8_t green;
+	uint8_t blue;
+} oreoled_rgbset_t;
+
+/*
+  structure passed to OREOLED_RUN_MACRO ioctl()
+ */
+typedef struct {
+	uint8_t instance;
+	oreoled_macro macro;
+} oreoled_macrorun_t;
+
+/*
+  structure passed to send_bytes method (only used for testing)
+ */
+typedef struct {
+	uint8_t led_num;
+	uint8_t num_bytes;
+	uint8_t buff[OREOLED_CMD_LENGTH_MAX];
+} oreoled_cmd_t;
+

src/drivers/drv_pwm_input.h

@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 <sys/types.h>
+#include <stdbool.h>
+
+#include <time.h>
+#include <queue.h>
+
+#define PWMIN0_DEVICE_PATH		"/dev/pwmin0"
+
+__BEGIN_DECLS
+
+/*
+ * Initialise the timer
+ */
+__EXPORT extern int	pwm_input_main(int argc, char * argv[]);
+
+__END_DECLS

src/drivers/gps/ubx.cpp

@@ -45,7 +45,7 @@
  *   (rework, add ubx7+ compatibility)
  *
  * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf
- * @see http://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8-V15_ReceiverDescriptionProtocolSpec_Public_%28UBX-13003221%29.pdf
+ * @see http://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8_ReceiverDescriptionProtocolSpec_%28UBX-13003221%29_Public.pdf
  */
 
 #include <assert.h>

src/drivers/mpu6000/mpu6000.cpp

@@ -1881,6 +1881,7 @@ MPU6000	*g_dev_int; // on internal bus
 MPU6000	*g_dev_ext; // on external bus
 
 void	start(bool, enum Rotation);
+void	stop(bool);
 void	test(bool);
 void	reset(bool);
 void	info(bool);
@@ -1946,6 +1947,20 @@ fail:
 	errx(1, "driver start failed");
 }
 
+void
+stop(bool external_bus)
+{
+	MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
+	if (*g_dev_ptr != nullptr) {
+		delete *g_dev_ptr;
+		*g_dev_ptr = nullptr;
+	} else {
+		/* warn, but not an error */
+		warnx("already stopped.");
+	}
+	exit(0);
+}
+
 /**
  * Perform some basic functional tests on the driver;
  * make sure we can collect data from the sensor in polled
@@ -2111,7 +2126,7 @@ factorytest(bool external_bus)
 void
 usage()
 {
-	warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'factorytest', 'testerror'");
+	warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'factorytest', 'testerror'");
 	warnx("options:");
 	warnx("    -X    (external bus)");
 	warnx("    -R rotation");
@@ -2147,38 +2162,50 @@ mpu6000_main(int argc, char *argv[])
 	 * Start/load the driver.
 
 	 */
-	if (!strcmp(verb, "start"))
+	if (!strcmp(verb, "start")) {
 		mpu6000::start(external_bus, rotation);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		mpu6000::stop(external_bus);
+	}
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(verb, "test"))
+	if (!strcmp(verb, "test")) {
 		mpu6000::test(external_bus);
+	}
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(verb, "reset"))
+	if (!strcmp(verb, "reset")) {
 		mpu6000::reset(external_bus);
+	}
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(verb, "info"))
+	if (!strcmp(verb, "info")) {
 		mpu6000::info(external_bus);
+	}
 
 	/*
 	 * Print register information.
 	 */
-	if (!strcmp(verb, "regdump"))
+	if (!strcmp(verb, "regdump")) {
 		mpu6000::regdump(external_bus);
+	}
 
-	if (!strcmp(verb, "factorytest"))
+	if (!strcmp(verb, "factorytest")) {
 		mpu6000::factorytest(external_bus);
+	}
 
-	if (!strcmp(verb, "testerror"))
+	if (!strcmp(verb, "testerror")) {
 		mpu6000::testerror(external_bus);
+	}
 
-	errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'regdump', 'factorytest' or 'testerror'");
+	mpu6000::usage();
+	exit(1);
 }

src/drivers/oreoled/module.mk

@@ -0,0 +1,8 @@
+#
+# Oreo LED driver
+#
+
+MODULE_COMMAND	 = oreoled
+SRCS		 = oreoled.cpp
+
+MAXOPTIMIZATION	 = -Os

src/drivers/oreoled/oreoled.cpp

@@ -0,0 +1,700 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Randy Mackay <rmackay9@yahoo.com>
+ *
+ * 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 oreoled.cpp
+ *
+ * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C.
+ *
+ */
+
+#include <nuttx/config.h>
+
+#include <drivers/device/i2c.h>
+#include <drivers/drv_hrt.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <ctype.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+#include <board_config.h>
+
+#include <drivers/drv_oreoled.h>
+#include <drivers/device/ringbuffer.h>
+
+#define OREOLED_NUM_LEDS		4			///< maximum number of LEDs the oreo led driver can support
+#define OREOLED_BASE_I2C_ADDR	0x68		///< base i2c address (7-bit)
+#define OREOLED_TIMEOUT_MS		10000000U	///< timeout looking for battery 10seconds after startup
+#define OREOLED_GENERALCALL_US	4000000U	///< general call sent every 4 seconds
+#define OREOLED_GENERALCALL_CMD	0x00		///< general call command sent at regular intervals
+
+#define OREOLED_STARTUP_INTERVAL_US		(1000000U / 10U)	///< time in microseconds, measure at 10hz
+#define OREOLED_UPDATE_INTERVAL_US		(1000000U / 50U)	///< time in microseconds, measure at 10hz
+
+#define OREOLED_CMD_QUEUE_SIZE	10		///< up to 10 messages can be queued up to send to the LEDs
+
+class OREOLED : public device::I2C
+{
+public:
+	OREOLED(int bus, int i2c_addr);
+	virtual ~OREOLED();
+
+	virtual int		init();
+	virtual int		probe();
+	virtual int		info();
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/* send general call on I2C bus to syncronise all LEDs */
+	int				send_general_call();
+
+	/* send cmd to an LEDs (used for testing only) */
+	int				send_cmd(oreoled_cmd_t sb);
+
+private:
+
+	/**
+	 * Start periodic updates to the LEDs
+	 */
+	void			start();
+
+	/**
+	 * Stop periodic updates to the LEDs
+	 */
+	void			stop();
+
+	/**
+	 * static function that is called by worker queue
+	 */
+	static void		cycle_trampoline(void *arg);
+
+	/**
+	 * update the colours displayed by the LEDs
+	 */
+	void			cycle();
+
+	/* internal variables */
+	work_s			_work;							///< work queue for scheduling reads
+	bool			_healthy[OREOLED_NUM_LEDS];		///< health of each LED
+	uint8_t			_num_healthy;					///< number of healthy LEDs
+	RingBuffer		*_cmd_queue;					///< buffer of commands to send to LEDs
+	uint64_t		_last_gencall;
+	uint64_t		_start_time;					///< system time we first attempt to communicate with battery
+};
+
+/* for now, we only support one OREOLED */
+namespace
+{
+OREOLED *g_oreoled = nullptr;
+}
+
+void oreoled_usage();
+
+extern "C" __EXPORT int oreoled_main(int argc, char *argv[]);
+
+/* constructor */
+OREOLED::OREOLED(int bus, int i2c_addr) :
+	I2C("oreoled", OREOLED0_DEVICE_PATH, bus, i2c_addr, 100000),
+	_work{},
+	_num_healthy(0),
+	_cmd_queue(nullptr),
+	_last_gencall(0)
+{
+	/* initialise to unhealthy */
+	memset(_healthy, 0, sizeof(_healthy));
+
+	/* capture startup time */
+	_start_time = hrt_absolute_time();
+}
+
+/* destructor */
+OREOLED::~OREOLED()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* clear command queue */
+	if (_cmd_queue != nullptr) {
+		delete _cmd_queue;
+	}
+}
+
+int
+OREOLED::init()
+{
+	int ret;
+
+	/* initialise I2C bus */
+	ret = I2C::init();
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	/* allocate command queue */
+	_cmd_queue = new RingBuffer(OREOLED_CMD_QUEUE_SIZE, sizeof(oreoled_cmd_t));
+
+	if (_cmd_queue == nullptr) {
+		return ENOTTY;
+
+	} else {
+		/* start work queue */
+		start();
+	}
+
+	return OK;
+}
+
+int
+OREOLED::probe()
+{
+	/* always return true */
+	return OK;
+}
+
+int
+OREOLED::info()
+{
+	/* print health info on each LED */
+	for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+		if (!_healthy[i]) {
+			log("oreo %u: BAD", (int)i);
+
+		} else {
+			log("oreo %u: OK", (int)i);
+		}
+	}
+
+	return OK;
+}
+
+void
+OREOLED::start()
+{
+	/* schedule a cycle to start things */
+	work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this, 1);
+}
+
+void
+OREOLED::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+OREOLED::cycle_trampoline(void *arg)
+{
+	OREOLED *dev = (OREOLED *)arg;
+
+	/* check global oreoled and cycle */
+	if (g_oreoled != nullptr) {
+		dev->cycle();
+	}
+}
+
+void
+OREOLED::cycle()
+{
+	/* check time since startup */
+	uint64_t now = hrt_absolute_time();
+	bool startup_timeout = (now - _start_time > OREOLED_TIMEOUT_MS);
+
+	/* if not leds found during start-up period, exit without rescheduling */
+	if (startup_timeout && _num_healthy == 0) {
+		warnx("did not find oreoled");
+		return;
+	}
+
+	/* during startup period keep searching for unhealthy LEDs */
+	if (!startup_timeout && _num_healthy < OREOLED_NUM_LEDS) {
+		/* prepare command to turn off LED*/
+		uint8_t msg[] = {OREOLED_PATTERN_OFF};
+
+		/* attempt to contact each unhealthy LED */
+		for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+			if (!_healthy[i]) {
+				/* set I2C address */
+				set_address(OREOLED_BASE_I2C_ADDR + i);
+
+				/* send I2C command and record health*/
+				if (transfer(msg, sizeof(msg), nullptr, 0) == OK) {
+					_healthy[i] = true;
+					_num_healthy++;
+					warnx("oreoled %d ok", (unsigned)i);
+				}
+			}
+		}
+
+		/* schedule another attempt in 0.1 sec */
+		work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this,
+			   USEC2TICK(OREOLED_STARTUP_INTERVAL_US));
+		return;
+	}
+
+	/* get next command from queue */
+	oreoled_cmd_t next_cmd;
+
+	while (_cmd_queue->get(&next_cmd, sizeof(oreoled_cmd_t))) {
+		/* send valid messages to healthy LEDs */
+		if ((next_cmd.led_num < OREOLED_NUM_LEDS) && _healthy[next_cmd.led_num]
+		    && (next_cmd.num_bytes <= OREOLED_CMD_LENGTH_MAX)) {
+			/* set I2C address */
+			set_address(OREOLED_BASE_I2C_ADDR + next_cmd.led_num);
+			/* send I2C command */
+			transfer(next_cmd.buff, next_cmd.num_bytes, nullptr, 0);
+		}
+	}
+
+	/* send general call every 4 seconds*/
+	if ((now - _last_gencall) > OREOLED_GENERALCALL_US) {
+		send_general_call();
+	}
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this,
+		   USEC2TICK(OREOLED_UPDATE_INTERVAL_US));
+}
+
+int
+OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	int ret = -ENODEV;
+	oreoled_cmd_t new_cmd;
+
+	switch (cmd) {
+	case OREOLED_SET_RGB:
+		/* set the specified color */
+		new_cmd.led_num = ((oreoled_rgbset_t *) arg)->instance;
+		new_cmd.buff[0] = ((oreoled_rgbset_t *) arg)->pattern;
+		new_cmd.buff[1] = OREOLED_PARAM_BIAS_RED;
+		new_cmd.buff[2] = ((oreoled_rgbset_t *) arg)->red;
+		new_cmd.buff[3] = OREOLED_PARAM_BIAS_GREEN;
+		new_cmd.buff[4] = ((oreoled_rgbset_t *) arg)->green;
+		new_cmd.buff[5] = OREOLED_PARAM_BIAS_BLUE;
+		new_cmd.buff[6] = ((oreoled_rgbset_t *) arg)->blue;
+		new_cmd.num_bytes = 7;
+
+		/* special handling for request to set all instances rgb values */
+		if (new_cmd.led_num == OREOLED_ALL_INSTANCES) {
+			for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+				/* add command to queue for all healthy leds */
+				if (_healthy[i]) {
+					new_cmd.led_num = i;
+					_cmd_queue->force(&new_cmd);
+					ret = OK;
+				}
+			}
+
+		} else if (new_cmd.led_num < OREOLED_NUM_LEDS) {
+			/* request to set individual instance's rgb value */
+			if (_healthy[new_cmd.led_num]) {
+				_cmd_queue->force(&new_cmd);
+				ret = OK;
+			}
+		}
+
+		return ret;
+
+	case OREOLED_RUN_MACRO:
+		/* run a macro */
+		new_cmd.led_num = ((oreoled_macrorun_t *) arg)->instance;
+		new_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE;
+		new_cmd.buff[1] = OREOLED_PARAM_MACRO;
+		new_cmd.buff[2] = ((oreoled_macrorun_t *) arg)->macro;
+		new_cmd.num_bytes = 3;
+
+		/* special handling for request to set all instances */
+		if (new_cmd.led_num == OREOLED_ALL_INSTANCES) {
+			for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+				/* add command to queue for all healthy leds */
+				if (_healthy[i]) {
+					new_cmd.led_num = i;
+					_cmd_queue->force(&new_cmd);
+					ret = OK;
+				}
+			}
+
+		} else if (new_cmd.led_num < OREOLED_NUM_LEDS) {
+			/* request to set individual instance's rgb value */
+			if (_healthy[new_cmd.led_num]) {
+				_cmd_queue->force(&new_cmd);
+				ret = OK;
+			}
+		}
+
+		return ret;
+
+	case OREOLED_SEND_BYTES:
+		/* send bytes */
+		new_cmd = *((oreoled_cmd_t *) arg);
+
+		/* special handling for request to set all instances */
+		if (new_cmd.led_num == OREOLED_ALL_INSTANCES) {
+			for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
+				/* add command to queue for all healthy leds */
+				if (_healthy[i]) {
+					new_cmd.led_num = i;
+					_cmd_queue->force(&new_cmd);
+					ret = OK;
+				}
+			}
+
+		} else if (new_cmd.led_num < OREOLED_NUM_LEDS) {
+			/* request to set individual instance's rgb value */
+			if (_healthy[new_cmd.led_num]) {
+				_cmd_queue->force(&new_cmd);
+				ret = OK;
+			}
+		}
+
+		return ret;
+
+	default:
+		/* see if the parent class can make any use of it */
+		ret = CDev::ioctl(filp, cmd, arg);
+		break;
+	}
+
+	return ret;
+}
+
+/* send general call on I2C bus to syncronise all LEDs */
+int
+OREOLED::send_general_call()
+{
+	int ret = -ENODEV;
+
+	/* set I2C address to zero */
+	set_address(0);
+
+	/* prepare command : 0x01 = general hardware call, 0x00 = I2C address of master (but we don't act as a slave so set to zero)*/
+	uint8_t msg[] = {0x01, 0x00};
+
+	/* send I2C command */
+	if (transfer(msg, sizeof(msg), nullptr, 0) == OK) {
+		ret = OK;
+	}
+
+	/* record time */
+	_last_gencall = hrt_absolute_time();
+
+	return ret;
+}
+
+/* send a cmd to an LEDs (used for testing only) */
+int
+OREOLED::send_cmd(oreoled_cmd_t new_cmd)
+{
+	int ret = -ENODEV;
+
+	/* sanity check led number, health and cmd length */
+	if ((new_cmd.led_num < OREOLED_NUM_LEDS) && _healthy[new_cmd.led_num] && (new_cmd.num_bytes < OREOLED_CMD_LENGTH_MAX)) {
+		/* set I2C address */
+		set_address(OREOLED_BASE_I2C_ADDR + new_cmd.led_num);
+
+		/* add to queue */
+		_cmd_queue->force(&new_cmd);
+		ret = OK;
+	}
+
+	return ret;
+}
+
+void
+oreoled_usage()
+{
+	warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50' 'macro 4' 'gencall' 'bytes <lednum> 7 9 6'");
+	warnx("options:");
+	warnx("    -b i2cbus (%d)", PX4_I2C_BUS_LED);
+	warnx("    -a addr (0x%x)", OREOLED_BASE_I2C_ADDR);
+}
+
+int
+oreoled_main(int argc, char *argv[])
+{
+	int i2cdevice = -1;
+	int i2c_addr = OREOLED_BASE_I2C_ADDR; /* 7bit */
+
+	int ch;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
+		switch (ch) {
+		case 'a':
+			i2c_addr = (int)strtol(optarg, NULL, 0);
+			break;
+
+		case 'b':
+			i2cdevice = (int)strtol(optarg, NULL, 0);
+			break;
+
+		default:
+			oreoled_usage();
+			exit(0);
+		}
+	}
+
+	if (optind >= argc) {
+		oreoled_usage();
+		exit(1);
+	}
+
+	const char *verb = argv[optind];
+
+	int ret;
+
+	/* start driver */
+	if (!strcmp(verb, "start")) {
+		if (g_oreoled != nullptr) {
+			errx(1, "already started");
+		}
+
+		/* by default use LED bus */
+		if (i2cdevice == -1) {
+			i2cdevice = PX4_I2C_BUS_LED;
+		}
+
+		/* instantiate driver */
+		g_oreoled = new OREOLED(i2cdevice, i2c_addr);
+
+		/* check if object was created */
+		if (g_oreoled == nullptr) {
+			errx(1, "failed to allocated memory for driver");
+		}
+
+		/* check object was created successfully */
+		if (g_oreoled->init() != OK) {
+			delete g_oreoled;
+			g_oreoled = nullptr;
+			errx(1, "failed to start driver");
+		}
+
+		exit(0);
+	}
+
+	/* need the driver past this point */
+	if (g_oreoled == nullptr) {
+		warnx("not started");
+		oreoled_usage();
+		exit(1);
+	}
+
+	if (!strcmp(verb, "test")) {
+		int fd = open(OREOLED0_DEVICE_PATH, O_RDWR);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " OREOLED0_DEVICE_PATH);
+		}
+
+		/* structure to hold desired colour */
+		oreoled_rgbset_t rgb_set_red = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_SOLID, 0xFF, 0x0, 0x0};
+		oreoled_rgbset_t rgb_set_blue = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_SOLID, 0x0, 0x0, 0xFF};
+		oreoled_rgbset_t rgb_set_off = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_OFF, 0x0, 0x0, 0x0};
+
+		/* flash red and blue for 3 seconds */
+		for (uint8_t i = 0; i < 30; i++) {
+			/* red */
+			if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_red)) != OK) {
+				errx(1, " failed to update rgb");
+			}
+
+			/* sleep for 0.05 seconds */
+			usleep(50000);
+
+			/* blue */
+			if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_blue)) != OK) {
+				errx(1, " failed to update rgb");
+			}
+
+			/* sleep for 0.05 seconds */
+			usleep(50000);
+		}
+
+		/* turn off LED */
+		if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_off)) != OK) {
+			errx(1, " failed to turn off led");
+		}
+
+		close(fd);
+		exit(ret);
+	}
+
+	/* display driver status */
+	if (!strcmp(verb, "info")) {
+		g_oreoled->info();
+		exit(0);
+	}
+
+	if (!strcmp(verb, "off") || !strcmp(verb, "stop")) {
+		int fd = open(OREOLED0_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " OREOLED0_DEVICE_PATH);
+		}
+
+		/* turn off LED */
+		oreoled_rgbset_t rgb_set_off = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_OFF, 0x0, 0x0, 0x0};
+		ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_off);
+
+		close(fd);
+
+		/* delete the oreoled object if stop was requested, in addition to turning off the LED. */
+		if (!strcmp(verb, "stop")) {
+			OREOLED *tmp_oreoled = g_oreoled;
+			g_oreoled = nullptr;
+			delete tmp_oreoled;
+			exit(0);
+		}
+
+		exit(ret);
+	}
+
+	/* send rgb request to all LEDS */
+	if (!strcmp(verb, "rgb")) {
+		if (argc < 5) {
+			errx(1, "Usage: oreoled rgb <red> <green> <blue>");
+		}
+
+		int fd = open(OREOLED0_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " OREOLED0_DEVICE_PATH);
+		}
+
+		uint8_t red = (uint8_t)strtol(argv[2], NULL, 0);
+		uint8_t green = (uint8_t)strtol(argv[3], NULL, 0);
+		uint8_t blue = (uint8_t)strtol(argv[4], NULL, 0);
+		oreoled_rgbset_t rgb_set = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_SOLID, red, green, blue};
+
+		if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set)) != OK) {
+			errx(1, "failed to set rgb");
+		}
+
+		close(fd);
+		exit(ret);
+	}
+
+	/* send macro request to all LEDS */
+	if (!strcmp(verb, "macro")) {
+		if (argc < 3) {
+			errx(1, "Usage: oreoled macro <macro_num>");
+		}
+
+		int fd = open(OREOLED0_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " OREOLED0_DEVICE_PATH);
+		}
+
+		uint8_t macro = (uint8_t)strtol(argv[2], NULL, 0);
+
+		/* sanity check macro number */
+		if (macro > OREOLED_PARAM_MACRO_ENUM_COUNT) {
+			errx(1, "invalid macro number %d", (int)macro);
+			exit(ret);
+		}
+
+		oreoled_macrorun_t macro_run = {OREOLED_ALL_INSTANCES, (enum oreoled_macro)macro};
+
+		if ((ret = ioctl(fd, OREOLED_RUN_MACRO, (unsigned long)&macro_run)) != OK) {
+			errx(1, "failed to run macro");
+		}
+
+		close(fd);
+		exit(ret);
+	}
+
+	/* send general hardware call to all LEDS */
+	if (!strcmp(verb, "gencall")) {
+		ret = g_oreoled->send_general_call();
+		warnx("sent general call");
+		exit(ret);
+	}
+
+	/* send a string of bytes to an LED using send_bytes function */
+	if (!strcmp(verb, "bytes")) {
+		if (argc < 3) {
+			errx(1, "Usage: oreoled bytes <led_num> <byte1> <byte2> <byte3> ...");
+		}
+
+		/* structure to be sent */
+		oreoled_cmd_t sendb;
+
+		/* maximum of 20 bytes can be sent */
+		if (argc > 20 + 3) {
+			errx(1, "Max of 20 bytes can be sent");
+		}
+
+		/* check led num */
+		sendb.led_num = (uint8_t)strtol(argv[2], NULL, 0);
+
+		if (sendb.led_num > 3) {
+			errx(1, "led number must be between 0 ~ 3");
+		}
+
+		/* get bytes */
+		sendb.num_bytes = argc - 3;
+		uint8_t byte_count;
+
+		for (byte_count = 0; byte_count < sendb.num_bytes; byte_count++) {
+			sendb.buff[byte_count] = (uint8_t)strtol(argv[byte_count + 3], NULL, 0);
+		}
+
+		/* send bytes */
+		if ((ret = g_oreoled->send_cmd(sendb)) != OK) {
+			errx(1, "failed to send command");
+
+		} else {
+			warnx("sent %d bytes", (int)sendb.num_bytes);
+		}
+
+		exit(ret);
+	}
+
+	oreoled_usage();
+	exit(0);
+}

src/drivers/pwm_input/module.mk

@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# Makefile to build the PWM input driver.
+#
+
+MODULE_COMMAND	= pwm_input
+
+SRCS		= pwm_input.cpp
+

src/drivers/pwm_input/pwm_input.cpp

@@ -0,0 +1,603 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 Andrew Tridgell. 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 pwm_input.cpp 
+ *
+ * PWM input driver based on earlier driver from Evan Slatyer,
+ * which in turn was based on drv_hrt.c
+ */
+
+#include <nuttx/config.h>
+#include <nuttx/arch.h>
+#include <nuttx/irq.h>
+
+#include <sys/types.h>
+#include <stdbool.h>
+
+#include <assert.h>
+#include <debug.h>
+#include <time.h>
+#include <queue.h>
+#include <errno.h>
+#include <string.h>
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <board_config.h>
+#include <drivers/drv_pwm_input.h>
+#include <drivers/drv_hrt.h>
+
+#include "chip.h"
+#include "up_internal.h"
+#include "up_arch.h"
+
+#include "stm32.h"
+#include "stm32_gpio.h"
+#include "stm32_tim.h"
+#include <systemlib/err.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/pwm_input.h>
+#include <uORB/topics/subsystem_info.h>
+
+#include <drivers/drv_device.h>
+#include <drivers/device/device.h>
+#include <drivers/device/ringbuffer.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+
+#if HRT_TIMER == PWMIN_TIMER
+#error cannot share timer between HRT and PWMIN
+#endif
+
+#if !defined(GPIO_PWM_IN) || !defined(PWMIN_TIMER) || !defined(PWMIN_TIMER_CHANNEL)
+#error PWMIN defines are needed in board_config.h for this board
+#endif
+
+/* PWMIN configuration */
+#if   PWMIN_TIMER == 1
+# define PWMIN_TIMER_BASE	STM32_TIM1_BASE
+# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB2ENR
+# define PWMIN_TIMER_POWER_BIT	RCC_APB2ENR_TIM1EN
+# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM1CC
+# define PWMIN_TIMER_CLOCK	STM32_APB2_TIM1_CLKIN
+#elif PWMIN_TIMER == 2
+# define PWMIN_TIMER_BASE	STM32_TIM2_BASE
+# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define PWMIN_TIMER_POWER_BIT	RCC_APB2ENR_TIM2EN
+# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM2
+# define PWMIN_TIMER_CLOCK	STM32_APB1_TIM2_CLKIN
+#elif PWMIN_TIMER == 3
+# define PWMIN_TIMER_BASE	STM32_TIM3_BASE
+# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define PWMIN_TIMER_POWER_BIT	RCC_APB1ENR_TIM3EN
+# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM3
+# define PWMIN_TIMER_CLOCK	STM32_APB1_TIM3_CLKIN
+#elif PWMIN_TIMER == 4
+# define PWMIN_TIMER_BASE	STM32_TIM4_BASE
+# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define PWMIN_TIMER_POWER_BIT	RCC_APB1ENR_TIM4EN
+# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM4
+# define PWMIN_TIMER_CLOCK	STM32_APB1_TIM4_CLKIN
+#elif PWMIN_TIMER == 5
+# define PWMIN_TIMER_BASE	STM32_TIM5_BASE
+# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define PWMIN_TIMER_POWER_BIT	RCC_APB2ENR_TIM5EN
+# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM5
+# define PWMIN_TIMER_CLOCK	STM32_APB1_TIM5_CLKIN
+#elif PWMIN_TIMER == 8
+# define PWMIN_TIMER_BASE	STM32_TIM8_BASE
+# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB2ENR
+# define PWMIN_TIMER_POWER_BIT	RCC_APB2ENR_TIM8EN
+# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM8CC
+# define PWMIN_TIMER_CLOCK	STM32_APB2_TIM8_CLKIN
+#elif PWMIN_TIMER == 9
+# define PWMIN_TIMER_BASE	STM32_TIM9_BASE
+# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define PWMIN_TIMER_POWER_BIT	RCC_APB2ENR_TIM9EN
+# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM1BRK
+# define PWMIN_TIMER_CLOCK	STM32_APB1_TIM9_CLKIN
+#elif PWMIN_TIMER == 10
+# define PWMIN_TIMER_BASE	STM32_TIM10_BASE
+# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define PWMIN_TIMER_POWER_BIT	RCC_APB2ENR_TIM10EN
+# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM1UP
+# define PWMIN_TIMER_CLOCK	STM32_APB2_TIM10_CLKIN
+#elif PWMIN_TIMER == 11
+# define PWMIN_TIMER_BASE	STM32_TIM11_BASE
+# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define PWMIN_TIMER_POWER_BIT	RCC_APB2ENR_TIM11EN
+# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM1TRGCOM
+# define PWMIN_TIMER_CLOCK	STM32_APB2_TIM11_CLKIN
+#elif PWMIN_TIMER == 12
+# define PWMIN_TIMER_BASE	STM32_TIM12_BASE
+# define PWMIN_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define PWMIN_TIMER_POWER_BIT	RCC_APB2ENR_TIM12EN
+# define PWMIN_TIMER_VECTOR	STM32_IRQ_TIM1TRGCOM
+# define PWMIN_TIMER_CLOCK	STM32_APB2_TIM12_CLKIN
+#else
+# error PWMIN_TIMER must be a value between 1 and 12
+#endif
+
+/*
+ * HRT clock must be at least 1MHz
+ */
+#if PWMIN_TIMER_CLOCK <= 1000000
+# error PWMIN_TIMER_CLOCK must be greater than 1MHz
+#endif
+
+/*
+ * Timer register accessors
+ */
+#define REG(_reg)	(*(volatile uint32_t *)(PWMIN_TIMER_BASE + _reg))
+
+#define rCR1		REG(STM32_GTIM_CR1_OFFSET)
+#define rCR2		REG(STM32_GTIM_CR2_OFFSET)
+#define rSMCR		REG(STM32_GTIM_SMCR_OFFSET)
+#define rDIER		REG(STM32_GTIM_DIER_OFFSET)
+#define rSR		REG(STM32_GTIM_SR_OFFSET)
+#define rEGR		REG(STM32_GTIM_EGR_OFFSET)
+#define rCCMR1		REG(STM32_GTIM_CCMR1_OFFSET)
+#define rCCMR2		REG(STM32_GTIM_CCMR2_OFFSET)
+#define rCCER		REG(STM32_GTIM_CCER_OFFSET)
+#define rCNT		REG(STM32_GTIM_CNT_OFFSET)
+#define rPSC		REG(STM32_GTIM_PSC_OFFSET)
+#define rARR		REG(STM32_GTIM_ARR_OFFSET)
+#define rCCR1		REG(STM32_GTIM_CCR1_OFFSET)
+#define rCCR2		REG(STM32_GTIM_CCR2_OFFSET)
+#define rCCR3		REG(STM32_GTIM_CCR3_OFFSET)
+#define rCCR4		REG(STM32_GTIM_CCR4_OFFSET)
+#define rDCR		REG(STM32_GTIM_DCR_OFFSET)
+#define rDMAR		REG(STM32_GTIM_DMAR_OFFSET)
+
+/*
+ * Specific registers and bits used by HRT sub-functions
+ */
+#if PWMIN_TIMER_CHANNEL == 1
+ #define rCCR_PWMIN_A		rCCR1			/* compare register for PWMIN */
+ #define DIER_PWMIN_A		(GTIM_DIER_CC1IE) 	/* interrupt enable for PWMIN */
+ #define SR_INT_PWMIN_A		GTIM_SR_CC1IF		/* interrupt status for PWMIN */
+ #define rCCR_PWMIN_B		rCCR2 			/* compare register for PWMIN */
+ #define SR_INT_PWMIN_B		GTIM_SR_CC2IF		/* interrupt status for PWMIN */
+ #define CCMR1_PWMIN		((0x02 << GTIM_CCMR1_CC2S_SHIFT) | (0x01 << GTIM_CCMR1_CC1S_SHIFT))
+ #define CCMR2_PWMIN		0
+ #define CCER_PWMIN		(GTIM_CCER_CC2P | GTIM_CCER_CC1E | GTIM_CCER_CC2E)
+ #define SR_OVF_PWMIN		(GTIM_SR_CC1OF | GTIM_SR_CC2OF)
+ #define SMCR_PWMIN_1		(0x05 << GTIM_SMCR_TS_SHIFT)
+ #define SMCR_PWMIN_2		((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1)
+#elif PWMIN_TIMER_CHANNEL == 2
+ #define rCCR_PWMIN_A		rCCR2			/* compare register for PWMIN */
+ #define DIER_PWMIN_A		(GTIM_DIER_CC2IE)	/* interrupt enable for PWMIN */
+ #define SR_INT_PWMIN_A		GTIM_SR_CC2IF		/* interrupt status for PWMIN */
+ #define rCCR_PWMIN_B		rCCR1			/* compare register for PWMIN */
+ #define DIER_PWMIN_B		GTIM_DIER_CC1IE		/* interrupt enable for PWMIN */
+ #define SR_INT_PWMIN_B		GTIM_SR_CC1IF		/* interrupt status for PWMIN */
+ #define CCMR1_PWMIN		((0x01 << GTIM_CCMR1_CC2S_SHIFT) | (0x02 << GTIM_CCMR1_CC1S_SHIFT))
+ #define CCMR2_PWMIN		0
+ #define CCER_PWMIN		(GTIM_CCER_CC1P | GTIM_CCER_CC1E | GTIM_CCER_CC2E)
+ #define SR_OVF_PWMIN		(GTIM_SR_CC1OF | GTIM_SR_CC2OF)
+ #define SMCR_PWMIN_1		(0x06 << GTIM_SMCR_TS_SHIFT)
+ #define SMCR_PWMIN_2		((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1)
+#else
+ #error PWMIN_TIMER_CHANNEL must be either 1 and 2.
+#endif
+
+
+class PWMIN : device::CDev
+{
+public:
+	PWMIN();
+	virtual ~PWMIN();
+
+	virtual int init();
+	virtual int open(struct file *filp);
+	virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
+	virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	void _publish(uint16_t status, uint32_t period, uint32_t pulse_width);
+	void _print_info(void);
+
+private:
+	uint32_t error_count;
+	uint32_t pulses_captured;
+	uint32_t last_period;
+	uint32_t last_width;
+	RingBuffer *reports;
+	bool timer_started;
+
+	void timer_init(void);
+};
+
+static int pwmin_tim_isr(int irq, void *context);
+static void pwmin_start(void);
+static void pwmin_info(void);
+static void pwmin_test(void);
+static void pwmin_reset(void);
+
+static PWMIN *g_dev;
+
+PWMIN::PWMIN() :
+	CDev("pwmin", PWMIN0_DEVICE_PATH),
+	error_count(0),
+	pulses_captured(0),
+	last_period(0),
+	last_width(0),
+	reports(nullptr),
+	timer_started(false)
+{
+}
+
+PWMIN::~PWMIN()
+{
+	if (reports != nullptr)
+		delete reports;
+}
+
+/*
+  initialise the driver. This doesn't actually start the timer (that
+  is done on open). We don't start the timer to allow for this driver
+  to be started in init scripts when the user may be using the input
+  pin as PWM output
+ */
+int
+PWMIN::init()
+{
+	// we just register the device in /dev, and only actually
+	// activate the timer when requested to when the device is opened
+	CDev::init();
+
+	reports = new RingBuffer(2, sizeof(struct pwm_input_s));
+	if (reports == nullptr) {
+		return -ENOMEM;
+	}
+
+	return OK;
+}
+
+/*
+ * Initialise the timer we are going to use.
+ */
+void PWMIN::timer_init(void) 
+{
+	// run with interrupts disabled in case the timer is already
+	// setup. We don't want it firing while we are doing the setup
+	irqstate_t flags = irqsave();
+	stm32_configgpio(GPIO_PWM_IN);
+
+	/* claim our interrupt vector */
+	irq_attach(PWMIN_TIMER_VECTOR, pwmin_tim_isr);
+
+	/* Clear no bits, set timer enable bit.*/
+	modifyreg32(PWMIN_TIMER_POWER_REG, 0, PWMIN_TIMER_POWER_BIT); 
+
+	/* disable and configure the timer */
+	rCR1 = 0;
+	rCR2 = 0;
+	rSMCR = 0;
+	rDIER = DIER_PWMIN_A;
+	rCCER = 0;		/* unlock CCMR* registers */
+	rCCMR1 = CCMR1_PWMIN;
+	rCCMR2 = CCMR2_PWMIN;
+	rSMCR = SMCR_PWMIN_1;	/* Set up mode */
+	rSMCR = SMCR_PWMIN_2;	/* Enable slave mode controller */
+	rCCER = CCER_PWMIN;
+	rDCR = 0;
+
+	// for simplicity scale by the clock in MHz. This gives us
+	// readings in microseconds which is typically what is needed
+	// for a PWM input driver
+	uint32_t prescaler = PWMIN_TIMER_CLOCK/1000000UL;
+
+	/* 
+	 * define the clock speed. We want the highest possible clock 
+	 * speed that avoids overflows.
+	 */
+	rPSC = prescaler - 1;
+
+	/* run the full span of the counter. All timers can handle
+	 * uint16 */
+	rARR = UINT16_MAX;
+
+	/* generate an update event; reloads the counter, all registers */
+	rEGR = GTIM_EGR_UG;
+
+	/* enable the timer */
+	rCR1 = GTIM_CR1_CEN;
+
+	/* enable interrupts */
+	up_enable_irq(PWMIN_TIMER_VECTOR);
+
+	irqrestore(flags);
+
+	timer_started = true;
+}
+
+/*
+  hook for open of the driver. We start the timer at this point, then
+  leave it running
+ */
+int
+PWMIN::open(struct file *filp)
+{
+	if (g_dev == nullptr) {
+		return -EIO;
+	}
+	int ret = CDev::open(filp);	
+	if (ret == OK && !timer_started) {
+		g_dev->timer_init();
+	}
+	return ret;
+}
+
+
+/*
+  handle ioctl requests
+ */
+int
+PWMIN::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+	case SENSORIOCSQUEUEDEPTH: {
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 1) || (arg > 500))
+				return -EINVAL;
+
+			irqstate_t flags = irqsave();
+			if (!reports->resize(arg)) {
+				irqrestore(flags);
+				return -ENOMEM;
+			}
+			irqrestore(flags);
+
+			return OK;
+		}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return reports->size();
+
+	case SENSORIOCRESET:
+		/* user has asked for the timer to be reset. This may
+		   be needed if the pin was used for a different
+		   purpose (such as PWM output)
+		*/
+		timer_init();
+		return OK;
+
+	default:
+		/* give it to the superclass */
+		return CDev::ioctl(filp, cmd, arg);
+	}
+}
+
+
+/*
+  read some samples from the device
+ */
+ssize_t
+PWMIN::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct pwm_input_s);
+	struct pwm_input_s *buf = reinterpret_cast<struct pwm_input_s *>(buffer);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1)
+		return -ENOSPC;
+
+	while (count--) {
+		if (reports->get(buf)) {
+			ret += sizeof(struct pwm_input_s);
+			buf++;
+		}
+	}
+
+	/* if there was no data, warn the caller */
+	return ret ? ret : -EAGAIN;
+}
+
+/*
+  publish some data from the ISR in the ring buffer
+ */
+void PWMIN::_publish(uint16_t status, uint32_t period, uint32_t pulse_width)
+{
+	/* if we missed an edge, we have to give up */
+	if (status & SR_OVF_PWMIN) {
+		error_count++;
+		return;
+	}
+
+	struct pwm_input_s pwmin_report;
+	pwmin_report.timestamp = hrt_absolute_time();
+	pwmin_report.error_count = error_count;
+	pwmin_report.period = period;
+	pwmin_report.pulse_width = pulse_width;
+
+	reports->force(&pwmin_report);
+
+	last_period = period;
+	last_width = pulse_width;
+	pulses_captured++;
+}
+
+/*
+  print information on the last captured
+ */
+void PWMIN::_print_info(void)
+{
+	if (!timer_started) {
+		printf("timer not started - try the 'test' command\n");
+	} else {
+		printf("count=%u period=%u width=%u\n",
+		       (unsigned)pulses_captured,
+		       (unsigned)last_period,
+		       (unsigned)last_width);
+	}
+}
+
+
+/*
+  Handle the interupt, gathering pulse data
+ */
+static int pwmin_tim_isr(int irq, void *context)
+{
+	uint16_t status = rSR;
+	uint32_t period = rCCR_PWMIN_A;
+	uint32_t pulse_width = rCCR_PWMIN_B;
+
+	/* ack the interrupts we just read */
+	rSR = 0;
+
+	if (g_dev != nullptr) {
+		g_dev->_publish(status, period, pulse_width);
+	}
+
+	return OK;
+}
+
+/*
+  start the driver
+ */
+static void pwmin_start(void)
+{
+	if (g_dev != nullptr) {
+		printf("driver already started\n");
+		exit(1);
+	}
+	g_dev = new PWMIN();
+	if (g_dev == nullptr) {
+		errx(1, "driver allocation failed");
+	}
+	if (g_dev->init() != OK) {
+		errx(1, "driver init failed");
+	}
+	exit(0);
+}
+
+/*
+  test the driver
+ */
+static void pwmin_test(void)
+{
+	int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY);
+	if (fd == -1) {
+		errx(1, "Failed to open device");
+	}
+	uint64_t start_time = hrt_absolute_time();
+
+	printf("Showing samples for 5 seconds\n");
+
+	while (hrt_absolute_time() < start_time+5U*1000UL*1000UL) {
+		struct pwm_input_s buf;
+		if (::read(fd, &buf, sizeof(buf)) == sizeof(buf)) {
+			printf("period=%u width=%u error_count=%u\n",
+			       (unsigned)buf.period,
+			       (unsigned)buf.pulse_width,
+			       (unsigned)buf.error_count);
+		}
+	}
+	close(fd);
+	exit(0);
+}
+
+/*
+  reset the timer
+ */
+static void pwmin_reset(void)
+{
+	int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY);
+	if (fd == -1) {
+		errx(1, "Failed to open device");
+	}
+	if (ioctl(fd, SENSORIOCRESET, 0) != OK) {
+		errx(1, "reset failed");
+	}
+	close(fd);
+	exit(0);
+}
+
+/*
+  show some information on the driver
+ */
+static void pwmin_info(void)
+{
+	if (g_dev == nullptr) {
+		printf("driver not started\n");
+		exit(1);
+	}
+	g_dev->_print_info();
+	exit(0);
+}
+
+
+/*
+  driver entry point
+ */
+int pwm_input_main(int argc, char * argv[]) 
+{
+	const char *verb = argv[1];
+
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(verb, "start")) {
+		pwmin_start();
+	}
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(verb, "info")) {
+		pwmin_info();
+	}
+
+	/*
+	 * print test results
+	 */
+	if (!strcmp(verb, "test")) {
+		pwmin_test();
+	}
+
+	/*
+	 * reset the timer
+	 */
+	if (!strcmp(verb, "reset")) {
+		pwmin_reset();
+	}
+
+	errx(1, "unrecognized command, try 'start', 'info', 'reset' or 'test'");
+	return 0;
+}

src/drivers/px4fmu/fmu.cpp

@@ -182,6 +182,7 @@ private:
 
 	void		gpio_reset(void);
 	void		sensor_reset(int ms);
+	void		peripheral_reset(int ms);
 	void		gpio_set_function(uint32_t gpios, int function);
 	void		gpio_write(uint32_t gpios, int function);
 	uint32_t	gpio_read(void);
@@ -1376,6 +1377,29 @@ PX4FMU::sensor_reset(int ms)
 #endif
 }
 
+void
+PX4FMU::peripheral_reset(int ms)
+{
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+
+	if (ms < 1) {
+		ms = 10;
+	}
+
+	/* set the peripheral rails off */
+	stm32_configgpio(GPIO_VDD_5V_PERIPH_EN);
+	stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 1);
+
+	/* wait for the peripheral rail to reach GND */
+	usleep(ms * 1000);
+	warnx("reset done, %d ms", ms);
+
+	/* re-enable power */
+
+	/* switch the peripheral rail back on */
+	stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 0);
+#endif
+}
 
 void
 PX4FMU::gpio_reset(void)
@@ -1488,6 +1512,10 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
 		sensor_reset(arg);
 		break;
 
+	case GPIO_PERIPHERAL_RAIL_RESET:
+		peripheral_reset(arg);
+		break;
+
 	case GPIO_SET_OUTPUT:
 	case GPIO_SET_INPUT:
 	case GPIO_SET_ALT_1:
@@ -1529,6 +1557,7 @@ enum PortMode {
 	PORT_GPIO_AND_SERIAL,
 	PORT_PWM_AND_SERIAL,
 	PORT_PWM_AND_GPIO,
+	PORT_PWM4,
 };
 
 PortMode g_port_mode;
@@ -1564,6 +1593,13 @@ fmu_new_mode(PortMode new_mode)
 #endif
 		break;
 
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+	case PORT_PWM4:
+		/* select 4-pin PWM mode */
+		servo_mode = PX4FMU::MODE_4PWM;
+		break;
+#endif
+
 		/* mixed modes supported on v1 board only */
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 
@@ -1666,6 +1702,24 @@ sensor_reset(int ms)
 	close(fd);
 }
 
+void
+peripheral_reset(int ms)
+{
+	int	 fd;
+
+	fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
+
+	if (fd < 0) {
+		errx(1, "open fail");
+	}
+
+	if (ioctl(fd, GPIO_PERIPHERAL_RAIL_RESET, ms) < 0) {
+		warnx("peripheral rail reset failed");
+	}
+
+	close(fd);
+}
+
 void
 test(void)
 {
@@ -1836,6 +1890,10 @@ fmu_main(int argc, char *argv[])
 	} else if (!strcmp(verb, "mode_pwm")) {
 		new_mode = PORT_FULL_PWM;
 
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+	} else if (!strcmp(verb, "mode_pwm4")) {
+		new_mode = PORT_PWM4;
+#endif
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 
 	} else if (!strcmp(verb, "mode_serial")) {
@@ -1883,6 +1941,19 @@ fmu_main(int argc, char *argv[])
 		exit(0);
 	}
 
+	if (!strcmp(verb, "peripheral_reset")) {
+		if (argc > 2) {
+			int reset_time = strtol(argv[2], 0, 0);
+			peripheral_reset(reset_time);
+
+		} else {
+			peripheral_reset(0);
+			warnx("resettet default time");
+		}
+
+		exit(0);
+	}
+
 	if (!strcmp(verb, "i2c")) {
 		if (argc > 3) {
 			int bus = strtol(argv[2], 0, 0);