sf0x: move to PX4Rangefinder and cleanup

src/drivers/distance_sensor/sf0x/CMakeLists.txt

@@ -40,6 +40,9 @@ px4_add_module(
 		SF0X.hpp
 		sf0x_main.cpp
 		sf0x_parser.cpp
+	DEPENDS
+		drivers_rangefinder
+		px4_work_queue
 	MODULE_CONFIG
 		module.yaml
 	)

src/drivers/distance_sensor/sf0x/SF0X.cpp

@@ -33,26 +33,16 @@
 
 #include "SF0X.hpp"
 
+#include <termios.h>
+
+/* Configuration Constants */
+#define SF0X_TAKE_RANGE_REG		'd'
+
 SF0X::SF0X(const char *port, uint8_t rotation) :
-	CDev(RANGE_FINDER0_DEVICE_PATH),
 	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
-	_rotation(rotation),
+	_px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, rotation),
-	_min_distance(0.30f),
+	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
-	_max_distance(40.0f),
+	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err"))
-	_conversion_interval(83334),
-	_reports(nullptr),
-	_measure_interval(0),
-	_collect_phase(false),
-	_fd(-1),
-	_linebuf_index(0),
-	_parse_state(SF0X_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, "sf0x_read")),
-	_comms_errors(perf_alloc(PC_COUNT, "sf0x_com_err"))
 {
 	/* store port name */
 	strncpy(_port, port, sizeof(_port) - 1);
@@ -60,22 +50,13 @@ SF0X::SF0X(const char *port, uint8_t rotation) :
 	/* enforce null termination */
 	_port[sizeof(_port) - 1] = '\0';
 
+	_px4_rangefinder.set_device_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER);
 }
 
 SF0X::~SF0X()
 {
-	/* 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);
 }
@@ -83,40 +64,39 @@ SF0X::~SF0X()
 int
 SF0X::init()
 {
-	int hw_model;
+	int32_t hw_model = 0;
 	param_get(param_find("SENS_EN_SF0X"), &hw_model);
 
 	switch (hw_model) {
-
 	case 1: /* SF02 (40m, 12 Hz)*/
-		_min_distance = 0.3f;
+		_px4_rangefinder.set_min_distance(0.30f);
-		_max_distance = 40.0f;
+		_px4_rangefinder.set_max_distance(40.0f);
-		_conversion_interval =	83334;
+		_interval = 83334;
 		break;
 
 	case 2:  /* SF10/a (25m 32Hz) */
-		_min_distance = 0.01f;
+		_px4_rangefinder.set_min_distance(0.01f);
-		_max_distance = 25.0f;
+		_px4_rangefinder.set_max_distance(25.0f);
-		_conversion_interval = 31250;
+		_interval = 31250;
 		break;
 
 	case 3:  /* SF10/b (50m 32Hz) */
-		_min_distance = 0.01f;
+		_px4_rangefinder.set_min_distance(0.01f);
-		_max_distance = 50.0f;
+		_px4_rangefinder.set_max_distance(50.0f);
-		_conversion_interval = 31250;
+		_interval = 31250;
 		break;
 
 	case 4:  /* SF10/c (100m 16Hz) */
-		_min_distance = 0.01f;
+		_px4_rangefinder.set_min_distance(0.01f);
-		_max_distance = 100.0f;
+		_px4_rangefinder.set_max_distance(100.0f);
-		_conversion_interval = 62500;
+		_interval = 62500;
 		break;
 
 	case 5:
 		/* SF11/c (120m 20Hz) */
-		_min_distance = 0.01f;
+		_px4_rangefinder.set_min_distance(0.01f);
-		_max_distance = 120.0f;
+		_px4_rangefinder.set_max_distance(120.0f);
-		_conversion_interval = 50000;
+		_interval = 50000;
 		break;
 
 	default:
@@ -124,197 +104,16 @@ SF0X::init()
 		return -1;
 	}
 
-	/* status */
+	start();
-	int ret = 0;
 
