move all distance sensor drivers to the same folder

src/drivers/distance_sensor/CMakeLists.txt

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#add_subdirectory(hc_sr04) # not currently supported
+add_subdirectory(ll40ls)
+add_subdirectory(mb12xx)
+add_subdirectory(sf0x)
+add_subdirectory(sf1xx)
+add_subdirectory(srf02)
+add_subdirectory(srf02_i2c)
+add_subdirectory(teraranger)
+add_subdirectory(tfmini)
+add_subdirectory(ulanding)

src/drivers/distance_sensor/hc_sr04/CMakeLists.txt

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_module(
+	MODULE drivers__hc_sr04
+	MAIN hc_sr04
+	STACK_MAIN 1200
+	COMPILE_FLAGS
+	SRCS
+		hc_sr04.cpp
+	DEPENDS
+		platforms__common
+	)

src/drivers/distance_sensor/hc_sr04/hc_sr04.cpp

@@ -0,0 +1,947 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015, 2016 Airmind 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 Airmind 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 hc_sr04.cpp
+ *
+ * Driver for the hc_sr04 sonar range finders .
+ */
+
+#include <px4_config.h>
+#include <px4_workqueue.h>
+#include <drivers/device/device.h>
+#include <px4_defines.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 <vector>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_range_finder.h>
+#include <drivers/device/ringbuffer.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/distance_sensor.h>
+
+#define SR04_MAX_RANGEFINDERS 6
+#define SR04_ID_BASE	 0x10
+
+/* Configuration Constants */
+#define SR04_DEVICE_PATH	"/dev/hc_sr04"
+
+#define SUBSYSTEM_TYPE_RANGEFINDER 131072
+/* Device limits */
+#define SR04_MIN_DISTANCE 	(0.10f)
+#define SR04_MAX_DISTANCE 	(4.00f)
+
+#define SR04_CONVERSION_INTERVAL 	100000 /* 100ms for one sonar */
+
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+class HC_SR04 : public device::CDev
+{
+public:
+	HC_SR04(unsigned sonars = 6);
+	virtual ~HC_SR04();
+
+	virtual int 		init();
+
+	virtual ssize_t		read(device::file_t *filp, char *buffer, size_t buflen);
+	virtual int			ioctl(device::file_t *filp, int cmd, unsigned long arg);
+
+	/**
+	* Diagnostics - print some basic information about the driver.
+	*/
+	void				print_info();
+	void                            interrupt(unsigned time);
+
+protected:
+	virtual int			probe();
+
+private:
+	float				_min_distance;
+	float				_max_distance;
+	work_s				_work;
+	ringbuffer::RingBuffer	*_reports;
+	bool				_sensor_ok;
+	int					_measure_ticks;
+	bool				_collect_phase;
+	int					_class_instance;
+	int					_orb_class_instance;
+
+	orb_advert_t		_distance_sensor_topic;
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_comms_errors;
+
+	uint8_t				_cycle_counter;	/* counter in cycle to change i2c adresses */
+	int					_cycling_rate;	/* */
+	uint8_t				_index_counter;	/* temporary sonar i2c address */
+
+	std::vector<float>
+	_latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */
+	unsigned 		_sonars;
+	struct GPIOConfig {
+		uint32_t        trig_port;
+		uint32_t        echo_port;
+		uint32_t        alt;
+	};
+	static const GPIOConfig _gpio_tab[];
+	unsigned 		_raising_time;
+	unsigned 		_falling_time;
+	unsigned 		_status;
+	/**
+	* 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();
+
+	/**
+	* 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 MB12XX_MIN_DISTANCE
+	* and MB12XX_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);
+
+
+};
+
+const HC_SR04::GPIOConfig HC_SR04::_gpio_tab[] = {
+	{GPIO_GPIO6_OUTPUT,      GPIO_GPIO7_INPUT,       0},
+	{GPIO_GPIO6_OUTPUT,      GPIO_GPIO8_INPUT,       0},
+	{GPIO_GPIO6_OUTPUT,      GPIO_GPIO9_INPUT,       0},
+	{GPIO_GPIO6_OUTPUT,      GPIO_GPIO10_INPUT,       0},
+	{GPIO_GPIO6_OUTPUT,      GPIO_GPIO11_INPUT,       0},
+	{GPIO_GPIO6_OUTPUT,      GPIO_GPIO12_INPUT,       0}
+};
+
+/*
+ * Driver 'main' command.
+ */
+extern "C"  __EXPORT int hc_sr04_main(int argc, char *argv[]);
+static int sonar_isr(int irq, void *context);
+
+HC_SR04::HC_SR04(unsigned sonars) :
+	CDev("HC_SR04", SR04_DEVICE_PATH, 0),
+	_min_distance(SR04_MIN_DISTANCE),
+	_max_distance(SR04_MAX_DISTANCE),
+	_reports(nullptr),
+	_sensor_ok(false),
+	_measure_ticks(0),
+	_collect_phase(false),
+	_class_instance(-1),
+	_orb_class_instance(-1),
+	_distance_sensor_topic(nullptr),
+	_sample_perf(perf_alloc(PC_ELAPSED, "hc_sr04_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "hc_sr04_comms_errors")),
+	_cycle_counter(0),	/* initialising counter for cycling function to zero */
+	_cycling_rate(0),	/* initialising cycling rate (which can differ depending on one sonar or multiple) */
+	_index_counter(0), 	/* initialising temp sonar i2c address to zero */
+	_sonars(sonars),
+	_raising_time(0),
+	_falling_time(0),
+	_status(0)
+
+{
+	/* enable debug() calls */
+	_debug_enabled = false;
+
+	/* work_cancel in the dtor will explode if we don't do this... */
+	memset(&_work, 0, sizeof(_work));
+}
+
+HC_SR04::~HC_SR04()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr) {
+		delete _reports;
+	}
+
+	if (_class_instance != -1) {
+		unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance);
+	}
+
+	/* free perf counters */
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+}
+
+int
+HC_SR04::init()
+{
+	int ret = PX4_ERROR;
+
+	/* do I2C init (and probe) first */
+	if (CDev::init() != OK) {
+		return PX4_ERROR;
+	}
+
+	/* allocate basic report buffers */
+	_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
+
+	if (_reports == nullptr) {
+		return PX4_ERROR;
+	}
+
+	_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
+
+	/* get a publish handle on the range finder topic */
+	struct distance_sensor_s ds_report = {};
+
+	_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
+				 &_orb_class_instance, ORB_PRIO_LOW);
+
+	if (_distance_sensor_topic == nullptr) {
+		DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
+	}
+
+	/* init echo port : */
+	for (unsigned i = 0; i <= _sonars; i++) {
+		px4_arch_configgpio(_gpio_tab[i].trig_port);
+		px4_arch_gpiowrite(_gpio_tab[i].trig_port, false);
+		px4_arch_configgpio(_gpio_tab[i].echo_port);
+		_latest_sonar_measurements.push_back(0);
+	}
+
+	usleep(200000); /* wait for 200ms; */
+
+	_cycling_rate = SR04_CONVERSION_INTERVAL;
+
+	/* show the connected sonars in terminal */
+	DEVICE_DEBUG("Number of sonars set: %d", _sonars);
+
+	ret = OK;
+	/* sensor is ok, but we don't really know if it is within range */
+	_sensor_ok = true;
+
+	return ret;
+}
+
+int
+HC_SR04::probe()
+{
+	return (OK);
+}
+
+void
+HC_SR04::set_minimum_distance(float min)
+{
+	_min_distance = min;
+}
+
+void
+HC_SR04::set_maximum_distance(float max)
+{
+	_max_distance = max;
+}
+
+float
+HC_SR04::get_minimum_distance()
+{
+	return _min_distance;
+}
+
+float
+HC_SR04::get_maximum_distance()
+{
+	return _max_distance;
+}
+void
+HC_SR04::interrupt(unsigned time)
+{
+	if (_status == 0) {
+		_raising_time = time;
+		_status++;
+		return;
+
+	} else if (_status == 1) {
+		_falling_time = time;
+		_status++;
+		return;
+	}
+
+	return;
+}
+
+int
+HC_SR04::ioctl(device::file_t *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(_cycling_rate);
+
+					/* 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 */
+					int ticks = USEC2TICK(1000000 / arg);
+
+					/* check against maximum rate */
+					if (ticks < USEC2TICK(_cycling_rate)) {
+						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;
+			}
+
+			ATOMIC_ENTER;
+
+			if (!_reports->resize(arg)) {
+				ATOMIC_LEAVE;
+				return -ENOMEM;
+			}
+
+			ATOMIC_LEAVE;
+
+			return OK;
+		}
+
+	case SENSORIOCRESET:
+		/* XXX implement this */
+		return -EINVAL;
+
+	default:
+		/* give it to the superclass */
+		return CDev::ioctl(filp, cmd, arg);
+	}
+}
+
+ssize_t
+HC_SR04::read(device::file_t *filp, char *buffer, size_t buflen)
+{
+
+	unsigned count = buflen / sizeof(struct distance_sensor_s);
+	struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(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(_cycling_rate * 2);
+
+		/* 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
+HC_SR04::measure()
+{
+
+	int ret;
+	/*
+	 * Send a plus begin a measurement.
+	 */
+	px4_arch_gpiowrite(_gpio_tab[_cycle_counter].trig_port, true);
+	usleep(10);  // 10us
+	px4_arch_gpiowrite(_gpio_tab[_cycle_counter].trig_port, false);
+
+	px4_arch_gpiosetevent(_gpio_tab[_cycle_counter].echo_port, true, true, false, sonar_isr);
+	_status = 0;
+	ret = OK;
+
+	return ret;
+}
+
+int
+HC_SR04::collect()
+{
+	int	ret = -EIO;
+#if 0
+	perf_begin(_sample_perf);
+
+	/* read from the sensor */
+	if (_status != 2) {
+		DEVICE_DEBUG("erro sonar %d ,status=%d", _cycle_counter, _status);
+		px4_arch_gpiosetevent(_gpio_tab[_cycle_counter].echo_port, true, true, false, nullptr);
+		perf_end(_sample_perf);
+		return (ret);
+	}
+
+	unsigned  distance_time = _falling_time - _raising_time ;
+
+	float si_units = (distance_time * 0.000170f) ; /* meter */
+	struct distance_sensor_s 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);
+
+	/* if only one sonar, write it to the original distance parameter so that it's still used as altitude sonar */
+	if (_sonars == 1) {
+		report.distance = si_units;
+
+		for (unsigned i = 0; i < (SRF02_MAX_RANGEFINDERS); i++) {
+			report.id[i] = 0;
+			report.distance_vector[i] = 0;
+		}
+
+		report.id[0] = SR04_ID_BASE;
+		report.distance_vector[0] = si_units; //  将测量值填入向量中，适应test()的要求
+		report.just_updated = 1;
+
+	} else {
+		/* for multiple sonars connected */
+
+		_latest_sonar_measurements[_cycle_counter] = si_units;
+		report.just_updated = 0;
+
+		for (unsigned i = 0; i < SRF02_MAX_RANGEFINDERS; i++) {
+			if (i < _sonars) {
+				report.distance_vector[i] = _latest_sonar_measurements[i];
+				report.id[i] = SR04_ID_BASE + i;
+				report.just_updated++;
+
+			} else {
+				report.distance_vector[i] = 0;
+				report.id[i] = 0;
+			}
+
+		}
+
+		report.distance =  _latest_sonar_measurements[0]; //
+	}
+
+	report.minimum_distance = get_minimum_distance();
+	report.maximum_distance = get_maximum_distance();
+	report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
+
+	/* publish it, if we are the primary */
+	if (_distance_sensor_topic != nullptr) {
+		orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
+	}
+
+	_reports->force(&report);
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	ret = OK;
+
+	px4_arch_gpiosetevent(_gpio_tab[_cycle_counter].echo_port, true, true, false, nullptr); /* close interrupt */
+	perf_end(_sample_perf);
+#endif
+	return ret;
+}
+
+void
+HC_SR04::start()
+{
+
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+	_reports->flush();
+
+	measure();  /* begin measure */
+
+	/* schedule a cycle to start things */
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&HC_SR04::cycle_trampoline,
+		   this,
+		   USEC2TICK(_cycling_rate));
+
+
+	/* notify about state change */
+	struct subsystem_info_s info = {};
+	info.present = true;
+	info.enabled = true;
+	info.ok = true;
+	info.subsystem_type = SUBSYSTEM_TYPE_RANGEFINDER;
+
+	static orb_advert_t pub = nullptr;
+
+	if (pub != nullptr) {
+		orb_publish(ORB_ID(subsystem_info), pub, &info);
+
+
+	} else {
+		pub = orb_advertise(ORB_ID(subsystem_info), &info);
+
+	}
+}
+
+void
+HC_SR04::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+HC_SR04::cycle_trampoline(void *arg)
+{
+
+	HC_SR04 *dev = (HC_SR04 *)arg;
+
+	dev->cycle();
+
+}
+
+void
+HC_SR04::cycle()
+{
+	/*_circle_count 计录当前sonar　*/
+	/* perform collection */
+	if (OK != collect()) {
+		DEVICE_DEBUG("collection error");
+	}
+
+	/* change to next sonar */
+	_cycle_counter = _cycle_counter + 1;
+
+	if (_cycle_counter >= _sonars) {
+		_cycle_counter = 0;
+	}
+
+	/* 测量next sonar */
+	if (OK != measure()) {
+		DEVICE_DEBUG("measure error sonar adress %d", _cycle_counter);
+	}
+
+
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&HC_SR04::cycle_trampoline,
+		   this,
+		   USEC2TICK(_cycling_rate));
+
+}
+
+void
+HC_SR04::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	printf("poll interval:  %u ticks\n", _measure_ticks);
+	_reports->print_info("report queue");
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace  hc_sr04
+{
+
+HC_SR04	*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 HC_SR04();
+
+	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(SR04_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		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()
+{
+#if 0
+	struct distance_sensor_s report;
+	ssize_t sz;
+	int ret;
+
+	int fd = open(SR04_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		err(1, "%s open failed (try 'hc_sr04 start' if the driver is not running", SR04_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 of sonar %d,id=%d", (double)report.distance_vector[report.just_updated], report.just_updated,
+	      report.id[report.just_updated]);
+	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);
+
+		/* Print the sonar rangefinder report sonar distance vector */
+		for (uint8_t count = 0; count < SRF02_MAX_RANGEFINDERS; count++) {
+			warnx("measurement: %0.3f of sonar %u, id=%d", (double)report.distance_vector[count], count + 1, report.id[count]);
+		}
+
+		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");
+#endif
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(SR04_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 */
+
+static int sonar_isr(int irq, void *context)
+{
+	unsigned time = hrt_absolute_time();
+	/* ack the interrupts we just read */
+
+	if (hc_sr04::g_dev != nullptr) {
+		hc_sr04::g_dev->interrupt(time);
+	}
+
+	return OK;
+}
+
+
+
+int
+hc_sr04_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[1], "start")) {
+		hc_sr04::start();
+	}
+
+	/*
+	 * Stop the driver
+	 */
+	if (!strcmp(argv[1], "stop")) {
+		hc_sr04::stop();
+	}
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test")) {
+		hc_sr04::test();
+	}
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset")) {
+		hc_sr04::reset();
+	}
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
+		hc_sr04::info();
+	}
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+}

src/drivers/distance_sensor/ll40ls/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__ll40ls
+	MAIN ll40ls
+	COMPILE_FLAGS
+	SRCS
+		ll40ls.cpp
+		LidarLite.cpp
+		LidarLiteI2C.cpp
+		LidarLitePWM.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :

src/drivers/distance_sensor/ll40ls/LidarLite.cpp

