From 5e0ee580c6f11c48fb0ae7eeabbb2b5f1b0c8abb Mon Sep 17 00:00:00 2001
From: mcsauder <mcsauder@gmail.com>
Date: Mon, 2 Sep 2019 00:16:09 -0600
Subject: [PATCH] Refactor the terraranger driver.

---
 .../distance_sensor/teraranger/parameters.c   |   1 +
 .../distance_sensor/teraranger/teraranger.cpp | 886 +++++-------------
 2 files changed, 249 insertions(+), 638 deletions(-)

diff --git a/src/drivers/distance_sensor/teraranger/parameters.c b/src/drivers/distance_sensor/teraranger/parameters.c
index ed7c996719a0..56253fe7a98c 100644
--- a/src/drivers/distance_sensor/teraranger/parameters.c
+++ b/src/drivers/distance_sensor/teraranger/parameters.c
@@ -43,5 +43,6 @@
  * @value 2 TROne
  * @value 3 TREvo60m
  * @value 4 TREvo600Hz
+ * @value 5 TREvo3m
  */
 PARAM_DEFINE_INT32(SENS_EN_TRANGER, 0);
diff --git a/src/drivers/distance_sensor/teraranger/teraranger.cpp b/src/drivers/distance_sensor/teraranger/teraranger.cpp
index 14851308953f..56a2f2a1b311 100644
--- a/src/drivers/distance_sensor/teraranger/teraranger.cpp
+++ b/src/drivers/distance_sensor/teraranger/teraranger.cpp
@@ -38,105 +38,59 @@
  * Driver for the TeraRanger One range finders connected via I2C.
  */
 
+#include <drivers/device/i2c.h>
+#include <perf/perf_counter.h>
 #include <px4_config.h>
 #include <px4_defines.h>
 #include <px4_getopt.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <px4_module.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 <stdio.h>
-#include <math.h>
-#include <unistd.h>
-
-#include <perf/perf_counter.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 <uORB/uORB.h>
 
-#include <board_config.h>
+using namespace time_literals;
 
 /* Configuration Constants */
-#define TERARANGER_BUS_DEFAULT           PX4_I2C_BUS_EXPANSION
-#define TRONE_BASEADDR      0x30 /* 7-bit address */
-#define TREVO_BASEADDR      0x31 /* 7-bit address */
-#define TERARANGER_DEVICE_PATH   	"/dev/teraranger"
+#define TERARANGER_BUS_DEFAULT                  PX4_I2C_BUS_EXPANSION
+#define TERARANGER_DEVICE_PATH                  "/dev/teraranger"
+#define TERARANGER_ONE_BASEADDR                 0x30 // 7-bit address.
+#define TERARANGER_EVO_BASEADDR                 0x31 // 7-bit address.
 
 /* 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
 
-#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 TERARANGER_ONE_MAX_DISTANCE             (14.00f)
+#define TERARANGER_ONE_MIN_DISTANCE             (0.20f)
 
+#define TERARANGER_EVO_3M_MAX_DISTANCE          (3.0f)
+#define TERARANGER_EVO_3M_MIN_DISTANCE          (0.10f)
 
-/* Device limits */
-#define TRONE_MIN_DISTANCE (0.20f)
-#define TRONE_MAX_DISTANCE (14.00f)
-#define TREVO_60M_MIN_DISTANCE (0.50f)
-#define TREVO_60M_MAX_DISTANCE (60.0f)
-#define TREVO_600HZ_MIN_DISTANCE (0.75f)
-#define TREVO_600HZ_MAX_DISTANCE (8.0f)
+#define TERARANGER_EVO_60M_MAX_DISTANCE         (60.0f)
+#define TERARANGER_EVO_60M_MIN_DISTANCE         (0.50f)
+
+#define TERARANGER_EVO_600HZ_MAX_DISTANCE       (8.0f)
+#define TERARANGER_EVO_600HZ_MIN_DISTANCE       (0.75f)
 