-	do { /* create a scope to handle exit conditions using break */
+	return PX4_OK;
-
-		/* 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) {
-			PX4_ERR("alloc failed");
-			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) {
-			PX4_ERR("failed to create distance_sensor object");
-		}
-
-	} while (0);
-
-	return ret;
 }
 
-void
+int SF0X::measure()
-SF0X::set_minimum_distance(float min)
 {
-	_min_distance = min;
+	// Send the command to begin a measurement.
-}
-
-void
-SF0X::set_maximum_distance(float max)
-{
-	_max_distance = max;
-}
-
-float
-SF0X::get_minimum_distance()
-{
-	return _min_distance;
-}
-
-float
-SF0X::get_maximum_distance()
-{
-	return _max_distance;
-}
-
-int
-SF0X::ioctl(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 = (_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 < (_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 CDev::ioctl(filp, cmd, arg);
-	}
-}
-
-ssize_t
-SF0X::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(_conversion_interval);
-
-		/* run the collection phase */
-		if (OK != collect()) {
-			ret = -EIO;
-			break;
-		}
-
-		/* state machine will have generated a report, copy it out */
-		if (_reports->get(rbuf)) {
-			ret = sizeof(*rbuf);
-		}
-
-	} while (0);
-
-	return ret;
-}
-
-int
-SF0X::measure()
-{
-	int ret;
-
-	/*
-	 * Send the command to begin a measurement.
-	 */
 	char cmd = SF0X_TAKE_RANGE_REG;
-	ret = ::write(_fd, &cmd, 1);
+	int ret = ::write(_fd, &cmd, 1);
 
 	if (ret != sizeof(cmd)) {
 		perf_count(_comms_errors);
@@ -322,13 +121,10 @@ SF0X::measure()
 		return ret;
 	}
 
-	ret = OK;
+	return PX4_OK;
-
-	return ret;
 }
 