@@ -0,0 +1,157 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014, 2015 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 LidarLite.h
+ * @author Johan Jansen <jnsn.johan@gmail.com>
+ *
+ * Generic interface driver for the PulsedLight Lidar-Lite range finders.
+ */
+
+#include "LidarLite.h"
+
+LidarLite::LidarLite() :
+	_min_distance(LL40LS_MIN_DISTANCE),
+	_max_distance(LL40LS_MAX_DISTANCE),
+	_measure_ticks(0)
+{
+}
+
+LidarLite::~LidarLite()
+{
+}
+
+void LidarLite::set_minimum_distance(const float min)
+{
+	_min_distance = min;
+}
+
+void LidarLite::set_maximum_distance(const float max)
+{
+	_max_distance = max;
+}
+
+float LidarLite::get_minimum_distance() const
+{
+	return _min_distance;
+}
+
+float LidarLite::get_maximum_distance() const
+{
+	return _max_distance;
+}
+
+uint32_t LidarLite::getMeasureTicks() const
+{
+	return _measure_ticks;
+}
+
+int LidarLite::ioctl(device::file_t *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(LL40LS_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(LL40LS_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 SENSORIOCRESET:
+		reset_sensor();
+		return OK;
+
+	default:
+		return -EINVAL;
+	}
+}

src/drivers/distance_sensor/ll40ls/LidarLite.h

@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014, 2015 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 LidarLite.h
+ * @author Johan Jansen <jnsn.johan@gmail.com>
+ *
+ * Generic interface driver for the PulsedLight Lidar-Lite range finders.
+ */
+#pragma once
+
+#include <drivers/device/device.h>
+#include <drivers/drv_range_finder.h>
+
+/* Device limits */
+#define LL40LS_MIN_DISTANCE (0.05f)
+#define LL40LS_MAX_DISTANCE (25.00f)
+
+// normal conversion wait time
+#define LL40LS_CONVERSION_INTERVAL 50*1000UL /* 50ms */
+
+// maximum time to wait for a conversion to complete.
+#define LL40LS_CONVERSION_TIMEOUT 100*1000UL /* 100ms */
+
+class LidarLite
+{
+public:
+	LidarLite();
+
+	virtual ~LidarLite();
+
+	virtual int init() = 0;
+
+	virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
+
+	virtual void start() = 0;
+
+	virtual void stop() = 0;
+
+	/**
+	* @brief
+	*   Diagnostics - print some basic information about the driver.
+	*/
+	virtual void print_info() = 0;
+
+	/**
+	 * @brief
+	 *   print registers to console
+	 */
+	virtual void print_registers() = 0;
+
+	virtual const char *get_dev_name() = 0;
+
+protected:
+	/**
+	* 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 LL40LS_MIN_DISTANCE
+	* and LL40LS_MAX_DISTANCE
+	*/
+	void                set_minimum_distance(const float min);
+	void                set_maximum_distance(const float max);
+	float               get_minimum_distance() const;
+	float               get_maximum_distance() const;
+
+	uint32_t            getMeasureTicks() const;
+
+	virtual int         measure() = 0;
+	virtual int         collect() = 0;
+
+	virtual int         reset_sensor() = 0;
+
+private:
+	float               _min_distance;
+	float               _max_distance;
+	uint32_t            _measure_ticks;
+};

src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp

@@ -0,0 +1,565 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014, 2015 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 LidarLiteI2C.cpp
+ * @author Allyson Kreft
+ *
+ * Driver for the PulsedLight Lidar-Lite range finders connected via I2C.
+ */
+
+#include "LidarLiteI2C.h"
+#include <px4_defines.h>
+#include <semaphore.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <poll.h>
+#include <string.h>
+#include <stdio.h>
+#include <drivers/drv_hrt.h>
+
+LidarLiteI2C::LidarLiteI2C(int bus, const char *path, uint8_t rotation, int address) :
+	I2C("LL40LS", path, bus, address, 100000),
+	_rotation(rotation),
+	_work{},
+	_reports(nullptr),
+	_sensor_ok(false),
+	_collect_phase(false),
+	_class_instance(-1),
+	_orb_class_instance(-1),
+	_distance_sensor_topic(nullptr),
+	_sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_i2c_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "ll40ls_i2c_comms_errors")),
+	_sensor_resets(perf_alloc(PC_COUNT, "ll40ls_i2c_resets")),
+	_sensor_zero_resets(perf_alloc(PC_COUNT, "ll40ls_i2c_zero_resets")),
+	_last_distance(0),
+	_zero_counter(0),
+	_acquire_time_usec(0),
+	_pause_measurements(false),
+	_hw_version(0),
+	_sw_version(0)
+{
+	// up the retries since the device misses the first measure attempts
+	_retries = 3;
+
+	// work_cancel in the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
+}
+
+LidarLiteI2C::~LidarLiteI2C()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr) {
+		delete _reports;
+	}
+
+	if (_class_instance != -1) {
+		unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance);
+	}
+
+	// free perf counters
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+	perf_free(_sensor_resets);
+	perf_free(_sensor_zero_resets);
+}
+
+int LidarLiteI2C::init()
+{
+	int ret = PX4_ERROR;
+
+	/* do I2C init (and probe) first */
+	if (I2C::init() != OK) {
+		return ret;
+	}
+
+	/* allocate basic report buffers */
+	_reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s));
+
+	if (_reports == nullptr) {
+		return ret;
+	}
+
+	_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
+
+	/* get a publish handle on the range finder topic */
+	struct distance_sensor_s ds_report = {};
+	measure();
+	_reports->get(&ds_report);
+	_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
+				 &_orb_class_instance, ORB_PRIO_LOW);
+
+	if (_distance_sensor_topic == nullptr) {
+		DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?");
+	}
+
+	ret = OK;
+	/* sensor is ok, but we don't really know if it is within range */
+	_sensor_ok = true;
+
+	return ret;
+}
+
+int LidarLiteI2C::read_reg(uint8_t reg, uint8_t &val)
+{
+	return lidar_transfer(&reg, 1, &val, 1);
+}
+
+int LidarLiteI2C::write_reg(uint8_t reg, uint8_t val)
+{
+	const uint8_t cmd[2] = { reg, val };
+	return transfer(&cmd[0], 2, nullptr, 0);
+}
+
+/*
+  LidarLite specific transfer() function that avoids a stop condition
+  with SCL low
+ */
+int LidarLiteI2C::lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
+{
+	if (send != nullptr && send_len > 0) {
+		int ret = transfer(send, send_len, nullptr, 0);
+
+		if (ret != OK) {
+			return ret;
+		}
+	}
+
+	if (recv != nullptr && recv_len > 0) {
+		return transfer(nullptr, 0, recv, recv_len);
+	}
+
+	return OK;
+}
+
+int LidarLiteI2C::probe()
+{
+	// cope with both old and new I2C bus address
+	const uint8_t addresses[2] = {LL40LS_BASEADDR, LL40LS_BASEADDR_OLD};
+
+	// more retries for detection
+	_retries = 10;
+
+	for (uint8_t i = 0; i < sizeof(addresses); i++) {
+		/*
+		  check for hw and sw versions. It would be better if
+		  we had a proper WHOAMI register
+		 */
+		if (read_reg(LL40LS_HW_VERSION, _hw_version) == OK && _hw_version > 0 &&
+		    read_reg(LL40LS_SW_VERSION, _sw_version) == OK && _sw_version > 0) {
+			goto ok;
+		}
+
+		DEVICE_DEBUG("probe failed hw_version=0x%02x sw_version=0x%02x\n",
+			     (unsigned)_hw_version,
+			     (unsigned)_sw_version);
+	}
+
+	// not found on any address
+	return -EIO;
+
+ok:
+	_retries = 3;
+	return reset_sensor();
+}
+
+int LidarLiteI2C::ioctl(device::file_t *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+	case SENSORIOCSQUEUEDEPTH: {
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 1) || (arg > 100)) {
+				return -EINVAL;
+			}
+
+			ATOMIC_ENTER;
+
+			if (!_reports->resize(arg)) {
+				ATOMIC_LEAVE;
+				return -ENOMEM;
+			}
+
+			ATOMIC_LEAVE;
+
+			return OK;
+		}
+
+	default: {
+			int result = LidarLite::ioctl(filp, cmd, arg);
+
+			if (result == -EINVAL) {
+				result = I2C::ioctl(filp, cmd, arg);
+			}
+
+			return result;
+		}
+	}
+}
+
+ssize_t LidarLiteI2C::read(device::file_t *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct distance_sensor_s);
+	struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(buffer);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1) {
+		return -ENOSPC;
+	}
+
+	/* if automatic measurement is enabled */
+	if (getMeasureTicks() > 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(LL40LS_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 LidarLiteI2C::measure()
+{
+	int ret;
+
+	if (_pause_measurements) {
+		// we are in print_registers() and need to avoid
+		// acquisition to keep the I2C peripheral on the
+		// sensor active
+		return OK;
+	}
+
+	/*
+	 * Send the command to begin a measurement.
+	 */
+	const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE };
+	ret = lidar_transfer(cmd, sizeof(cmd), nullptr, 0);
+
+	if (OK != ret) {
+		perf_count(_comms_errors);
+		DEVICE_DEBUG("i2c::transfer returned %d", ret);
+
+		// if we are getting lots of I2C transfer errors try
+		// resetting the sensor
+		if (perf_event_count(_comms_errors) % 10 == 0) {
+			perf_count(_sensor_resets);
+			reset_sensor();
+		}
+
+		return ret;
+	}
+
+	// remember when we sent the acquire so we can know when the
+	// acquisition has timed out
+	_acquire_time_usec = hrt_absolute_time();
+	ret = OK;
+
+	return ret;
+}
+
+/*
+  reset the sensor to power on defaults
+ */
+int LidarLiteI2C::reset_sensor()
+{
+	int ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET);
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	// wait for sensor reset to complete
+	usleep(1000);
+
+	return OK;
+}
+
+/*
+  dump sensor registers for debugging
+ */
+void LidarLiteI2C::print_registers()
+{
+	_pause_measurements = true;
+	printf("ll40ls registers\n");
+	// wait for a while to ensure the lidar is in a ready state
+	usleep(50000);
+
+	for (uint8_t reg = 0; reg <= 0x67; reg++) {
+		uint8_t val = 0;
+		int ret = lidar_transfer(&reg, 1, &val, 1);
+
+		if (ret != OK) {
+			printf("%02x:XX ", (unsigned)reg);
+
+		} else {
+			printf("%02x:%02x ", (unsigned)reg, (unsigned)val);
+		}
+
+		if (reg % 16 == 15) {
+			printf("\n");
+		}
+	}
+
+	printf("\n");
+	_pause_measurements = false;
+}
+
+int LidarLiteI2C::collect()
+{
+	int ret = -EIO;
+
+	/* read from the sensor */
+	uint8_t val[2] = {0, 0};
+
+	perf_begin(_sample_perf);
+
+	// read the high and low byte distance registers
+	uint8_t distance_reg = LL40LS_DISTHIGH_REG | LL40LS_AUTO_INCREMENT;
+	ret = lidar_transfer(&distance_reg, 1, &val[0], sizeof(val));
+
+	// if the transfer failed or if the high bit of distance is
+	// set then the distance is invalid
+	if (ret < 0 || (val[0] & 0x80)) {
+		if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT) {
+			/*
+			  NACKs from the sensor are expected when we
+			  read before it is ready, so only consider it
+			  an error if more than 100ms has elapsed.
+			 */
+			DEVICE_DEBUG("error reading from sensor: %d", ret);
+			perf_count(_comms_errors);
+
+			if (perf_event_count(_comms_errors) % 10 == 0) {
+				perf_count(_sensor_resets);
+				reset_sensor();
+			}
+		}
+
+		perf_end(_sample_perf);
+		// if we are getting lots of I2C transfer errors try
+		// resetting the sensor
+		return ret;
+	}
+
+	uint16_t distance_cm = (val[0] << 8) | val[1];
+	float distance_m = float(distance_cm) * 1e-2f;
+	struct distance_sensor_s report;
+
+	if (distance_cm == 0) {
+		_zero_counter++;
+
+		if (_zero_counter == 20) {
+			/* we have had 20 zeros in a row - reset the
+			   sensor. This is a known bad state of the
+			   sensor where it returns 16 bits of zero for
+			   the distance with a trailing NACK, and
+			   keeps doing that even when the target comes
+			   into a valid range.
+			*/
+			_zero_counter = 0;
+			perf_end(_sample_perf);
+			perf_count(_sensor_zero_resets);
+			return reset_sensor();
+		}
+
+	} else {
+		_zero_counter = 0;
+	}
+
+	_last_distance = distance_cm;
+
+
+	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
+	report.timestamp = hrt_absolute_time();
+	report.current_distance = distance_m;
+	report.min_distance = get_minimum_distance();
+	report.max_distance = get_maximum_distance();
+	report.covariance = 0.0f;
+	/* the sensor is in fact a laser + sonar but there is no enum for this */
+	report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
+	report.orientation = _rotation;
+	/* TODO: set proper ID */
+	report.id = 0;
+
+	/* publish it, if we are the primary */
+	if (_distance_sensor_topic != nullptr) {
+		orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
+	}
+
+	_reports->force(&report);
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	ret = OK;
+
+	perf_end(_sample_perf);
+	return ret;
+}
+
+void LidarLiteI2C::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)&LidarLiteI2C::cycle_trampoline, this, 1);
+}
+
+void LidarLiteI2C::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void LidarLiteI2C::cycle_trampoline(void *arg)
+{
+	LidarLiteI2C *dev = (LidarLiteI2C *)arg;
+
+	dev->cycle();
+}
+
+void LidarLiteI2C::cycle()
+{
+	/* collection phase? */
+	if (_collect_phase) {
+
+		/* try a collection */
+		if (OK != collect()) {
+			DEVICE_DEBUG("collection error");
+
+			/* if we've been waiting more than 200ms then
+			   send a new acquire */
+			if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT * 2) {
+				_collect_phase = false;
+			}
+
+		} else {
+			/* next phase is measurement */
+			_collect_phase = false;
+
+			/*
+			 * Is there a collect->measure gap?
+			 */
+			if (getMeasureTicks() > USEC2TICK(LL40LS_CONVERSION_INTERVAL)) {
+
+				/* schedule a fresh cycle call when we are ready to measure again */
+				work_queue(HPWORK,
+					   &_work,
+					   (worker_t)&LidarLiteI2C::cycle_trampoline,
+					   this,
+					   getMeasureTicks() - USEC2TICK(LL40LS_CONVERSION_INTERVAL));
+
+				return;
+			}
+		}
+	}
+
+	if (_collect_phase == false) {
+		/* measurement phase */
+		if (OK != measure()) {
+			DEVICE_DEBUG("measure error");
+
+		} else {
+			/* next phase is collection. Don't switch to
+			   collection phase until we have a successful
+			   acquire request I2C transfer */
+			_collect_phase = true;
+		}
+	}
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&LidarLiteI2C::cycle_trampoline,
+		   this,
+		   USEC2TICK(LL40LS_CONVERSION_INTERVAL));
+}
+
+void LidarLiteI2C::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	perf_print_counter(_sensor_resets);
+	perf_print_counter(_sensor_zero_resets);
+	printf("poll interval:  %u ticks\n", getMeasureTicks());
+	_reports->print_info("report queue");
+	printf("distance: %ucm (0x%04x)\n",
+	       (unsigned)_last_distance, (unsigned)_last_distance);
+}
+
+const char *LidarLiteI2C::get_dev_name()
+{
+	return get_devname();
+}

src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h

@@ -0,0 +1,179 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014, 2015 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 LidarLiteI2C.h
+ * @author Allyson Kreft
+ *
+ * Driver for the PulsedLight Lidar-Lite range finders connected via I2C.
+ */
+
+#pragma once
+
+#include "LidarLite.h"
+
+#include <px4_workqueue.h>
+
+#include <systemlib/perf_counter.h>
+
+#include <drivers/device/i2c.h>
+#include <drivers/device/ringbuffer.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/distance_sensor.h>
+
+
+/* Configuration Constants */
+#define LL40LS_BASEADDR     0x62 /* 7-bit address */
+#define LL40LS_BASEADDR_OLD     0x42 /* previous 7-bit address */
+
+/* LL40LS Registers addresses */
+
+#define LL40LS_MEASURE_REG      0x00        /* Measure range register */
+#define LL40LS_MSRREG_RESET     0x00        /* reset to power on defaults */
+#define LL40LS_MSRREG_ACQUIRE       0x04        /* Value to initiate a measurement, varies based on sensor revision */
+#define LL40LS_DISTHIGH_REG     0x0F        /* High byte of distance register, auto increment */
+#define LL40LS_AUTO_INCREMENT   0x80
+#define LL40LS_HW_VERSION         0x41
+#define LL40LS_SW_VERSION         0x4f
+#define LL40LS_SIGNAL_STRENGTH_REG  0x5b
+
+class LidarLiteI2C : public LidarLite, public device::I2C
+{
+public:
+	LidarLiteI2C(int bus, const char *path,
+		     uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING,
+		     int address = LL40LS_BASEADDR);
+	virtual ~LidarLiteI2C();
+
+	int         init() override;
+
+	ssize_t     read(device::file_t *filp, char *buffer, size_t buflen) override;
+	int         ioctl(device::file_t *filp, int cmd, unsigned long arg) override;
+
+	/**
+	* Diagnostics - print some basic information about the driver.
+	*/
+	void print_info() override;
+
+	/**
+	 * print registers to console
+	 */
+	void print_registers() override;
+
+	const char *get_dev_name() override;
+
+protected:
+	int         probe() override;
+	int         read_reg(uint8_t reg, uint8_t &val);
+	int         write_reg(uint8_t reg, uint8_t val);
+
+	int                 measure() override;
+	int                 reset_sensor() override;
+
+private:
+	uint8_t _rotation;
+	work_s              _work;
+	ringbuffer::RingBuffer          *_reports;
+	bool                _sensor_ok;
+	bool                _collect_phase;
+	int                 _class_instance;
+	int		    _orb_class_instance;
+
+	orb_advert_t        _distance_sensor_topic;
+
+	perf_counter_t      _sample_perf;
+	perf_counter_t      _comms_errors;
+	perf_counter_t      _sensor_resets;
+	perf_counter_t      _sensor_zero_resets;
+	uint16_t        _last_distance;
+	uint16_t        _zero_counter;
+	uint64_t        _acquire_time_usec;
+	volatile bool       _pause_measurements;
+	uint8_t		_hw_version;
+	uint8_t		_sw_version;
+
+	/**
+	 * LidarLite specific transfer function. This is needed
+	 * to avoid a stop transition with SCL high
+	 *
+	 * @param send		Pointer to bytes to send.
+	 * @param send_len	Number of bytes to send.
+	 * @param recv		Pointer to buffer for bytes received.
+	 * @param recv_len	Number of bytes to receive.
+	 * @return		OK if the transfer was successful, -errno
+	 *			otherwise.
+	 */
+	int lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len);
+
+	/**
+	* 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() override;
+
+	/**
+	* Stop the automatic measurement state machine.
+	*/
+	void                stop() override;
+
+	/**
+	* Perform a poll cycle; collect from the previous measurement
+	* and start a new one.
+	*/
+	void                cycle();
+	int                 collect() override;
+
+	/**
+	* 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);
+
+private:
+	LidarLiteI2C(const LidarLiteI2C &copy) = delete;
+	LidarLiteI2C operator=(const LidarLiteI2C &assignment) = delete;
+};

src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp

@@ -0,0 +1,289 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014, 2015 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 LidarLitePWM.h
+ * @author Johan Jansen <jnsn.johan@gmail.com>
+ * @author Ban Siesta <bansiesta@gmail.com>
+ *
+ * Driver for the PulsedLight Lidar-Lite range finders connected via PWM.
+ *
+ * This driver accesses the pwm_input published by the pwm_input driver.
+ */
+
+#include "LidarLitePWM.h"
+#include <stdio.h>
+#include <string.h>
+#include <px4_defines.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_pwm_input.h>
+
+LidarLitePWM::LidarLitePWM(const char *path, uint8_t rotation) :
+	CDev("LidarLitePWM", path),
+	_rotation(rotation),
+	_work{},
+	_reports(nullptr),
+	_class_instance(-1),
+	_orb_class_instance(-1),
+	_pwmSub(-1),
+	_pwm{},
+	_distance_sensor_topic(nullptr),
+	_range{},
+	_sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_pwm_read")),
+	_read_errors(perf_alloc(PC_COUNT, "ll40ls_pwm_read_errors")),
+	_sensor_zero_resets(perf_alloc(PC_COUNT, "ll40ls_pwm_zero_resets"))
+{
+}
+
+LidarLitePWM::~LidarLitePWM()
+{
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr) {
+		delete _reports;
+	}
+
+	if (_class_instance != -1) {
+		unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance);
+	}
+
+	/* free perf counters */
+	perf_free(_sample_perf);
+	perf_free(_sensor_zero_resets);
+}
+
+
+int LidarLitePWM::init()
+{
+	/* do regular cdev init */
+	int ret = CDev::init();
+
+	if (ret != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	/* allocate basic report buffers */
+	_reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s));
+
+	if (_reports == nullptr) {
+		return PX4_ERROR;
+	}
+
+	_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
+
+	/* get a publish handle on the distance_sensor topic */
+	struct distance_sensor_s ds_report = {};
+	measure();
+	_reports->get(&ds_report);
+	_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
+				 &_orb_class_instance, ORB_PRIO_LOW);
+
+	if (_distance_sensor_topic == nullptr) {
+		DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?");
+	}
+
+	return PX4_OK;
+}
+
+void LidarLitePWM::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_read_errors);
+	perf_print_counter(_sensor_zero_resets);
+	warnx("poll interval:  %u ticks", getMeasureTicks());
+	warnx("distance: %.3fm", (double)_range.current_distance);
+}
+
+void LidarLitePWM::print_registers()
+{
+	printf("Not supported in PWM mode\n");
+}
+
+void LidarLitePWM::start()
+{
+	/* schedule a cycle to start things */
+	work_queue(HPWORK, &_work, (worker_t)&LidarLitePWM::cycle_trampoline, this, 1);
+}
+
+void LidarLitePWM::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void LidarLitePWM::cycle_trampoline(void *arg)
+{
+	LidarLitePWM *dev = (LidarLitePWM *)arg;
+
+	dev->cycle();
+}
+
+void LidarLitePWM::cycle()
+{
+	measure();
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&LidarLitePWM::cycle_trampoline,
+		   this,
+		   getMeasureTicks());
+}
+
+int LidarLitePWM::measure()
+{
+	perf_begin(_sample_perf);
+
+	if (PX4_OK != collect()) {
+		DEVICE_DEBUG("collection error");
+		perf_count(_read_errors);
+		perf_end(_sample_perf);
+		return PX4_ERROR;
+	}
+
+	_range.timestamp = hrt_absolute_time();
+	_range.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
+	_range.max_distance = get_maximum_distance();
+	_range.min_distance = get_minimum_distance();
+	_range.current_distance = float(_pwm.pulse_width) * 1e-3f;   /* 10 usec = 1 cm distance for LIDAR-Lite */
+	_range.covariance = 0.0f;
+	_range.orientation = _rotation;
+	/* TODO: set proper ID */
+	_range.id = 0;
+
+	/* Due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0) */
+	if (_range.current_distance <= 0.0f) {
+		perf_count(_sensor_zero_resets);
+		perf_end(_sample_perf);
+		return reset_sensor();
+	}
+
+	if (_distance_sensor_topic != nullptr) {
+		orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &_range);
+	}
+
+	_reports->force(&_range);
+
+	poll_notify(POLLIN);
+	perf_end(_sample_perf);
+	return PX4_OK;
+}
+
+ssize_t LidarLitePWM::read(device::file_t *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct distance_sensor_s);
+	struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(buffer);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1) {
+		return -ENOSPC;
+	}
+
+	/* if automatic measurement is enabled */
+	if (getMeasureTicks() > 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;
+
+	} else {
+
+		_reports->flush();
+		measure();
+
+		if (_reports->get(rbuf)) {
+			ret = sizeof(*rbuf);
+		}
+	}
+
+	return ret;
+}
+
+int
+LidarLitePWM::ioctl(device::file_t *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+	/* no custom ioctls implemented for now */
+	default:
+		/* give it to the superclass */
+		return LidarLite::ioctl(filp, cmd, arg);
+	}
+}
+
+int LidarLitePWM::collect()
+{
+	int fd = ::open(PWMIN0_DEVICE_PATH, O_RDONLY);
+
+	if (fd == -1) {
+		return PX4_ERROR;
+	}
+
+	if (::read(fd, &_pwm, sizeof(_pwm)) == sizeof(_pwm)) {
+		::close(fd);
+		return PX4_OK;
+	}
+
+	::close(fd);
+	return EAGAIN;
+}
+
+int LidarLitePWM::reset_sensor()
+{
+
+	int fd = ::open(PWMIN0_DEVICE_PATH, O_RDONLY);
+
+	if (fd == -1) {
+		return PX4_ERROR;
+	}
+
+	int ret = ::ioctl(fd, SENSORIOCRESET, 0);
+	::close(fd);
+	return ret;
+}
+
+const char *LidarLitePWM::get_dev_name()
+{
+	return get_devname();
+}

src/drivers/distance_sensor/ll40ls/LidarLitePWM.h

