Merge remote-tracking branch 'upstream/master' into diff_press_filter

Conflicts:
	src/modules/sdlog2/sdlog2.c

ROMFS/px4fmu_common/init.d/10016_3dr_iris

@@ -10,11 +10,11 @@ sh /etc/init.d/rc.mc_defaults
 if [ $DO_AUTOCONFIG == yes ]
 then
 	# TODO tune roll/pitch separately
-	param set MC_ROLL_P 9.0
+	param set MC_ROLL_P 7.0
 	param set MC_ROLLRATE_P 0.13
 	param set MC_ROLLRATE_I 0.0
 	param set MC_ROLLRATE_D 0.004
-	param set MC_PITCH_P 9.0
+	param set MC_PITCH_P 7.0
 	param set MC_PITCHRATE_P 0.13
 	param set MC_PITCHRATE_I 0.0
 	param set MC_PITCHRATE_D 0.004

ROMFS/px4fmu_common/init.d/4008_ardrone

@@ -23,13 +23,15 @@ then
     param set MC_PITCHRATE_I 0.0
     param set MC_PITCHRATE_D 0.0
     param set MC_YAW_P 1.0
-    param set MC_YAW_D 0.1
     param set MC_YAWRATE_P 0.15
     param set MC_YAWRATE_I 0.0
     param set MC_YAWRATE_D 0.0
     param set MC_YAW_FF 0.15
+    param set BAT_V_SCALING 0.00838095238
 fi
 
 set OUTPUT_MODE ardrone
 set USE_IO no
 set MIXER skip
+# set MAV_TYPE because no specific mixer is set
+set MAV_TYPE 2

ROMFS/px4fmu_common/init.d/rcS

@@ -118,6 +118,7 @@ then
 	set MKBLCTRL_MODE none
 	set FMU_MODE pwm
 	set MAV_TYPE none
+	set LOAD_DEFAULT_APPS yes
 	
 	#
 	# Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts
@@ -465,8 +466,11 @@ then
 		sh /etc/init.d/rc.interface
 		
 		# Start standard fixedwing apps
+		if [ LOAD_DEFAULT_APPS == yes ]
+		then
 			sh /etc/init.d/rc.fw_apps
 		fi
+	fi
 
 	#
 	# Multicopters setup
@@ -521,8 +525,11 @@ then
 		sh /etc/init.d/rc.interface
 		
 		# Start standard multicopter apps
+		if [ LOAD_DEFAULT_APPS == yes ]
+		then
 			sh /etc/init.d/rc.mc_apps
 		fi
+	fi
 
 	#
 	# Generic setup (autostart ID not found)

Tools/fetch_log.py

@@ -0,0 +1,133 @@
+#!/usr/bin/env python
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Log fetcher
+#
+# Print list of logs:
+#     python fetch_log.py
+#
+# Fetch log:
+#     python fetch_log.py sess001/log001.bin
+#
+
+import serial, time, sys, os
+
+def wait_for_string(ser, s, timeout=1.0, debug=False):
+    t0 = time.time()
+    buf = []
+    res = []
+    n = 0
+    while (True):
+        c = ser.read()
+        if debug:
+            sys.stderr.write(c)
+        buf.append(c)
+        if len(buf) > len(s):
+            res.append(buf.pop(0))
+            n += 1
+            if n % 10000 == 0:
+                sys.stderr.write(str(n) + "\n")
+        if "".join(buf) == s:
+            break
+        if timeout > 0.0 and time.time() - t0 > timeout:
+            raise Exception("Timeout while waiting for: " + s)
+    return "".join(res)
+
+def exec_cmd(ser, cmd, timeout):
+    ser.write(cmd + "\n")
+    ser.flush()
+    wait_for_string(ser, cmd + "\r\n", timeout)
+    return wait_for_string(ser, "nsh> \x1b[K", timeout)
+
+def ls_dir(ser, dir, timeout=1.0):
+    res = []
+    for line in exec_cmd(ser, "ls -l " + dir, timeout).splitlines()[1:]:
+        res.append((line[20:], int(line[11:19].strip()), line[1] == "d"))
+    return res
+
+def list_logs(ser):
+    logs_dir = "/fs/microsd/log"
+    res = []
+    for d in ls_dir(ser, logs_dir):
+        if d[2]:
+            sess_dir = d[0][:-1]
+            for f in ls_dir(ser, logs_dir + "/" + sess_dir):
+                log_file = f[0]
+                log_size = f[1]
+                res.append(sess_dir + "/" + log_file + "\t" + str(log_size))
+    return "\n".join(res)
+
+def fetch_log(ser, fn, timeout):
+    cmd = "dumpfile " + fn
+    ser.write(cmd + "\n")
+    ser.flush()
+    wait_for_string(ser, cmd + "\r\n", timeout, True)
+    res = wait_for_string(ser, "\n", timeout, True)
+    data = []
+    if res.startswith("OK"):
+        size = int(res.split()[1])
+        n = 0
+        print "Reading data:"
+        while (n < size):
+            buf = ser.read(min(size - n, 8192))
+            data.append(buf)
+            n += len(buf)
+            sys.stdout.write(".")
+            sys.stdout.flush()
+        print
+    else:
+        raise Exception("Error reading log")
+    wait_for_string(ser, "nsh> \x1b[K", timeout)
+    return "".join(data)
+
+def main():
+    dev = "/dev/tty.usbmodem1"
+    ser = serial.Serial(dev, "115200", timeout=0.2)
+    if len(sys.argv) < 2:
+        print list_logs(ser)
+    else:
+        log_file = sys.argv[1]
+        data = fetch_log(ser, "/fs/microsd/log/" + log_file, 1.0)
+        try:
+            os.mkdir(log_file.split("/")[0])
+        except:
+            pass
+        fout = open(log_file, "wb")
+        fout.write(data)
+        fout.close()
+    ser.close()
+
+if __name__ == "__main__":
+    main()

makefiles/config_px4fmu-v1_default.mk

@@ -56,6 +56,7 @@ MODULES		+= systemcmds/tests
 MODULES		+= systemcmds/config
 MODULES		+= systemcmds/nshterm
 MODULES		+= systemcmds/hw_ver
+MODULES		+= systemcmds/dumpfile
 
 #
 # General system control

makefiles/config_px4fmu-v2_default.mk

@@ -27,6 +27,7 @@ MODULES		+= drivers/l3gd20
 MODULES		+= drivers/hmc5883
 MODULES		+= drivers/ms5611
 MODULES		+= drivers/mb12xx
+MODULES		+= drivers/sf0x
 MODULES		+= drivers/gps
 MODULES		+= drivers/hil
 MODULES		+= drivers/hott/hott_telemetry
@@ -63,6 +64,7 @@ MODULES		+= systemcmds/config
 MODULES		+= systemcmds/nshterm
 MODULES		+= systemcmds/mtd
 MODULES		+= systemcmds/hw_ver
+MODULES		+= systemcmds/dumpfile
 
 #
 # General system control
@@ -71,6 +73,7 @@ MODULES		+= modules/commander
 MODULES		+= modules/navigator
 MODULES		+= modules/mavlink
 MODULES		+= modules/mavlink_onboard
+MODULES		+= modules/gpio_led
 
 #
 # Estimation modules (EKF/ SO3 / other filters)

src/drivers/drv_px4flow.h

@@ -0,0 +1,84 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 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 Rangefinder driver interface.
+ */
+
+#ifndef _DRV_PX4FLOW_H
+#define _DRV_PX4FLOW_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_sensor.h"
+#include "drv_orb_dev.h"
+
+#define PX4FLOW_DEVICE_PATH	"/dev/px4flow"
+
+/**
+ * Optical flow in NED body frame in SI units.
+ *
+ * @see http://en.wikipedia.org/wiki/International_System_of_Units
+ */
+struct px4flow_report {
+
+	uint64_t timestamp;		/**< in microseconds since system start          */
+
+	int16_t flow_raw_x;		/**< flow in pixels in X direction, not rotation-compensated */
+	int16_t flow_raw_y;		/**< flow in pixels in Y direction, not rotation-compensated */
+	float flow_comp_x_m;		/**< speed over ground in meters, rotation-compensated */
+	float flow_comp_y_m;		/**< speed over ground in meters, rotation-compensated */
+	float ground_distance_m;	/**< Altitude / distance to ground in meters */
+	uint8_t	quality;		/**< Quality of the measurement, 0: bad quality, 255: maximum quality */
+	uint8_t sensor_id;		/**< id of the sensor emitting the flow value */
+
+};
+
+/*
+ * ObjDev tag for px4flow data.
+ */
+ORB_DECLARE(optical_flow);
+
+/*
+ * ioctl() definitions
+ *
+ * px4flow drivers also implement the generic sensor driver
+ * interfaces from drv_sensor.h
+ */
+
+#define _PX4FLOWIOCBASE			(0x7700)
+#define __PX4FLOWIOC(_n)		(_IOC(_PX4FLOWIOCBASE, _n))
+
+
+#endif /* _DRV_PX4FLOW_H */

src/drivers/mb12xx/mb12xx.cpp

@@ -200,7 +200,7 @@ MB12XX::MB12XX(int bus, int address) :
 	_buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows"))
 {
 	// enable debug() calls
-	_debug_enabled = true;
+	_debug_enabled = false;
 
 	// work_cancel in the dtor will explode if we don't do this...
 	memset(&_work, 0, sizeof(_work));
@@ -212,8 +212,9 @@ MB12XX::~MB12XX()
 	stop();
 
 	/* free any existing reports */
-	if (_reports != nullptr)
+	if (_reports != nullptr) {
 		delete _reports;
+	}
 }
 
 int
@@ -222,22 +223,25 @@ MB12XX::init()
 	int ret = ERROR;
 
 	/* do I2C init (and probe) first */
-	if (I2C::init() != OK)
+	if (I2C::init() != OK) {
 		goto out;
+	}
 
 	/* allocate basic report buffers */
 	_reports = new RingBuffer(2, sizeof(range_finder_report));
 
-	if (_reports == nullptr)
+	if (_reports == nullptr) {
 		goto out;
+	}
 
 	/* get a publish handle on the range finder topic */
 	struct range_finder_report zero_report;
 	memset(&zero_report, 0, sizeof(zero_report));
 	_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report);
 
-	if (_range_finder_topic < 0)
+	if (_range_finder_topic < 0) {
 		debug("failed to create sensor_range_finder object. Did you start uOrb?");
+	}
 
 	ret = OK;
 	/* sensor is ok, but we don't really know if it is within range */
@@ -307,8 +311,9 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
 					_measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL);
 
 					/* if we need to start the poll state machine, do it */
-					if (want_start)
+					if (want_start) {
 						start();
+					}
 
 					return OK;
 				}
@@ -322,15 +327,17 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
 					unsigned ticks = USEC2TICK(1000000 / arg);
 
 					/* check against maximum rate */