-#define TERARANGER_CONVERSION_INTERVAL 50000 /* 50ms */
+#define TERARANGER_MEASUREMENT_INTERVAL         50_us
 
 class TERARANGER : public device::I2C, public px4::ScheduledWorkItem
 {
 public:
-	TERARANGER(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING,
-		   int bus = TERARANGER_BUS_DEFAULT, int address = TRONE_BASEADDR);
-	virtual ~TERARANGER();
+	TERARANGER(const int bus = TERARANGER_BUS_DEFAULT,
+		   const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING,
+		   const int address = TERARANGER_ONE_BASEADDR);
 
-	virtual int 		init() override;
+	virtual ~TERARANGER();
 
-	virtual ssize_t		read(device::file_t *filp, char *buffer, size_t buflen) override;
-	virtual int			ioctl(device::file_t *filp, int cmd, unsigned long arg) override;
+	virtual int init() override;
 
 	/**
 	* Diagnostics - print some basic information about the driver.
 	*/
-	void				print_info();
-
-protected:
-	virtual int			probe() override;
-
-private:
-	uint8_t _rotation;
-	float				_min_distance;
-	float				_max_distance;
-	ringbuffer::RingBuffer		*_reports;
-	bool				_sensor_ok;
-	uint8_t				_valid;
-	int				_measure_interval;
-	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);
+	void print_info();
 
 	/**
 	* Initialise the automatic measurement state machine and start it.
@@ -144,31 +98,57 @@ class TERARANGER : public device::I2C, public px4::ScheduledWorkItem
 	* @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();
+	void start();
 
 	/**
 	* Stop the automatic measurement state machine.
 	*/
-	void				stop();
+	void stop();
+
+protected:
+
+	virtual int probe() override;
+
+private:
 
 	/**
-	* 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
+	 * Collects the most recent sensor measurement data from the i2c bus.
+	 */
+	int collect();
+
+	/**
+	 * Sends an i2c measure command to the sensors.
+	 */
+	int measure();
+
+	/**
+	* 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.
 	*/
-	void				set_minimum_distance(float min);
-	void				set_maximum_distance(float max);
-	float				get_minimum_distance();
-	float				get_maximum_distance();
+	int probe_address(const uint8_t address);
 
 	/**
 	* Perform a poll cycle; collect from the previous measurement
 	* and start a new one.
 	*/
-	void					Run() override;
-	int					measure();
-	int					collect();
+	void Run() override;
+
+	bool _collect_phase{false};
+
+	int _orb_class_instance{-1};
 
+	uint8_t _orientation{0};
+
+	float _max_distance{0};
+	float _min_distance{0};
+
+	orb_advert_t _distance_sensor_topic{nullptr};
+
+	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, "tr1_comm_err")};
+	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, "tr1_read")};
 };
 
 static const uint8_t crc_table[] = {
@@ -209,27 +189,11 @@ static uint8_t crc8(uint8_t *p, uint8_t len)
 	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) :
+TERARANGER::TERARANGER(const int bus, const uint8_t orientation, const int address) :
 	I2C("TERARANGER", TERARANGER_DEVICE_PATH, bus, address, 100000),
 	ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())),