@@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014, 2015 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 LidarLitePWM.h
+ * @author Johan Jansen <jnsn.johan@gmail.com>
+ * @author Ban Siesta <bansiesta@gmail.com>
+ *
+ * Driver for the PulsedLight Lidar-Lite range finders connected via PWM.
+ *
+ * This driver accesses the pwm_input published by the pwm_input driver.
+ */
+#pragma once
+
+#include "LidarLite.h"
+
+#include <px4_workqueue.h>
+
+
+#include <drivers/device/ringbuffer.h>
+#include <systemlib/perf_counter.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/pwm_input.h>
+#include <uORB/topics/distance_sensor.h>
+
+
+
+class LidarLitePWM : public LidarLite, public device::CDev
+{
+public:
+	LidarLitePWM(const char *path, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
+	virtual ~LidarLitePWM();
+
+	int init() override;
+
+	ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override;
+	int	ioctl(device::file_t *filp, int cmd, unsigned long arg) override;
+
+	void start() override;
+
+	void stop() override;
+
+	void cycle();
+
+	/**
+	* @brief
+	*   Diagnostics - print some basic information about the driver.
+	*/
+	void print_info() override;
+
+	/**
+	 * @brief
+	 *   print registers to console
+	 */
+	void print_registers() override;
+
+	/**
+	* 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);
+
+	const char *get_dev_name() override;
+
+protected:
+
+	int measure() override;
+
+	int collect() override;
+
+	int reset_sensor() override;
+
+	void task_main_trampoline(int argc, char *argv[]);
+
+private:
+	uint8_t _rotation;
+	work_s			_work;
+	ringbuffer::RingBuffer	*_reports;
+	int			_class_instance;
+	int			_orb_class_instance;
+	int			_pwmSub;
+	struct pwm_input_s	_pwm;
+	orb_advert_t	        _distance_sensor_topic;
+	struct distance_sensor_s _range;
+
+	perf_counter_t	        _sample_perf;
+	perf_counter_t	        _read_errors;
+	perf_counter_t	        _sensor_zero_resets;
+};

src/drivers/distance_sensor/ll40ls/ll40ls.cpp

@@ -0,0 +1,425 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014, 2015 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 ll40ls.cpp
+ * @author Allyson Kreft
+ * @author Johan Jansen <jnsn.johan@gmail.com>
+ * @author Ban Siesta <bansiesta@gmail.com>
+ * @author James Goppert <james.goppert@gmail.com>
+ *
+ * Interface for the PulsedLight Lidar-Lite range finders.
+ */
+
+#include "LidarLiteI2C.h"
+#include "LidarLitePWM.h"
+#include <board_config.h>
+#include <systemlib/err.h>
+#include <fcntl.h>
+#include <cstdlib>
+#include <string.h>
+#include <stdio.h>
+#include <platforms/px4_getopt.h>
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+#define LL40LS_DEVICE_PATH_PWM  "/dev/ll40ls_pwm"
+
+enum LL40LS_BUS {
+	LL40LS_BUS_I2C_ALL = 0,
+	LL40LS_BUS_I2C_INTERNAL,
+	LL40LS_BUS_I2C_EXTERNAL,
+	LL40LS_BUS_PWM
+};
+
+static struct ll40ls_bus_option {
+	enum LL40LS_BUS busid;
+	const char *devname;
+	uint8_t busnum;
+} bus_options[] = {
+#ifdef PX4_I2C_BUS_EXPANSION
+	{ LL40LS_BUS_I2C_EXTERNAL, "/dev/ll40ls_ext", PX4_I2C_BUS_EXPANSION },
+#endif
+#ifdef PX4_I2C_BUS_EXPANSION1
+	{ LL40LS_BUS_I2C_EXTERNAL, "/dev/ll40ls_ext1", PX4_I2C_BUS_EXPANSION1 },
+#endif
+#ifdef PX4_I2C_BUS_ONBOARD
+	{ LL40LS_BUS_I2C_INTERNAL, "/dev/ll40ls_int", PX4_I2C_BUS_ONBOARD },
+#endif
+};
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]);
+
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace ll40ls
+{
+
+LidarLite *instance = nullptr;
+
+void	start(enum LL40LS_BUS busid, uint8_t rotation);
+void	stop();
+void	test();
+void	reset();
+void	info();
+void	regdump();
+void	usage();
+
+/**
+ * Start the driver.
+ */
+void start(enum LL40LS_BUS busid, uint8_t rotation)
+{
+	int fd, ret;
+
+	if (instance) {
+		PX4_INFO("driver already started");
+	}
+
+	if (busid == LL40LS_BUS_PWM) {
+		instance = new LidarLitePWM(LL40LS_DEVICE_PATH_PWM, rotation);
+
+		if (!instance) {
+			PX4_ERR("Failed to instantiate LidarLitePWM");
+			return;
+		}
+
+		if (instance->init() != PX4_OK) {
+			PX4_ERR("failed to initialize LidarLitePWM");
+			goto fail;
+		}
+
+	} else {
+		for (uint8_t i = 0; i < (sizeof(bus_options) / sizeof(bus_options[0])); i++) {
+			if (busid != LL40LS_BUS_I2C_ALL && busid != bus_options[i].busid) {
+				continue;
+			}
+
+			instance = new LidarLiteI2C(bus_options[i].busnum, bus_options[i].devname, rotation);
+
+			if (!instance) {
+				PX4_ERR("Failed to instantiate LidarLiteI2C");
+				return;
+			}
+
+			if (instance->init() == PX4_OK) {
+				break;
+			}
+
+			PX4_ERR("failed to initialize LidarLiteI2C on busnum=%u", bus_options[i].busnum);
+			delete instance;
+			instance = nullptr;
+		}
+	}
+
+	if (!instance) {
+		PX4_WARN("No LidarLite found");
+		return;
+	}
+
+	fd = px4_open(instance->get_dev_name(), O_RDONLY);
+
+	if (fd == -1) {
+		PX4_ERR("Error opening fd");
+		goto fail;
+	}
+
+	ret = px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
+	px4_close(fd);
+
+	if (ret < 0) {
+		PX4_ERR("pollrate fail");
+		goto fail;
+	}
+
+	return;
+
+fail:
+	delete instance;
+	instance = nullptr;
+}
+
+/**
+ * Stop the driver
+ */
+void stop()
+{
+	delete instance;
+	instance = nullptr;
+}
+
+/**
+ * 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 distance_sensor_s report;
+	ssize_t sz;
+	int ret;
+
+	if (!instance) {
+		PX4_ERR("No ll40ls driver running");
+		return;
+	}
+
+	int fd = px4_open(instance->get_dev_name(), O_RDONLY);
+
+	if (fd < 0) {
+		PX4_ERR("Error opening fd");
+		return;
+	}
+
+	/* do a simple demand read */
+	sz = px4_read(fd, &report, sizeof(report));
+
+	if (sz != sizeof(report)) {
+		PX4_ERR("immediate read failed");
+		return;
+	}
+
+	PX4_INFO("single read");
+	PX4_INFO("measurement: %0.2f m", (double)report.current_distance);
+	PX4_INFO("time:        %lld", report.timestamp);
+
+	/* start the sensor polling at 2Hz */
+	if (PX4_OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
+		PX4_ERR("failed to set 2Hz poll rate");
+		return;
+	}
+
+	/* read the sensor 5 times and report each value */
+	for (unsigned i = 0; i < 5; i++) {
+		px4_pollfd_struct_t fds;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = px4_poll(&fds, 1, 2000);
+
+		if (ret != 1) {
+			PX4_WARN("timed out waiting for sensor data");
+			return;
+		}
+
+		/* now go get it */
+		sz = px4_read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report)) {
+			PX4_WARN("periodic read failed");
+			return;
+		}
+
+		PX4_INFO("periodic read %u", i);
+		PX4_INFO("valid %u", (float)report.current_distance > report.min_distance
+			 && (float)report.current_distance < report.max_distance ? 1 : 0);
+		PX4_INFO("measurement: %0.3f m", (double)report.current_distance);
+		PX4_INFO("time:        %lld", report.timestamp);
+	}
+
+	/* reset the sensor polling to default rate */
+	if (PX4_OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
+		PX4_WARN("failed to set default poll rate");
+	}
+
+	px4_close(fd);
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	if (!instance) {
+		PX4_WARN("No ll40ls driver running");
+		return;
+	}
+
+	int fd = px4_open(instance->get_dev_name(), O_RDONLY);
+
+	if (fd < 0) {
+		PX4_ERR("Error opening fd");
+		return;
+	}
+
+	if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) {
+		PX4_ERR("driver reset failed");
+		goto error;
+	}
+
+	if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		PX4_ERR("driver poll restart failed");
+		goto error;
+	}
+
+error:
+	px4_close(fd);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (!instance) {
+		warnx("No ll40ls driver running");
+		return;
+	}
+
+	printf("state @ %p\n", instance);
+	instance->print_info();
+}
+
+/**
+ * Dump registers
+ */
+void
+regdump()
+{
+	if (!instance) {
+		warnx("No ll40ls driver running");
+		return;
+	}
+
+	printf("regdump @ %p\n", instance);
+	instance->print_registers();
+}
+
+void
+usage()
+{
+	PX4_INFO("missing command: try 'start', 'stop', 'info', 'test', 'reset', 'info' or 'regdump' [i2c|pwm]");
+	PX4_INFO("options for I2C:");
+	PX4_INFO("    -X only external bus");
+#ifdef PX4_I2C_BUS_ONBOARD
+	PX4_INFO("    -I only internal bus");
+#endif
+	PX4_INFO("E.g. ll40ls start i2c -R 0");
+}
+
+} // namespace
+
+int
+ll40ls_main(int argc, char *argv[])
+{
+	int ch;
+	int myoptind = 1;
+	const char *myoptarg = nullptr;
+	enum LL40LS_BUS busid = LL40LS_BUS_I2C_ALL;
+	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
+
+	while ((ch = px4_getopt(argc, argv, "IXR:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+#ifdef PX4_I2C_BUS_ONBOARD
+
+		case 'I':
+			busid = LL40LS_BUS_I2C_INTERNAL;
+			break;
+#endif
+
+		case 'X':
+			busid = LL40LS_BUS_I2C_EXTERNAL;
+			break;
+
+		case 'R':
+			rotation = (uint8_t)atoi(myoptarg);
+			PX4_INFO("Setting Lidar orientation to %d", (int)rotation);
+			break;
+
+		default:
+			ll40ls::usage();
+			return 0;
+		}
+	}
+
+	/* determine protocol first because it's needed next */
+	if (argc > myoptind + 1) {
+		const char *protocol = argv[myoptind + 1];
+
+		if (!strcmp(protocol, "pwm")) {
+			busid = LL40LS_BUS_PWM;;
+
+		} else if (!strcmp(protocol, "i2c")) {
+			// Do nothing
+
+		} else {
+			warnx("unknown protocol, choose pwm or i2c");
+			ll40ls::usage();
+			return 0;
+		}
+	}
+
+	/* now determine action */
+	if (argc > myoptind) {
+		const char *verb = argv[myoptind];
+
+		if (!strcmp(verb, "start")) {
+			ll40ls::start(busid, rotation);
+
+		} else if (!strcmp(verb, "stop")) {
+			ll40ls::stop();
+
+		} else if (!strcmp(verb, "test")) {
+			ll40ls::test();
+
+		} else if (!strcmp(verb, "reset")) {
+			ll40ls::reset();
+
+		} else if (!strcmp(verb, "regdump")) {
+			ll40ls::regdump();
+
+		} else if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
+			ll40ls::info();
+
+		} else {
+			ll40ls::usage();
+		}
+
+		return 0;
+	}
+
+	warnx("unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'");
+	ll40ls::usage();
+	return 0;
+}

src/drivers/distance_sensor/mb12xx/CMakeLists.txt

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__mb12xx
+	MAIN mb12xx
+	COMPILE_FLAGS
+		-Wno-sign-compare
+	SRCS
+		mb12xx.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :

src/drivers/distance_sensor/mb12xx/mb12xx.cpp

@@ -0,0 +1,944 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 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 mb12xx.cpp
+ * @author Greg Hulands
+ * @author Jon Verbeke <jon.verbeke@kuleuven.be>
+ *
+ * Driver for the Maxbotix sonar range finders connected via I2C.
+ */
+
+#include <px4_config.h>
+#include <px4_getopt.h>
+#include <px4_workqueue.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 <vector>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_range_finder.h>
+#include <drivers/device/ringbuffer.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/distance_sensor.h>
+
+#include <board_config.h>
+
+/* Configuration Constants */
+#define MB12XX_BUS 		PX4_I2C_BUS_EXPANSION
+#define MB12XX_BASEADDR 	0x70 /* 7-bit address. 8-bit address is 0xE0 */
+#define MB12XX_DEVICE_PATH	"/dev/mb12xx"
+
+/* MB12xx Registers addresses */
+
+#define MB12XX_TAKE_RANGE_REG	0x51		/* Measure range Register */
+#define MB12XX_SET_ADDRESS_1	0xAA		/* Change address 1 Register */
+#define MB12XX_SET_ADDRESS_2	0xA5		/* Change address 2 Register */
+
+/* Device limits */
+#define MB12XX_MIN_DISTANCE 	(0.20f)
+#define MB12XX_MAX_DISTANCE 	(7.65f)
+
+#define MB12XX_CONVERSION_INTERVAL 	100000 /* 60ms for one sonar */
+#define TICKS_BETWEEN_SUCCESIVE_FIRES 	100000 /* 30ms between each sonar measurement (watch out for interference!) */
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+class MB12XX : public device::I2C
+{
+public:
+	MB12XX(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING,
+	       int bus = MB12XX_BUS, int address = MB12XX_BASEADDR);
+	virtual ~MB12XX();
+
+	virtual int 		init();
+
+	virtual ssize_t		read(device::file_t *filp, char *buffer, size_t buflen);
+	virtual int			ioctl(device::file_t *filp, int cmd, unsigned long arg);
+
+	/**
+	* Diagnostics - print some basic information about the driver.
+	*/
+	void				print_info();
+
+protected:
+	virtual int			probe();
+
+private:
+	uint8_t _rotation;
+	float				_min_distance;
+	float				_max_distance;
+	work_s				_work;
+	ringbuffer::RingBuffer	*_reports;
+	bool				_sensor_ok;
+	int					_measure_ticks;
+	bool				_collect_phase;
+	int				_class_instance;
+	int				_orb_class_instance;
+
+	orb_advert_t		_distance_sensor_topic;
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_comms_errors;
+
+	uint8_t				_cycle_counter;	/* counter in cycle to change i2c adresses */
+	int					_cycling_rate;	/* */
+	uint8_t				_index_counter;	/* temporary sonar i2c address */
+	std::vector<uint8_t>	addr_ind; 	/* temp sonar i2c address vector */
+	std::vector<float>
+	_latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */
+
+
+	/**
+	* 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();
+
+	/**
+	* 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 MB12XX_MIN_DISTANCE
+	* and MB12XX_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 mb12xx_main(int argc, char *argv[]);
+
+MB12XX::MB12XX(uint8_t rotation, int bus, int address) :
+	I2C("MB12xx", MB12XX_DEVICE_PATH, bus, address, 100000),
+	_rotation(rotation),
+	_min_distance(MB12XX_MIN_DISTANCE),
+	_max_distance(MB12XX_MAX_DISTANCE),
+	_reports(nullptr),
+	_sensor_ok(false),
+	_measure_ticks(0),
+	_collect_phase(false),
+	_class_instance(-1),
+	_orb_class_instance(-1),
+	_distance_sensor_topic(nullptr),
+	_sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "mb12xx_com_err")),
+	_cycle_counter(0),	/* initialising counter for cycling function to zero */
+	_cycling_rate(0),	/* initialising cycling rate (which can differ depending on one sonar or multiple) */
+	_index_counter(0) 	/* initialising temp sonar i2c address to zero */
+
+{
+	/* enable debug() calls */
+	_debug_enabled = false;
+
+	/* work_cancel in the dtor will explode if we don't do this... */
+	memset(&_work, 0, sizeof(_work));
+}
+
+MB12XX::~MB12XX()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr) {
+		delete _reports;
+	}
+
+	if (_class_instance != -1) {
+		unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance);
+	}
+
+	/* free perf counters */
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+}
+
+int
+MB12XX::init()
+{
+	int ret = PX4_ERROR;
+
+	/* do I2C init (and probe) first */
+	if (I2C::init() != OK) {
+		return ret;
+	}
+
+	/* allocate basic report buffers */
+	_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
+
+	_index_counter = MB12XX_BASEADDR;	/* set temp sonar i2c address to base adress */
+	set_device_address(_index_counter);		/* set I2c port to temp sonar i2c adress */
+
+	if (_reports == nullptr) {
+		return ret;
+	}
+
+	_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
+
+	/* get a publish handle on the range finder topic */
+	struct distance_sensor_s ds_report = {};
+
+	_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
+				 &_orb_class_instance, ORB_PRIO_LOW);
+
+	if (_distance_sensor_topic == nullptr) {
+		DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
+	}
+
+	// XXX we should find out why we need to wait 200 ms here
+	usleep(200000);
+
+	/* check for connected rangefinders on each i2c port:
+	   We start from i2c base address (0x70 = 112) and count downwards
+	   So second iteration it uses i2c address 111, third iteration 110 and so on*/
+	for (unsigned counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) {
+		_index_counter = MB12XX_BASEADDR - counter;	/* set temp sonar i2c address to base adress - counter */
+		set_device_address(_index_counter);			/* set I2c port to temp sonar i2c adress */
+		int ret2 = measure();
+
+		if (ret2 == 0) { /* sonar is present -> store address_index in array */
+			addr_ind.push_back(_index_counter);
+			DEVICE_DEBUG("sonar added");
+			_latest_sonar_measurements.push_back(200);
+		}
+	}
+
+	_index_counter = MB12XX_BASEADDR;
+	set_device_address(_index_counter); /* set i2c port back to base adress for rest of driver */
+
+	/* if only one sonar detected, no special timing is required between firing, so use default */
+	if (addr_ind.size() == 1) {
+		_cycling_rate = MB12XX_CONVERSION_INTERVAL;
+
+	} else {
+		_cycling_rate = TICKS_BETWEEN_SUCCESIVE_FIRES;
+	}
+
+	/* show the connected sonars in terminal */
+	for (unsigned i = 0; i < addr_ind.size(); i++) {
+		DEVICE_LOG("sonar %d with address %d added", (i + 1), addr_ind[i]);
+	}
+
+	DEVICE_DEBUG("Number of sonars connected: %lu", addr_ind.size());
+
+	ret = OK;
+	/* sensor is ok, but we don't really know if it is within range */
+	_sensor_ok = true;
+
+	return ret;
+}
+
+int
+MB12XX::probe()
+{
+	return measure();
+}
+
+void
+MB12XX::set_minimum_distance(float min)
+{
+	_min_distance = min;
+}
+
+void
+MB12XX::set_maximum_distance(float max)
+{
+	_max_distance = max;
+}
+
+float
+MB12XX::get_minimum_distance()
+{
+	return _min_distance;
+}
+
+float
+MB12XX::get_maximum_distance()
+{
+	return _max_distance;
+}
+
+int
+MB12XX::ioctl(device::file_t *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(_cycling_rate);
+
+					/* 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 */
+					int ticks = USEC2TICK(1000000 / arg);
+
+					/* check against maximum rate */
+					if (ticks < USEC2TICK(_cycling_rate)) {
+						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;
+			}
+
+			ATOMIC_ENTER;
+
+			if (!_reports->resize(arg)) {
+				ATOMIC_LEAVE;
+				return -ENOMEM;
+			}
+
+			ATOMIC_LEAVE;
+
+			return OK;
+		}
+
+	case SENSORIOCRESET:
+		/* XXX implement this */
+		return -EINVAL;
+
+	default:
+		/* give it to the superclass */
+		return I2C::ioctl(filp, cmd, arg);
+	}
+}
+
+ssize_t
+MB12XX::read(device::file_t *filp, char *buffer, size_t buflen)
+{
+
+	unsigned count = buflen / sizeof(struct distance_sensor_s);
+	struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(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(_cycling_rate * 2);
+
+		/* 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
+MB12XX::measure()
+{
+
+	int ret;
+
+	/*
+	 * Send the command to begin a measurement.
+	 */
+
+	uint8_t cmd = MB12XX_TAKE_RANGE_REG;
+	ret = transfer(&cmd, 1, nullptr, 0);
+
+	if (OK != ret) {
+		perf_count(_comms_errors);
+		DEVICE_DEBUG("i2c::transfer returned %d", ret);
+		return ret;
+	}
+
+	ret = OK;
+
+	return ret;
+}
+
+int
+MB12XX::collect()
+{
+	int	ret = -EIO;
+
+	/* read from the sensor */
+	uint8_t val[2] = {0, 0};
+
+	perf_begin(_sample_perf);
+
+	ret = transfer(nullptr, 0, &val[0], 2);
+
+	if (ret < 0) {
+		DEVICE_DEBUG("error reading from sensor: %d", ret);
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+		return ret;
+	}
+
+	uint16_t distance_cm = val[0] << 8 | val[1];
+	float distance_m = float(distance_cm) * 1e-2f;
+
+	struct distance_sensor_s report;
+	report.timestamp = hrt_absolute_time();
+	report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
+	report.orientation = _rotation;
+	report.current_distance = distance_m;
+	report.min_distance = get_minimum_distance();
+	report.max_distance = get_maximum_distance();
+	report.covariance = 0.0f;
+	/* TODO: set proper ID */
+	report.id = 0;
+
+	/* publish it, if we are the primary */
+	if (_distance_sensor_topic != nullptr) {
+		orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
+	}
+
+	_reports->force(&report);
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	ret = OK;
+
+	perf_end(_sample_perf);
+	return ret;
+}
+
+void
+MB12XX::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)&MB12XX::cycle_trampoline, this, 5);
+
+	/* notify about state change */
+	struct subsystem_info_s info = {};
+	info.present = true;
+	info.enabled = true;
+	info.ok = true;
+	info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
+
+	static orb_advert_t pub = nullptr;
+
+	if (pub != nullptr) {
+		orb_publish(ORB_ID(subsystem_info), pub, &info);
+
+
+	} else {
+		pub = orb_advertise(ORB_ID(subsystem_info), &info);
+
+	}
+}
+
+void
+MB12XX::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+MB12XX::cycle_trampoline(void *arg)
+{
+
+	MB12XX *dev = (MB12XX *)arg;
+
+	dev->cycle();
+
+}
+
+void
+MB12XX::cycle()
+{
+	if (_collect_phase) {
+		_index_counter = addr_ind[_cycle_counter]; /*sonar from previous iteration collect is now read out */
+		set_device_address(_index_counter);
+
+		/* perform collection */
+		if (OK != collect()) {
+			DEVICE_DEBUG("collection error");
+			/* if error restart the measurement state machine */
+			start();
+			return;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+
+		/* change i2c adress to next sonar */
+		_cycle_counter = _cycle_counter + 1;
+
+		if (_cycle_counter >= addr_ind.size()) {
+			_cycle_counter = 0;
+		}
+
+		/* Is there a collect->measure gap? Yes, and the timing is set equal to the cycling_rate
+		   Otherwise the next sonar would fire without the first one having received its reflected sonar pulse */
+
+		if (_measure_ticks > USEC2TICK(_cycling_rate)) {
+
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&MB12XX::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(_cycling_rate));
+			return;
+		}
+	}
+
+	/* Measurement (firing) phase */
+
+	/* ensure sonar i2c adress is still correct */
+	_index_counter = addr_ind[_cycle_counter];
+	set_device_address(_index_counter);
+
+	/* Perform measurement */
+	if (OK != measure()) {
+		DEVICE_DEBUG("measure error sonar adress %d", _index_counter);
+	}
+
+	/* next phase is collection */
+	_collect_phase = true;
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&MB12XX::cycle_trampoline,
+		   this,
+		   USEC2TICK(_cycling_rate));
+
+}
+
+void
+MB12XX::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	printf("poll interval:  %u ticks\n", _measure_ticks);
+	_reports->print_info("report queue");
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace mb12xx
+{
+
+MB12XX	*g_dev;
+
+void	start(uint8_t rotation);
+void	stop();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start(uint8_t rotation)
+{
+	int fd;
+
+	if (g_dev != nullptr) {
+		errx(1, "already started");
+	}
+
+	/* create the driver */
+	g_dev = new MB12XX(rotation, MB12XX_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(MB12XX_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		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 distance_sensor_s report;
+	ssize_t sz;
+	int ret;
+
+	int fd = open(MB12XX_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		err(1, "%s open failed (try 'mb12xx start' if the driver is not running", MB12XX_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.current_distance);
+	warnx("time:        %llu", 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("valid %u", (float)report.current_distance > report.min_distance
+		      && (float)report.current_distance < report.max_distance ? 1 : 0);
+		warnx("measurement: %0.3f", (double)report.current_distance);
+		warnx("time:        %llu", 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(MB12XX_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
+mb12xx_main(int argc, char *argv[])
+{
+	// check for optional arguments
+	int ch;
+	int myoptind = 1;
+	const char *myoptarg = nullptr;
+	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
+
+
+	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'R':
+			rotation = (uint8_t)atoi(myoptarg);
+			PX4_INFO("Setting distance sensor orientation to %d", (int)rotation);
+			break;
+
+		default:
+			PX4_WARN("Unknown option!");
+		}
+	}
+
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[myoptind], "start")) {
+		mb12xx::start(rotation);
+	}
+
+	/*
+	 * Stop the driver
+	 */
+	if (!strcmp(argv[myoptind], "stop")) {
+		mb12xx::stop();
+	}
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[myoptind], "test")) {
+		mb12xx::test();
+	}
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[myoptind], "reset")) {
+		mb12xx::reset();
+	}
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[1], "status")) {
+		mb12xx::info();
+	}
+
+	PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
+	return PX4_ERROR;
+}