-					if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL))
+					if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL)) {
 						return -EINVAL;
+					}
 
 					/* update interval for next measurement */
 					_measure_ticks = ticks;
 
 					/* if we need to start the poll state machine, do it */
-					if (want_start)
+					if (want_start) {
 						start();
+					}
 
 					return OK;
 				}
@@ -338,21 +345,25 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
 		}
 
 	case SENSORIOCGPOLLRATE:
-		if (_measure_ticks == 0)
+		if (_measure_ticks == 0) {
 			return SENSOR_POLLRATE_MANUAL;
+		}
 
 		return (1000 / _measure_ticks);
 
 	case SENSORIOCSQUEUEDEPTH: {
 			/* lower bound is mandatory, upper bound is a sanity check */
-		if ((arg < 1) || (arg > 100))
+			if ((arg < 1) || (arg > 100)) {
 				return -EINVAL;
+			}
 
 			irqstate_t flags = irqsave();
+
 			if (!_reports->resize(arg)) {
 				irqrestore(flags);
 				return -ENOMEM;
 			}
+
 			irqrestore(flags);
 
 			return OK;
@@ -365,18 +376,18 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
 		/* XXX implement this */
 		return -EINVAL;
 
-	case RANGEFINDERIOCSETMINIUMDISTANCE:
+	case RANGEFINDERIOCSETMINIUMDISTANCE: {
-	{
 			set_minimum_distance(*(float *)arg);
 			return 0;
 		}
 		break;
-	case RANGEFINDERIOCSETMAXIUMDISTANCE:
+
-	{
+	case RANGEFINDERIOCSETMAXIUMDISTANCE: {
 			set_maximum_distance(*(float *)arg);
 			return 0;
 		}
 		break;
+
 	default:
 		/* give it to the superclass */
 		return I2C::ioctl(filp, cmd, arg);
@@ -391,8 +402,9 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
 	int ret = 0;
 
 	/* buffer must be large enough */
-	if (count < 1)
+	if (count < 1) {
 		return -ENOSPC;
+	}
 
 	/* if automatic measurement is enabled */
 	if (_measure_ticks > 0) {
@@ -453,12 +465,12 @@ MB12XX::measure()
 	uint8_t cmd = MB12XX_TAKE_RANGE_REG;
 	ret = transfer(&cmd, 1, nullptr, 0);
 
-	if (OK != ret)
+	if (OK != ret) {
-	{
 		perf_count(_comms_errors);
 		log("i2c::transfer returned %d", ret);
 		return ret;
 	}
+
 	ret = OK;
 
 	return ret;
@@ -476,8 +488,7 @@ MB12XX::collect()
 
 	ret = transfer(nullptr, 0, &val[0], 2);
 
-	if (ret < 0)
+	if (ret < 0) {
-	{
 		log("error reading from sensor: %d", ret);
 		perf_count(_comms_errors);
 		perf_end(_sample_perf);
@@ -485,7 +496,7 @@ MB12XX::collect()
 	}
 
 	uint16_t distance = val[0] << 8 | val[1];
-	float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */
+	float si_units = (distance * 1.0f) / 100.0f; /* cm to m */
 	struct range_finder_report report;
 
 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
@@ -525,11 +536,13 @@ MB12XX::start()
 		true,
 		true,
 		true,
-		SUBSYSTEM_TYPE_RANGEFINDER};
+		SUBSYSTEM_TYPE_RANGEFINDER
+	};
 	static orb_advert_t pub = -1;
 
 	if (pub > 0) {
 		orb_publish(ORB_ID(subsystem_info), pub, &info);
+
 	} else {
 		pub = orb_advertise(ORB_ID(subsystem_info), &info);
 	}
@@ -583,8 +596,9 @@ MB12XX::cycle()
 	}
 
 	/* measurement phase */
-	if (OK != measure())
+	if (OK != measure()) {
 		log("measure error");
+	}
 
 	/* next phase is collection */
 	_collect_phase = true;
@@ -635,33 +649,37 @@ start()
 {
 	int fd;
 
-	if (g_dev != nullptr)
+	if (g_dev != nullptr) {
 		errx(1, "already started");
+	}
 
 	/* create the driver */
 	g_dev = new MB12XX(MB12XX_BUS);
 
-	if (g_dev == nullptr)
+	if (g_dev == nullptr) {
 		goto fail;
+	}
 
-	if (OK != g_dev->init())
+	if (OK != g_dev->init()) {
 		goto fail;
+	}
 
 	/* set the poll rate to default, starts automatic data collection */
 	fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		goto fail;
+	}
 
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
 		goto fail;
+	}
 
 	exit(0);
 
 fail:
 
-	if (g_dev != nullptr) 
+	if (g_dev != nullptr) {
-	{
 		delete g_dev;
 		g_dev = nullptr;
 	}
@@ -674,15 +692,14 @@ fail:
  */
 void stop()
 {
-	if (g_dev != nullptr)
+	if (g_dev != nullptr) {
-	{
 		delete g_dev;
 		g_dev = nullptr;
-	}
+
-	else
+	} else {
-	{
 		errx(1, "driver not running");
 	}
+
 	exit(0);
 }
 
@@ -700,22 +717,25 @@ test()
 
 	int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		err(1, "%s open failed (try 'mb12xx start' if the driver is not running", RANGE_FINDER_DEVICE_PATH);
+	}
 
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
 
-	if (sz != sizeof(report))
+	if (sz != sizeof(report)) {
 		err(1, "immediate read failed");
+	}
 
 	warnx("single read");
 	warnx("measurement: %0.2f m", (double)report.distance);
 	warnx("time:        %lld", report.timestamp);
 
 	/* start the sensor polling at 2Hz */
-	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
 		errx(1, "failed to set 2Hz poll rate");
+	}
 
 	/* read the sensor 5x and report each value */
 	for (unsigned i = 0; i < 5; i++) {
@@ -726,20 +746,27 @@ test()
 		fds.events = POLLIN;
 		ret = poll(&fds, 1, 2000);
 
-		if (ret != 1)
+		if (ret != 1) {
 			errx(1, "timed out waiting for sensor data");
+		}
 
 		/* now go get it */
 		sz = read(fd, &report, sizeof(report));
 
-		if (sz != sizeof(report))
+		if (sz != sizeof(report)) {
 			err(1, "periodic read failed");
+		}
 
 		warnx("periodic read %u", i);
 		warnx("measurement: %0.3f", (double)report.distance);
 		warnx("time:        %lld", report.timestamp);
 	}
 
+	/* reset the sensor polling to default rate */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
+		errx(1, "failed to set default poll rate");
+	}
+
 	errx(0, "PASS");
 }
 
@@ -751,14 +778,17 @@ reset()
 {
 	int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		err(1, "failed ");
+	}
 
-	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
 		err(1, "driver reset failed");
+	}
 
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
 		err(1, "driver poll restart failed");
+	}
 
 	exit(0);
 }
@@ -769,8 +799,9 @@ reset()
 void
 info()
 {
-	if (g_dev == nullptr)
+	if (g_dev == nullptr) {
 		errx(1, "driver not running");
+	}
 
 	printf("state @ %p\n", g_dev);
 	g_dev->print_info();
@@ -786,32 +817,37 @@ mb12xx_main(int argc, char *argv[])
 	/*
 	 * Start/load the driver.
 	 */
-	if (!strcmp(argv[1], "start"))
+	if (!strcmp(argv[1], "start")) {
 		mb12xx::start();
+	}
 
 	/*
 	 * Stop the driver
 	 */
-	 if (!strcmp(argv[1], "stop"))
+	if (!strcmp(argv[1], "stop")) {
 		mb12xx::stop();
+	}
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(argv[1], "test"))
+	if (!strcmp(argv[1], "test")) {
 		mb12xx::test();
+	}
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset"))
+	if (!strcmp(argv[1], "reset")) {
 		mb12xx::reset();
+	}
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
 		mb12xx::info();
+	}
 
 	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
 }

src/drivers/px4flow/module.mk

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 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 PX4FLOW driver.
+#
+
+MODULE_COMMAND	= px4flow
+
+SRCS		= px4flow.cpp

src/drivers/px4flow/px4flow.cpp