-int
+int SF0X::collect()
-SF0X::collect()
 {
 	perf_begin(_sample_perf);
 
@@ -340,6 +136,7 @@ SF0X::collect()
 	unsigned readlen = sizeof(readbuf) - 1;
 
 	/* read from the sensor (uart buffer) */
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
 	int ret = ::read(_fd, &readbuf[0], readlen);
 
 	if (ret < 0) {
@@ -348,7 +145,7 @@ SF0X::collect()
 		perf_end(_sample_perf);
 
 		/* only throw an error if we time out */
-		if (read_elapsed > (_conversion_interval * 2)) {
+		if (read_elapsed > (_interval * 2)) {
 			return ret;
 
 		} else {
@@ -376,52 +173,28 @@ SF0X::collect()
 
 	PX4_DEBUG("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((valid) ? "OK" : "NO"));
 
-	struct distance_sensor_s report;
+	_px4_rangefinder.update(timestamp_sample, distance_m);
-
-	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.variance = 0.0f;
-	report.signal_quality = -1;
-	/* 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;
+
+	return PX4_OK;
 }
 
-void
+void SF0X::start()
-SF0X::start()
 {
 	/* reset the report ring and state machine */
 	_collect_phase = false;
-	_reports->flush();
 
 	/* schedule a cycle to start things */
 	ScheduleNow();
 }
 
-void
+void SF0X::stop()
-SF0X::stop()
 {
 	ScheduleClear();
 }
 
-void
+void SF0X::Run()
-SF0X::Run()
 {
 	/* fds initialized? */
 	if (_fd < 0) {
@@ -447,7 +220,7 @@ SF0X::Run()
 		uart_config.c_cflag &= ~(CSTOPB | PARENB);
 
 		/* if distance sensor model is SF11/C, then set baudrate 115200, else 9600 */
-		int hw_model;
+		int32_t hw_model = 0;
 
 		param_get(param_find("SENS_EN_SF0X"), &hw_model);
 
@@ -507,17 +280,6 @@ SF0X::Run()
 
 		/* next phase is measurement */
 		_collect_phase = false;
-
-		/*
-		 * Is there a collect->measure gap?
-		 */
-		if (_measure_interval > (_conversion_interval)) {
-
-			/* schedule a fresh cycle call when we are ready to measure again */
-			ScheduleDelayed(_measure_interval - _conversion_interval);
-
-			return;
-		}
 	}
 
 	/* measurement phase */
@@ -529,14 +291,13 @@ SF0X::Run()
 	_collect_phase = true;
 
 	/* schedule a fresh cycle call when the measurement is done */
-	ScheduleDelayed(_conversion_interval);
+	ScheduleDelayed(_interval);
 }
 
-void
+void SF0X::print_info()
-SF0X::print_info()
 {
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
-	printf("poll interval:  %d\n", _measure_interval);
+
-	_reports->print_info("report queue");
+	_px4_rangefinder.print_status();
 }

src/drivers/distance_sensor/sf0x/SF0X.hpp

@@ -43,111 +43,47 @@
 
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-
+#include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
-#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 <perf/perf_counter.h>
-
 #include <drivers/drv_hrt.h>
-#include <drivers/drv_range_finder.h>
-#include <drivers/device/device.h>
-#include <drivers/device/ringbuffer.h>
 #include <lib/parameters/param.h>
-
+#include <lib/perf/perf_counter.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/distance_sensor.h>
 
 #include "sf0x_parser.h"
 
-/* Configuration Constants */
+class SF0X : public px4::ScheduledWorkItem
-
-#define SF0X_TAKE_RANGE_REG		'd'
-
-// designated SERIAL4/5 on Pixhawk
-#define SF0X_DEFAULT_PORT		"/dev/ttyS6"
-
-class SF0X : public cdev::CDev, public px4::ScheduledWorkItem
 {
 public:
-	SF0X(const char *port = SF0X_DEFAULT_PORT, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
+	SF0X(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
-	virtual ~SF0X();
+	~SF0X() override;
 
-	virtual int 			init() override;
+	int 			init();
-
-	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;
-
-	/**
-	* 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;
-	ringbuffer::RingBuffer		*_reports;
-	int				_measure_interval;
-	bool				_collect_phase;
-	int				_fd;
-	char				_linebuf[10];
-	unsigned			_linebuf_index;
-	enum SF0X_PARSE_STATE		_parse_state;
-	hrt_abstime			_last_read;
 
-	int				_class_instance;
+	void				start();
-	int				_orb_class_instance;
+	void				stop();
+	void				Run() override;
+	int				measure();
+	int				collect();
 
-	orb_advert_t			_distance_sensor_topic;
+
+	PX4Rangefinder                  _px4_rangefinder;
+
+	char 				_port[20] {};
+	int         		        _interval{100000};
+	bool				_collect_phase{false};
+	int				_fd{-1};
+	char				_linebuf[10] {};
+	unsigned			_linebuf_index{0};
+	enum SF0X_PARSE_STATE		_parse_state {SF0X_PARSE_STATE0_UNSYNC};
+	hrt_abstime			_last_read{0};
 
 	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 SF0X_MIN_DISTANCE
-	* and SF0X_MAX_DISTANCE
-	*/
-	void				set_minimum_distance(float min);
-	void				set_maximum_distance(float max);
-	float				get_minimum_distance();
-	float				get_maximum_distance();
-
-	/**
-	* Perform a poll cycle; collect from the previous measurement
-	* and start a new one.
-	*/
-	void				Run() override;
-	int				measure();
-	int				collect();
 
 };

src/drivers/distance_sensor/sf0x/sf0x_main.cpp

@@ -34,34 +34,15 @@
 #include "SF0X.hpp"
 
 #include <px4_platform_common/getopt.h>
+#include <px4_platform_common/module.h>
 
-/*
- * Driver 'main' command.
- */
-extern "C" __EXPORT int sf0x_main(int argc, char *argv[]);
-
-/**
- * Local functions in support of the shell command.
- */
 namespace sf0x
 {
 
-SF0X	*g_dev;
+SF0X *g_dev{nullptr};
 
-int	start(const char *port, uint8_t rotation);
+static int start(const char *port, uint8_t rotation)
-int	stop();
-int	test();
-int	reset();
-int	info();
-
-/**
- * Start the driver.
- */
-int
-start(const char *port, uint8_t rotation)
 {
-	int fd;
-
 	if (g_dev != nullptr) {
 		PX4_WARN("already started");
 		return -1;
@@ -71,41 +52,19 @@ start(const char *port, uint8_t rotation)
 	g_dev = new SF0X(port, rotation);
 
 	if (g_dev == nullptr) {
-		goto fail;
+		return -1;
 	}
 
-	if (OK != g_dev->init()) {
+	if (g_dev->init() != PX4_OK) {
-		goto fail;
+		delete g_dev;
-	}
+		g_dev = nullptr;
-
+		return -1;
-	/* set the poll rate to default, starts automatic data collection */
-	fd = open(RANGE_FINDER0_DEVICE_PATH, 0);
-
-	if (fd < 0) {
-		PX4_ERR("device open fail (%i)", errno);
-		goto fail;
-	}
-
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
-		goto fail;
 	}
 
 	return 0;
-
-fail:
-
-	if (g_dev != nullptr) {
-		delete g_dev;
-		g_dev = nullptr;
-	}
-
-	return -1;
 }
 
-/**
+static int stop()
- * Stop the driver
- */
-int stop()
 {
 	if (g_dev != nullptr) {
 		delete g_dev;
@@ -118,124 +77,53 @@ int stop()
 	return 0;
 }
 
-/**
+static int status()
- * Perform some basic functional tests on the driver;
- * make sure we can collect data from the sensor in polled
- * and automatic modes.
- */
-int
-test()
-{
-	struct distance_sensor_s report;
-	ssize_t sz;
-
-	int fd = open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0) {
-		PX4_ERR("%s open failed (try 'sf0x start' if the driver is not running", RANGE_FINDER0_DEVICE_PATH);
-		return -1;
-	}
-
-	/* do a simple demand read */
-	sz = read(fd, &report, sizeof(report));
-
-	if (sz != sizeof(report)) {
-		PX4_ERR("immediate read failed");
-		return -1;
-	}
-
-	print_message(report);
-
-	/* start the sensor polling at 2 Hz rate */
-	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
-		PX4_ERR("failed to set 2Hz poll rate");
-		return -1;
-	}
-
-	/* 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) {
-			PX4_ERR("timed out");
-			break;
-		}
-
-		/* now go get it */
-		sz = read(fd, &report, sizeof(report));
-
-		if (sz != sizeof(report)) {
-			PX4_ERR("read failed: got %zi vs exp. %zu", sz, sizeof(report));
-			break;
-		}
-
-		print_message(report);
-	}
-
-	/* reset the sensor polling to the default rate */
-	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
-		PX4_ERR("ioctl SENSORIOCSPOLLRATE failed");
-		return -1;
-	}
-
-	return 0;
-}
-
-/**
- * Reset the driver.
- */
-int
-reset()
-{
-	int fd = open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0) {
-		PX4_ERR("open failed (%i)", errno);
-		return -1;
-	}
-
-	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
-		PX4_ERR("driver reset failed");
-		return -1;
-	}
-
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
-		PX4_ERR("driver poll restart failed");
-		return -1;
-	}
-
-	return 0;
-}
-
-/**
- * Print a little info about the driver.
- */
-int
-info()
 {
 	if (g_dev == nullptr) {
 		PX4_ERR("driver not running");
 		return -1;
 	}
 
-	printf("state @ %p\n", g_dev);
 	g_dev->print_info();
 
 	return 0;
 }
 
+static int usage()
+{
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+
+Serial bus driver for the LightWare SF02/F, SF10/a, SF10/b, SF10/c, SF11/c Laser rangefinders.
+
+Most boards are configured to enable/start the driver on a specified UART using the SENS_SF0X_CFG parameter.
+
+Setup/usage information: https://docs.px4.io/en/sensor/sfxx_lidar.html
+
+### Examples
+
+Attempt to start driver on a specified serial device.
+$ sf0x start -d /dev/ttyS1
+Stop driver
+$ sf0x stop
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("sf0x", "driver");
+	PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver");
+	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);
+	PRINT_MODULE_USAGE_PARAM_INT('R', 25, 1, 25, "Sensor rotation - downward facing by default", true);
+	PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver");
+	return PX4_OK;
+}
+
 } // namespace
 