src/drivers/distance_sensor/sf0x/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__sf0x
+	MAIN sf0x
+	COMPILE_FLAGS
+		-Wno-sign-compare # TODO: fix all sign-compare
+	SRCS
+		sf0x.cpp
+		sf0x_parser.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :

src/drivers/distance_sensor/sf0x/sf0x_parser.cpp

@@ -0,0 +1,155 @@
+/****************************************************************************
+ *
+ *   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_parser.cpp
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Driver for the Lightware SF0x laser rangefinder series
+ */
+
+#include "sf0x_parser.h"
+#include <string.h>
+#include <stdlib.h>
+
+//#define SF0X_DEBUG
+
+#ifdef SF0X_DEBUG
+#include <stdio.h>
+
+const char *parser_state[] = {
+	"0_UNSYNC",
+	"1_SYNC",
+	"2_GOT_DIGIT0",
+	"3_GOT_DOT",
+	"4_GOT_DIGIT1",
+	"5_GOT_DIGIT2",
+	"6_GOT_CARRIAGE_RETURN"
+};
+#endif
+
+int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist)
+{
+	int ret = -1;
+	char *end;
+
+	switch (*state) {
+	case SF0X_PARSE_STATE0_UNSYNC:
+		if (c == '\n') {
+			*state = SF0X_PARSE_STATE1_SYNC;
+			(*parserbuf_index) = 0;
+		}
+
+		break;
+
+	case SF0X_PARSE_STATE1_SYNC:
+		if (c >= '0' && c <= '9') {
+			*state = SF0X_PARSE_STATE2_GOT_DIGIT0;
+			parserbuf[*parserbuf_index] = c;
+			(*parserbuf_index)++;
+		}
+
+		break;
+
+	case SF0X_PARSE_STATE2_GOT_DIGIT0:
+		if (c >= '0' && c <= '9') {
+			*state = SF0X_PARSE_STATE2_GOT_DIGIT0;
+			parserbuf[*parserbuf_index] = c;
+			(*parserbuf_index)++;
+
+		} else if (c == '.') {
+			*state = SF0X_PARSE_STATE3_GOT_DOT;
+			parserbuf[*parserbuf_index] = c;
+			(*parserbuf_index)++;
+
+		} else {
+			*state = SF0X_PARSE_STATE0_UNSYNC;
+		}
+
+		break;
+
+	case SF0X_PARSE_STATE3_GOT_DOT:
+		if (c >= '0' && c <= '9') {
+			*state = SF0X_PARSE_STATE4_GOT_DIGIT1;
+			parserbuf[*parserbuf_index] = c;
+			(*parserbuf_index)++;
+
+		} else {
+			*state = SF0X_PARSE_STATE0_UNSYNC;
+		}
+
+		break;
+
+	case SF0X_PARSE_STATE4_GOT_DIGIT1:
+		if (c >= '0' && c <= '9') {
+			*state = SF0X_PARSE_STATE5_GOT_DIGIT2;
+			parserbuf[*parserbuf_index] = c;
+			(*parserbuf_index)++;
+
+		} else {
+			*state = SF0X_PARSE_STATE0_UNSYNC;
+		}
+
+		break;
+
+	case SF0X_PARSE_STATE5_GOT_DIGIT2:
+		if (c == '\r') {
+			*state = SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN;
+
+		} else {
+			*state = SF0X_PARSE_STATE0_UNSYNC;
+		}
+
+		break;
+
+	case SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN:
+		if (c == '\n') {
+			parserbuf[*parserbuf_index] = '\0';
+			*dist = strtod(parserbuf, &end);
+			*state = SF0X_PARSE_STATE1_SYNC;
+			*parserbuf_index = 0;
+			ret = 0;
+
+		} else {
+			*state = SF0X_PARSE_STATE0_UNSYNC;
+		}
+
+		break;
+	}
+
+#ifdef SF0X_DEBUG
+	printf("state: SF0X_PARSE_STATE%s\n", parser_state[*state]);
+#endif
+
+	return ret;
+}

src/drivers/distance_sensor/sf0x/sf0x_parser.h

@@ -0,0 +1,53 @@
+/****************************************************************************
+ *
+ *   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_parser.cpp
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Declarations of parser for the Lightware SF0x laser rangefinder series
+ */
+
+#pragma once
+
+enum SF0X_PARSE_STATE {
+	SF0X_PARSE_STATE0_UNSYNC = 0,
+	SF0X_PARSE_STATE1_SYNC,
+	SF0X_PARSE_STATE2_GOT_DIGIT0,
+	SF0X_PARSE_STATE3_GOT_DOT,
+	SF0X_PARSE_STATE4_GOT_DIGIT1,
+	SF0X_PARSE_STATE5_GOT_DIGIT2,
+	SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN
+};
+
+int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist);

src/drivers/distance_sensor/sf0x/sf0x_tests/CMakeLists.txt

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__sf0x__sf0x_tests
+	MAIN sf0x_tests
+	COMPILE_FLAGS
+		-Wno-sign-compare # TODO: fix all sign-compare
+	SRCS
+		SF0XTest.cpp
+		../sf0x_parser.cpp
+	DEPENDS
+		platforms__common
+	)

src/drivers/distance_sensor/sf0x/sf0x_tests/SF0XTest.cpp

@@ -0,0 +1,91 @@
+#include <unit_test.h>
+
+#include "../sf0x_parser.h"
+
+#include <systemlib/err.h>
+
+#include <cstdio>
+#include <cstring>
+#include <unistd.h>
+
+extern "C" __EXPORT int sf0x_tests_main(int argc, char *argv[]);
+
+class SF0XTest : public UnitTest
+{
+public:
+	virtual bool run_tests();
+
+private:
+	bool sf0xTest();
+};
+
+bool SF0XTest::run_tests()
+{
+	ut_run_test(sf0xTest);
+
+	return (_tests_failed == 0);
+}
+
+bool SF0XTest::sf0xTest()
+{
+	const char _LINE_MAX = 20;
+	//char _linebuf[_LINE_MAX];
+	//_linebuf[0] = '\0';
+
+	const char *lines[] = {"0.01\r\n",
+			       "0.02\r\n",
+			       "0.03\r\n",
+			       "0.04\r\n",
+			       "0",
+			       ".",
+			       "0",
+			       "5",
+			       "\r",
+			       "\n",
+			       "0",
+			       "3\r",
+			       "\n",
+			       "\r\n",
+			       "0.06",
+			       "\r\n"
+			      };
+
+	enum SF0X_PARSE_STATE state = SF0X_PARSE_STATE0_UNSYNC;
+	float dist_m;
+	char _parserbuf[_LINE_MAX];
+	unsigned _parsebuf_index = 0;
+
+	for (unsigned l = 0; l < sizeof(lines) / sizeof(lines[0]); l++) {
+		//printf("\n%s", _linebuf);
+
+		int parse_ret;
+
+		for (int i = 0; i < strlen(lines[l]); i++) {
+			parse_ret = sf0x_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m);
+
+			if (parse_ret == 0) {
+				if (l == 0) {
+					ut_test(dist_m - 0.010000f < 0.001f);
+
+				} else if (l == 1) {
+					ut_test(dist_m - 0.020000f < 0.001f);
+
+				} else if (l == 2) {
+					ut_test(dist_m - 0.030000f < 0.001f);
+
+				} else if (l == 3) {
+					ut_test(dist_m - 0.040000f < 0.001f);
+				}
+
+				//printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : "");
+			}
+		}
+
+		//printf("%s", lines[l]);
+
+	}
+
+	return true;
+}
+
+ut_declare_test_c(sf0x_tests_main, SF0XTest)

src/drivers/distance_sensor/sf1xx/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+
+	MODULE drivers__sf1xx
+	MAIN sf1xx
+	COMPILE_FLAGS
+		-Wno-sign-compare
+	SRCS
+		sf1xx.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :

src/drivers/distance_sensor/sf1xx/sf1xx.cpp

@@ -0,0 +1,881 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file sf1xx.cpp
+ *
+ * @author ecmnet <ecm@gmx.de>
+ * @author Vasily Evseenko <svpcom@gmail.com>
+ *
+ * Driver for the Lightware SF1xx lidar range finder series.
+ * Default I2C address 0x66 is used.
+ */
+
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_getopt.h>
+#include <px4_workqueue.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 <vector>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_range_finder.h>
+#include <drivers/device/ringbuffer.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/distance_sensor.h>
+
+#include <board_config.h>
+
+/* Configuration Constants */
+#define SF1XX_BUS 		PX4_I2C_BUS_EXPANSION
+#define SF1XX_BASEADDR 	0x66
+#define SF1XX_DEVICE_PATH	"/dev/sf1xx"
+
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+class SF1XX : public device::I2C
+{
+public:
+	SF1XX(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, int bus = SF1XX_BUS,
+	      int address = SF1XX_BASEADDR);
+	virtual ~SF1XX();
+
+	virtual int 		init();
+
+	virtual ssize_t		read(device::file_t *filp, char *buffer, size_t buflen);
+	virtual int			ioctl(device::file_t *filp, int cmd, unsigned long arg);
+
+	/**
+	* Diagnostics - print some basic information about the driver.
+	*/
+	void				print_info();
+
+protected:
+	virtual int			probe();
+
+private:
+	uint8_t _rotation;
+	float				_min_distance;
+	float				_max_distance;
+	int                             _conversion_interval;
+	work_s				_work;
+	ringbuffer::RingBuffer  *_reports;
+	bool				_sensor_ok;
+	int				_measure_ticks;
+	int				_class_instance;
+	int				_orb_class_instance;
+
+	orb_advert_t		_distance_sensor_topic;
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_comms_errors;
+
+
+
+	/**
+	* 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();
+
+	/**
+	* 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 SF1XX_MIN_DISTANCE
+	* and SF1XX_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 sf1xx_main(int argc, char *argv[]);
+
+SF1XX::SF1XX(uint8_t rotation, int bus, int address) :
+	I2C("SF1XX", SF1XX_DEVICE_PATH, bus, address, 400000),
+	_rotation(rotation),
+	_min_distance(-1.0f),
+	_max_distance(-1.0f),
+	_conversion_interval(-1),
+	_reports(nullptr),
+	_sensor_ok(false),
+	_measure_ticks(0),
+	_class_instance(-1),
+	_orb_class_instance(-1),
+	_distance_sensor_topic(nullptr),
+	_sample_perf(perf_alloc(PC_ELAPSED, "sf1xx_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "sf1xx_com_err"))
+
+{
+	/* enable debug() calls */
+	_debug_enabled = false;
+
+	/* work_cancel in the dtor will explode if we don't do this... */
+	memset(&_work, 0, sizeof(_work));
+}
+
+SF1XX::~SF1XX()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr) {
+		delete _reports;
+	}
+
+	if (_distance_sensor_topic != nullptr) {
+		orb_unadvertise(_distance_sensor_topic);
+	}
+
+	if (_class_instance != -1) {
+		unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance);
+	}
+
+	/* free perf counters */
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+}
+
+int
+SF1XX::init()
+{
+	int ret = PX4_ERROR;
+	int hw_model;
+	param_get(param_find("SENS_EN_SF1XX"), &hw_model);
+
+	switch (hw_model) {
+	case 0:
+		DEVICE_LOG("disabled.");
+		return ret;
+
+	case 1:  /* SF10/a (25m 32Hz) */
+		_min_distance = 0.01f;
+		_max_distance = 25.0f;
+		_conversion_interval = 31250;
+		break;
+
+	case 2:  /* SF10/b (50m 32Hz) */
+		_min_distance = 0.01f;
+		_max_distance = 50.0f;
+		_conversion_interval = 31250;
+		break;
+
+	case 3:  /* SF10/c (100m 16Hz) */
+		_min_distance = 0.01f;
+		_max_distance = 100.0f;
+		_conversion_interval = 62500;
+		break;
+
+	case 4:
+		/* SF11/c (120m 20Hz) */
+		_min_distance = 0.01f;
+		_max_distance = 120.0f;
+		_conversion_interval = 50000;
+		break;
+
+	case 5:
+		/* SF20/LW20 (100m 48-388Hz) */
+		_min_distance = 0.001f;
+		_max_distance = 100.0f;
+		_conversion_interval = 20834;
+		break;
+
+	default:
+		DEVICE_LOG("invalid HW model %d.", hw_model);
+		return ret;
+	}
+
+	/* do I2C init (and probe) first */
+	if (I2C::init() != OK) {
+		return ret;
+	}
+
+	/* allocate basic report buffers */
+	_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
+
+	set_device_address(SF1XX_BASEADDR);
+
+	if (_reports == nullptr) {
+		return ret;
+	}
+
+	_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
+
+	/* get a publish handle on the range finder topic */
+	struct distance_sensor_s ds_report = {};
+
+	_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
+				 &_orb_class_instance, ORB_PRIO_HIGH);
+
+	if (_distance_sensor_topic == nullptr) {
+		DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
+	}
+
+	// Select altitude register
+	int ret2 = measure();
+
+	if (ret2 == 0) {
+		ret = OK;
+		_sensor_ok = true;
+		DEVICE_LOG("(%dm %dHz) with address %d found", (int)_max_distance,
+			   (int)(1e6f / _conversion_interval), SF1XX_BASEADDR);
+	}
+
+	return ret;
+}
+
+int
+SF1XX::probe()
+{
+	return measure();
+}
+
+void
+SF1XX::set_minimum_distance(float min)
+{
+	_min_distance = min;
+}
+
+void
+SF1XX::set_maximum_distance(float max)
+{
+	_max_distance = max;
+}
+
+float
+SF1XX::get_minimum_distance()
+{
+	return _min_distance;
+}
+
+float
+SF1XX::get_maximum_distance()
+{
+	return _max_distance;
+}
+
+int
+SF1XX::ioctl(device::file_t *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(_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 */
+					int ticks = USEC2TICK(1000000 / arg);
+
+					/* check against maximum rate */
+					if (ticks < USEC2TICK(_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;
+			}
+
+			ATOMIC_ENTER;
+
+			if (!_reports->resize(arg)) {
+				ATOMIC_LEAVE;
+				return -ENOMEM;
+			}
+
+			ATOMIC_LEAVE;
+
+			return OK;
+		}
+
+	case SENSORIOCRESET:
+		/* XXX implement this */
+		return -EINVAL;
+
+	default:
+		/* give it to the superclass */
+		return I2C::ioctl(filp, cmd, arg);
+	}
+}
+
+ssize_t
+SF1XX::read(device::file_t *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct distance_sensor_s);
+	struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(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(_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
+SF1XX::measure()
+{
+	int ret;
+
+	/*
+	 * Send the command '0' -- read altitude
+	 */
+
+	uint8_t cmd = 0;
+	ret = transfer(&cmd, 1, nullptr, 0);
+
+	if (OK != ret) {
+		perf_count(_comms_errors);
+		DEVICE_DEBUG("i2c::transfer returned %d", ret);
+		return ret;
+	}
+
+	ret = OK;
+
+	return ret;
+}
+
+int
+SF1XX::collect()
+{
+	int	ret = -EIO;
+
+	/* read from the sensor */
+	uint8_t val[2] = {0, 0};
+	perf_begin(_sample_perf);
+
+	ret = transfer(nullptr, 0, &val[0], 2);
+
+	if (ret < 0) {
+		DEVICE_DEBUG("error reading from sensor: %d", ret);
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+		return ret;
+	}
+
+	uint16_t distance_cm = val[0] << 8 | val[1];
+	float distance_m = float(distance_cm) * 1e-2f;
+
+	struct distance_sensor_s report;
+	report.timestamp = hrt_absolute_time();
+	report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
+	report.orientation = _rotation;
+	report.current_distance = distance_m;
+	report.min_distance = get_minimum_distance();
+	report.max_distance = get_maximum_distance();
+	report.covariance = 0.0f;
+	/* TODO: set proper ID */
+	report.id = 0;
+
+	/* publish it, if we are the primary */
+	if (_distance_sensor_topic != nullptr) {
+		orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
+	}
+
+	_reports->force(&report);
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	ret = OK;
+
+	perf_end(_sample_perf);
+	return ret;
+}
+
+void
+SF1XX::start()
+{
+	/* reset the report ring and state machine */
+	_reports->flush();
+
+	/* set register to '0' */
+	measure();
+
+	/* schedule a cycle to start things */
+	work_queue(HPWORK, &_work, (worker_t)&SF1XX::cycle_trampoline, this, USEC2TICK(_conversion_interval));
+}
+
+void
+SF1XX::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+SF1XX::cycle_trampoline(void *arg)
+{
+	SF1XX *dev = (SF1XX *)arg;
+
+	dev->cycle();
+}
+
+void
+SF1XX::cycle()
+{
+	/* Collect results */
+	if (OK != collect()) {
+		DEVICE_DEBUG("collection error");
+		/* if error restart the measurement state machine */
+		start();
+		return;
+	}
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&SF1XX::cycle_trampoline,
+		   this,
+		   USEC2TICK(_conversion_interval));
+
+}
+
+void
+SF1XX::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	printf("poll interval:  %u ticks\n", _measure_ticks);
+	_reports->print_info("report queue");
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace sf1xx
+{
+
+SF1XX	*g_dev;
+
+void	start(uint8_t rotation);
+void	stop();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start(uint8_t rotation)
+{
+	int fd = -1;
+
+	if (g_dev != nullptr) {
+		errx(1, "already started");
+	}
+
+	/* create the driver */
+	g_dev = new SF1XX(rotation, SF1XX_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(SF1XX_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		goto fail;
+	}
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		::close(fd);
+		goto fail;
+	}
+
+	::close(fd);
+	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 distance_sensor_s report;
+	ssize_t sz;
+	int ret;
+
+	int fd = open(SF1XX_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		err(1, "%s open failed (try 'sf1xx start' if the driver is not running", SF1XX_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.current_distance);
+	warnx("time:        %llu", 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("valid %u", (float)report.current_distance > report.min_distance
+		      && (float)report.current_distance < report.max_distance ? 1 : 0);
+		warnx("measurement: %0.3f", (double)report.current_distance);
+		warnx("time:        %llu", 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");
+	}
+
+	::close(fd);
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(SF1XX_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");
+	}
+
+	::close(fd);
+	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
+sf1xx_main(int argc, char *argv[])
+{
+	// check for optional arguments
+	int ch;
+	int myoptind = 1;
+	const char *myoptarg = nullptr;
+	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
+
+
+	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'R':
+			rotation = (uint8_t)atoi(myoptarg);
+			PX4_INFO("Setting distance sensor orientation to %d", (int)rotation);
+			break;
+
+		default:
+			PX4_WARN("Unknown option!");
+		}
+	}
+
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[myoptind], "start")) {
+		sf1xx::start(rotation);
+	}
+
+	/*
+	 * Stop the driver
+	 */
+	if (!strcmp(argv[myoptind], "stop")) {
+		sf1xx::stop();
+	}
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[myoptind], "test")) {
+		sf1xx::test();
+	}
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[myoptind], "reset")) {
+		sf1xx::reset();
+	}
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
+		sf1xx::info();
+	}
+
+	PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
+	return PX4_ERROR;
+}