@@ -0,0 +1,806 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 px4flow.cpp
+ * @author Dominik Honegger
+ *
+ * Driver for the PX4FLOW module connected via I2C.
+ */
+	 
+#include <nuttx/config.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_px4flow.h>
+#include <drivers/device/ringbuffer.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/subsystem_info.h>
+//#include <uORB/topics/optical_flow.h>
+
+#include <board_config.h>
+
+/* Configuration Constants */
+#define PX4FLOW_BUS 			PX4_I2C_BUS_EXPANSION
+#define I2C_FLOW_ADDRESS 		0x45 //* 7-bit address. 8-bit address is 0x8A
+//range 0x42 - 0x49
+
+/* PX4FLOW Registers addresses */
+#define PX4FLOW_REG	0x00		/* Measure Register */
+
+#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+//struct i2c_frame
+//{
+//    uint16_t frame_count;
+//    int16_t pixel_flow_x_sum;
+//    int16_t pixel_flow_y_sum;
+//    int16_t flow_comp_m_x;
+//    int16_t flow_comp_m_y;
+//    int16_t qual;
+//    int16_t gyro_x_rate;
+//    int16_t gyro_y_rate;
+//    int16_t gyro_z_rate;
+//    uint8_t gyro_range;
+//    uint8_t sonar_timestamp;
+//    int16_t ground_distance;
+//};
+//
+//struct i2c_frame f;
+
+class PX4FLOW : public device::I2C
+{
+public:
+	PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS);
+	virtual ~PX4FLOW();
+	
+	virtual int 		init();
+	
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
+	
+	/**
+	* Diagnostics - print some basic information about the driver.
+	*/
+	void				print_info();
+	
+protected:
+	virtual int			probe();
+
+private:
+
+	work_s				_work;
+	RingBuffer			*_reports;
+	bool				_sensor_ok;
+	int					_measure_ticks;
+	bool				_collect_phase;
+	
+	orb_advert_t		_px4flow_topic;
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_comms_errors;
+	perf_counter_t		_buffer_overflows;
+	
+	/**
+	* Test whether the device supported by the driver is present at a
+	* specific address.
+	*
+	* @param address	The I2C bus address to probe.
+	* @return		True if the device is present.
+	*/
+	int					probe_address(uint8_t address);
+	
+	/**
+	* Initialise the automatic measurement state machine and start it.
+	*
+	* @note This function is called at open and error time.  It might make sense
+	*       to make it more aggressive about resetting the bus in case of errors.
+	*/
+	void				start();
+	
+	/**
+	* Stop the automatic measurement state machine.
+	*/
+	void				stop();
+	
+	/**
+	* Perform a poll cycle; collect from the previous measurement
+	* and start a new one.
+	*/
+	void				cycle();
+	int					measure();
+	int					collect();
+	/**
+	* Static trampoline from the workq context; because we don't have a
+	* generic workq wrapper yet.
+	*
+	* @param arg		Instance pointer for the driver that is polling.
+	*/
+	static void		cycle_trampoline(void *arg);
+	
+	
+};
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int px4flow_main(int argc, char *argv[]);
+
+PX4FLOW::PX4FLOW(int bus, int address) :
+	I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000),//400khz
+	_reports(nullptr),
+	_sensor_ok(false),
+	_measure_ticks(0),
+	_collect_phase(false),
+	_px4flow_topic(-1),
+	_sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")),
+	_buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows"))
+{
+	// enable debug() calls
+	_debug_enabled = true;
+	
+	// work_cancel in the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
+}
+
+PX4FLOW::~PX4FLOW()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr)
+		delete _reports;
+}
+
+int
+PX4FLOW::init()
+{
+	int ret = ERROR;
+
+	/* do I2C init (and probe) first */
+	if (I2C::init() != OK)
+		goto out;
+
+	/* allocate basic report buffers */
+	_reports = new RingBuffer(2, sizeof(px4flow_report));
+
+	if (_reports == nullptr)
+		goto out;
+
+	/* get a publish handle on the px4flow topic */
+	struct px4flow_report zero_report;
+	memset(&zero_report, 0, sizeof(zero_report));
+	_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report);
+
+	if (_px4flow_topic < 0)
+		debug("failed to create px4flow object. Did you start uOrb?");
+
+	ret = OK;
+	/* sensor is ok, but we don't really know if it is within range */
+	_sensor_ok = true;
+out:
+	return ret;
+}
+
+int
+PX4FLOW::probe()
+{
+	return measure();
+}
+
+int
+PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+			switch (arg) {
+
+				/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_measure_ticks = 0;
+				return OK;
+
+				/* external signalling (DRDY) not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+				/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+				/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* set interval for next measurement to minimum legal value */
+					_measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL);
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+
+				/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* convert hz to tick interval via microseconds */
+					unsigned ticks = USEC2TICK(1000000 / arg);
+
+					/* check against maximum rate */
+					if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL))
+						return -EINVAL;
+
+					/* update interval for next measurement */
+					_measure_ticks = ticks;
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		if (_measure_ticks == 0)
+			return SENSOR_POLLRATE_MANUAL;
+
+		return (1000 / _measure_ticks);
+
+	case SENSORIOCSQUEUEDEPTH: {
+		/* lower bound is mandatory, upper bound is a sanity check */
+		if ((arg < 1) || (arg > 100))
+			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:
+		/* XXX implement this */
+		return -EINVAL;
+	
+	default:
+		/* give it to the superclass */
+		return I2C::ioctl(filp, cmd, arg);
+	}
+}
+
+ssize_t
+PX4FLOW::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct px4flow_report);
+	struct px4flow_report *rbuf = reinterpret_cast<struct px4flow_report *>(buffer);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1)
+		return -ENOSPC;
+
+	/* if automatic measurement is enabled */
+	if (_measure_ticks > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 * Note that we may be pre-empted by the workq thread while we are doing this;
+		 * we are careful to avoid racing with them.
+		 */
+		while (count--) {
+			if (_reports->get(rbuf)) {
+				ret += sizeof(*rbuf);
+				rbuf++;
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement - run one conversion */
+	do {
+		_reports->flush();
+
+		/* trigger a measurement */
+		if (OK != measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* wait for it to complete */
+		usleep(PX4FLOW_CONVERSION_INTERVAL);
+
+		/* run the collection phase */
+		if (OK != collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* state machine will have generated a report, copy it out */
+		if (_reports->get(rbuf)) {
+			ret = sizeof(*rbuf);
+		}
+
+	} while (0);
+
+	return ret;
+}
+
+int
+PX4FLOW::measure()
+{
+	int ret;
+
+	/*
+	 * Send the command to begin a measurement.
+	 */
+	uint8_t cmd = PX4FLOW_REG;
+	ret = transfer(&cmd, 1, nullptr, 0);
+
+	if (OK != ret)
+	{
+		perf_count(_comms_errors);
+		log("i2c::transfer returned %d", ret);
+		 printf("i2c::transfer flow returned %d");
+		return ret;
+	}
+	ret = OK;
+	
+	return ret;
+}
+
+int
+PX4FLOW::collect()
+{
+	int	ret = -EIO;
+	
+	/* read from the sensor */
+	uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0};
+	
+	perf_begin(_sample_perf);
+	
+	ret = transfer(nullptr, 0, &val[0], 22);
+	
+	if (ret < 0)
+	{
+		log("error reading from sensor: %d", ret);
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+		return ret;
+	}
+	
+//	f.frame_count = val[1] << 8 | val[0];
+//	f.pixel_flow_x_sum= val[3] << 8 | val[2];
+//	f.pixel_flow_y_sum= val[5] << 8 | val[4];
+//	f.flow_comp_m_x= val[7] << 8 | val[6];
+//	f.flow_comp_m_y= val[9] << 8 | val[8];
+//	f.qual= val[11] << 8 | val[10];
+//	f.gyro_x_rate= val[13] << 8 | val[12];
+//	f.gyro_y_rate= val[15] << 8 | val[14];
+//	f.gyro_z_rate= val[17] << 8 | val[16];
+//	f.gyro_range= val[18];
+//	f.sonar_timestamp= val[19];
+//	f.ground_distance= val[21] << 8 | val[20];
+
+	int16_t flowcx = val[7] << 8 | val[6];
+	int16_t flowcy = val[9] << 8 | val[8];
+	int16_t gdist = val[21] << 8 | val[20];
+
+	struct px4flow_report report;
+	report.flow_comp_x_m = float(flowcx)/1000.0f;
+	report.flow_comp_y_m = float(flowcy)/1000.0f;
+	report.flow_raw_x= val[3] << 8 | val[2];
+	report.flow_raw_y= val[5] << 8 | val[4];
+	report.ground_distance_m =float(gdist)/1000.0f;
+	report.quality=  val[10];
+	report.sensor_id = 0;
+	report.timestamp = hrt_absolute_time();
+
+
+	/* publish it */
+	orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
+
+	/* post a report to the ring */
+	if (_reports->force(&report)) {
+		perf_count(_buffer_overflows);
+	}
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	ret = OK;
+
+	perf_end(_sample_perf);
+	return ret;
+}
+
+void
+PX4FLOW::start()
+{
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+	_reports->flush();
+
+	/* schedule a cycle to start things */
+	work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1);
+	
+	/* notify about state change */
+	struct subsystem_info_s info = {
+		true,
+		true,
+		true,
+		SUBSYSTEM_TYPE_OPTICALFLOW};
+	static orb_advert_t pub = -1;
+
+	if (pub > 0) {
+		orb_publish(ORB_ID(subsystem_info), pub, &info);
+	} else {
+		pub = orb_advertise(ORB_ID(subsystem_info), &info);
+	}
+}
+
+void
+PX4FLOW::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+PX4FLOW::cycle_trampoline(void *arg)
+{
+	PX4FLOW *dev = (PX4FLOW *)arg;
+
+	dev->cycle();
+}
+
+void
+PX4FLOW::cycle()
+{
+	/* collection phase? */
+	if (_collect_phase) {
+
+		/* perform collection */
+		if (OK != collect()) {
+			log("collection error");
+			/* restart the measurement state machine */
+			start();
+			return;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+
+		/*
+		 * Is there a collect->measure gap?
+		 */
+		if (_measure_ticks > USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) {
+
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&PX4FLOW::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(PX4FLOW_CONVERSION_INTERVAL));
+
+			return;
+		}
+	}
+
+	/* measurement phase */
+	if (OK != measure())
+		log("measure error");
+
+	/* next phase is collection */
+	_collect_phase = true;
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&PX4FLOW::cycle_trampoline,
+		   this,
+		   USEC2TICK(PX4FLOW_CONVERSION_INTERVAL));
+}
+
+void
+PX4FLOW::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	perf_print_counter(_buffer_overflows);
+	printf("poll interval:  %u ticks\n", _measure_ticks);
+	_reports->print_info("report queue");
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace px4flow
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+const int ERROR = -1;
+
+PX4FLOW	*g_dev;
+
+void	start();
+void	stop();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start()
+{
+	int fd;
+
+	if (g_dev != nullptr)
+		errx(1, "already started");
+
+	/* create the driver */
+	g_dev = new PX4FLOW(PX4FLOW_BUS);
+
+	if (g_dev == nullptr)
+		goto fail;
+
+	if (OK != g_dev->init())
+		goto fail;
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		goto fail;
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0)
+		goto fail;
+
+	exit(0);
+
+fail:
+
+	if (g_dev != nullptr) 
+	{
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Stop the driver
+ */
+void stop()
+{
+	if (g_dev != nullptr)
+	{
+		delete g_dev;
+		g_dev = nullptr;
+	}
+	else
+	{
+		errx(1, "driver not running");
+	}
+	exit(0);
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+	struct px4flow_report report;
+	ssize_t sz;
+	int ret;
+
+	int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "%s open failed (try 'px4flow start' if the driver is not running", PX4FLOW_DEVICE_PATH);
+
+	/* do a simple demand read */
+	sz = read(fd, &report, sizeof(report));
+
+	if (sz != sizeof(report))
+	//	err(1, "immediate read failed");
+
+	warnx("single read");
+	warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m);
+	warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m);
+	warnx("time:        %lld", report.timestamp);
+
+
+	/* start the sensor polling at 2Hz */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
+		errx(1, "failed to set 2Hz poll rate");
+
+	/* read the sensor 5x and report each value */
+	for (unsigned i = 0; i < 5; i++) {
+		struct pollfd fds;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = poll(&fds, 1, 2000);
+
+		if (ret != 1)
+			errx(1, "timed out waiting for sensor data");
+
+		/* now go get it */
+		sz = read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report))
+			err(1, "periodic read failed");
+
+		warnx("periodic read %u", i);
+		warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m);
+		warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m);
+		warnx("time:        %lld", report.timestamp);
+
+
+	}
+
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "failed ");
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+		err(1, "driver reset failed");
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		err(1, "driver poll restart failed");
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running");
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+} // namespace
+
+int
+px4flow_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[1], "start"))
+		px4flow::start();
+	
+	 /*
+	  * Stop the driver
+	  */
+	 if (!strcmp(argv[1], "stop"))
+		 px4flow::stop();
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		px4flow::test();
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset"))
+		px4flow::reset();
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
+		px4flow::info();
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+}

src/drivers/px4io/px4io.cpp

@@ -1921,12 +1921,14 @@ PX4IO::print_status()
 	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE));
 #endif
 	printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG));
-	printf("controls");
+	for (unsigned group = 0; group < 4; group++) {
+		printf("controls %u:", group);
 
 		for (unsigned i = 0; i < _max_controls; i++)
-		printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, i));
+			printf(" %d", (int16_t) io_reg_get(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT + i));
 
 		printf("\n");