-int
+extern "C" __EXPORT int sf0x_main(int argc, char *argv[])
-sf0x_main(int argc, char *argv[])
 {
 	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
-	const char *device_path = SF0X_DEFAULT_PORT;
+	const char *device_path = nullptr;
 	int ch;
 	int myoptind = 1;
 	const char *myoptarg = nullptr;
@@ -251,51 +139,26 @@ sf0x_main(int argc, char *argv[])
 			break;
 
 		default:
-			PX4_WARN("Unknown option!");
+			sf0x::usage();
 			return -1;
 		}
 	}
 
 	if (myoptind >= argc) {
-		goto out_error;
+		sf0x::usage();
+		return -1;
 	}
 
-	/*
-	 * Start/load the driver.
-	 */
 	if (!strcmp(argv[myoptind], "start")) {
 		return sf0x::start(device_path, rotation);
-	}
 
-	/*
+	} else if (!strcmp(argv[myoptind], "stop")) {
-	 * Stop the driver
-	 */
-	if (!strcmp(argv[myoptind], "stop")) {
 		return sf0x::stop();
+
+	} else if (!strcmp(argv[myoptind], "status")) {
+		return sf0x::status();
 	}
 
-	/*
+	sf0x::usage();
-	 * Test the driver/device.
-	 */
-	if (!strcmp(argv[myoptind], "test")) {
-		return sf0x::test();
-	}
-
-	/*
-	 * Reset the driver.
-	 */
-	if (!strcmp(argv[myoptind], "reset")) {
-		return sf0x::reset();
-	}
-
-	/*
-	 * Print driver information.
-	 */
-	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
-		return sf0x::info();
-	}
-
-out_error:
-	PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
 	return -1;
 }