src/drivers/distance_sensor/srf02/CMakeLists.txt

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__srf02
+	MAIN srf02
+	COMPILE_FLAGS
+		-Wno-sign-compare
+	SRCS
+		srf02.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :

src/drivers/distance_sensor/srf02/srf02.cpp

@@ -0,0 +1,948 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 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 srf02.cpp
+ *
+ * Driver for the SRF02 sonar range finder adapted from the Maxbotix sonar range finder driver (mb12xx).
+ */
+
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_getopt.h>
+#include <px4_workqueue.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 <vector>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_range_finder.h>
+#include <drivers/device/ringbuffer.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/distance_sensor.h>
+
+#include <board_config.h>
+
+/* Configuration Constants */
+#define SRF02_BUS 		PX4_I2C_BUS_EXPANSION
+#define SRF02_BASEADDR 	0x70 /* 7-bit address. 8-bit address is 0xE0 */
+#define SRF02_DEVICE_PATH	"/dev/srf02"
+
+/* MB12xx Registers addresses */
+
+#define SRF02_TAKE_RANGE_REG	0x51		/* Measure range Register */
+#define SRF02_SET_ADDRESS_0	0xA0		/* Change address 0 Register */
+#define SRF02_SET_ADDRESS_1	0xAA		/* Change address 1 Register */
+#define SRF02_SET_ADDRESS_2	0xA5		/* Change address 2 Register */
+
+/* Device limits */
+#define SRF02_MIN_DISTANCE 	(0.20f)
+#define SRF02_MAX_DISTANCE 	(7.65f)
+
+#define SRF02_CONVERSION_INTERVAL 	100000 /* 60ms for one sonar */
+#define TICKS_BETWEEN_SUCCESIVE_FIRES 	100000 /* 30ms between each sonar measurement (watch out for interference!) */
+
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+class SRF02 : public device::I2C
+{
+public:
+	SRF02(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, int bus = SRF02_BUS,
+	      int address = SRF02_BASEADDR);
+	virtual ~SRF02();
+
+	virtual int 		init();
+
+	virtual ssize_t		read(device::file_t *filp, char *buffer, size_t buflen);
+	virtual int			ioctl(device::file_t *filp, int cmd, unsigned long arg);
+
+	/**
+	* Diagnostics - print some basic information about the driver.
+	*/
+	void				print_info();
+
+protected:
+	virtual int			probe();
+
+private:
+	uint8_t _rotation;
+	float				_min_distance;
+	float				_max_distance;
+	work_s				_work;
+	ringbuffer::RingBuffer	*_reports;
+	bool				_sensor_ok;
+	int					_measure_ticks;
+	bool				_collect_phase;
+	int				_class_instance;
+	int				_orb_class_instance;
+
+	orb_advert_t		_distance_sensor_topic;
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_comms_errors;
+
+	uint8_t				_cycle_counter;	/* counter in cycle to change i2c adresses */
+	int					_cycling_rate;	/* */
+	uint8_t				_index_counter;	/* temporary sonar i2c address */
+	std::vector<uint8_t>	addr_ind; 	/* temp sonar i2c address vector */
+	std::vector<float>
+	_latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */
+
+
+	/**
+	* 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();
+
+	/**
+	* 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 SRF02_MIN_DISTANCE
+	* and SRF02_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 srf02_main(int argc, char *argv[]);
+
+SRF02::SRF02(uint8_t rotation, int bus, int address) :
+	I2C("MB12xx", SRF02_DEVICE_PATH, bus, address, 100000),
+	_rotation(rotation),
+	_min_distance(SRF02_MIN_DISTANCE),
+	_max_distance(SRF02_MAX_DISTANCE),
+	_reports(nullptr),
+	_sensor_ok(false),
+	_measure_ticks(0),
+	_collect_phase(false),
+	_class_instance(-1),
+	_orb_class_instance(-1),
+	_distance_sensor_topic(nullptr),
+	_sample_perf(perf_alloc(PC_ELAPSED, "srf02_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "srf02_com_err")),
+	_cycle_counter(0),	/* initialising counter for cycling function to zero */
+	_cycling_rate(0),	/* initialising cycling rate (which can differ depending on one sonar or multiple) */
+	_index_counter(0) 	/* initialising temp sonar i2c address to zero */
+
+{
+	/* enable debug() calls */
+	_debug_enabled = false;
+
+	/* work_cancel in the dtor will explode if we don't do this... */
+	memset(&_work, 0, sizeof(_work));
+}
+
+SRF02::~SRF02()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr) {
+		delete _reports;
+	}
+
+	if (_class_instance != -1) {
+		unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance);
+	}
+
+	/* free perf counters */
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+}
+
+int
+SRF02::init()
+{
+	int ret = PX4_ERROR;
+
+	/* do I2C init (and probe) first */
+	if (I2C::init() != OK) {
+		return ret;
+	}
+
+	/* allocate basic report buffers */
+	_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
+
+	_index_counter = SRF02_BASEADDR;	/* set temp sonar i2c address to base adress */
+	set_device_address(_index_counter);		/* set I2c port to temp sonar i2c adress */
+
+	if (_reports == nullptr) {
+		return ret;
+	}
+
+	_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
+
+	/* get a publish handle on the range finder topic */
+	struct distance_sensor_s ds_report = {};
+
+	_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
+				 &_orb_class_instance, ORB_PRIO_LOW);
+
+	if (_distance_sensor_topic == nullptr) {
+		DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
+	}
+
+	// XXX we should find out why we need to wait 200 ms here
+	usleep(200000);
+
+	/* check for connected rangefinders on each i2c port:
+	   We start from i2c base address (0x70 = 112) and count downwards
+	   So second iteration it uses i2c address 111, third iteration 110 and so on*/
+	for (unsigned counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) {
+		_index_counter = SRF02_BASEADDR - counter;	/* set temp sonar i2c address to base adress - counter */
+		set_device_address(_index_counter);			/* set I2c port to temp sonar i2c adress */
+		int ret2 = measure();
+
+		if (ret2 == 0) { /* sonar is present -> store address_index in array */
+			addr_ind.push_back(_index_counter);
+			DEVICE_DEBUG("sonar added");
+			_latest_sonar_measurements.push_back(200);
+		}
+	}
+
+	_index_counter = SRF02_BASEADDR;
+	set_device_address(_index_counter); /* set i2c port back to base adress for rest of driver */
+
+	/* if only one sonar detected, no special timing is required between firing, so use default */
+	if (addr_ind.size() == 1) {
+		_cycling_rate = SRF02_CONVERSION_INTERVAL;
+
+	} else {
+		_cycling_rate = TICKS_BETWEEN_SUCCESIVE_FIRES;
+	}
+
+	/* show the connected sonars in terminal */
+	for (unsigned i = 0; i < addr_ind.size(); i++) {
+		DEVICE_LOG("sonar %d with address %d added", (i + 1), addr_ind[i]);
+	}
+
+	DEVICE_DEBUG("Number of sonars connected: %zu", addr_ind.size());
+
+	ret = OK;
+	/* sensor is ok, but we don't really know if it is within range */
+	_sensor_ok = true;
+
+	return ret;
+}
+
+int
+SRF02::probe()
+{
+	return measure();
+}
+
+void
+SRF02::set_minimum_distance(float min)
+{
+	_min_distance = min;
+}
+
+void
+SRF02::set_maximum_distance(float max)
+{
+	_max_distance = max;
+}
+
+float
+SRF02::get_minimum_distance()
+{
+	return _min_distance;
+}
+
+float
+SRF02::get_maximum_distance()
+{
+	return _max_distance;
+}
+
+int
+SRF02::ioctl(device::file_t *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(_cycling_rate);
+
+					/* 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 */
+					int ticks = USEC2TICK(1000000 / arg);
+
+					/* check against maximum rate */
+					if (ticks < USEC2TICK(_cycling_rate)) {
+						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;
+			}
+
+			ATOMIC_ENTER;
+
+			if (!_reports->resize(arg)) {
+				ATOMIC_LEAVE;
+				return -ENOMEM;
+			}
+
+			ATOMIC_LEAVE;
+
+			return OK;
+		}
+
+	case SENSORIOCRESET:
+		/* XXX implement this */
+		return -EINVAL;
+
+	default:
+		/* give it to the superclass */
+		return I2C::ioctl(filp, cmd, arg);
+	}
+}
+
+ssize_t
+SRF02::read(device::file_t *filp, char *buffer, size_t buflen)
+{
+
+	unsigned count = buflen / sizeof(struct distance_sensor_s);
+	struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(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(_cycling_rate * 2);
+
+		/* 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
+SRF02::measure()
+{
+
+	int ret;
+
+	/*
+	 * Send the command to begin a measurement.
+	 */
+
+	uint8_t cmd[2];
+	cmd[0] = 0x00;
+	cmd[1] = SRF02_TAKE_RANGE_REG;
+	ret = transfer(cmd, 2, nullptr, 0);
+
+	if (OK != ret) {
+		perf_count(_comms_errors);
+		DEVICE_DEBUG("i2c::transfer returned %d", ret);
+		return ret;
+	}
+
+	ret = OK;
+
+	return ret;
+}
+
+int
+SRF02::collect()
+{
+	int	ret = -EIO;
+
+	/* read from the sensor */
+	uint8_t val[2] = {0, 0};
+	uint8_t cmd = 0x02;
+	perf_begin(_sample_perf);
+
+	ret = transfer(&cmd, 1, nullptr, 0);
+	ret = transfer(nullptr, 0, &val[0], 2);
+
+	if (ret < 0) {
+		DEVICE_DEBUG("error reading from sensor: %d", ret);
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+		return ret;
+	}
+
+	uint16_t distance_cm = val[0] << 8 | val[1];
+	float distance_m = float(distance_cm) * 1e-2f;
+
+	struct distance_sensor_s report;
+	report.timestamp = hrt_absolute_time();
+	report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
+	report.orientation = _rotation;
+	report.current_distance = distance_m;
+	report.min_distance = get_minimum_distance();
+	report.max_distance = get_maximum_distance();
+	report.covariance = 0.0f;
+	/* TODO: set proper ID */
+	report.id = 0;
+
+	/* publish it, if we are the primary */
+	if (_distance_sensor_topic != nullptr) {
+		orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
+	}
+
+	_reports->force(&report);
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	ret = OK;
+
+	perf_end(_sample_perf);
+	return ret;
+}
+
+void
+SRF02::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)&SRF02::cycle_trampoline, this, 5);
+
+	/* notify about state change */
+	struct subsystem_info_s info = {};
+	info.present = true;
+	info.enabled = true;
+	info.ok = true;
+	info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
+
+	static orb_advert_t pub = nullptr;
+
+	if (pub != nullptr) {
+		orb_publish(ORB_ID(subsystem_info), pub, &info);
+
+
+	} else {
+		pub = orb_advertise(ORB_ID(subsystem_info), &info);
+
+	}
+}
+
+void
+SRF02::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+SRF02::cycle_trampoline(void *arg)
+{
+
+	SRF02 *dev = (SRF02 *)arg;
+
+	dev->cycle();
+
+}
+
+void
+SRF02::cycle()
+{
+	if (_collect_phase) {
+		_index_counter = addr_ind[_cycle_counter]; /*sonar from previous iteration collect is now read out */
+		set_device_address(_index_counter);
+
+		/* perform collection */
+		if (OK != collect()) {
+			DEVICE_DEBUG("collection error");
+			/* if error restart the measurement state machine */
+			start();
+			return;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+
+		/* change i2c adress to next sonar */
+		_cycle_counter = _cycle_counter + 1;
+
+		if (_cycle_counter >= addr_ind.size()) {
+			_cycle_counter = 0;
+		}
+
+		/* Is there a collect->measure gap? Yes, and the timing is set equal to the cycling_rate
+		   Otherwise the next sonar would fire without the first one having received its reflected sonar pulse */
+
+		if (_measure_ticks > USEC2TICK(_cycling_rate)) {
+
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&SRF02::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(_cycling_rate));
+			return;
+		}
+	}
+
+	/* Measurement (firing) phase */
+
+	/* ensure sonar i2c adress is still correct */
+	_index_counter = addr_ind[_cycle_counter];
+	set_device_address(_index_counter);
+
+	/* Perform measurement */
+	if (OK != measure()) {
+		DEVICE_DEBUG("measure error sonar adress %d", _index_counter);
+	}
+
+	/* next phase is collection */
+	_collect_phase = true;
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&SRF02::cycle_trampoline,
+		   this,
+		   USEC2TICK(_cycling_rate));
+
+}
+
+void
+SRF02::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	printf("poll interval:  %u ticks\n", _measure_ticks);
+	_reports->print_info("report queue");
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace srf02
+{
+
+SRF02	*g_dev;
+
+void	start(uint8_t rotation);
+void	stop();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start(uint8_t rotation)
+{
+	int fd;
+
+	if (g_dev != nullptr) {
+		errx(1, "already started");
+	}
+
+	/* create the driver */
+	g_dev = new SRF02(rotation, SRF02_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(SRF02_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		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 distance_sensor_s report;
+	ssize_t sz;
+	int ret;
+
+	int fd = open(SRF02_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		err(1, "%s open failed (try 'srf02 start' if the driver is not running", SRF02_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.current_distance);
+	warnx("time:        %llu", 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("valid %u", (float)report.current_distance > report.min_distance
+		      && (float)report.current_distance < report.max_distance ? 1 : 0);
+		warnx("measurement: %0.3f", (double)report.current_distance);
+		warnx("time:        %llu", 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(SRF02_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
+srf02_main(int argc, char *argv[])
+{
+	// check for optional arguments
+	int ch;
+	int myoptind = 1;
+	const char *myoptarg = nullptr;
+	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
+
+
+	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'R':
+			rotation = (uint8_t)atoi(myoptarg);
+			PX4_INFO("Setting distance sensor orientation to %d", (int)rotation);
+			break;
+
+		default:
+			PX4_WARN("Unknown option!");
+		}
+	}
+
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[myoptind], "start")) {
+		srf02::start(rotation);
+	}
+
+	/*
+	 * Stop the driver
+	 */
+	if (!strcmp(argv[myoptind], "stop")) {
+		srf02::stop();
+	}
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[myoptind], "test")) {
+		srf02::test();
+	}
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[myoptind], "reset")) {
+		srf02::reset();
+	}
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
+		srf02::info();
+	}
+
+	PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
+	return PX4_ERROR;
+}

src/drivers/distance_sensor/srf02_i2c/CMakeLists.txt

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__srf02_i2c
+	MAIN srf02_i2c
+	COMPILE_FLAGS
+		-Wno-sign-compare
+	SRCS
+		srf02_i2c.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :

src/drivers/distance_sensor/srf02_i2c/srf02_i2c.cpp