+	}
 
 	for (unsigned i = 0; i < _max_rc_input; i++) {
 		unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;

src/drivers/sf0x/module.mk

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2014 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 Lightware laser range finder driver.
+#
+
+MODULE_COMMAND	= sf0x
+
+SRCS		= sf0x.cpp

src/drivers/sf0x/sf0x.cpp

@@ -0,0 +1,977 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 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 sf0x.cpp
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Greg Hulands
+ *
+ * Driver for the Lightware SF0x laser rangefinder series
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+#include <termios.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_range_finder.h>
+#include <drivers/device/device.h>
+#include <drivers/device/ringbuffer.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/subsystem_info.h>
+
+#include <board_config.h>
+
+/* Configuration Constants */
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+#define SF0X_CONVERSION_INTERVAL	83334
+#define SF0X_TAKE_RANGE_REG		'd'
+#define SF02F_MIN_DISTANCE		0.0f
+#define SF02F_MAX_DISTANCE		40.0f
+#define SF0X_DEFAULT_PORT		"/dev/ttyS2"
+
+class SF0X : public device::CDev
+{
+public:
+	SF0X(const char *port = SF0X_DEFAULT_PORT);
+	virtual ~SF0X();
+
+	virtual int 			init();
+
+	virtual ssize_t			read(struct file *filp, char *buffer, size_t buflen);
+	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/**
+	* Diagnostics - print some basic information about the driver.
+	*/
+	void				print_info();
+
+protected:
+	virtual int			probe();
+
+private:
+	float				_min_distance;
+	float				_max_distance;
+	work_s				_work;
+	RingBuffer			*_reports;
+	bool				_sensor_ok;
+	int				_measure_ticks;
+	bool				_collect_phase;
+	int				_fd;
+	char				_linebuf[10];
+	unsigned			_linebuf_index;
+	hrt_abstime			_last_read;
+
+	orb_advert_t			_range_finder_topic;
+
+	perf_counter_t			_sample_perf;
+	perf_counter_t			_comms_errors;
+	perf_counter_t			_buffer_overflows;
+
+	/**
+	* Initialise the automatic measurement state machine and start it.
+	*
+	* @note This function is called at open and error time.  It might make sense
+	*       to make it more aggressive about resetting the bus in case of errors.
+	*/
+	void				start();
+
+	/**
+	* Stop the automatic measurement state machine.
+	*/
+	void				stop();
+
+	/**
+	* Set the min and max distance thresholds if you want the end points of the sensors
+	* range to be brought in at all, otherwise it will use the defaults SF0X_MIN_DISTANCE
+	* and SF0X_MAX_DISTANCE
+	*/
+	void				set_minimum_distance(float min);
+	void				set_maximum_distance(float max);
+	float				get_minimum_distance();
+	float				get_maximum_distance();
+
+	/**
+	* Perform a poll cycle; collect from the previous measurement
+	* and start a new one.
+	*/
+	void				cycle();
+	int				measure();
+	int				collect();
+	/**
+	* Static trampoline from the workq context; because we don't have a
+	* generic workq wrapper yet.
+	*
+	* @param arg		Instance pointer for the driver that is polling.
+	*/
+	static void			cycle_trampoline(void *arg);
+
+
+};
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int sf0x_main(int argc, char *argv[]);
+
+SF0X::SF0X(const char *port) :
+	CDev("SF0X", RANGE_FINDER_DEVICE_PATH),
+	_min_distance(SF02F_MIN_DISTANCE),
+	_max_distance(SF02F_MAX_DISTANCE),
+	_reports(nullptr),
+	_sensor_ok(false),
+	_measure_ticks(0),
+	_collect_phase(false),
+	_fd(-1),
+	_linebuf_index(0),
+	_last_read(0),
+	_range_finder_topic(-1),
+	_sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")),
+	_buffer_overflows(perf_alloc(PC_COUNT, "sf0x_buffer_overflows"))
+{
+	/* open fd */
+	_fd = ::open(port, O_RDWR | O_NOCTTY | O_NONBLOCK);
+
+	if (_fd < 0) {
+		warnx("FAIL: laser fd");
+	}
+
+	/* tell it to stop auto-triggering */
+	char stop_auto = ' ';
+	(void)::write(_fd, &stop_auto, 1);
+	usleep(100);
+	(void)::write(_fd, &stop_auto, 1);
+
+	struct termios uart_config;
+
+	int termios_state;
+
+	/* fill the struct for the new configuration */
+	tcgetattr(_fd, &uart_config);
+
+	/* clear ONLCR flag (which appends a CR for every LF) */
+	uart_config.c_oflag &= ~ONLCR;
+	/* no parity, one stop bit */
+	uart_config.c_cflag &= ~(CSTOPB | PARENB);
+
+	unsigned speed = B9600;
+
+	/* set baud rate */
+	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
+		warnx("ERR CFG: %d ISPD", termios_state);
+	}
+
+	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
+		warnx("ERR CFG: %d OSPD\n", termios_state);
+	}
+
+	if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
+		warnx("ERR baud %d ATTR", termios_state);
+	}
+
+	// disable debug() calls
+	_debug_enabled = false;
+
+	// work_cancel in the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
+}
+
+SF0X::~SF0X()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr) {
+		delete _reports;
+	}
+}
+
+int
+SF0X::init()
+{
+	int ret = ERROR;
+	unsigned i = 0;
+
+	/* do regular cdev init */
+	if (CDev::init() != OK) {
+		goto out;
+	}
+
+	/* allocate basic report buffers */
+	_reports = new RingBuffer(2, sizeof(range_finder_report));
+
+	if (_reports == nullptr) {
+		warnx("mem err");
+		goto out;
+	}
+
+	/* get a publish handle on the range finder topic */
+	struct range_finder_report zero_report;
+	memset(&zero_report, 0, sizeof(zero_report));
+	_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report);
+
+	if (_range_finder_topic < 0) {
+		warnx("advert err");
+	}
+
+	/* attempt to get a measurement 5 times */
+	while (ret != OK && i < 5) {
+
+		if (measure()) {
+			ret = ERROR;
+			_sensor_ok = false;
+		}
+
+		usleep(100000);
+
+		if (collect()) {
+			ret = ERROR;
+			_sensor_ok = false;
+
+		} else {
+			ret = OK;
+			/* sensor is ok, but we don't really know if it is within range */
+			_sensor_ok = true;
+		}
+
+		i++;
+	}
+
+	/* close the fd */
+	::close(_fd);
+	_fd = -1;
+out:
+	return ret;
+}
+
+int
+SF0X::probe()
+{
+	return measure();
+}
+
+void
+SF0X::set_minimum_distance(float min)
+{
+	_min_distance = min;
+}
+
+void
+SF0X::set_maximum_distance(float max)
+{
+	_max_distance = max;
+}
+
+float
+SF0X::get_minimum_distance()
+{
+	return _min_distance;
+}
+
+float
+SF0X::get_maximum_distance()
+{
+	return _max_distance;
+}
+
+int
+SF0X::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+			switch (arg) {
+
+			/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_measure_ticks = 0;
+				return OK;
+
+			/* external signalling (DRDY) not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+			/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+			/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* set interval for next measurement to minimum legal value */
+					_measure_ticks = USEC2TICK(SF0X_CONVERSION_INTERVAL);
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start) {
+						start();
+					}
+
+					return OK;
+				}
+
+			/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* convert hz to tick interval via microseconds */
+					unsigned ticks = USEC2TICK(1000000 / arg);
+
+					/* check against maximum rate */
+					if (ticks < USEC2TICK(SF0X_CONVERSION_INTERVAL)) {
+						return -EINVAL;
+					}
+
+					/* update interval for next measurement */
+					_measure_ticks = ticks;
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start) {
+						start();
+					}
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		if (_measure_ticks == 0) {
+			return SENSOR_POLLRATE_MANUAL;
+		}
+
+		return (1000 / _measure_ticks);
+
+	case SENSORIOCSQUEUEDEPTH: {
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 1) || (arg > 100)) {
+				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:
+		/* XXX implement this */
+		return -EINVAL;
+
+	case RANGEFINDERIOCSETMINIUMDISTANCE: {
+			set_minimum_distance(*(float *)arg);
+			return 0;
+		}
+		break;
+
+	case RANGEFINDERIOCSETMAXIUMDISTANCE: {
+			set_maximum_distance(*(float *)arg);
+			return 0;
+		}
+		break;
+
+	default:
+		/* give it to the superclass */
+		return CDev::ioctl(filp, cmd, arg);
+	}
+}
+
+ssize_t
+SF0X::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct range_finder_report);
+	struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1) {
+		return -ENOSPC;
+	}
+
+	/* if automatic measurement is enabled */
+	if (_measure_ticks > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 * Note that we may be pre-empted by the workq thread while we are doing this;
+		 * we are careful to avoid racing with them.
+		 */
+		while (count--) {
+			if (_reports->get(rbuf)) {
+				ret += sizeof(*rbuf);
+				rbuf++;
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement - run one conversion */
+	do {
+		_reports->flush();
+
+		/* trigger a measurement */
+		if (OK != measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* wait for it to complete */
+		usleep(SF0X_CONVERSION_INTERVAL);
+
+		/* run the collection phase */
+		if (OK != collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* state machine will have generated a report, copy it out */
+		if (_reports->get(rbuf)) {
+			ret = sizeof(*rbuf);
+		}
+
+	} while (0);
+
+	return ret;
+}
+
+int
+SF0X::measure()
+{
+	int ret;
+
+	/*
+	 * Send the command to begin a measurement.
+	 */
+	char cmd = SF0X_TAKE_RANGE_REG;
+	ret = ::write(_fd, &cmd, 1);
+
+	if (ret != sizeof(cmd)) {
+		perf_count(_comms_errors);
+		log("write fail %d", ret);
+		return ret;
+	}
+
+	ret = OK;
+
+	return ret;
+}
+
+int
+SF0X::collect()
+{
+	int	ret;
+
+	perf_begin(_sample_perf);
+
+	/* clear buffer if last read was too long ago */
+	uint64_t read_elapsed = hrt_elapsed_time(&_last_read);
+
+	if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
+		_linebuf_index = 0;
+	}
+
+	/* read from the sensor (uart buffer) */
+	ret = ::read(_fd, &_linebuf[_linebuf_index], sizeof(_linebuf) - _linebuf_index);
+
+	if (ret < 0) {
+		_linebuf[sizeof(_linebuf) - 1] = '\0';
+		debug("read err: %d lbi: %d buf: %s", ret, (int)_linebuf_index, _linebuf);
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+
+		/* only throw an error if we time out */
+		if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
+			return ret;
+
+		} else {
+			return -EAGAIN;
+		}
+	}
+
+	_linebuf_index += ret;
+
+	if (_linebuf_index >= sizeof(_linebuf)) {
+		_linebuf_index = 0;
+	}
+
+	_last_read = hrt_absolute_time();
+
+	if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') {
+		/* incomplete read, reschedule ourselves */
+		return -EAGAIN;
+	}
+
+	char *end;
+	float si_units;
+	bool valid;
+
+	/* enforce line ending */
+	_linebuf[sizeof(_linebuf) - 1] = '\0';
+
+	if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') {
+		si_units = -1.0f;
+		valid = false;
+
+	} else {
+		si_units = strtod(_linebuf, &end);
+		valid = true;
+	}
+
+	debug("val (float): %8.4f, raw: %s\n", si_units, _linebuf);
+
+	/* done with this chunk, resetting */
+	_linebuf_index = 0;
+
+	struct range_finder_report report;
+
+	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
+	report.timestamp = hrt_absolute_time();
+	report.error_count = perf_event_count(_comms_errors);
+	report.distance = si_units;
+	report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0);
+
+	/* publish it */
+	orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
+
+	if (_reports->force(&report)) {
+		perf_count(_buffer_overflows);
+	}
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	ret = OK;
+
+	perf_end(_sample_perf);
+	return ret;
+}
+
+void
+SF0X::start()
+{
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+	_reports->flush();
+
+	/* schedule a cycle to start things */
+	work_queue(HPWORK, &_work, (worker_t)&SF0X::cycle_trampoline, this, 1);
+
+	// /* notify about state change */
+	// struct subsystem_info_s info = {
+	// 	true,
+	// 	true,
+	// 	true,
+	// 	SUBSYSTEM_TYPE_RANGEFINDER
+	// };
+	// static orb_advert_t pub = -1;
+
+	// if (pub > 0) {
+	// 	orb_publish(ORB_ID(subsystem_info), pub, &info);
+
+	// } else {
+	// 	pub = orb_advertise(ORB_ID(subsystem_info), &info);
+	// }
+}
+
+void
+SF0X::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+SF0X::cycle_trampoline(void *arg)
+{
+	SF0X *dev = static_cast<SF0X *>(arg);
+
+	dev->cycle();
+}
+
+void
+SF0X::cycle()
+{
+	/* fds initialized? */
+	if (_fd < 0) {
+		/* open fd */
+		_fd = ::open(SF0X_DEFAULT_PORT, O_RDWR | O_NOCTTY | O_NONBLOCK);
+	}
+
+	/* collection phase? */
+	if (_collect_phase) {
+
+		/* perform collection */
+		int collect_ret = collect();
+
+		if (collect_ret == -EAGAIN) {
+			/* reschedule to grab the missing bits, time to transmit 10 bytes @9600 bps */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&SF0X::cycle_trampoline,
+				   this,
+				   USEC2TICK(1100));
+			return;
+		}
+
+		if (OK != collect_ret) {
+			log("collection error");
+			/* restart the measurement state machine */
+			start();
+			return;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+
+		/*
+		 * Is there a collect->measure gap?
+		 */
+		if (_measure_ticks > USEC2TICK(SF0X_CONVERSION_INTERVAL)) {
+
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&SF0X::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(SF0X_CONVERSION_INTERVAL));
+
+			return;
+		}
+	}
+
+	/* measurement phase */
+	if (OK != measure()) {
+		log("measure error");
+	}
+
+	/* next phase is collection */
+	_collect_phase = true;
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&SF0X::cycle_trampoline,
+		   this,
+		   USEC2TICK(SF0X_CONVERSION_INTERVAL));
+}
+
+void
+SF0X::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	perf_print_counter(_buffer_overflows);
+	printf("poll interval:  %d ticks\n", _measure_ticks);
+	_reports->print_info("report queue");
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace sf0x
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+const int ERROR = -1;
+
+SF0X	*g_dev;
+
+void	start();
+void	stop();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start(const char *port)
+{
+	int fd;
+
+	if (g_dev != nullptr) {
+		errx(1, "already started");
+	}
+
+	/* create the driver */
+	g_dev = new SF0X(port);
+
+	if (g_dev == nullptr) {
+		goto fail;
+	}
+
+	if (OK != g_dev->init()) {
+		goto fail;
+	}
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(RANGE_FINDER_DEVICE_PATH, 0);
+
+	if (fd < 0) {
+		warnx("device open fail");
+		goto fail;
+	}
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		goto fail;
+	}
+
+	exit(0);
+
+fail:
+
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Stop the driver
+ */
+void stop()
+{
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+
+	} else {
+		errx(1, "driver not running");
+	}
+
+	exit(0);
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+	struct range_finder_report report;
+	ssize_t sz;
+
+	int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		err(1, "%s open failed (try 'sf0x start' if the driver is not running", RANGE_FINDER_DEVICE_PATH);
+	}
+
+	/* do a simple demand read */
+	sz = read(fd, &report, sizeof(report));
+
+	if (sz != sizeof(report)) {
+		err(1, "immediate read failed");
+	}
+
+	warnx("single read");
+	warnx("measurement: %0.2f m", (double)report.distance);
+	warnx("time:        %lld", report.timestamp);
+
+	/* start the sensor polling at 2Hz */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
+		errx(1, "failed to set 2Hz poll rate");
+	}
+
+	/* read the sensor 5x and report each value */
+	for (unsigned i = 0; i < 5; i++) {
+		struct pollfd fds;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		int ret = poll(&fds, 1, 2000);
+
+		if (ret != 1) {
+			errx(1, "timed out waiting for sensor data");
+		}
+
+		/* now go get it */
+		sz = read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report)) {
+			err(1, "periodic read failed");
+		}
+
+		warnx("periodic read %u", i);
+		warnx("measurement: %0.3f", (double)report.distance);
+		warnx("time:        %lld", report.timestamp);
+	}
+
+	/* reset the sensor polling to default rate */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
+		errx(1, "failed to set default poll rate");
+	}
+
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		err(1, "failed ");
+	}
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
+		err(1, "driver reset failed");
+	}
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		err(1, "driver poll restart failed");
+	}
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr) {
+		errx(1, "driver not running");
+	}
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+} // namespace
+
+int
+sf0x_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[1], "start")) {
+		if (argc > 2) {
+			sf0x::start(argv[2]);
+
+		} else {
+			sf0x::start(SF0X_DEFAULT_PORT);
+		}
+	}
+
+	/*
+	 * Stop the driver
+	 */
+	if (!strcmp(argv[1], "stop")) {
+		sf0x::stop();
+	}
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test")) {
+		sf0x::test();
+	}
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset")) {
+		sf0x::reset();
+	}
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
+		sf0x::info();
+	}
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+}