-	_rotation(rotation),
-	_min_distance(-1.0f),
-	_max_distance(-1.0f),
-	_reports(nullptr),
-	_sensor_ok(false),
-	_valid(0),
-	_measure_interval(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"))
+	_orientation(orientation)
 {
 	// up the retries since the device misses the first measure attempts
 	I2C::_retries = 3;
@@ -237,125 +201,173 @@ TERARANGER::TERARANGER(uint8_t rotation, int bus, int address) :
 
 TERARANGER::~TERARANGER()
 {
-	/* make sure we are truly inactive */
+	// Ensure 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);
+	// Unadvertise the distance sensor topic.
+	if (_distance_sensor_topic != nullptr) {
+		orb_unadvertise(_distance_sensor_topic);
 	}
 
-	// free perf counters
+	// Free perf counters.
 	perf_free(_sample_perf);
 	perf_free(_comms_errors);
 }
 
+int
+TERARANGER::collect()
+{
+	if (!_collect_phase) {
+		return measure();
+	}
+
+	perf_begin(_sample_perf);
+
+	uint8_t val[3] = {};
+
+	// Transfer data from the bus.
+	int ret_val = transfer(nullptr, 0, &val[0], 3);
+
+	if (ret_val < 0) {
+		PX4_ERR("error reading from sensor: %d", ret_val);
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+		return ret_val;
+	}
+
+	uint16_t distance_mm = (val[0] << 8) | val[1];
+	float distance_m = static_cast<float>(distance_mm) * 1e-3f;
+
+	distance_sensor_s report {};
+	report.current_distance = distance_m;
+	report.id               = 0; // TODO: set proper ID.
+	report.min_distance     = _min_distance;
+	report.max_distance     = _max_distance;
+	report.orientation      = _orientation;
+	report.signal_quality   = -1;
+	report.timestamp        = hrt_absolute_time();
+	// NOTE: There is no enum item for a combined LASER and ULTRASOUND which this should be.
+	report.type             = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
+	report.variance         = 0.0f;
+
+	if (crc8(val, 2) == val[2] &&
+	    (float)report.current_distance > report.min_distance &&
+	    (float)report.current_distance < report.max_distance) {
+		int instance_id;
+		orb_publish_auto(ORB_ID(distance_sensor), &_distance_sensor_topic, &report, &instance_id, ORB_PRIO_DEFAULT);
+	}
+
+	// Next phase is measurement.
+	_collect_phase = false;
+
+	perf_count(_sample_perf);
+	perf_end(_sample_perf);
+	return PX4_OK;
+}
+
 int
 TERARANGER::init()
 {
-	int ret = PX4_ERROR;
-	int hw_model;
+	int hw_model = 0;
 	param_get(param_find("SENS_EN_TRANGER"), &hw_model);
 
 	switch (hw_model) {
-	case 0: /* Disabled */
+	case 0: // Disabled
 		PX4_WARN("Disabled");
-		return ret;
+		return PX4_ERROR;
 
-	case 1: /* Autodetect */
-		/* Assume TROne */
-		set_device_address(TRONE_BASEADDR);
+	case 1: // Autodetect - assume default Teraranger One
+		set_device_address(TERARANGER_ONE_BASEADDR);
 
 		if (I2C::init() != OK) {
-			set_device_address(TREVO_BASEADDR);
+			set_device_address(TERARANGER_EVO_BASEADDR);
 
 			if (I2C::init() != OK) {
-				goto out;
+				return PX4_ERROR;
 
 			} else {
-				_min_distance = TREVO_60M_MIN_DISTANCE;
-				_max_distance = TREVO_60M_MAX_DISTANCE;
+				// Assume minimum and maximum possible distances acros Evo family
+				_min_distance = TERARANGER_EVO_3M_MIN_DISTANCE;
+				_max_distance = TERARANGER_EVO_60M_MAX_DISTANCE;
 			}
 
 		} else {
-			_min_distance = TRONE_MIN_DISTANCE;
-			_max_distance = TRONE_MAX_DISTANCE;
+			_min_distance = TERARANGER_ONE_MIN_DISTANCE;
+			_max_distance = TERARANGER_ONE_MAX_DISTANCE;
 		}
 
 		break;
 
-	case 2: /* TROne */
-		set_device_address(TRONE_BASEADDR);
+	case 2: // Teraranger One.
+		set_device_address(TERARANGER_ONE_BASEADDR);
 
 		if (I2C::init() != OK) {
-			goto out;
+			return PX4_ERROR;
 		}
 
-		_min_distance = TRONE_MIN_DISTANCE;
-		_max_distance = TRONE_MAX_DISTANCE;
+		_min_distance = TERARANGER_ONE_MIN_DISTANCE;
+		_max_distance = TERARANGER_ONE_MAX_DISTANCE;
 		break;
 
-	case 3: /* TREvo60m */
-		set_device_address(TREVO_BASEADDR);
+	case 3: // Teraranger Evo60m.
+		set_device_address(TERARANGER_EVO_BASEADDR);
 
-		/* do I2C init (and probe) first */
+		// I2C init (and probe) first.
 		if (I2C::init() != OK) {
-			goto out;
+			return PX4_ERROR;
 		}
 
-		_min_distance = TREVO_60M_MIN_DISTANCE;
-		_max_distance = TREVO_60M_MAX_DISTANCE;
+		_min_distance = TERARANGER_EVO_60M_MIN_DISTANCE;
+		_max_distance = TERARANGER_EVO_60M_MAX_DISTANCE;
 		break;
 
-	case 4: /* TREvo600Hz */
-		set_device_address(TREVO_BASEADDR);
+	case 4: // Teraranger Evo600Hz.
+		set_device_address(TERARANGER_EVO_BASEADDR);
 
-		/* do I2C init (and probe) first */
+		// I2C init (and probe) first.
 		if (I2C::init() != OK) {
-			goto out;
+			return PX4_ERROR;
 		}
 
-		_min_distance = TREVO_600HZ_MIN_DISTANCE;
-		_max_distance = TREVO_600HZ_MAX_DISTANCE;
+		_min_distance = TERARANGER_EVO_600HZ_MIN_DISTANCE;
+		_max_distance = TERARANGER_EVO_600HZ_MAX_DISTANCE;
 		break;
 
-	default:
-		PX4_ERR("invalid HW model %d.", hw_model);
-		return ret;
-	}
+	case 5: // Teraranger Evo3m.
+		set_device_address(TERARANGER_EVO_BASEADDR);
 
-	/* allocate basic report buffers */
-	_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
+		// I2C init (and probe) first.
+		if (I2C::init() != OK) {
+			return PX4_ERROR;
+		}
 
-	if (_reports == nullptr) {
-		goto out;
-	}
+		_min_distance = TERARANGER_EVO_3M_MIN_DISTANCE;
+		_max_distance = TERARANGER_EVO_3M_MAX_DISTANCE;
+		break;
 
-	_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
+	default:
+		PX4_ERR("invalid HW model %d.", hw_model);
+		return PX4_ERROR;
+	}
 
-	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);
+	return PX4_OK;
+}
 