@@ -0,0 +1,948 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015, 2016 Airmind 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 Airmind 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 srf02_i2c.cpp
+ * @author Greg Hulands
+ * @author Jon Verbeke <jon.verbeke@kuleuven.be>
+ *
+ * Driver for the Maxbotix sonar range finders connected via I2C.
+ */
+
+#include <px4_workqueue.h>
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_getopt.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 <vector>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_range_finder.h>
+#include <drivers/device/ringbuffer.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/distance_sensor.h>
+
+#include <board_config.h>
+
+/* Configuration Constants */
+#define SRF02_I2C_BUS 		PX4_I2C_BUS_EXPANSION
+#define SRF02_I2C_BASEADDR 	0x70 /* 7-bit address. 8-bit address is 0xE0 */
+#define SRF02_DEVICE_PATH	"/dev/srf02"
+
+/* MB12xx Registers addresses */
+
+#define SRF02_TAKE_RANGE_REG	0x51		/* Measure range Register */
+#define SRF02_SET_ADDRESS_0	0xA0		/* Change address 0 Register */
+#define SRF02_SET_ADDRESS_1	0xAA		/* Change address 1 Register */
+#define SRF02_SET_ADDRESS_2	0xA5		/* Change address 2 Register */
+
+/* Device limits */
+#define SRF02_MIN_DISTANCE 	(0.20f)
+#define SRF02_MAX_DISTANCE 	(6.00f)
+
+#define SRF02_CONVERSION_INTERVAL 	100000 /* 60ms for one sonar */
+#define TICKS_BETWEEN_SUCCESIVE_FIRES 	100000 /* 30ms between each sonar measurement (watch out for interference!) */
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+class SRF02_I2C : public device::I2C
+{
+public:
+	SRF02_I2C(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, int bus = SRF02_I2C_BUS,
+		  int address = SRF02_I2C_BASEADDR);
+	virtual ~SRF02_I2C();
+
+	virtual int 		init();
+
+	virtual ssize_t		read(device::file_t *filp, char *buffer, size_t buflen);
+	virtual int			ioctl(device::file_t *filp, int cmd, unsigned long arg);
+
+	/**
+	* Diagnostics - print some basic information about the driver.
+	*/
+	void				print_info();
+
+protected:
+	virtual int			probe();
+
+private:
+	uint8_t _rotation;
+	float				_min_distance;
+	float				_max_distance;
+	work_s				_work;
+	ringbuffer::RingBuffer		*_reports;
+	bool				_sensor_ok;
+	int					_measure_ticks;
+	bool				_collect_phase;
+	int					_class_instance;
+	int					_orb_class_instance;
+
+	orb_advert_t		_distance_sensor_topic;
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_comms_errors;
+
+	uint8_t				_cycle_counter;	/* counter in cycle to change i2c adresses */
+	int					_cycling_rate;	/* */
+	uint8_t				_index_counter;	/* temporary sonar i2c address */
+	std::vector<uint8_t>	addr_ind; 	/* temp sonar i2c address vector */
+	std::vector<float>
+	_latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */
+
+
+	/**
+	* 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();
+
+	/**
+	* 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 SRF02_MIN_DISTANCE
+	* and SRF02_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 srf02_i2c_main(int argc, char *argv[]);}
+
+SRF02_I2C::SRF02_I2C(uint8_t rotation, int bus, int address) :
+	I2C("MB12xx", SRF02_DEVICE_PATH, bus, address, 100000),
+	_rotation(rotation),
+	_min_distance(SRF02_MIN_DISTANCE),
+	_max_distance(SRF02_MAX_DISTANCE),
+	_reports(nullptr),
+	_sensor_ok(false),
+	_measure_ticks(0),
+	_collect_phase(false),
+	_class_instance(-1),
+	_orb_class_instance(-1),
+	_distance_sensor_topic(nullptr),
+	_sample_perf(perf_alloc(PC_ELAPSED, "srf02_i2c_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "srf02_i2c_comms_errors")),
+	_cycle_counter(0),	/* initialising counter for cycling function to zero */
+	_cycling_rate(0),	/* initialising cycling rate (which can differ depending on one sonar or multiple) */
+	_index_counter(0) 	/* initialising temp sonar i2c address to zero */
+
+{
+	/* enable debug() calls */
+	_debug_enabled = false;
+
+	/* work_cancel in the dtor will explode if we don't do this... */
+	memset(&_work, 0, sizeof(_work));
+}
+
+SRF02_I2C::~SRF02_I2C()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr) {
+		delete _reports;
+	}
+
+	if (_class_instance != -1) {
+		unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance);
+	}
+
+	/* free perf counters */
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+}
+
+int
+SRF02_I2C::init()
+{
+	int ret = PX4_ERROR;
+
+	/* do I2C init (and probe) first */
+	if (I2C::init() != OK) {
+		return ret;
+	}
+
+	/* allocate basic report buffers */
+	_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
+
+	_index_counter = SRF02_I2C_BASEADDR;	/* set temp sonar i2c address to base adress */
+	set_device_address(_index_counter);		/* set I2c port to temp sonar i2c adress */
+
+	if (_reports == nullptr) {
+		return ret;
+	}
+
+	_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
+
+	/* get a publish handle on the range finder topic */
+	struct distance_sensor_s ds_report = {};
+
+	_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
+				 &_orb_class_instance, ORB_PRIO_LOW);
+
+	if (_distance_sensor_topic == nullptr) {
+		DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
+	}
+
+	// XXX we should find out why we need to wait 200 ms here
+	usleep(200000);
+
+	/* check for connected rangefinders on each i2c port:
+	   We start from i2c base address (0x70 = 112) and count downwards
+	   So second iteration it uses i2c address 111, third iteration 110 and so on*/
+	for (unsigned counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) {
+		_index_counter = SRF02_I2C_BASEADDR + counter * 2;	/* set temp sonar i2c address to base adress - counter */
+		set_device_address(_index_counter);			/* set I2c port to temp sonar i2c adress */
+		int ret2 = measure();
+
+		if (ret2 == 0) { /* sonar is present -> store address_index in array */
+			addr_ind.push_back(_index_counter);
+			DEVICE_DEBUG("sonar added");
+			_latest_sonar_measurements.push_back(200);
+		}
+	}
+
+	_index_counter = SRF02_I2C_BASEADDR;
+	set_device_address(_index_counter); /* set i2c port back to base adress for rest of driver */
+
+	/* if only one sonar detected, no special timing is required between firing, so use default */
+	if (addr_ind.size() == 1) {
+		_cycling_rate = SRF02_CONVERSION_INTERVAL;
+
+	} else {
+		_cycling_rate = TICKS_BETWEEN_SUCCESIVE_FIRES;
+	}
+
+	/* show the connected sonars in terminal */
+	for (unsigned i = 0; i < addr_ind.size(); i++) {
+		DEVICE_LOG("sonar %d with address %d added", (i + 1), addr_ind[i]);
+	}
+
+	DEVICE_DEBUG("Number of sonars connected: %zu", addr_ind.size());
+
+	ret = OK;
+	/* sensor is ok, but we don't really know if it is within range */
+	_sensor_ok = true;
+
+	return ret;
+}
+
+int
+SRF02_I2C::probe()
+{
+	return measure();
+}
+
+void
+SRF02_I2C::set_minimum_distance(float min)
+{
+	_min_distance = min;
+}
+
+void
+SRF02_I2C::set_maximum_distance(float max)
+{
+	_max_distance = max;
+}
+
+float
+SRF02_I2C::get_minimum_distance()
+{
+	return _min_distance;
+}
+
+float
+SRF02_I2C::get_maximum_distance()
+{
+	return _max_distance;
+}
+
+int
+SRF02_I2C::ioctl(device::file_t *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(_cycling_rate);
+
+					/* 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 */
+					int ticks = USEC2TICK(1000000 / arg);
+
+					/* check against maximum rate */
+					if (ticks < USEC2TICK(_cycling_rate)) {
+						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;
+			}
+
+			ATOMIC_ENTER;
+
+			if (!_reports->resize(arg)) {
+				ATOMIC_LEAVE;
+				return -ENOMEM;
+			}
+
+			ATOMIC_LEAVE;
+
+			return OK;
+		}
+
+	case SENSORIOCRESET:
+		/* XXX implement this */
+		return -EINVAL;
+
+	default:
+		/* give it to the superclass */
+		return I2C::ioctl(filp, cmd, arg);
+	}
+}
+
+ssize_t
+SRF02_I2C::read(device::file_t *filp, char *buffer, size_t buflen)
+{
+
+	unsigned count = buflen / sizeof(struct distance_sensor_s);
+	struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(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(_cycling_rate * 2);
+
+		/* 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
+SRF02_I2C::measure()
+{
+
+	int ret;
+
+	/*
+	 * Send the command to begin a measurement.
+	 */
+
+	uint8_t cmd[2];
+	cmd[0] = 0x00;
+	cmd[1] = SRF02_TAKE_RANGE_REG;
+	ret = transfer(cmd, 2, nullptr, 0);
+
+	if (OK != ret) {
+		perf_count(_comms_errors);
+		DEVICE_DEBUG("i2c::transfer returned %d", ret);
+		return ret;
+	}
+
+	ret = OK;
+
+	return ret;
+}
+
+int
+SRF02_I2C::collect()
+{
+	int	ret = -EIO;
+
+	/* read from the sensor */
+	uint8_t val[2] = {0, 0};
+	uint8_t cmd = 0x02;
+	perf_begin(_sample_perf);
+
+	ret = transfer(&cmd, 1, nullptr, 0);
+	ret = transfer(nullptr, 0, &val[0], 2);
+
+	if (ret < 0) {
+		DEVICE_DEBUG("error reading from sensor: %d", ret);
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+		return ret;
+	}
+
+	uint16_t distance_cm = val[0] << 8 | val[1];
+	float distance_m = float(distance_cm) * 1e-2f;
+
+	struct distance_sensor_s report;
+	report.timestamp = hrt_absolute_time();
+	report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
+	report.orientation = _rotation;
+	report.current_distance = distance_m;
+	report.min_distance = get_minimum_distance();
+	report.max_distance = get_maximum_distance();
+	report.covariance = 0.0f;
+	/* TODO: set proper ID */
+	report.id = 0;
+
+	/* publish it, if we are the primary */
+	if (_distance_sensor_topic != nullptr) {
+		orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
+	}
+
+	_reports->force(&report);
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	ret = OK;
+
+	perf_end(_sample_perf);
+	return ret;
+}
+
+void
+SRF02_I2C::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)&SRF02_I2C::cycle_trampoline, this, 5);
+
+	/* notify about state change */
+	struct subsystem_info_s info = {};
+	info.present = true;
+	info.enabled = true;
+	info.ok = true;
+	info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
+
+	static orb_advert_t pub = nullptr;
+
+	if (pub != nullptr) {
+		orb_publish(ORB_ID(subsystem_info), pub, &info);
+
+	} else {
+		pub = orb_advertise(ORB_ID(subsystem_info), &info);
+
+	}
+}
+
+void
+SRF02_I2C::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+SRF02_I2C::cycle_trampoline(void *arg)
+{
+
+	SRF02_I2C *dev = (SRF02_I2C *)arg;
+
+	dev->cycle();
+
+}
+
+void
+SRF02_I2C::cycle()
+{
+	if (_collect_phase) {
+		_index_counter = addr_ind[_cycle_counter]; /*sonar from previous iteration collect is now read out */
+		set_device_address(_index_counter);
+
+		/* perform collection */
+		if (OK != collect()) {
+			DEVICE_DEBUG("collection error");
+			/* if error restart the measurement state machine */
+			start();
+			return;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+
+		/* change i2c adress to next sonar */
+		_cycle_counter = _cycle_counter + 1;
+
+		if (_cycle_counter >= addr_ind.size()) {
+			_cycle_counter = 0;
+		}
+
+		/* Is there a collect->measure gap? Yes, and the timing is set equal to the cycling_rate
+		   Otherwise the next sonar would fire without the first one having received its reflected sonar pulse */
+
+		if (_measure_ticks > USEC2TICK(_cycling_rate)) {
+
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&SRF02_I2C::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(_cycling_rate));
+			return;
+		}
+	}
+
+	/* Measurement (firing) phase */
+
+	/* ensure sonar i2c adress is still correct */
+	_index_counter = addr_ind[_cycle_counter];
+	set_device_address(_index_counter);
+
+	/* Perform measurement */
+	if (OK != measure()) {
+		DEVICE_DEBUG("measure error sonar adress %d", _index_counter);
+	}
+
+	/* next phase is collection */
+	_collect_phase = true;
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&SRF02_I2C::cycle_trampoline,
+		   this,
+		   USEC2TICK(_cycling_rate));
+
+}
+
+void
+SRF02_I2C::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	printf("poll interval:  %u ticks\n", _measure_ticks);
+	_reports->print_info("report queue");
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace  srf02_i2c
+{
+
+SRF02_I2C	*g_dev;
+
+void	start(uint8_t rotation);
+void	stop();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start(uint8_t rotation)
+{
+	int fd;
+
+	if (g_dev != nullptr) {
+		errx(1, "already started");
+	}
+
+	/* create the driver */
+	g_dev = new SRF02_I2C(rotation, SRF02_I2C_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(SRF02_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		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 distance_sensor_s report;
+	ssize_t sz;
+	int ret;
+
+	int fd = open(SRF02_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		err(1, "%s open failed (try 'srf02_i2c start' if the driver is not running", SRF02_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.current_distance);
+	warnx("time:        %llu", 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("valid %u", (float)report.current_distance > report.min_distance
+		      && (float)report.current_distance < report.max_distance ? 1 : 0);
+		warnx("measurement: %0.3f", (double)report.current_distance);
+		warnx("time:        %llu", 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(SRF02_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
+srf02_i2c_main(int argc, char *argv[])
+{
+	// check for optional arguments
+	int ch;
+	int myoptind = 1;
+	const char *myoptarg = nullptr;
+	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
+
+
+	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'R':
+			rotation = (uint8_t)atoi(myoptarg);
+			PX4_INFO("Setting distance sensor orientation to %d", (int)rotation);
+			break;
+
+		default:
+			PX4_WARN("Unknown option!");
+		}
+	}
+
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[myoptind], "start")) {
+		srf02_i2c::start(rotation);
+	}
+
+	/*
+	 * Stop the driver
+	 */
+	if (!strcmp(argv[myoptind], "stop")) {
+		srf02_i2c::stop();
+	}
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[myoptind], "test")) {
+		srf02_i2c::test();
+	}
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[myoptind], "reset")) {
+		srf02_i2c::reset();
+	}
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
+		srf02_i2c::info();
+	}
+
+	PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
+	return PX4_ERROR;
+}

src/drivers/distance_sensor/teraranger/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__teraranger
+	MAIN teraranger
+	STACK_MAIN 1200
+	COMPILE_FLAGS
+		-Wno-sign-compare
+	SRCS
+		teraranger.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :

src/drivers/distance_sensor/teraranger/teraranger.cpp

@@ -0,0 +1,988 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file teraranger.cpp
+ * @author Luis Rodrigues
+ *
+ * Driver for the TeraRanger One range finders connected via I2C.
+ */
+
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_getopt.h>
+#include <px4_workqueue.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 <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_range_finder.h>
+#include <drivers/device/ringbuffer.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/distance_sensor.h>
+
+#include <board_config.h>
+
+/* Configuration Constants */
+#define TERARANGER_BUS           PX4_I2C_BUS_EXPANSION
+#define TRONE_BASEADDR      0x30 /* 7-bit address */
+#define TREVO_BASEADDR      0x31 /* 7-bit address */
+#define TERARANGER_DEVICE_PATH   	"/dev/teraranger"
+
+/* TERARANGER Registers addresses */
+
+#define TERARANGER_MEASURE_REG	0x00		/* Measure range register */
+#define TERARANGER_WHO_AM_I_REG  0x01        /* Who am I test register */
+#define TERARANGER_WHO_AM_I_REG_VAL 0xA1
+
+
+/* Device limits */
+#define TRONE_MIN_DISTANCE (0.20f)
+#define TRONE_MAX_DISTANCE (14.00f)
+#define TREVO_MIN_DISTANCE (0.50f)
+#define TREVO_MAX_DISTANCE (60.0f)
+
+#define TERARANGER_CONVERSION_INTERVAL 50000 /* 50ms */
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+class TERARANGER : public device::I2C
+{
+public:
+	TERARANGER(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING,
+		   int bus = TERARANGER_BUS, int address = TRONE_BASEADDR);
+	virtual ~TERARANGER();
+
+	virtual int 		init();
+
+	virtual ssize_t		read(device::file_t *filp, char *buffer, size_t buflen);
+	virtual int			ioctl(device::file_t *filp, int cmd, unsigned long arg);
+
+	/**
+	* Diagnostics - print some basic information about the driver.
+	*/
+	void				print_info();
+
+protected:
+	virtual int			probe();
+
+private:
+	uint8_t _rotation;
+	float				_min_distance;
+	float				_max_distance;
+	work_s				_work;
+	ringbuffer::RingBuffer		*_reports;
+	bool				_sensor_ok;
+	uint8_t				_valid;
+	int					_measure_ticks;
+	bool				_collect_phase;
+	int				_class_instance;
+	int				_orb_class_instance;
+
+	orb_advert_t		_distance_sensor_topic;
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_comms_errors;
+
+	/**
+	* 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();
+
+	/**
+	* 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 TRONE_MIN_DISTANCE
+	* and TRONE_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);
+
+
+};
+
+static const uint8_t crc_table[] = {
+	0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31,
+	0x24, 0x23, 0x2a, 0x2d, 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65,
+	0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, 0xe0, 0xe7, 0xee, 0xe9,
+	0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd,
+	0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1,
+	0xb4, 0xb3, 0xba, 0xbd, 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2,
+	0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, 0xb7, 0xb0, 0xb9, 0xbe,
+	0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a,
+	0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16,
+	0x03, 0x04, 0x0d, 0x0a, 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42,
+	0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, 0x89, 0x8e, 0x87, 0x80,
+	0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4,
+	0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8,
+	0xdd, 0xda, 0xd3, 0xd4, 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c,
+	0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, 0x19, 0x1e, 0x17, 0x10,
+	0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34,
+	0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f,
+	0x6a, 0x6d, 0x64, 0x63, 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b,
+	0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, 0xae, 0xa9, 0xa0, 0xa7,
+	0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83,
+	0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef,
+	0xfa, 0xfd, 0xf4, 0xf3
+};
+
+static uint8_t crc8(uint8_t *p, uint8_t len)
+{
+	uint16_t i;
+	uint16_t crc = 0x0;
+
+	while (len--) {
+		i = (crc ^ *p++) & 0xFF;
+		crc = (crc_table[i] ^ (crc << 8)) & 0xFF;
+	}
+
+	return crc & 0xFF;
+}
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int teraranger_main(int argc, char *argv[]);
+
+TERARANGER::TERARANGER(uint8_t rotation, int bus, int address) :
+	I2C("TERARANGER", TERARANGER_DEVICE_PATH, bus, address, 100000),
+	_rotation(rotation),
+	_min_distance(-1.0f),
+	_max_distance(-1.0f),
+	_reports(nullptr),
+	_sensor_ok(false),
+	_valid(0),
+	_measure_ticks(0),
+	_collect_phase(false),
+	_class_instance(-1),
+	_orb_class_instance(-1),
+	_distance_sensor_topic(nullptr),
+	_sample_perf(perf_alloc(PC_ELAPSED, "tr1_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "tr1_com_err"))
+{
+	// up the retries since the device misses the first measure attempts
+	I2C::_retries = 3;
+
+	// enable debug() calls
+	_debug_enabled = false;
+
+	// work_cancel in the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
+}
+
+TERARANGER::~TERARANGER()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr) {
+		delete _reports;
+	}
+
+	if (_class_instance != -1) {
+		unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance);
+	}
+
+	// free perf counters
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+}
+
+int
+TERARANGER::init()
+{
+	int ret = PX4_ERROR;
+	int hw_model;
+	param_get(param_find("SENS_EN_TRANGER"), &hw_model);
+
+	switch (hw_model) {
+	case 0: /* Disabled */
+		DEVICE_LOG("Disabled");
+		return ret;
+
+	case 1: /* Autodetect */
+		/* Assume TROne */
+		set_device_address(TRONE_BASEADDR);
+
+		if (I2C::init() != OK) {
+			set_device_address(TREVO_BASEADDR);
+
+			if (I2C::init() != OK) {
+				goto out;
+
+			} else {
+				_min_distance = TREVO_MIN_DISTANCE;
+				_max_distance = TREVO_MAX_DISTANCE;
+			}
+
+		} else {
+			_min_distance = TRONE_MIN_DISTANCE;
+			_max_distance = TRONE_MAX_DISTANCE;
+		}
+
+		break;
+
+	case 2: /* TROne */
+		set_device_address(TRONE_BASEADDR);
+
+		if (I2C::init() != OK) {
+			goto out;
+		}
+
+		_min_distance = TRONE_MIN_DISTANCE;
+		_max_distance = TRONE_MAX_DISTANCE;
+		break;
+
+	case 3: /* TREvo */
+		set_device_address(TREVO_BASEADDR);
+
+		/* do I2C init (and probe) first */
+		if (I2C::init() != OK) {
+			goto out;
+		}
+
+		_min_distance = TREVO_MIN_DISTANCE;
+		_max_distance = TREVO_MAX_DISTANCE;
+		break;
+
+	default:
+		DEVICE_LOG("invalid HW model %d.", hw_model);
+		return ret;
+	}
+
+	/* allocate basic report buffers */
+	_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
+
+	if (_reports == nullptr) {
+		goto out;
+	}
+
+	_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
+
+	if (_class_instance == CLASS_DEVICE_PRIMARY) {
+		/* get a publish handle on the range finder topic */
+		struct distance_sensor_s ds_report;
+		measure();
+		_reports->get(&ds_report);
+
+		_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
+					 &_orb_class_instance, ORB_PRIO_LOW);
+
+		if (_distance_sensor_topic == nullptr) {
+			DEVICE_LOG("failed to create distance_sensor 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
+TERARANGER::probe()
+{
+	uint8_t who_am_i = 0;
+
+	const uint8_t cmd = TERARANGER_WHO_AM_I_REG;
+
+	// can't use a single transfer as Teraranger needs a bit of time for internal processing
+	if (transfer(&cmd, 1, nullptr, 0) == OK) {
+		if (transfer(nullptr, 0, &who_am_i, 1) == OK && who_am_i == TERARANGER_WHO_AM_I_REG_VAL) {
+			return measure();
+		}
+	}
+
+	DEVICE_DEBUG("WHO_AM_I byte mismatch 0x%02x should be 0x%02x\n",
+		     (unsigned)who_am_i,
+		     TERARANGER_WHO_AM_I_REG_VAL);
+
+	// not found on any address
+	return -EIO;
+}
+
+void
+TERARANGER::set_minimum_distance(float min)
+{
+	_min_distance = min;
+}
+
+void
+TERARANGER::set_maximum_distance(float max)
+{
+	_max_distance = max;
+}
+
+float
+TERARANGER::get_minimum_distance()
+{
+	return _min_distance;
+}
+
+float
+TERARANGER::get_maximum_distance()
+{
+	return _max_distance;
+}
+
+int
+TERARANGER::ioctl(device::file_t *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(TERARANGER_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(TERARANGER_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;
+			}
+
+			ATOMIC_ENTER;
+
+			if (!_reports->resize(arg)) {
+				ATOMIC_LEAVE;
+				return -ENOMEM;
+			}
+
+			ATOMIC_LEAVE;
+
+			return OK;
+		}
+
+	case SENSORIOCRESET:
+		/* XXX implement this */
+		return -EINVAL;
+
+	default:
+		/* give it to the superclass */
+		return I2C::ioctl(filp, cmd, arg);
+	}
+}
+
+ssize_t
+TERARANGER::read(device::file_t *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct distance_sensor_s);
+	struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(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(TERARANGER_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
+TERARANGER::measure()
+{
+	int ret;
+
+	/*
+	 * Send the command to begin a measurement.
+	 */
+	const uint8_t cmd = TERARANGER_MEASURE_REG;
+	ret = transfer(&cmd, sizeof(cmd), nullptr, 0);
+
+	if (OK != ret) {
+		perf_count(_comms_errors);
+		DEVICE_LOG("i2c::transfer returned %d", ret);
+		return ret;
+	}
+
+	ret = OK;
+
+	return ret;
+}
+
+int
+TERARANGER::collect()
+{
+	int ret = -EIO;
+
+	/* read from the sensor */
+	uint8_t val[3] = {0, 0, 0};
+
+	perf_begin(_sample_perf);
+
+	ret = transfer(nullptr, 0, &val[0], 3);
+
+	if (ret < 0) {
+		DEVICE_LOG("error reading from sensor: %d", ret);
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+		return ret;
+	}
+
+	uint16_t distance_mm = (val[0] << 8) | val[1];
+	float distance_m = float(distance_mm) *  1e-3f;
+	struct distance_sensor_s report;
+
+	report.timestamp = hrt_absolute_time();
+	/* there is no enum item for a combined LASER and ULTRASOUND which it should be */
+	report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
+	report.orientation = _rotation;
+	report.current_distance = distance_m;
+	report.min_distance = get_minimum_distance();
+	report.max_distance = get_maximum_distance();
+	report.covariance = 0.0f;
+	/* TODO: set proper ID */
+	report.id = 0;
+
+	// This validation check can be used later
+	_valid = crc8(val, 2) == val[2] && (float)report.current_distance > report.min_distance
+		 && (float)report.current_distance < report.max_distance ? 1 : 0;
+
+	/* publish it, if we are the primary */
+	if (_distance_sensor_topic != nullptr) {
+		orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
+	}
+
+	_reports->force(&report);
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	ret = OK;
+
+	perf_end(_sample_perf);
+	return ret;
+}
+
+void
+TERARANGER::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)&TERARANGER::cycle_trampoline, this, 1);
+
+	/* notify about state change */
+	struct subsystem_info_s info = {};
+	info.present = true;
+	info.enabled = true;
+	info.ok = true;
+	info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
+
+	static orb_advert_t pub = nullptr;
+
+	if (pub != nullptr) {
+		orb_publish(ORB_ID(subsystem_info), pub, &info);
+
+	} else {
+		pub = orb_advertise(ORB_ID(subsystem_info), &info);
+	}
+}
+
+void
+TERARANGER::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+TERARANGER::cycle_trampoline(void *arg)
+{
+	TERARANGER *dev = (TERARANGER *)arg;
+
+	dev->cycle();
+}
+
+void
+TERARANGER::cycle()
+{
+	/* collection phase? */
+	if (_collect_phase) {
+
+		/* perform collection */
+		if (OK != collect()) {
+			DEVICE_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(TERARANGER_CONVERSION_INTERVAL)) {
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&TERARANGER::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(TERARANGER_CONVERSION_INTERVAL));
+
+			return;
+		}
+	}
+
+	/* measurement phase */
+	if (OK != measure()) {
+		DEVICE_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)&TERARANGER::cycle_trampoline,
+		   this,
+		   USEC2TICK(TERARANGER_CONVERSION_INTERVAL));
+}
+
+void
+TERARANGER::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	printf("poll interval:  %u ticks\n", _measure_ticks);
+	_reports->print_info("report queue");
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace teraranger
+{
+
+TERARANGER	*g_dev;
+
+void	start(uint8_t rotation);
+void	stop();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start(uint8_t rotation)
+{
+	int fd;
+
+	if (g_dev != nullptr) {
+		errx(1, "already started");
+	}
+
+	/* create the driver */
+	g_dev = new TERARANGER(rotation, TERARANGER_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(TERARANGER_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		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 distance_sensor_s report;
+	ssize_t sz;
+	int ret;
+
+	int fd = open(TERARANGER_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		err(1, "%s open failed (try 'teraranger start' if the driver is not running", TERARANGER_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.current_distance);
+	warnx("time:        %llu", 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 50x and report each value */
+	for (unsigned i = 0; i < 50; 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("measurement: %0.3f", (double)report.current_distance);
+		warnx("time:        %llu", 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(TERARANGER_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
+teraranger_main(int argc, char *argv[])
+{
+	int ch;
+	int myoptind = 1;
+	const char *myoptarg = nullptr;
+	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
+
+	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'R':
+			rotation = (uint8_t)atoi(myoptarg);
+			PX4_INFO("Setting sensor orientation to %d", (int)rotation);
+			break;
+
+		default:
+			PX4_WARN("Unknown option!");
+		}
+	}
+
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[myoptind], "start")) {
+		teraranger::start(rotation);
+	}
+
+	/*
+	 * Stop the driver
+	 */
+	if (!strcmp(argv[myoptind], "stop")) {
+		teraranger::stop();
+	}
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[myoptind], "test")) {
+		teraranger::test();
+	}
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[myoptind], "reset")) {
+		teraranger::reset();
+	}
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[1], "status")) {
+		teraranger::info();
+	}
+
+	PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
+	return PX4_ERROR;
+}