src/modules/gpio_led/gpio_led.c

@@ -51,7 +51,6 @@
 #include <systemlib/err.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/actuator_armed.h>
 #include <poll.h>
 #include <drivers/drv_gpio.h>
 #include <modules/px4iofirmware/protocol.h>
@@ -63,8 +62,6 @@ struct gpio_led_s {
 	int pin;
 	struct vehicle_status_s status;
 	int vehicle_status_sub;
-	struct actuator_armed_s armed;
-	int actuator_armed_sub;
 	bool led_state;
 	int counter;
 };
@@ -81,6 +78,7 @@ void gpio_led_cycle(FAR void *arg);
 int gpio_led_main(int argc, char *argv[])
 {
 	if (argc < 2) {
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 		errx(1, "usage: gpio_led {start|stop} [-p <1|2|a1|a2|r1|r2>]\n"
 		     "\t-p\tUse pin:\n"
 		     "\t\t1\tPX4FMU GPIO_EXT1 (default)\n"
@@ -88,7 +86,14 @@ int gpio_led_main(int argc, char *argv[])
 		     "\t\ta1\tPX4IO ACC1\n"
 		     "\t\ta2\tPX4IO ACC2\n"
 		     "\t\tr1\tPX4IO RELAY1\n"
-		     "\t\tr2\tPX4IO RELAY2");
+		     "\t\tr2\tPX4IO RELAY2"
+		    );
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		errx(1, "usage: gpio_led {start|stop} [-p <n>]\n"
+		     "\t-p <n>\tUse specified AUX OUT pin number (default: 1)"
+		    );
+#endif
 
 	} else {
 
@@ -98,37 +103,70 @@ int gpio_led_main(int argc, char *argv[])
 			}
 
 			bool use_io = false;
-			int pin = GPIO_EXT_1;
+
+			/* by default use GPIO_EXT_1 on FMUv1 and GPIO_SERVO_1 on FMUv2 */
+			int pin = 1;
+
+			/* pin name to display */
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+			char *pin_name = "PX4FMU GPIO_EXT1";
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+			char pin_name[] = "AUX OUT 1";
+#endif
 
 			if (argc > 2) {
 				if (!strcmp(argv[2], "-p")) {
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+
 					if (!strcmp(argv[3], "1")) {
 						use_io = false;
 						pin = GPIO_EXT_1;
+						pin_name = "PX4FMU GPIO_EXT1";
 
 					} else if (!strcmp(argv[3], "2")) {
 						use_io = false;
 						pin = GPIO_EXT_2;
+						pin_name = "PX4FMU GPIO_EXT2";
 
 					} else if (!strcmp(argv[3], "a1")) {
 						use_io = true;
 						pin = PX4IO_P_SETUP_RELAYS_ACC1;
+						pin_name = "PX4IO ACC1";
 
 					} else if (!strcmp(argv[3], "a2")) {
 						use_io = true;
 						pin = PX4IO_P_SETUP_RELAYS_ACC2;
+						pin_name = "PX4IO ACC2";
 
 					} else if (!strcmp(argv[3], "r1")) {
 						use_io = true;
 						pin = PX4IO_P_SETUP_RELAYS_POWER1;
+						pin_name = "PX4IO RELAY1";
 
 					} else if (!strcmp(argv[3], "r2")) {
 						use_io = true;
 						pin = PX4IO_P_SETUP_RELAYS_POWER2;
+						pin_name = "PX4IO RELAY2";
 
 					} else {
 						errx(1, "unsupported pin: %s", argv[3]);
 					}
+
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+					unsigned int n = strtoul(argv[3], NULL, 10);
+
+					if (n >= 1 && n <= 6) {
+						use_io = false;
+						pin = 1 << (n - 1);
+						snprintf(pin_name, sizeof(pin_name), "AUX OUT %d", n);
+
+					} else {
+						errx(1, "unsupported pin: %s", argv[3]);
+					}
+
+#endif
 				}
 			}
 
@@ -142,21 +180,6 @@ int gpio_led_main(int argc, char *argv[])
 
 			} else {
 				gpio_led_started = true;
-				char pin_name[24];
-
-				if (use_io) {
-					if (pin & (PX4IO_P_SETUP_RELAYS_ACC1 | PX4IO_P_SETUP_RELAYS_ACC2)) {
-						sprintf(pin_name, "PX4IO ACC%i", (pin >> 3));
-
-					} else {
-						sprintf(pin_name, "PX4IO RELAY%i", pin);
-					}
-
-				} else {
-					sprintf(pin_name, "PX4FMU GPIO_EXT%i", pin);
-
-				}
-
 				warnx("start, using pin: %s", pin_name);
 			}
 