-		_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
-					 &_orb_class_instance, ORB_PRIO_LOW);
+int
+TERARANGER::measure()
+{
+	// Send the command to begin a measurement.
+	const uint8_t cmd = TERARANGER_MEASURE_REG;
+	int ret_val = transfer(&cmd, sizeof(cmd), nullptr, 0);
 
-		if (_distance_sensor_topic == nullptr) {
-			PX4_ERR("failed to create distance_sensor object");
-		}
+	if (ret_val != PX4_OK) {
+		perf_count(_comms_errors);
+		PX4_DEBUG("i2c::transfer returned %d", ret_val);
+		return ret_val;
 	}
 
-	ret = OK;
-	/* sensor is ok, but we don't really know if it is within range */
-	_sensor_ok = true;
-out:
-	return ret;
+	_collect_phase = true;
+	return PX4_OK;
 }
 
 int
@@ -365,7 +377,7 @@ TERARANGER::probe()
 
 	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
+	// 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();
@@ -376,240 +388,32 @@ TERARANGER::probe()
 		  (unsigned)who_am_i,
 		  TERARANGER_WHO_AM_I_REG_VAL);
 
-	// not found on any address
+	// Not found on any address.
 	return -EIO;
 }
 
 void
-TERARANGER::set_minimum_distance(float min)
+TERARANGER::print_info()
 {
-	_min_distance = min;
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	printf("poll interval:  %u\n", TERARANGER_MEASUREMENT_INTERVAL);
 }
 
 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) {
-
-			/* zero would be bad */
-			case 0:
-				return -EINVAL;
-
-			/* set default polling rate */
-			case SENSOR_POLLRATE_DEFAULT: {
-					/* do we need to start internal polling? */
-					bool want_start = (_measure_interval == 0);
-
-					/* set interval for next measurement to minimum legal value */
-					_measure_interval = (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_interval == 0);
-
-					/* convert hz to tick interval via microseconds */
-					int interval = (1000000 / arg);
-
-					/* check against maximum rate */
-					if (interval < (TERARANGER_CONVERSION_INTERVAL)) {
-						return -EINVAL;
-					}
-
-					/* update interval for next measurement */
-					_measure_interval = interval;
-
-					/* if we need to start the poll state machine, do it */
-					if (want_start) {
-						start();
-					}
-
-					return OK;
-				}
-			}
-		}
-
-	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_interval > 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 */
-		px4_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);
-		PX4_DEBUG("i2c::transfer returned %d", ret);
-		return ret;
-	}
-
-	ret = OK;
-
-	return ret;
-}
-
-int
-TERARANGER::collect()
+TERARANGER::Run()
 {
-	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) {
-		PX4_DEBUG("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.variance = 0.0f;
-	report.signal_quality = -1;
-	/* 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;
+	// Perform data collection.
+	collect();
 }
 
 void
 TERARANGER::start()
 {
-	/* reset the report ring and state machine */
 	_collect_phase = false;
-	_reports->flush();
 
-	/* schedule a cycle to start things */
-	ScheduleNow();
+	// Schedule the driver to run on a set interval
+	ScheduleOnInterval(TERARANGER_MEASUREMENT_INTERVAL);
 }
 
 void
@@ -618,69 +422,20 @@ TERARANGER::stop()
 	ScheduleClear();
 }
 