src/drivers/distance_sensor/tfmini/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__tfmini
+	MAIN tfmini
+	COMPILE_FLAGS
+		-Wno-sign-compare
+	SRCS
+		tfmini.cpp
+		tfmini_parser.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :

src/drivers/distance_sensor/tfmini/tfmini.cpp

@@ -0,0 +1,967 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file tfmini.cpp
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Greg Hulands
+ * @author Ayush Gaud <ayush.gaud@gmail.com>
+ *
+ * Driver for the Benewake TFmini laser rangefinder series
+ */
+
+#include <px4_config.h>
+#include <px4_workqueue.h>
+#include <px4_getopt.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 <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 <uORB/topics/distance_sensor.h>
+
+#include <board_config.h>
+
+#include "tfmini_parser.h"
+
+/* Configuration Constants */
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+// designated SERIAL4/5 on Pixhawk
+#define TFMINI_DEFAULT_PORT		"/dev/ttyS6"
+
+class TFMINI : public device::CDev
+{
+public:
+	TFMINI(const char *port = TFMINI_DEFAULT_PORT, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
+	virtual ~TFMINI();
+
+	virtual int 			init();
+
+	virtual ssize_t			read(device::file_t *filp, char *buffer, size_t buflen);
+	virtual int			ioctl(device::file_t *filp, int cmd, unsigned long arg);
+
+	/**
+	* Diagnostics - print some basic information about the driver.
+	*/
+	void				print_info();
+
+private:
+	char 				_port[20];
+	uint8_t 			_rotation;
+	float				_min_distance;
+	float				_max_distance;
+	int				_conversion_interval;
+	work_s				_work;
+	ringbuffer::RingBuffer		*_reports;
+	int				_measure_ticks;
+	bool				_collect_phase;
+	int				_fd;
+	char				_linebuf[10];
+	unsigned			_linebuf_index;
+	enum TFMINI_PARSE_STATE		_parse_state;
+
+	hrt_abstime			_last_read;
+
+	int				_class_instance;
+	int				_orb_class_instance;
+
+	orb_advert_t			_distance_sensor_topic;
+
+	unsigned			_consecutive_fail_count;
+
+	perf_counter_t			_sample_perf;
+	perf_counter_t			_comms_errors;
+
+	/**
+	* 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 TFMINI_MIN_DISTANCE
+	* and TFMINI_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				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 tfmini_main(int argc, char *argv[]);
+
+TFMINI::TFMINI(const char *port, uint8_t rotation) :
+	CDev("TFMINI", RANGE_FINDER0_DEVICE_PATH),
+	_rotation(rotation),
+	_min_distance(0.30f),
+	_max_distance(12.0f),
+	_conversion_interval(10000),
+	_reports(nullptr),
+	_measure_ticks(0),
+	_collect_phase(false),
+	_fd(-1),
+	_linebuf_index(0),
+	_parse_state(TFMINI_PARSE_STATE0_UNSYNC),
+	_last_read(0),
+	_class_instance(-1),
+	_orb_class_instance(-1),
+	_distance_sensor_topic(nullptr),
+	_consecutive_fail_count(0),
+	_sample_perf(perf_alloc(PC_ELAPSED, "tfmini_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "tfmini_com_err"))
+{
+	/* store port name */
+	strncpy(_port, port, sizeof(_port));
+	/* enforce null termination */
+	_port[sizeof(_port) - 1] = '\0';
+
+	/* open fd */
+	_fd = ::open(_port, O_RDWR | O_NOCTTY | O_SYNC);
+
+	/*baudrate 115200, 8 bits, no parity, 1 stop bit */
+	unsigned speed = B115200;
+
+	struct termios uart_config;
+
+	int termios_state;
+
+	tcgetattr(_fd, &uart_config);
+
+	/* clear ONLCR flag (which appends a CR for every LF) */
+	uart_config.c_oflag &= ~ONLCR;
+
+	/* 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);
+	}
+
+	uart_config.c_cflag |= (CLOCAL | CREAD);    /* ignore modem controls */
+	uart_config.c_cflag &= ~CSIZE;
+	uart_config.c_cflag |= CS8;         /* 8-bit characters */
+	uart_config.c_cflag &= ~PARENB;     /* no parity bit */
+	uart_config.c_cflag &= ~CSTOPB;     /* only need 1 stop bit */
+	uart_config.c_cflag &= ~CRTSCTS;    /* no hardware flowcontrol */
+
+	/* setup for non-canonical mode */
+	uart_config.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
+	uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
+	uart_config.c_oflag &= ~OPOST;
+
+	/* fetch bytes as they become available */
+	uart_config.c_cc[VMIN] = 1;
+	uart_config.c_cc[VTIME] = 1;
+
+	if (_fd < 0) {
+		warnx("FAIL: laser fd");
+	}
+
+	// disable debug() calls
+	_debug_enabled = false;
+
+	// work_cancel in the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
+}
+
+TFMINI::~TFMINI()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr) {
+		delete _reports;
+	}
+
+	if (_class_instance != -1) {
+		unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance);
+	}
+
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+}
+
+int
+TFMINI::init()
+{
+	int hw_model;
+	param_get(param_find("SENS_EN_TFMINI"), &hw_model);
+
+	switch (hw_model) {
+	case 0:
+		DEVICE_LOG("disabled.");
+		return 0;
+
+	case 1: /* TFMINI (12m, 100 Hz)*/
+		_min_distance = 0.3f;
+		_max_distance = 12.0f;
+		_conversion_interval =	10000;
+		break;
+
+	default:
+		DEVICE_LOG("invalid HW model %d.", hw_model);
+		return -1;
+	}
+
+	/* status */
+	int ret = 0;
+
+	do { /* create a scope to handle exit conditions using break */
+
+		/* do regular cdev init */
+		ret = CDev::init();
+
+		if (ret != OK) { break; }
+
+		/* allocate basic report buffers */
+		_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
+
+		if (_reports == nullptr) {
+			warnx("mem err");
+			ret = -1;
+			break;
+		}
+
+		_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
+
+		/* get a publish handle on the range finder topic */
+		struct distance_sensor_s ds_report = {};
+
+		_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
+					 &_orb_class_instance, ORB_PRIO_HIGH);
+
+		if (_distance_sensor_topic == nullptr) {
+			DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
+		}
+
+	} while (0);
+
+	/* close the fd */
+	::close(_fd);
+	_fd = -1;
+
+	return OK;
+}
+
+void
+TFMINI::set_minimum_distance(float min)
+{
+	_min_distance = min;
+}
+
+void
+TFMINI::set_maximum_distance(float max)
+{
+	_max_distance = max;
+}
+
+float
+TFMINI::get_minimum_distance()
+{
+	return _min_distance;
+}
+
+float
+TFMINI::get_maximum_distance()
+{
+	return _max_distance;
+}
+
+int
+TFMINI::ioctl(device::file_t *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(_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(_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;
+			}
+
+			ATOMIC_ENTER;
+
+			if (!_reports->resize(arg)) {
+				ATOMIC_LEAVE;
+				return -ENOMEM;
+			}
+
+			ATOMIC_LEAVE;
+
+			return OK;
+		}
+
+	case SENSORIOCRESET:
+		/* XXX implement this */
+		return -EINVAL;
+
+	default:
+		/* give it to the superclass */
+		return CDev::ioctl(filp, cmd, arg);
+	}
+}
+
+ssize_t
+TFMINI::read(device::file_t *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct distance_sensor_s);
+	struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(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();
+
+		/* wait for it to complete */
+		usleep(_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
+TFMINI::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);
+
+	/* the buffer for read chars is buflen minus null termination */
+	char readbuf[sizeof(_linebuf)];
+	unsigned readlen = sizeof(readbuf) - 1;
+
+	/* read from the sensor (uart buffer) */
+	ret = ::read(_fd, &readbuf[0], readlen);
+
+	if (ret < 0) {
+		DEVICE_DEBUG("read err: %d", ret);
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+
+		/* only throw an error if we time out */
+		if (read_elapsed > (_conversion_interval * 2)) {
+			return ret;
+
+		} else {
+			return -EAGAIN;
+		}
+
+	} else if (ret == 0) {
+		return -EAGAIN;
+	}
+
+	_last_read = hrt_absolute_time();
+
+	float distance_m = -1.0f;
+	bool valid = false;
+
+	for (int i = 0; i < ret; i++) {
+		if (OK == tfmini_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m)) {
+			valid = true;
+		}
+	}
+
+	if (!valid) {
+		return -EAGAIN;
+	}
+
+	DEVICE_DEBUG("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((valid) ? "OK" : "NO"));
+
+	struct distance_sensor_s report;
+
+	report.timestamp = hrt_absolute_time();
+	report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
+	report.orientation = _rotation;
+	report.current_distance = distance_m;
+	report.min_distance = get_minimum_distance();
+	report.max_distance = get_maximum_distance();
+	report.covariance = 0.0f;
+	/* TODO: set proper ID */
+	report.id = 0;
+
+	/* publish it */
+	orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
+
+	_reports->force(&report);
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	ret = OK;
+
+	perf_end(_sample_perf);
+	return ret;
+}
+
+void
+TFMINI::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)&TFMINI::cycle_trampoline, this, 1);
+
+	/* notify about state change */
+	struct subsystem_info_s info = {};
+	info.present = true;
+	info.enabled = true;
+	info.ok = true;
+	info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
+
+	static orb_advert_t pub = nullptr;
+
+	if (pub != nullptr) {
+		orb_publish(ORB_ID(subsystem_info), pub, &info);
+
+
+	} else {
+		pub = orb_advertise(ORB_ID(subsystem_info), &info);
+
+	}
+}
+
+void
+TFMINI::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+TFMINI::cycle_trampoline(void *arg)
+{
+	TFMINI *dev = static_cast<TFMINI *>(arg);
+
+	dev->cycle();
+}
+
+void
+TFMINI::cycle()
+{
+	/* fds initialized? */
+	if (_fd < 0) {
+		/* open fd */
+		_fd = ::open(TFMINI_DEFAULT_PORT, O_RDWR | O_NOCTTY | O_SYNC);
+	}
+
+	/* collection phase? */
+	if (_collect_phase) {
+
+		/* perform collection */
+		int collect_ret = collect();
+
+		if (collect_ret == -EAGAIN) {
+			/* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&TFMINI::cycle_trampoline,
+				   this,
+				   USEC2TICK(1042 * 8));
+			return;
+		}
+
+		if (OK != collect_ret) {
+
+			/* we know the sensor needs about four seconds to initialize */
+			if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {
+				DEVICE_LOG("collection error #%u", _consecutive_fail_count);
+			}
+
+			_consecutive_fail_count++;
+
+			/* restart the measurement state machine */
+			start();
+			return;
+
+		} else {
+			/* apparently success */
+			_consecutive_fail_count = 0;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+
+		/*
+		 * Is there a collect->measure gap?
+		 */
+		if (_measure_ticks > USEC2TICK(_conversion_interval)) {
+
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&TFMINI::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(_conversion_interval));
+
+			return;
+		}
+	}
+
+	/* next phase is collection */
+	_collect_phase = true;
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&TFMINI::cycle_trampoline,
+		   this,
+		   USEC2TICK(_conversion_interval));
+}
+
+void
+TFMINI::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	printf("poll interval:  %d ticks\n", _measure_ticks);
+	_reports->print_info("report queue");
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace tfmini
+{
+
+TFMINI	*g_dev;
+
+void	start(const char *port, uint8_t rotation);
+void	stop();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start(const char *port, uint8_t rotation)
+{
+	int fd;
+
+	if (g_dev != nullptr) {
+		errx(1, "already started");
+	}
+
+	/* create the driver */
+	g_dev = new TFMINI(port, rotation);
+
+	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_FINDER0_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 distance_sensor_s report;
+	ssize_t sz;
+
+	int fd = open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		err(1, "%s open failed (try 'tfmini start' if the driver is not running", RANGE_FINDER0_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.current_distance);
+	warnx("time: %llu", report.timestamp);
+
+	/* start the sensor polling at 2 Hz rate */
+	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) {
+			warnx("timed out");
+			break;
+		}
+
+		/* now go get it */
+		sz = read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report)) {
+			warnx("read failed: got %d vs exp. %d", sz, sizeof(report));
+			break;
+		}
+
+		warnx("read #%u", i);
+		warnx("valid %u", (float)report.current_distance > report.min_distance
+		      && (float)report.current_distance < report.max_distance ? 1 : 0);
+		warnx("measurement:  %0.3f m", (double)report.current_distance);
+		warnx("time: %llu", report.timestamp);
+	}
+
+	/* reset the sensor polling to the default rate */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
+		errx(1, "ERR: DEF RATE");
+	}
+
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(RANGE_FINDER0_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
+tfmini_main(int argc, char *argv[])
+{
+	// check for optional arguments
+	int ch;
+	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
+	int myoptind = 1;
+	const char *myoptarg = nullptr;
+
+
+	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'R':
+			rotation = (uint8_t)atoi(myoptarg);
+			PX4_INFO("Setting distance sensor orientation to %d", (int)rotation);
+			break;
+
+		default:
+			PX4_WARN("Unknown option!");
+		}
+	}
+
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[myoptind], "start")) {
+		if (argc > myoptind + 1) {
+			tfmini::start(argv[myoptind + 1], rotation);
+
+		} else {
+			tfmini::start(TFMINI_DEFAULT_PORT, rotation);
+		}
+	}
+
+	/*
+	 * Stop the driver
+	 */
+	if (!strcmp(argv[myoptind], "stop")) {
+		tfmini::stop();
+	}
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[myoptind], "test")) {
+		tfmini::test();
+	}
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[myoptind], "reset")) {
+		tfmini::reset();
+	}
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[1], "status")) {
+		tfmini::info();
+	}
+
+	PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
+	return PX4_ERROR;
+}