@@ -186,6 +209,7 @@ void gpio_led_start(FAR void *arg)
 
 	if (priv->use_io) {
 		gpio_dev = PX4IO_DEVICE_PATH;
+
 	} else {
 		gpio_dev = PX4FMU_DEVICE_PATH;
 	}
@@ -204,8 +228,10 @@ void gpio_led_start(FAR void *arg)
 	/* px4fmu only, px4io doesn't support GPIO_SET_OUTPUT and will ignore */
 	ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin);
 
-	/* subscribe to vehicle status topic */
+	/* initialize vehicle status structure */
 	memset(&priv->status, 0, sizeof(priv->status));
+
+	/* subscribe to vehicle status topic */
 	priv->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 
 	/* add worker to queue */
@@ -224,38 +250,33 @@ void gpio_led_cycle(FAR void *arg)
 	FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg;
 
 	/* check for status updates*/
-	bool status_updated;
+	bool updated;
-	orb_check(priv->vehicle_status_sub, &status_updated);
+	orb_check(priv->vehicle_status_sub, &updated);
 
-	if (status_updated)
+	if (updated) {
 		orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status);
-
+	}
-	orb_check(priv->vehicle_status_sub, &status_updated);
-
-	if (status_updated)
-		orb_copy(ORB_ID(actuator_armed), priv->actuator_armed_sub, &priv->armed);
 
 	/* select pattern for current status */
 	int pattern = 0;
 
-	if (priv->armed.armed) {
+	if (priv->status.arming_state == ARMING_STATE_ARMED_ERROR) {
-		if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
+		pattern = 0x2A;	// *_*_*_ fast blink (armed, error)
+
+	} else if (priv->status.arming_state == ARMING_STATE_ARMED) {
+		if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && priv->status.failsafe_state == FAILSAFE_STATE_NORMAL) {
 			pattern = 0x3f;	// ****** solid (armed)
 
 		} else {
-			pattern = 0x2A;	// *_*_*_ fast blink (armed, battery warning)
+			pattern = 0x3e;	// *****_ slow blink (armed, battery low or failsafe)
 		}
 
-	} else {
+	} else if (priv->status.arming_state == ARMING_STATE_STANDBY) {
-		if (priv->armed.ready_to_arm) {
-			pattern = 0x00;	// ______ off (disarmed, preflight check)
-
-		} else if (priv->armed.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
 		pattern = 0x38;	// ***___ slow blink (disarmed, ready)
 
-		} else {
+	} else if (priv->status.arming_state == ARMING_STATE_STANDBY_ERROR) {
-			pattern = 0x28;	// *_*___ slow double blink (disarmed, not good to arm)
+		pattern = 0x28;	// *_*___ slow double blink (disarmed, error)
-		}
+
 	}
 
 	/* blink pattern */
@@ -266,6 +287,7 @@ void gpio_led_cycle(FAR void *arg)
 
 		if (led_state_new) {
 			ioctl(priv->gpio_fd, GPIO_SET, priv->pin);
+
 		} else {
 			ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
 		}
@@ -273,8 +295,9 @@ void gpio_led_cycle(FAR void *arg)
 
 	priv->counter++;
 