-void
-TERARANGER::Run()
-{
-	/* collection phase? */
-	if (_collect_phase) {
-
-		/* perform collection */
-		if (OK != collect()) {
-			PX4_DEBUG("collection error");
-			/* restart the measurement state machine */
-			start();
-			return;
-		}
-
-		/* next phase is measurement */
-		_collect_phase = false;
-
-		/*
-		 * Is there a collect->measure gap?
-		 */
-		if (_measure_interval > TERARANGER_CONVERSION_INTERVAL) {
-			/* schedule a fresh cycle call when we are ready to measure again */
-			ScheduleDelayed(_measure_interval - TERARANGER_CONVERSION_INTERVAL);
-
-			return;
-		}
-	}
-
-	/* measurement phase */
-	if (OK != measure()) {
-		PX4_DEBUG("measure error");
-	}
-
-	/* next phase is collection */
-	_collect_phase = true;
-
-	/* schedule a fresh cycle call when the measurement is done */
-	ScheduleDelayed(TERARANGER_CONVERSION_INTERVAL);
-}
-
-void
-TERARANGER::print_info()
-{
-	perf_print_counter(_sample_perf);
-	perf_print_counter(_comms_errors);
-	printf("poll interval:  %u\n", _measure_interval);
-	_reports->print_info("report queue");
-}
-
 /**
  * Local functions in support of the shell command.
  */
 namespace teraranger
 {
 
-TERARANGER	*g_dev;
+TERARANGER *g_dev;
 
-int 	start(uint8_t rotation);
-int 	start_bus(uint8_t rotation, int i2c_bus);
-int 	stop();
-int 	test();
-int 	reset();
-int 	info();
+int start(const uint8_t orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
+int start_bus(const int i2c_bus = TERARANGER_BUS_DEFAULT,
+	      const uint8_t orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
+int status();
+int stop();
+int usage();
 
 /**
  *
@@ -692,15 +447,15 @@ int 	info();
  *
  */
 int
-start(uint8_t rotation)
+start(const uint8_t orientation)
 {
 	if (g_dev != nullptr) {
 		PX4_ERR("already started");
 		return PX4_ERROR;
 	}
 
-	for (unsigned i = 0; i < NUM_I2C_BUS_OPTIONS; i++) {
-		if (start_bus(rotation, i2c_bus_options[i]) == PX4_OK) {
+	for (size_t i = 0; i < NUM_I2C_BUS_OPTIONS; i++) {
+		if (start_bus(i2c_bus_options[i], orientation) == PX4_OK) {
 			return PX4_OK;
 		}
 	}
@@ -715,53 +470,33 @@ start(uint8_t rotation)
  * or could not be detected successfully.
  */
 int
-start_bus(uint8_t rotation, int i2c_bus)
+start_bus(const int i2c_bus, const uint8_t orientation)
 {
-	int fd = -1;
-
 	if (g_dev != nullptr) {
 		PX4_ERR("already started");
-		return PX4_ERROR;
+		return PX4_OK;
 	}
 
-	/* create the driver */
-	g_dev = new TERARANGER(rotation, i2c_bus);
-
+	// Instantiate the driver.
+	g_dev = new TERARANGER(i2c_bus, orientation);
 
 	if (g_dev == nullptr) {
-		goto fail;
-	}
-
-	if (OK != g_dev->init()) {
-		goto fail;
-	}
-
-	/* set the poll rate to default, starts automatic data collection */
-	fd = px4_open(TERARANGER_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0) {
-		goto fail;
-	}
-
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
-		goto fail;
-	}
-
-	px4_close(fd);
-	return PX4_OK;
-
-fail:
-
-	if (fd >= 0) {
-		px4_close(fd);
+		delete g_dev;
+		return PX4_ERROR;
 	}
 
-	if (g_dev != nullptr) {
+	// Initialize the sensor.
+	if (g_dev->init() != PX4_OK) {
 		delete g_dev;
 		g_dev = nullptr;
+		return PX4_ERROR;
 	}
 
-	return PX4_ERROR;
+	// Start the driver.
+	g_dev->start();
+
+	PX4_INFO("driver started");
+	return PX4_OK;
 }
 
 /**
@@ -774,135 +509,30 @@ stop()
 		delete g_dev;
 		g_dev = nullptr;
 
-	} else {
-		PX4_ERR("driver not running");
-		return PX4_ERROR;
 	}
 
+	PX4_INFO("driver stopped");
 	return PX4_OK;
 }
 
 /**
- * Perform some basic functional tests on the driver;
- * make sure we can collect data from the sensor in polled
- * and automatic modes.
+ * Print the driver status.
  */
 int
-test()
-{
-	struct distance_sensor_s report;
-	ssize_t sz;
-	int ret;
-
-	int fd = px4_open(TERARANGER_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0) {
-		PX4_ERR("%s open failed (try 'teraranger start' if the driver is not running)", TERARANGER_DEVICE_PATH);
-		return PX4_ERROR;
-	}
-
-	/* do a simple demand read */
-	sz = read(fd, &report, sizeof(report));
-
-	if (sz != sizeof(report)) {
-		PX4_ERR("immediate read failed");
-		return PX4_ERROR;
-	}
-
-	print_message(report);
-
-	/* start the sensor polling at 2Hz */
-	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
-		PX4_ERR("failed to set 2Hz poll rate");
-		return PX4_ERROR;
-	}
-
-	/* 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) {
-			PX4_ERR("timed out waiting for sensor data");
-			return PX4_ERROR;
-		}
-
-		/* now go get it */
-		sz = read(fd, &report, sizeof(report));
-
-		if (sz != sizeof(report)) {
-			PX4_ERR("periodic read failed");
-			return PX4_ERROR;
-		}
-
-		print_message(report);
-	}
-
-	/* reset the sensor polling to default rate */
-	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
-		PX4_ERR("failed to set default poll rate");
-		return PX4_ERROR;
-	}
-
-	px4_close(fd);
-	PX4_INFO("PASS");
-	return PX4_OK;
-
-}
-
-/**
- * Reset the driver.
- */
-int
-reset()
-{
-	int fd = px4_open(TERARANGER_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0) {
-		PX4_ERR("failed");
-		return PX4_ERROR;
-	}
-
-	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
-		PX4_ERR("driver reset failed");
-		return PX4_ERROR;
-	}
-
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
-		PX4_ERR("driver poll restart failed");
-		return PX4_ERROR;
-	}
-
-	px4_close(fd);
-	return PX4_OK;
-}
-
-/**
- * Print a little info about the driver.
- */
-int
-info()
+status()
 {
 	if (g_dev == nullptr) {
 		PX4_ERR("driver not running");
 		return PX4_ERROR;
 	}
 
-	printf("state @ %p\n", g_dev);
 	g_dev->print_info();
 
 	return PX4_OK;
 }
 
-} // namespace
-
-
-static void
-teraranger_usage()
+int
+usage()
 {
 	PRINT_MODULE_DESCRIPTION(
 		R"DESCR_STR(
@@ -915,7 +545,6 @@ The sensor/driver must be enabled using the parameter SENS_EN_TRANGER.
 Setup/usage information: https://docs.px4.io/en/sensor/rangefinders.html#teraranger-rangefinders
 
 ### Examples
-
 Start driver on any bus (start on bus where first sensor found).
 $ teraranger start -a
 Start driver on specified bus
@@ -931,91 +560,72 @@ Stop driver
 	PRINT_MODULE_USAGE_PARAM_INT('b', 1, 1, 2000, "Start driver on specific I2C bus", true);
 	PRINT_MODULE_USAGE_PARAM_INT('R', 25, 1, 25, "Sensor rotation - downward facing by default", true);
 	PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("test","Test driver (basic functional tests)");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("reset","Reset driver");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("info","Print driver information");
-
-
+	PRINT_MODULE_USAGE_COMMAND_DESCR("status","Print driver information");
 
+	return PX4_OK;
 }
 
-int
-teraranger_main(int argc, char *argv[])
+} // namespace
+
+/**
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int teraranger_main(int argc, char *argv[])
 {
+	const char *myoptarg = nullptr;
+
 	int ch;
+	int i2c_bus = TERARANGER_BUS_DEFAULT;
 	int myoptind = 1;
-	const char *myoptarg = nullptr;
+
 	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
-	bool start_all = false;
 
-	int i2c_bus = TERARANGER_BUS_DEFAULT;
+	bool start_all = false;
 
 	while ((ch = px4_getopt(argc, argv, "ab:R:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
-		case 'R':
-			rotation = (uint8_t)atoi(myoptarg);
+		case 'a':
+			start_all = true;
 			break;
 
 		case 'b':
 			i2c_bus = atoi(myoptarg);
 			break;
 
-		case 'a':
-			start_all = true;
+		case 'R':
+			rotation = (uint8_t)atoi(myoptarg);
 			break;
 
 		default:
 			PX4_WARN("Unknown option!");
-			goto out_error;
+			return teraranger::usage();
 		}
 	}
 
 	if (myoptind >= argc) {
-		goto out_error;
+		return teraranger::usage();
 	}
 
-	/*
-	 * Start/load the driver.
-	 */
+	// Start/load the driver.
 	if (!strcmp(argv[myoptind], "start")) {
 
 		if (start_all) {
 			return teraranger::start(rotation);
 
 		} else {
-			return teraranger::start_bus(rotation, i2c_bus);
+			return teraranger::start_bus(i2c_bus, rotation);
 		}
 	}
 
-	/*
-	 * Stop the driver
-	 */
-	if (!strcmp(argv[myoptind], "stop")) {
-		return teraranger::stop();
-	}
-
-	/*
-	 * Test the driver/device.
-	 */
-	if (!strcmp(argv[myoptind], "test")) {
-		return teraranger::test();
-	}
-
-	/*
-	 * Reset the driver.
-	 */
-	if (!strcmp(argv[myoptind], "reset")) {
-		return teraranger::reset();
+	// Print the driver status.
+	if (!strcmp(argv[myoptind], "status")) {
+		return teraranger::status();
 	}
 
-	/*
-	 * Print driver information.
-	 */
-	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
-		return teraranger::info();
+	// Stop the driver.
+	if (!strcmp(argv[myoptind], "stop")) {
+		return teraranger::stop();
 	}
 
-out_error:
-	teraranger_usage();
-	return PX4_ERROR;
+	return teraranger::usage();
 }