src/drivers/distance_sensor/tfmini/tfmini_parser.cpp

@@ -0,0 +1,180 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file modified from sf0x_parser.cpp
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Chuong Nguyen <chnguye7@asu.edu>
+ * @author Ayush Gaud <ayush.gaud@gmail.com>
+ *
+ * Declarations of parser for the Benewake TFmini laser rangefinder series
+ */
+
+#include "tfmini_parser.h"
+#include <string.h>
+#include <stdlib.h>
+
+// #define TFMINI_DEBUG
+
+#ifdef TFMINI_DEBUG
+#include <stdio.h>
+
+const char *parser_state[] = {
+	"0_UNSYNC",
+	"1_SYNC_1",
+	"2_SYNC_2",
+	"3_GOT_DIST_L",
+	"4_GOT_DIST_H",
+	"5_GOT_STRENGTH_L",
+	"6_GOT_STRENGTH_H",
+	"7_GOT_PRESERVED",
+	"8_GOT_QUALITY"
+	"9_GOT_CHECKSUM"
+};
+#endif
+
+int tfmini_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum TFMINI_PARSE_STATE *state, float *dist)
+{
+	int ret = -1;
+	//char *end;
+
+	switch (*state) {
+	case TFMINI_PARSE_STATE6_GOT_CHECKSUM:
+		if (c == 'Y') {
+			*state = TFMINI_PARSE_STATE1_SYNC_1;
+			parserbuf[*parserbuf_index] = c;
+			(*parserbuf_index)++;
+
+		} else {
+			*state = TFMINI_PARSE_STATE0_UNSYNC;
+		}
+
+		break;
+
+	case TFMINI_PARSE_STATE0_UNSYNC:
+		if (c == 'Y') {
+			*state = TFMINI_PARSE_STATE1_SYNC_1;
+			parserbuf[*parserbuf_index] = c;
+			(*parserbuf_index)++;
+		}
+
+		break;
+
+	case TFMINI_PARSE_STATE1_SYNC_1:
+		if (c == 'Y') {
+			*state = TFMINI_PARSE_STATE1_SYNC_2;
+			parserbuf[*parserbuf_index] = c;
+			(*parserbuf_index)++;
+
+		} else {
+			*state = TFMINI_PARSE_STATE0_UNSYNC;
+			*parserbuf_index = 0;
+		}
+
+		break;
+
+	case TFMINI_PARSE_STATE1_SYNC_2:
+		*state = TFMINI_PARSE_STATE2_GOT_DIST_L;
+		parserbuf[*parserbuf_index] = c;
+		(*parserbuf_index)++;
+
+		break;
+
+	case TFMINI_PARSE_STATE2_GOT_DIST_L:
+		*state = TFMINI_PARSE_STATE2_GOT_DIST_H;
+		parserbuf[*parserbuf_index] = c;
+		(*parserbuf_index)++;
+
+		break;
+
+	case TFMINI_PARSE_STATE2_GOT_DIST_H:
+		*state = TFMINI_PARSE_STATE3_GOT_STRENGTH_L;
+		parserbuf[*parserbuf_index] = c;
+		(*parserbuf_index)++;
+
+		break;
+
+	case TFMINI_PARSE_STATE3_GOT_STRENGTH_L:
+		*state = TFMINI_PARSE_STATE3_GOT_STRENGTH_H;
+		parserbuf[*parserbuf_index] = c;
+		(*parserbuf_index)++;
+
+		break;
+
+	case TFMINI_PARSE_STATE3_GOT_STRENGTH_H:
+		*state = TFMINI_PARSE_STATE4_GOT_RESERVED;
+		parserbuf[*parserbuf_index] = c;
+		(*parserbuf_index)++;
+
+		break;
+
+	case TFMINI_PARSE_STATE4_GOT_RESERVED:
+		*state = TFMINI_PARSE_STATE5_GOT_QUALITY;
+		parserbuf[*parserbuf_index] = c;
+		(*parserbuf_index)++;
+
+		break;
+
+	case TFMINI_PARSE_STATE5_GOT_QUALITY:
+		// Find the checksum
+		unsigned char cksm = 0;
+
+		for (int i = 0; i < 8; i++) {
+			cksm += parserbuf[i];
+		}
+
+		if (c == cksm) {
+			parserbuf[*parserbuf_index] = '\0';
+			unsigned int t1 = parserbuf[2];
+			unsigned int t2 = parserbuf[3];
+			t2 <<= 8;
+			t2 += t1;
+			*dist = ((float)t2) / 100;
+			*state = TFMINI_PARSE_STATE6_GOT_CHECKSUM;
+			*parserbuf_index = 0;
+			ret = 0;
+
+		} else {
+			*state = TFMINI_PARSE_STATE0_UNSYNC;
+			*parserbuf_index = 0;
+		}
+
+		break;
+	}
+
+#ifdef TFMINI_DEBUG
+	printf("state: TFMINI_PARSE_STATE%s\n", parser_state[*state]);
+#endif
+
+	return ret;
+}

src/drivers/distance_sensor/tfmini/tfmini_parser.h

@@ -0,0 +1,72 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file modified from sf0x_parser.cpp
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Chuong Nguyen <chnguye7@asu.edu>
+ * @author Ayush Gaud <ayush.gaud@gmail.com>
+ *
+ * Declarations of parser for the Benewake TFmini laser rangefinder series
+ */
+
+#pragma once
+
+// Data Format for Benewake TFmini
+// ===============================
+// 9 bytes total per message:
+// 1) 0x59
+// 2) 0x59
+// 3) Dist_L (low 8bit)
+// 4) Dist_H (high 8bit)
+// 5) Strength_L (low 8bit)
+// 6) Strength_H (high 8bit)
+// 7) Reserved bytes
+// 8) Original signal quality degree
+// 9) Checksum parity bit (low 8bit), Checksum = Byte1 + Byte2 +...+Byte8. This is only a low 8bit though
+
+
+enum TFMINI_PARSE_STATE {
+	TFMINI_PARSE_STATE0_UNSYNC = 0,
+	TFMINI_PARSE_STATE1_SYNC_1,
+	TFMINI_PARSE_STATE1_SYNC_2,
+	TFMINI_PARSE_STATE2_GOT_DIST_L,
+	TFMINI_PARSE_STATE2_GOT_DIST_H,
+	TFMINI_PARSE_STATE3_GOT_STRENGTH_L,
+	TFMINI_PARSE_STATE3_GOT_STRENGTH_H,
+	TFMINI_PARSE_STATE4_GOT_RESERVED,
+	TFMINI_PARSE_STATE5_GOT_QUALITY,
+	TFMINI_PARSE_STATE6_GOT_CHECKSUM
+};
+
+int tfmini_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum TFMINI_PARSE_STATE *state, float *dist);

src/drivers/distance_sensor/ulanding/CMakeLists.txt

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__ulanding
+	MAIN ulanding_radar
+	COMPILE_FLAGS
+	SRCS
+		ulanding.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :

src/drivers/distance_sensor/ulanding/ulanding.cpp

@@ -0,0 +1,574 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ulanding.cpp
+ * @author Jessica Stockham <jessica@aerotenna.com>
+ * @author Roman Bapst <roman@uaventure.com>
+ *
+ * Driver for the uLanding radar from Aerotenna
+ */
+
+#include <px4_config.h>
+#include <px4_getopt.h>
+#include <px4_workqueue.h>
+#include <px4_defines.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 <vector>
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+#include <drivers/device/ringbuffer.h>
+#include <stdio.h>
+#include <uORB/uORB.h>
+#include <termios.h>
+
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_range_finder.h>
+#include <drivers/device/device.h>
+
+#include <uORB/topics/distance_sensor.h>
+
+namespace ulanding
+{
+
+#define ULANDING_MIN_DISTANCE		0.315f
+#define ULANDING_MAX_DISTANCE		50.0f
+#define ULANDING_VERSION	1
+
+#if defined(__PX4_POSIX_OCPOC)
+#define RADAR_DEFAULT_PORT		"/dev/ttyS6"	// default uLanding port on OCPOC
+#else
+#define RADAR_DEFAULT_PORT		"/dev/ttyS2"	// telem2 on Pixhawk
+#endif
+
+#if ULANDING_VERSION == 1
+#define ULANDING_HDR		254
+#define BUF_LEN 		18
+#else
+#define ULANDING_HDR		72
+#define BUF_LEN 		9
+#endif
+
+/* assume standard deviation to be equal to sensor resolution.
+   Static bench tests have shown that the sensor ouput does
+   not vary if the unit is not moved. */
+#define SENS_VARIANCE 		0.045f * 0.045f
+
+
+extern "C" __EXPORT int ulanding_radar_main(int argc, char *argv[]);
+
+class Radar : public device::CDev
+{
+public:
+	Radar(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, const char *port = RADAR_DEFAULT_PORT);
+	virtual ~Radar();
+
+	virtual int 			init();
+
+	int				start();
+
+private:
+	uint8_t _rotation;
+	bool				_task_should_exit;
+	int 				_task_handle;
+	char 				_port[20];
+	int				_class_instance;
+	int				_orb_class_instance;
+	orb_advert_t			_distance_sensor_topic;
+
+	unsigned 	_head;
+	unsigned 	_tail;
+	uint8_t 	_buf[BUF_LEN];
+
+	static void task_main_trampoline(int argc, char *argv[]);
+	void task_main();
+
+	bool read_and_parse(uint8_t *buf, int len, float *range);
+
+	bool is_header_byte(uint8_t c) {return (c == ULANDING_HDR);};
+};
+
+namespace radar
+{
+Radar	*g_dev;
+}
+
+Radar::Radar(uint8_t rotation, const char *port) :
+	CDev("Radar", RANGE_FINDER0_DEVICE_PATH),
+	_rotation(rotation),
+	_task_should_exit(false),
+	_task_handle(-1),
+	_class_instance(-1),
+	_orb_class_instance(-1),
+	_distance_sensor_topic(nullptr),
+	_head(0),
+	_tail(0)
+
+{
+	/* store port name */
+	strncpy(_port, port, sizeof(_port));
+	/* enforce null termination */
+	_port[sizeof(_port) - 1] = '\0';
+
+	// disable debug() calls
+	_debug_enabled = false;
+
+	memset(&_buf[0], 0, sizeof(_buf));
+}
+
+Radar::~Radar()
+{
+
+	if (_class_instance != -1) {
+		unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance);
+	}
+
+	if (_task_handle != -1) {
+		/* task wakes up every 100ms or so at the longest */
+		_task_should_exit = true;
+
+		/* wait for a second for the task to quit at our request */
+		unsigned i = 0;
+
+		do {
+			/* wait 20ms */
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 50) {
+				px4_task_delete(_task_handle);
+				break;
+			}
+		} while (_task_handle != -1);
+	}
+
+	if (_class_instance != -1) {
+		unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance);
+	}
+
+	orb_unadvertise(_distance_sensor_topic);
+}
+
+int
+Radar::init()
+{
+	/* status */
+	int ret = 0;
+
+	do { /* create a scope to handle exit conditions using break */
+
+		ret = CDev::init();
+
+		if (ret != OK) {
+			PX4_WARN("vdev init failed");
+			break;
+		}
+
+		/* open fd */
+		int fd = ::open(_port, O_RDWR | O_NOCTTY);
+		PX4_INFO("passed open port");
+
+		if (fd < 0) {
+			PX4_WARN("failed to open serial device");
+			ret = 1;
+			break;
+		}
+
+		struct termios uart_config;
+
+		int termios_state;
+
+		/* fill the struct for the new configuration */
+		tcgetattr(fd, &uart_config);
+
+		/** Input flags - Turn off input processing
+		 *
+		 * convert break to null byte, no CR to NL translation,
+		 * no NL to CR translation, don't mark parity errors or breaks
+		 * no input parity check, don't strip high bit off,
+		 * no XON/XOFF software flow control
+		 *
+		 */
+
+		uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
+					 INLCR | PARMRK | INPCK | ISTRIP | IXON);
+
+		/** No line processing
+		 *
+		 * echo off, echo newline off, canonical mode off,
+		 * extended input processing off, signal chars off
+		 *
+		 */
+
+		uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
+
+		/* 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 = B115200;
+
+		/* set baud rate */
+		if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
+			PX4_WARN("ERR CFG: %d ISPD", termios_state);
+			ret = 1;
+			break;
+		}
+
+		if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
+			PX4_WARN("ERR CFG: %d OSPD\n", termios_state);
+			ret = 1;
+			break;
+		}
+
+		if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
+			PX4_WARN("ERR baud %d ATTR", termios_state);
+			ret = 1;
+			break;
+		}
+
+		::close(fd);
+
+		_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
+
+		struct distance_sensor_s ds_report = {};
+		ds_report.timestamp = hrt_absolute_time();
+		ds_report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_RADAR;
+		ds_report.orientation = _rotation;
+		ds_report.id = 0;
+		ds_report.current_distance = -1.0f;	// make evident that this range sample is invalid
+		ds_report.covariance = SENS_VARIANCE;
+
+		_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
+					 &_orb_class_instance, ORB_PRIO_HIGH);
+
+		if (_distance_sensor_topic == nullptr) {
+			DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
+			ret = 1;
+			break;
+		}
+
+	} while (0);
+
+	return ret;
+}
+
+void
+Radar::task_main_trampoline(int argc, char *argv[])
+{
+	radar::g_dev->task_main();
+}
+
+int
+Radar::start()
+{
+	ASSERT(_task_handle == -1);
+
+	/* start the task */
+	_task_handle = px4_task_spawn_cmd("radar",
+					  SCHED_DEFAULT,
+					  SCHED_PRIORITY_MAX - 30,
+					  800,
+					  (px4_main_t)&Radar::task_main_trampoline,
+					  nullptr);
+
+	if (_task_handle < 0) {
+		PX4_WARN("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+bool Radar::read_and_parse(uint8_t *buf, int len, float *range)
+{
+	bool ret = false;
+
+	// write new data into a ring buffer
+	for (int i = 0; i < len; i++) {
+
+		_head++;
+
+		if (_head >= BUF_LEN) {
+			_head = 0;
+		}
+
+		if (_tail == _head) {
+			_tail = (_tail == BUF_LEN - 1) ? 0 : _head + 1;
+		}
+
+		_buf[_head] = buf[i];
+	}
+
+	// check how many bytes are in the buffer, return if it's lower than the size of one package
+	int num_bytes = _head >= _tail ? (_head - _tail + 1) : (_head + 1 + BUF_LEN - _tail);
+
+	if (num_bytes < BUF_LEN / 3) {
+		PX4_DEBUG("not enough bytes");
+		return false;
+	}
+
+	int index = _head;
+	uint8_t no_header_counter = 0;	// counter for bytes which are non header bytes
+
+	// go through the buffer backwards starting from the newest byte
+	// if we find a header byte and the previous two bytes weren't header bytes
+	// then we found the newest package.
+	for (int i = 0; i < num_bytes; i++) {
+		if (is_header_byte(_buf[index])) {
+			if (no_header_counter >= BUF_LEN / 3 - 1) {
+				if (ULANDING_VERSION == 1) {
+					if (((_buf[index + 1] + _buf[index + 2] + _buf[index + 3] + _buf[index + 4])) != (_buf[index + 5])
+					    || (_buf[index + 1] <= 0)) {
+						ret = false;
+						break;
+					}
+
+					int raw = _buf[index + 3] * 256 + _buf[index + 2];
+
+					*range = ((float)(raw / 100.)); // raw is in cm, converts range to meters;
+
+				} else {
+					int raw = (_buf[index + 1] & 0x7F);
+					raw += ((_buf[index + 2] & 0x7F) << 7);
+
+					*range = ((float)raw) * 0.045f;
+				}
+
+				// set the tail to one after the index because we neglect
+				// any data before the one we just read
+				_tail = index == BUF_LEN - 1 ? 0 : index++;
+				ret = true;
+				break;
+
+			}
+
+			no_header_counter = 0;
+
+		} else {
+			no_header_counter++;
+		}
+
+		index--;
+
+		if (index < 0) {
+			index = BUF_LEN - 1;
+		}
+
+	}
+
+	return ret;
+}
+
+void
+Radar::task_main()
+{
+	int fd = ::open(_port, O_RDWR | O_NOCTTY);
+
+	if (fd < 0) {
+		PX4_WARN("serial port not open");
+	}
+
+	if (!isatty(fd)) {
+		PX4_WARN("not a serial device");
+	}
+
+
+	// we poll on data from the serial port
+	pollfd fds[1];
+	fds[0].fd = fd;
+	fds[0].events = POLLIN;
+
+	// read buffer
+	uint8_t buf[BUF_LEN];
+
+	while (!_task_should_exit) {
+		// wait for up to 100ms for data
+		int pret = ::poll(fds, (sizeof(fds) / sizeof(fds[0])), 100);
+
+		// timed out
+		if (pret == 0) {
+			continue;
+		}
+
+		if (pret < 0) {
+			PX4_DEBUG("radar serial port poll error");
+			// sleep a bit before next try
+			usleep(100000);
+			continue;
+		}
+
+		if (fds[0].revents & POLLIN) {
+			memset(&buf[0], 0, sizeof(buf));
+			int len = ::read(fd, &buf[0], sizeof(buf));
+
+			if (len <= 0) {
+				PX4_DEBUG("error reading radar");
+			}
+
+			float range = 0;
+
+			if (read_and_parse(&buf[0], len, &range)) {
+
+				struct distance_sensor_s report = {};
+				report.timestamp = hrt_absolute_time();
+				report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
+				report.orientation = _rotation;
+				report.current_distance = range;
+				report.current_distance = report.current_distance > ULANDING_MAX_DISTANCE ? ULANDING_MAX_DISTANCE :
+							  report.current_distance;
+				report.current_distance = report.current_distance < ULANDING_MIN_DISTANCE ? ULANDING_MIN_DISTANCE :
+							  report.current_distance;
+				report.min_distance = ULANDING_MIN_DISTANCE;
+				report.max_distance = ULANDING_MAX_DISTANCE;
+				report.covariance = SENS_VARIANCE;
+				report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_RADAR;
+				report.id = 0;
+
+				// publish radar data
+				orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
+			}
+
+		}
+	}
+
+	::close(fd);
+}
+
+int ulanding_radar_main(int argc, char *argv[])
+{
+	if (argc <= 1) {
+		PX4_WARN("not enough arguments, usage: start [device_path], stop, info ");
+		return 1;
+	}
+
+	// check for optional arguments
+	int ch;
+	int myoptind = 1;
+	const char *myoptarg = nullptr;
+	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
+
+
+	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'R':
+			rotation = (uint8_t)atoi(myoptarg);
+			PX4_INFO("Setting distance sensor orientation to %d", (int)rotation);
+			break;
+
+		default:
+			PX4_WARN("Unknown option!");
+		}
+	}
+
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[myoptind], "start")) {
+		if (radar::g_dev != nullptr) {
+			PX4_WARN("driver already started");
+			return 0;
+		}
+
+		if (argc > myoptind + 1) {
+			radar::g_dev = new Radar(rotation, argv[myoptind + 1]);
+
+		} else {
+			radar::g_dev = new Radar(rotation, RADAR_DEFAULT_PORT);
+		}
+
+		if (radar::g_dev == nullptr) {
+			PX4_ERR("failed to create instance of Radar");
+			return 1;
+		}
+
+		if (PX4_OK != radar::g_dev->init()) {
+			delete radar::g_dev;
+			radar::g_dev = nullptr;
+			return 1;
+		}
+
+		if (OK != radar::g_dev->start()) {
+			delete radar::g_dev;
+			radar::g_dev = nullptr;
+			return 1;
+		}
+
+		return 0;
+	}
+
+	/*
+	 * Stop the driver
+	 */
+	if (!strcmp(argv[myoptind], "stop")) {
+		if (radar::g_dev != nullptr) {
+			delete radar::g_dev;
+			radar::g_dev = nullptr;
+
+		} else {
+			PX4_WARN("driver not running");
+		}
+
+		return 0;
+	}
+
+	if (!strcmp(argv[myoptind], "info")) {
+		PX4_INFO("ulanding radar from Aerotenna");
+		PX4_INFO("min distance %.2f m", (double)ULANDING_MIN_DISTANCE);
+		PX4_INFO("max distance %.2f m", (double)ULANDING_MAX_DISTANCE);
+		PX4_INFO("update rate: 500 Hz");
+
+		return 0;
+
+	}
+
+	PX4_WARN("unrecognized arguments, try: start [device_path], stop, info ");
+	return 1;
+}
+}; //namespace