-	if (priv->counter > 5)
+	if (priv->counter > 5) {
 		priv->counter = 0;
+	}
 
 	/* repeat cycle at 5 Hz */
 	if (gpio_led_started) {

src/modules/px4iofirmware/mixer.cpp

@@ -267,7 +267,7 @@ mixer_callback(uintptr_t handle,
 	       uint8_t control_index,
 	       float &control)
 {
-	if (control_group > 3)
+	if (control_group >= PX4IO_CONTROL_GROUPS)
 		return -1;
 
 	switch (source) {

src/modules/px4iofirmware/px4io.h

@@ -53,7 +53,7 @@
  */
 #define PX4IO_SERVO_COUNT		8
 #define PX4IO_CONTROL_CHANNELS		8
-#define PX4IO_CONTROL_GROUPS		2
+#define PX4IO_CONTROL_GROUPS		4
 #define PX4IO_RC_INPUT_CHANNELS		18
 #define PX4IO_RC_MAPPED_CONTROL_CHANNELS		8 /**< This is the maximum number of channels mapped/used */
 

src/modules/sdlog2/logbuffer.c

@@ -74,8 +74,9 @@ bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size)
 	// bytes available to write
 	int available = lb->read_ptr - lb->write_ptr - 1;
 
-	if (available < 0)
+	if (available < 0) {
 		available += lb->size;
+	}
 
 	if (size > available) {
 		// buffer overflow

src/modules/sdlog2/sdlog2.c

@@ -58,6 +58,8 @@
 #include <drivers/drv_hrt.h>
 #include <math.h>
 
+#include <drivers/drv_range_finder.h>
+
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/sensor_combined.h>
@@ -134,12 +136,6 @@ static uint64_t gps_time = 0;
 
 /* current state of logging */
 static bool logging_enabled = false;
-/* enable logging on start (-e option) */
-static bool log_on_start = false;
-/* enable logging when armed (-a option) */
-static bool log_when_armed = false;
-/* delay = 1 / rate (rate defined by -r option) */
-static useconds_t sleep_delay = 0;
 /* use date/time for naming directories and files (-t option) */
 static bool log_name_timestamp = false;
 
@@ -159,6 +155,8 @@ static void *logwriter_thread(void *arg);
  */
 __EXPORT int sdlog2_main(int argc, char *argv[]);
 
+static bool copy_if_updated(orb_id_t topic, int handle, void *buffer);
+
 /**
  * Mainloop of sd log deamon.
  */
@@ -220,8 +218,9 @@ static int open_log_file(void);
 static void
 sdlog2_usage(const char *reason)
 {
-	if (reason)
+	if (reason) {
 		fprintf(stderr, "%s\n", reason);
+	}
 
 	errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t\n"
 	     "\t-r\tLog rate in Hz, 0 means unlimited rate\n"
@@ -241,8 +240,9 @@ sdlog2_usage(const char *reason)
  */
 int sdlog2_main(int argc, char *argv[])
 {
-	if (argc < 2)
+	if (argc < 2) {
 		sdlog2_usage("missing command");
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
@@ -401,22 +401,29 @@ static void *logwriter_thread(void *arg)
 
 	int log_fd = open_log_file();
 
-	if (log_fd < 0)
+	if (log_fd < 0) {
-		return;
+		return NULL;
+	}
 
 	struct logbuffer_s *logbuf = (struct logbuffer_s *)arg;
 
 	/* write log messages formats, version and parameters */
 	log_bytes_written += write_formats(log_fd);
+
 	log_bytes_written += write_version(log_fd);
+
 	log_bytes_written += write_parameters(log_fd);
+
 	fsync(log_fd);
 
 	int poll_count = 0;
 
 	void *read_ptr;
+
 	int n = 0;
+
 	bool should_wait = false;
+
 	bool is_part = false;
 
 	while (true) {
@@ -449,7 +456,6 @@ static void *logwriter_thread(void *arg)
 				n = available;
 			}
 
-			lseek(log_fd, 0, SEEK_CUR);
 			n = write(log_fd, read_ptr, n);
 
 			should_wait = (n == available) && !is_part;
@@ -483,7 +489,7 @@ static void *logwriter_thread(void *arg)
 	fsync(log_fd);
 	close(log_fd);
 
-	return;
+	return NULL;
 }
 
 void sdlog2_start_log()
@@ -628,6 +634,19 @@ int write_parameters(int fd)
 	return written;
 }
 
+bool copy_if_updated(orb_id_t topic, int handle, void *buffer)
+{
+	bool updated;
+
+	orb_check(handle, &updated);
+
+	if (updated) {
+		orb_copy(topic, handle, buffer);
+	}
+
+	return updated;
+}
+
 int sdlog2_thread_main(int argc, char *argv[])
 {
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
@@ -636,12 +655,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 		warnx("failed to open MAVLink log stream, start mavlink app first");
 	}
 
-	/* log buffer size */
+	/* delay = 1 / rate (rate defined by -r option), default log rate: 50 Hz */
+	useconds_t sleep_delay = 20000;
 	int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT;
-
 	logging_enabled = false;
-	log_on_start = false;
+	/* enable logging on start (-e option) */
-	log_when_armed = false;
+	bool log_on_start = false;
+	/* enable logging when armed (-a option) */
+	bool log_when_armed = false;
 	log_name_timestamp = false;
 
 	flag_system_armed = false;
@@ -651,17 +672,20 @@ int sdlog2_thread_main(int argc, char *argv[])
 	argv += 2;
 	int ch;
 
+	/* don't exit from getopt loop to leave getopt global variables in consistent state,
+	 * set error flag instead */
+	bool err_flag = false;
+
 	while ((ch = getopt(argc, argv, "r:b:eat")) != EOF) {
 		switch (ch) {
 		case 'r': {
 				unsigned long r = strtoul(optarg, NULL, 10);
 
-				if (r == 0) {
+				if (r <= 0) {
-					sleep_delay = 0;
+					r = 1;
-
-				} else {
-					sleep_delay = 1000000 / r;
 				}
+
+				sleep_delay = 1000000 / r;
 			}
 			break;
 
@@ -698,20 +722,27 @@ int sdlog2_thread_main(int argc, char *argv[])
 			} else {
 				warnx("unknown option character `\\x%x'", optopt);
 			}
+			err_flag = true;
+			break;
 
 		default:
-			sdlog2_usage("unrecognized flag");
+			warnx("unrecognized flag");
-			errx(1, "exiting");
+			err_flag = true;
+			break;
 		}
 	}
 
+	if (err_flag) {
+		sdlog2_usage(NULL);
+	}
+
 	gps_time = 0;
 
 	/* create log root dir */
 	int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO);
 
 	if (mkdir_ret != 0 && errno != EEXIST) {
-		err("failed creating log root dir: %s", log_root);
+		err(1, "failed creating log root dir: %s", log_root);
 	}
 
 	/* copy conversion scripts */
@@ -734,8 +765,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 	struct vehicle_status_s buf_status;
 
+	struct vehicle_gps_position_s buf_gps_pos;
+
 	memset(&buf_status, 0, sizeof(buf_status));
 
+	memset(&buf_gps_pos, 0, sizeof(buf_gps_pos));
+
 	/* warning! using union here to save memory, elements should be used separately! */
 	union {
 		struct vehicle_command_s cmd;
@@ -749,7 +784,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct vehicle_local_position_setpoint_s local_pos_sp;
 		struct vehicle_global_position_s global_pos;
 		struct position_setpoint_triplet_s triplet;
-		struct vehicle_gps_position_s gps_pos;
 		struct vehicle_vicon_position_s vicon_pos;
 		struct optical_flow_s flow;
 		struct rc_channels_s rc;
@@ -759,34 +793,11 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct vehicle_global_velocity_setpoint_s global_vel_sp;
 		struct battery_status_s battery;
 		struct telemetry_status_s telemetry;
+		struct range_finder_report range_finder;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
 
-	struct {
-		int cmd_sub;
-		int status_sub;
-		int sensor_sub;
-		int att_sub;
-		int att_sp_sub;
-		int rates_sp_sub;
-		int act_outputs_sub;
-		int act_controls_sub;
-		int local_pos_sub;
-		int local_pos_sp_sub;
-		int global_pos_sub;
-		int triplet_sub;
-		int gps_pos_sub;
-		int vicon_pos_sub;
-		int flow_sub;
-		int rc_sub;
-		int airspeed_sub;
-		int esc_sub;
-		int global_vel_sp_sub;
-		int battery_sub;
-		int telemetry_sub;
-	} subs;
-
 	/* log message buffer: header + body */
 #pragma pack(push, 1)
 	struct {
@@ -821,154 +832,53 @@ int sdlog2_thread_main(int argc, char *argv[])
 #pragma pack(pop)
 	memset(&log_msg.body, 0, sizeof(log_msg.body));
 
-	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
+	struct {
-	/* number of messages */
+		int cmd_sub;
-	const ssize_t fdsc = 25;
+		int status_sub;
-	/* Sanity check variable and index */
+		int sensor_sub;
-	ssize_t fdsc_count = 0;
+		int att_sub;
-	/* file descriptors to wait for */
+		int att_sp_sub;
-	struct pollfd fds[fdsc];
+		int rates_sp_sub;
+		int act_outputs_sub;
+		int act_controls_sub;
+		int local_pos_sub;
+		int local_pos_sp_sub;
+		int global_pos_sub;
+		int triplet_sub;
+		int gps_pos_sub;
+		int vicon_pos_sub;
+		int flow_sub;
+		int rc_sub;
+		int airspeed_sub;
+		int esc_sub;
+		int global_vel_sp_sub;
+		int battery_sub;
+		int telemetry_sub;
+		int range_finder_sub;
+	} subs;
 
-	/* --- VEHICLE COMMAND --- */
 	subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
-	fds[fdsc_count].fd = subs.cmd_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- VEHICLE STATUS --- */
 	subs.status_sub = orb_subscribe(ORB_ID(vehicle_status));
-	fds[fdsc_count].fd = subs.status_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- GPS POSITION --- */
 	subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	fds[fdsc_count].fd = subs.gps_pos_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- SENSORS COMBINED --- */
 	subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	fds[fdsc_count].fd = subs.sensor_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- ATTITUDE --- */
 	subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	fds[fdsc_count].fd = subs.att_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- ATTITUDE SETPOINT --- */
 	subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	fds[fdsc_count].fd = subs.att_sp_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- RATES SETPOINT --- */
 	subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-	fds[fdsc_count].fd = subs.rates_sp_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- ACTUATOR OUTPUTS --- */
 	subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS);
-	fds[fdsc_count].fd = subs.act_outputs_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- ACTUATOR CONTROL --- */
 	subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-	fds[fdsc_count].fd = subs.act_controls_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- LOCAL POSITION --- */
 	subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-	fds[fdsc_count].fd = subs.local_pos_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- LOCAL POSITION SETPOINT --- */
 	subs.local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
-	fds[fdsc_count].fd = subs.local_pos_sp_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- GLOBAL POSITION --- */
 	subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	fds[fdsc_count].fd = subs.global_pos_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- GLOBAL POSITION SETPOINT--- */
 	subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
-	fds[fdsc_count].fd = subs.triplet_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- VICON POSITION --- */
 	subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
-	fds[fdsc_count].fd = subs.vicon_pos_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- OPTICAL FLOW --- */
 	subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
-	fds[fdsc_count].fd = subs.flow_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- RC CHANNELS --- */
 	subs.rc_sub = orb_subscribe(ORB_ID(rc_channels));
-	fds[fdsc_count].fd = subs.rc_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- AIRSPEED --- */
 	subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
-	fds[fdsc_count].fd = subs.airspeed_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- ESCs --- */
 	subs.esc_sub = orb_subscribe(ORB_ID(esc_status));
-	fds[fdsc_count].fd = subs.esc_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- GLOBAL VELOCITY SETPOINT --- */
 	subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
-	fds[fdsc_count].fd = subs.global_vel_sp_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- BATTERY --- */
 	subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
-	fds[fdsc_count].fd = subs.battery_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- TELEMETRY STATUS --- */
 	subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
-	fds[fdsc_count].fd = subs.telemetry_sub;
+	subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* WARNING: If you get the error message below,
-	 * then the number of registered messages (fdsc)
-	 * differs from the number of messages in the above list.
-	 */
-	if (fdsc_count > fdsc) {
-		warn("WARNING: Not enough space for poll fds allocated. Check %s:%d", __FILE__, __LINE__);
-		fdsc_count = fdsc;
-	}
-
-	/*
-	 * set up poll to block for new data,
-	 * wait for a maximum of 1000 ms
-	 */
-	const int poll_timeout = 1000;
 
 	thread_running = true;
 
@@ -990,8 +900,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 	if (log_on_start) {
 		/* check GPS topic to get GPS time */
 		if (log_name_timestamp) {
-			if (OK == orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos)) {
+			if (copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) {
-				gps_time = buf.gps_pos.time_gps_usec;
+				gps_time = buf_gps_pos.time_gps_usec;
 			}
 		}
 
@@ -999,61 +909,33 @@ int sdlog2_thread_main(int argc, char *argv[])
 	}
 
 	while (!main_thread_should_exit) {
-		/* decide use usleep() or blocking poll() */
+		usleep(sleep_delay);
-		bool use_sleep = sleep_delay > 0 && logging_enabled;
-
-		/* poll all topics if logging enabled or only management (first 3) if not */
-		int poll_ret = poll(fds, logging_enabled ? fdsc_count : 3, use_sleep ? 0 : poll_timeout);
-
-		/* handle the poll result */
-		if (poll_ret < 0) {
-			warnx("ERROR: poll error, stop logging");
-			main_thread_should_exit = true;
-
-		} else if (poll_ret > 0) {
-
-			/* check all data subscriptions only if logging enabled,
-			 * logging_enabled can be changed while checking vehicle_command and vehicle_status */
-			bool check_data = logging_enabled;
-			int ifds = 0;
-			int handled_topics = 0;
 
 		/* --- VEHICLE COMMAND - LOG MANAGEMENT --- */
-			if (fds[ifds++].revents & POLLIN) {
+		if (copy_if_updated(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd)) {
-				orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd);
-
 			handle_command(&buf.cmd);
-				handled_topics++;
 		}
 
 		/* --- VEHICLE STATUS - LOG MANAGEMENT --- */
-			if (fds[ifds++].revents & POLLIN) {
+		bool status_updated = copy_if_updated(ORB_ID(vehicle_status), subs.status_sub, &buf_status);
-				orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status);
 
+		if (status_updated) {
 			if (log_when_armed) {
 				handle_status(&buf_status);
 			}
-
-				handled_topics++;
 		}
 
 		/* --- GPS POSITION - LOG MANAGEMENT --- */
-			if (fds[ifds++].revents & POLLIN) {
+		bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos);
-				orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
 
-				if (log_name_timestamp) {
+		if (gps_pos_updated && log_name_timestamp) {
-					gps_time = buf.gps_pos.time_gps_usec;
+			gps_time = buf_gps_pos.time_gps_usec;
 		}
 
-				handled_topics++;
+		if (!logging_enabled) {
-			}
-
-			if (!logging_enabled || !check_data || handled_topics >= poll_ret) {
 			continue;
 		}
 
-			ifds = 1;	// begin from VEHICLE STATUS again
-
 		pthread_mutex_lock(&logbuffer_mutex);
 
 		/* write time stamp message */
@@ -1062,8 +944,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		LOGBUFFER_WRITE_AND_COUNT(TIME);
 
 		/* --- VEHICLE STATUS --- */
-			if (fds[ifds++].revents & POLLIN) {
+		if (status_updated) {
-				/* don't orb_copy, it's already done few lines above */
 			log_msg.msg_type = LOG_STAT_MSG;
 			log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
 			log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
@@ -1074,26 +955,24 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- GPS POSITION --- */
-			if (fds[ifds++].revents & POLLIN) {
+		if (gps_pos_updated) {
-				/* don't orb_copy, it's already done few lines above */
 			log_msg.msg_type = LOG_GPS_MSG;
-				log_msg.body.log_GPS.gps_time = buf.gps_pos.time_gps_usec;
+			log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec;
-				log_msg.body.log_GPS.fix_type = buf.gps_pos.fix_type;
+			log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type;
-				log_msg.body.log_GPS.eph = buf.gps_pos.eph_m;
+			log_msg.body.log_GPS.eph = buf_gps_pos.eph_m;
-				log_msg.body.log_GPS.epv = buf.gps_pos.epv_m;
+			log_msg.body.log_GPS.epv = buf_gps_pos.epv_m;
-				log_msg.body.log_GPS.lat = buf.gps_pos.lat;
+			log_msg.body.log_GPS.lat = buf_gps_pos.lat;
-				log_msg.body.log_GPS.lon = buf.gps_pos.lon;
+			log_msg.body.log_GPS.lon = buf_gps_pos.lon;
-				log_msg.body.log_GPS.alt = buf.gps_pos.alt * 0.001f;
+			log_msg.body.log_GPS.alt = buf_gps_pos.alt * 0.001f;
-				log_msg.body.log_GPS.vel_n = buf.gps_pos.vel_n_m_s;
+			log_msg.body.log_GPS.vel_n = buf_gps_pos.vel_n_m_s;
-				log_msg.body.log_GPS.vel_e = buf.gps_pos.vel_e_m_s;
+			log_msg.body.log_GPS.vel_e = buf_gps_pos.vel_e_m_s;
-				log_msg.body.log_GPS.vel_d = buf.gps_pos.vel_d_m_s;
+			log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s;
-				log_msg.body.log_GPS.cog = buf.gps_pos.cog_rad;
+			log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad;
 			LOGBUFFER_WRITE_AND_COUNT(GPS);
 		}
 
 		/* --- SENSOR COMBINED --- */
-			if (fds[ifds++].revents & POLLIN) {
+		if (copy_if_updated(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor)) {
-				orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor);
 			bool write_IMU = false;
 			bool write_SENS = false;
 
@@ -1148,8 +1027,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- ATTITUDE --- */
-			if (fds[ifds++].revents & POLLIN) {
+		if (copy_if_updated(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att)) {
-				orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
 			log_msg.msg_type = LOG_ATT_MSG;
 			log_msg.body.log_ATT.roll = buf.att.roll;
 			log_msg.body.log_ATT.pitch = buf.att.pitch;
@@ -1164,8 +1042,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- ATTITUDE SETPOINT --- */
-			if (fds[ifds++].revents & POLLIN) {
+		if (copy_if_updated(ORB_ID(vehicle_attitude_setpoint), subs.att_sp_sub, &buf.att_sp)) {
-				orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.att_sp_sub, &buf.att_sp);
 			log_msg.msg_type = LOG_ATSP_MSG;
 			log_msg.body.log_ATSP.roll_sp = buf.att_sp.roll_body;
 			log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body;
@@ -1175,8 +1052,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- RATES SETPOINT --- */
-			if (fds[ifds++].revents & POLLIN) {
+		if (copy_if_updated(ORB_ID(vehicle_rates_setpoint), subs.rates_sp_sub, &buf.rates_sp)) {
-				orb_copy(ORB_ID(vehicle_rates_setpoint), subs.rates_sp_sub, &buf.rates_sp);
 			log_msg.msg_type = LOG_ARSP_MSG;
 			log_msg.body.log_ARSP.roll_rate_sp = buf.rates_sp.roll;
 			log_msg.body.log_ARSP.pitch_rate_sp = buf.rates_sp.pitch;
@@ -1185,16 +1061,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- ACTUATOR OUTPUTS --- */
-			if (fds[ifds++].revents & POLLIN) {
+		if (copy_if_updated(ORB_ID(actuator_outputs_0), subs.act_outputs_sub, &buf.act_outputs)) {
-				orb_copy(ORB_ID(actuator_outputs_0), subs.act_outputs_sub, &buf.act_outputs);
 			log_msg.msg_type = LOG_OUT0_MSG;
 			memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output));
 			LOGBUFFER_WRITE_AND_COUNT(OUT0);
 		}
 
 		/* --- ACTUATOR CONTROL --- */
-			if (fds[ifds++].revents & POLLIN) {
+		if (copy_if_updated(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.act_controls_sub, &buf.act_controls)) {
-				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.act_controls_sub, &buf.act_controls);
 			log_msg.msg_type = LOG_ATTC_MSG;
 			log_msg.body.log_ATTC.roll = buf.act_controls.control[0];
 			log_msg.body.log_ATTC.pitch = buf.act_controls.control[1];
@@ -1204,8 +1078,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- LOCAL POSITION --- */
-			if (fds[ifds++].revents & POLLIN) {
+		if (copy_if_updated(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos)) {
-				orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
 			log_msg.msg_type = LOG_LPOS_MSG;
 			log_msg.body.log_LPOS.x = buf.local_pos.x;
 			log_msg.body.log_LPOS.y = buf.local_pos.y;
@@ -1224,6 +1097,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			if (buf.local_pos.dist_bottom_valid) {
 				dist_bottom_present = true;
 			}
+
 			if (dist_bottom_present) {
 				log_msg.msg_type = LOG_DIST_MSG;
 				log_msg.body.log_DIST.bottom = buf.local_pos.dist_bottom;
@@ -1234,8 +1108,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- LOCAL POSITION SETPOINT --- */
-			if (fds[ifds++].revents & POLLIN) {
+		if (copy_if_updated(ORB_ID(vehicle_local_position_setpoint), subs.local_pos_sp_sub, &buf.local_pos_sp)) {
-				orb_copy(ORB_ID(vehicle_local_position_setpoint), subs.local_pos_sp_sub, &buf.local_pos_sp);
 			log_msg.msg_type = LOG_LPSP_MSG;
 			log_msg.body.log_LPSP.x = buf.local_pos_sp.x;
 			log_msg.body.log_LPSP.y = buf.local_pos_sp.y;
@@ -1245,8 +1118,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- GLOBAL POSITION --- */
-			if (fds[ifds++].revents & POLLIN) {
+		if (copy_if_updated(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos)) {
-				orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
 			log_msg.msg_type = LOG_GPOS_MSG;
 			log_msg.body.log_GPOS.lat = buf.global_pos.lat * 1e7;
 			log_msg.body.log_GPOS.lon = buf.global_pos.lon * 1e7;
@@ -1260,8 +1132,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- GLOBAL POSITION SETPOINT --- */
-			if (fds[ifds++].revents & POLLIN) {
+		if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) {
-				orb_copy(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet);
 			log_msg.msg_type = LOG_GPSP_MSG;
 			log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state;
 			log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d);
@@ -1276,14 +1147,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- VICON POSITION --- */
-			if (fds[ifds++].revents & POLLIN) {
+		if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) {
-				orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos);
 			// TODO not implemented yet
 		}
 
 		/* --- FLOW --- */
-			if (fds[ifds++].revents & POLLIN) {
+		if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) {
-				orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
 			log_msg.msg_type = LOG_FLOW_MSG;
 			log_msg.body.log_FLOW.flow_raw_x = buf.flow.flow_raw_x;
 			log_msg.body.log_FLOW.flow_raw_y = buf.flow.flow_raw_y;
@@ -1296,8 +1165,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- RC CHANNELS --- */
-			if (fds[ifds++].revents & POLLIN) {
+		if (copy_if_updated(ORB_ID(rc_channels), subs.rc_sub, &buf.rc)) {
-				orb_copy(ORB_ID(rc_channels), subs.rc_sub, &buf.rc);
 			log_msg.msg_type = LOG_RC_MSG;
 			/* Copy only the first 8 channels of 14 */
 			memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel));
@@ -1306,8 +1174,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- AIRSPEED --- */
-			if (fds[ifds++].revents & POLLIN) {
+		if (copy_if_updated(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed)) {
-				orb_copy(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed);
 			log_msg.msg_type = LOG_AIRS_MSG;
 			log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s;
 			log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s;
@@ -1315,9 +1182,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- ESCs --- */
-			if (fds[ifds++].revents & POLLIN) {
+		if (copy_if_updated(ORB_ID(esc_status), subs.esc_sub, &buf.esc)) {
-				orb_copy(ORB_ID(esc_status), subs.esc_sub, &buf.esc);
-
 			for (uint8_t i = 0; i < buf.esc.esc_count; i++) {
 				log_msg.msg_type = LOG_ESC_MSG;
 				log_msg.body.log_ESC.counter = buf.esc.counter;
@@ -1337,8 +1202,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- GLOBAL VELOCITY SETPOINT --- */
-			if (fds[ifds++].revents & POLLIN) {
+		if (copy_if_updated(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp)) {
-				orb_copy(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp);
 			log_msg.msg_type = LOG_GVSP_MSG;
 			log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx;
 			log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy;
@@ -1347,8 +1211,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- BATTERY --- */
-			if (fds[ifds++].revents & POLLIN) {
+		if (copy_if_updated(ORB_ID(battery_status), subs.battery_sub, &buf.battery)) {
-				orb_copy(ORB_ID(battery_status), subs.battery_sub, &buf.battery);
 			log_msg.msg_type = LOG_BATT_MSG;
 			log_msg.body.log_BATT.voltage = buf.battery.voltage_v;
 			log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v;
@@ -1358,8 +1221,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- TELEMETRY --- */
-			if (fds[ifds++].revents & POLLIN) {
+		if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) {
-				orb_copy(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry);
 			log_msg.msg_type = LOG_TELE_MSG;
 			log_msg.body.log_TELE.rssi = buf.telemetry.rssi;
 			log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi;
@@ -1371,6 +1233,15 @@ int sdlog2_thread_main(int argc, char *argv[])
 			LOGBUFFER_WRITE_AND_COUNT(TELE);
 		}
 
+		/* --- BOTTOM DISTANCE --- */
+		if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) {
+			log_msg.msg_type = LOG_DIST_MSG;
+			log_msg.body.log_DIST.bottom = buf.range_finder.distance;
+			log_msg.body.log_DIST.bottom_rate = 0.0f;
+			log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0);
+			LOGBUFFER_WRITE_AND_COUNT(DIST);
+		}
+
 		/* signal the other thread new data, but not yet unlock */
 		if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
 			/* only request write if several packets can be written at once */
@@ -1381,13 +1252,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 		pthread_mutex_unlock(&logbuffer_mutex);
 	}
 
-		if (use_sleep) {
+	if (logging_enabled) {
-			usleep(sleep_delay);
-		}
-	}
-
-	if (logging_enabled)
 		sdlog2_stop_log();
+	}
 
 	pthread_mutex_destroy(&logbuffer_mutex);
 	pthread_cond_destroy(&logbuffer_cond);
@@ -1462,8 +1329,6 @@ int file_copy(const char *file_old, const char *file_new)
 
 void handle_command(struct vehicle_command_s *cmd)
 {
-	/* result of the command */
-	uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
 	int param;
 
 	/* request to set different system mode */

src/systemcmds/dumpfile/dumpfile.c

@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 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 dumpfile.c
+ *
+ * Dump file utility. Prints file size and contents in binary mode (don't replace LF with CR LF) to stdout.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <fcntl.h>
+#include <termios.h>
+
+#include <systemlib/err.h>
+
+__EXPORT int dumpfile_main(int argc, char *argv[]);
+
+int
+dumpfile_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		errx(1, "usage: dumpfile <filename>");
+	}
+
+	/* open input file */
+	FILE *f;
+	f = fopen(argv[1], "r");
+
+	if (f == NULL) {
+		printf("ERROR\n");
+		exit(1);
+	}
+
+	/* get file size */
+	fseek(f, 0L, SEEK_END);
+	int size = ftell(f);
+	fseek(f, 0L, SEEK_SET);
+
+	printf("OK %d\n", size);
+
+	/* configure stdout */
+	int out = fileno(stdout);
+
+	struct termios tc;
+	struct termios tc_old;
+	tcgetattr(out, &tc);
+
+	/* save old terminal attributes to restore it later on exit */
+	memcpy(&tc_old, &tc, sizeof(tc));
+
+	/* don't add CR on each LF*/
+	tc.c_oflag &= ~ONLCR;
+
+	if (tcsetattr(out, TCSANOW, &tc) < 0) {
+		warnx("ERROR setting stdout attributes");
+		exit(1);
+	}
+
+	char buf[512];
+	int nread;
+
+	/* dump file */
+	while ((nread = fread(buf, 1, sizeof(buf), f)) > 0) {
+		if (write(out, buf, nread) <= 0) {
+			warnx("error dumping file");
+			break;
+		}
+	}
+
+	fsync(out);
+	fclose(f);
+
+	/* restore old terminal attributes */
+	if (tcsetattr(out, TCSANOW, &tc_old) < 0) {
+		warnx("ERROR restoring stdout attributes");
+		exit(1);
+	}
+
+	return OK;
+}

src/systemcmds/dumpfile/module.mk

@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2014 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.
+#
+############################################################################
+
+#
+# Dump file utility
+#
+
+MODULE_COMMAND	 = dumpfile
+SRCS		 = dumpfile.c
+
+MAXOPTIMIZATION	 = -Os