crazyflie: optical flow and distace sensor driver fixes

src/drivers/boards/crazyflie/spi.c

@@ -21,8 +21,6 @@
 #include "board_config.h"
 #include <systemlib/err.h>
 
-//#include "stm32.h"
-
 /****************************************************************************
  * Pre-Processor Definitions
  ****************************************************************************/
@@ -76,15 +74,15 @@ __EXPORT int stm32_spi_bus_initialize(void)
 		return -ENODEV;
 	}
 
-	//#ifdef CONFIG_MMCSD
+#ifdef CONFIG_MMCSD
 	int ret = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi_expansion);
 
 	if (ret != OK) {
-		//message("[boot] FAILED to bind SPI port 1 to the MMCSD driver\n");
+		PX4_ERR("[boot] FAILED to bind SPI port 1 to the MMCSD driver\n");
 		return -ENODEV;
 	}
 
-	//#endif
+#endif
 
 
 	return OK;
@@ -104,7 +102,6 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool s
 	/* SPI select is active low, so write !selected to select the device */
 
 	int sel = (int) devid;
-	//ASSERT(PX4_SPI_BUS_ID(sel) == PX4_SPI_BUS_EXPANSION);
 
 	/* Making sure the other peripherals are not selected */
 
@@ -148,18 +145,10 @@ __EXPORT void board_spi_reset(int ms)
 	stm32_configgpio(GPIO_SPI1_MISO_OFF);
 	stm32_configgpio(GPIO_SPI1_MOSI_OFF);
 
-	/* set the sensor rail off */
-	//stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0);
-
 	/* wait for the sensor rail to reach GND */
 	usleep(ms * 1000);
 	warnx("reset done, %d ms", ms);
 
-	/* re-enable power */
-
-	/* switch the sensor rail back on */
-	//stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1);
-
 	/* wait a bit before starting SPI, different times didn't influence results */
 	usleep(100);
 

src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2018 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
@@ -58,7 +58,7 @@
 #include <math.h>
 #include <unistd.h>
 
-#include <systemlib/perf_counter.h>
+#include <perf/perf_counter.h>
 #include <systemlib/err.h>
 
 #include <drivers/drv_hrt.h>
@@ -74,41 +74,40 @@
 
 /* Configuration Constants */
 #ifdef PX4_I2C_BUS_EXPANSION
-#define VL53LXX_BUS 				PX4_I2C_BUS_EXPANSION
+#define VL53LXX_BUS PX4_I2C_BUS_EXPANSION
 #else
-#define VL53LXX_BUS 				0
+#define VL53LXX_BUS 0
 #endif
 
-#define VL53LXX_BASEADDR      		0b0101001 					// 7-bit address 
-#define VL53LXX_DEVICE_PATH   		"/dev/vl53lxx"
+#define VL53LXX_BASEADDR 0b0101001 // 7-bit address
+#define VL53LXX_DEVICE_PATH "/dev/vl53lxx"
 
 /* VL53LXX Registers addresses */
-#define VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG				0x89
-#define MSRC_CONFIG_CONTROL_REG								0x60
-#define FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT_REG		0x44
-#define SYSTEM_SEQUENCE_CONFIG_REG							0x01
-#define DYNAMIC_SPAD_REF_EN_START_OFFSET_REG				0x4F
-#define DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD_REG				0x4E
-#define GLOBAL_CONFIG_REF_EN_START_SELECT_REG				0xB6
-#define GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG				0xB0
-#define SYSTEM_INTERRUPT_CONFIG_GPIO_REG					0x0A
-#define SYSTEM_SEQUENCE_CONFIG_REG							0x01
-#define SYSRANGE_START_REG									0x00
-#define RESULT_INTERRUPT_STATUS_REG							0x13
-#define SYSTEM_INTERRUPT_CLEAR_REG							0x0B
-#define GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG				0xB0
-#define GPIO_HV_MUX_ACTIVE_HIGH_REG							0x84
-#define SYSTEM_INTERRUPT_CLEAR_REG							0x0B
-#define RESULT_RANGE_STATUS_REG								0x14
-
-#define VL53LXX_CONVERSION_INTERVAL 						1000 		/*  1ms */
-#define VL53LXX_SAMPLE_RATE									50000 		/* 50ms */
-
-#define VL53LXX_MAX_RANGING_DISTANCE						2.0f
-#define VL53LXX_MIN_RANGING_DISTANCE						0.0f
-
-#define VL53LXX_RA_IDENTIFICATION_MODEL_ID					0xC0
-#define VL53LXX_IDENTIFICATION_MODEL_ID 					0xEEAA
+#define VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG 0x89
+#define MSRC_CONFIG_CONTROL_REG 0x60
+#define FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT_REG 0x44
+#define SYSTEM_SEQUENCE_CONFIG_REG 0x01
+#define DYNAMIC_SPAD_REF_EN_START_OFFSET_REG 0x4F
+#define DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD_REG 0x4E
+#define GLOBAL_CONFIG_REF_EN_START_SELECT_REG 0xB6
+#define GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG 0xB0
+#define SYSTEM_INTERRUPT_CONFIG_GPIO_REG 0x0A
+#define SYSTEM_SEQUENCE_CONFIG_REG 0x01
+#define SYSRANGE_START_REG 0x00
+#define RESULT_INTERRUPT_STATUS_REG 0x13
+#define SYSTEM_INTERRUPT_CLEAR_REG 0x0B
+#define GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG 0xB0
+#define GPIO_HV_MUX_ACTIVE_HIGH_REG 0x84
+#define SYSTEM_INTERRUPT_CLEAR_REG 0x0B
+#define RESULT_RANGE_STATUS_REG 0x14
+#define VL53LXX_RA_IDENTIFICATION_MODEL_ID 0xC0
+#define VL53LXX_IDENTIFICATION_MODEL_ID 0xEEAA
+
+#define VL53LXX_MS 1000 /*  1ms */
+#define VL53LXX_SAMPLE_RATE 50000 /* 50ms */
+
+#define VL53LXX_MAX_RANGING_DISTANCE 2.0f
+#define VL53LXX_MIN_RANGING_DISTANCE 0.0f
 
 #ifndef CONFIG_SCHED_WORKQUEUE
 # error This requires CONFIG_SCHED_WORKQUEUE.
@@ -122,72 +121,69 @@ public:
 
 	virtual ~VL53LXX();
 
-	virtual int 				init();
+	virtual int init();
 
-	virtual ssize_t				read(device::file_t *filp, char *buffer, size_t buflen);			// NOT TESTED YET
+	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);
+	virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
 
 	/**
 	* Diagnostics - print some basic information about the driver.
 	*/
-	void						print_info();
+	void print_info();
 
 protected:
-	virtual int					probe();
+	virtual int probe();
 
 private:
-	uint8_t 					_rotation;
-	work_s						_work;
-	ringbuffer::RingBuffer		*_reports;
-	bool						_sensor_ok;
-	int							_measure_ticks;
-	bool 						_collect_phase;
-	bool 						_new_measurement;
-	bool 						_measurement_started;
+	uint8_t _rotation;
+	work_s _work;
+	ringbuffer::RingBuffer *_reports;
+	bool _sensor_ok;
+	int _measure_ticks;
+	bool _collect_phase;
+	bool _new_measurement;
+	bool _measurement_started;
 
-	int							_class_instance;
-	int							_orb_class_instance;
+	int _class_instance;
+	int _orb_class_instance;
 
-	orb_advert_t				_distance_sensor_topic;
+	orb_advert_t _distance_sensor_topic;
 
-	perf_counter_t				_sample_perf;
-	perf_counter_t				_comms_errors;
+	perf_counter_t _sample_perf;
+	perf_counter_t _comms_errors;
 
-	uint8_t 					stop_variable_;
+	uint8_t _stop_variable;
 
 
 	/**
 	* 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();
+	void start();
 
 	/**
 	* Stop the automatic measurement state machine.
 	*/
-	void						stop();
+	void stop();
 
 	/**
 	* Perform a poll cycle; collect from the previous measurement
 	* and start a new one.
 	*/
-	void						cycle();
-	int							measure();
-	int							collect();
+	void cycle();
+	int measure();
+	int collect();
 
-	int 						readRegister(uint8_t reg_address, uint8_t &value);
-	int 						writeRegister(uint8_t reg_address, uint8_t value);
+	int readRegister(uint8_t reg_address, uint8_t &value);
+	int writeRegister(uint8_t reg_address, uint8_t value);
 
-	int 						writeRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length);
-	int 						readRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length);
+	int writeRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length);
+	int readRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length);
 
-	int 						sensorInit();
-	bool 						spadCalculations();
-	bool 						sensorTuning();
-	bool						singleRefCalibration(uint8_t byte);
+	int sensorInit();
+	bool spadCalculations();
+	bool sensorTuning();
+	bool singleRefCalibration(uint8_t byte);
 
 	/**
 	* Static trampoline from the workq context; because we don't have a
@@ -195,8 +191,7 @@ private:
 	*
 	* @param arg		Instance pointer for the driver that is polling.
 	*/
-	static void					cycle_trampoline(void *arg);
-
+	static void cycle_trampoline(void *arg);
 
 };
 
@@ -222,7 +217,7 @@ VL53LXX::VL53LXX(uint8_t rotation, int bus, int address) :
 	_comms_errors(perf_alloc(PC_COUNT, "vl53lxx_com_err"))
 {
 	// up the retries since the device misses the first measure attempts
-	I2C::_retries = 2;
+	I2C::_retries = 3;
 
 	// enable debug() calls
 	_debug_enabled = false;
@@ -289,7 +284,7 @@ VL53LXX::sensorInit()
 		return ret;
 	}
 
-	stop_variable_ = val;
+	_stop_variable = val;
 
 	// disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks
 	readRegister(MSRC_CONFIG_CONTROL_REG, val);
@@ -305,8 +300,6 @@ VL53LXX::sensorInit()
 
 	spadCalculations();
 
-	ret = OK;
-
 	return ret;
 
 }
@@ -314,12 +307,13 @@ VL53LXX::sensorInit()
 int
 VL53LXX::init()
 {
-	int ret = PX4_ERROR;
+	int ret = OK;
 
 	set_device_address(VL53LXX_BASEADDR);
 
 	/* do I2C init (and probe) first */
 	if (I2C::init() != OK) {
+		ret = PX4_ERROR;
 		goto out;
 	}
 
@@ -327,26 +321,12 @@ VL53LXX::init()
 	_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
 
 	if (_reports == nullptr) {
+		ret = PX4_ERROR;
 		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;
-
-		_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;
 
@@ -381,7 +361,7 @@ VL53LXX::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 					bool want_start = (_measure_ticks == 0);
 
 					/* set interval for next measurement to minimum legal value */
-					_measure_ticks = USEC2TICK(VL53LXX_CONVERSION_INTERVAL);
+					_measure_ticks = USEC2TICK(VL53LXX_SAMPLE_RATE);
 
 					/* if we need to start the poll state machine, do it */
 					if (want_start) {
@@ -469,6 +449,8 @@ VL53LXX::read(device::file_t *filp, char *buffer, size_t buflen)
 			break;
 		}
 
+		while (!_collect_phase);
+
 		/* run the collection phase */
 		if (OK != collect()) {
 			ret = -EIO;
@@ -601,7 +583,7 @@ VL53LXX::writeRegisterMulti(uint8_t reg_address, uint8_t *value,
 int
 VL53LXX::measure()
 {
-	int ret;
+	int ret = OK;
 	uint8_t wait_for_measurement;
 	uint8_t system_start;
 
@@ -617,7 +599,7 @@ VL53LXX::measure()
 		writeRegister(0x80, 0x01);
 		writeRegister(0xFF, 0x01);
 		writeRegister(0x00, 0x00);
-		writeRegister(0x91, stop_variable_);
+		writeRegister(0x91, _stop_variable);
 		writeRegister(0x00, 0x01);
 		writeRegister(0xFF, 0x00);
 		writeRegister(0x80, 0x00);
@@ -628,7 +610,7 @@ VL53LXX::measure()
 
 		if ((system_start & 0x01) == 1) {
 			work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this,
-				   1000);		// reschedule every 1 ms until measurement is ready
+				   USEC2TICK(VL53LXX_MS));		// reschedule every 1 ms until measurement is ready
 			ret = OK;
 			return ret;
 
@@ -644,7 +626,7 @@ VL53LXX::measure()
 
 		if ((system_start & 0x01) == 1) {
 			work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this,
-				   1000);		// reschedule every 1 ms until measurement is ready
+				   USEC2TICK(VL53LXX_MS));		// reschedule every 1 ms until measurement is ready
 			ret = OK;
 			return ret;
 
@@ -657,7 +639,7 @@ VL53LXX::measure()
 
 	if ((wait_for_measurement & 0x07) == 0) {
 		work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this,
-			   1000);		// reschedule every 1 ms until measurement is ready
+			   USEC2TICK(VL53LXX_MS));		// reschedule every 1 ms until measurement is ready
 		ret = OK;
 		return ret;
 	}
@@ -672,8 +654,6 @@ VL53LXX::measure()
 		return ret;
 	}
 
-	ret = OK;
-
 	return ret;
 }
 
@@ -717,8 +697,9 @@ VL53LXX::collect()
 	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);
+	if (_class_instance == CLASS_DEVICE_PRIMARY) {
+		orb_publish_auto(ORB_ID(distance_sensor), &_distance_sensor_topic, &report, &_orb_class_instance,
+				 ORB_PRIO_DEFAULT);
 	}
 
 	_reports->force(&report);
@@ -741,7 +722,7 @@ VL53LXX::start()
 	_reports->flush();
 
 	/* schedule a cycle to start things */
-	work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, 1000);
+	work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, USEC2TICK(VL53LXX_MS));
 
 	/* notify about state change */
 	struct subsystem_info_s info = {};
@@ -753,11 +734,9 @@ VL53LXX::start()
 	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);
 	}
 }
@@ -795,7 +774,7 @@ VL53LXX::cycle()
 			   &_work,
 			   (worker_t)&VL53LXX::cycle_trampoline,
 			   this,
-			   USEC2TICK(VL53LXX_SAMPLE_RATE));
+			   _measure_ticks);
 	}
 
 }
@@ -1024,7 +1003,6 @@ VL53LXX	*g_dev;
 void	start(uint8_t rotation);
 void	stop();
 void	test();
-void	reset();
 void	info();
 
 /**
@@ -1098,45 +1076,28 @@ void stop()
 void
 test()
 {
-	//struct distance_sensor_s report;
+	struct distance_sensor_s report;
+	ssize_t sz;
 
 	int fd = open(VL53LXX_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0) {
-		err(1, "%s open failed (try 'vl53lxx start' if the driver is not running", VL53LXX_DEVICE_PATH);
+		err(1, "%s open failed (try 'vl53lxx start' if the driver is not running)", VL53LXX_DEVICE_PATH);
 	}
 
+	/* do a simple demand read */
+	sz = read(fd, &report, sizeof(report));
 
-	/* start the sensor polling at 2Hz */
-	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
-		errx(1, "failed to set 2Hz poll rate");
+	if (sz != sizeof(report)) {
+		PX4_ERR("ret: %d, expected: %d", sz, sizeof(report));
+		err(1, "immediate acc read failed");
 	}
 
+	print_message(report);
+
 	errx(0, "PASS");
 }
 
-/**
- * Reset the driver.
- */
-void
-reset()
-{
-	int fd = open(VL53LXX_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.
@@ -1160,27 +1121,13 @@ info()
 int
 vl53lxx_main(int argc, char *argv[])
 {
-	int ch;
 	int myoptind = 1;
-	const char *myoptarg = nullptr;
 	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
 
 	if (argc < 2) {
 		goto out;
 	}
 
-	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.
 	 */
@@ -1202,22 +1149,15 @@ vl53lxx_main(int argc, char *argv[])
 		vl53lxx::test();
 	}
 
-	/*
-	 * Reset the driver.
-	 */
-	if (!strcmp(argv[myoptind], "reset")) {
-		vl53lxx::reset();
-	}
-
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[1], "status")) {
+	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
 		vl53lxx::info();
 	}
 
 out:
 
-	PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
+	PX4_ERR("unrecognized command, try 'start', 'test', or 'info'");
 	return PX4_ERROR;
 }

src/drivers/pmw3901/CMakeLists.txt

@@ -5,7 +5,5 @@ px4_add_module(
 		-Wno-sign-compare
 	SRCS
 		pmw3901.cpp
-	DEPENDS
-		platforms__common
 	)
 # vim: set noet ft=cmake fenc=utf-8 ff=unix :

src/drivers/pmw3901/pmw3901.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2018 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
@@ -60,7 +60,7 @@
 
 #include <conversion/rotation.h>
 
-#include <systemlib/perf_counter.h>
+#include <perf/perf_counter.h>
 #include <systemlib/err.h>
 
 #include <drivers/drv_hrt.h>
@@ -78,27 +78,27 @@
 
 /* Configuration Constants */
 #ifdef PX4_SPI_BUS_EXPANSION
-#define PMW3901_BUS 							PX4_SPI_BUS_EXPANSION
+#define PMW3901_BUS PX4_SPI_BUS_EXPANSION
 #else
-#define PMW3901_BUS 							0
+#define PMW3901_BUS 0
 #endif
 
 #ifdef PX4_SPIDEV_EXPANSION_2
-#define PMW3901_SPIDEV 							PX4_SPIDEV_EXPANSION_2
+#define PMW3901_SPIDEV PX4_SPIDEV_EXPANSION_2
 #else
-#define PMW3901_SPIDEV 							0
+#define PMW3901_SPIDEV 0
 #endif
 
-#define PMW3901_SPI_BUS_SPEED    				(2000000L)      			// 2MHz
+#define PMW3901_SPI_BUS_SPEED (2000000L) // 2MHz
 
-#define DIR_WRITE(a)             				((a) | (1 << 7))
-#define DIR_READ(a)                    			((a) & 0x7f)
+#define DIR_WRITE(a) ((a) | (1 << 7))
+#define DIR_READ(a) ((a) & 0x7f)
 
-#define PMW3901_DEVICE_PATH   					"/dev/pmw3901"
+#define PMW3901_DEVICE_PATH "/dev/pmw3901"
 
 /* PMW3901 Registers addresses */
-#define PMW3901_CONVERSION_INTERVAL 			1000 						/*   1 ms */
-#define PMW3901_SAMPLE_RATE						10000 						/*  10 ms */
+#define PMW3901_MS 1000 /*   1 ms */
+#define PMW3901_SAMPLE_RATE 10000 /*  10 ms */
 
 
 #ifndef CONFIG_SCHED_WORKQUEUE
@@ -112,37 +112,34 @@ public:
 
 	virtual ~PMW3901();
 
-	virtual int 		init();
+	virtual int init();
 
-	virtual ssize_t		read(device::file_t *filp, char *buffer, size_t buflen);
+	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);
+	virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
 
 	/**
 	* Diagnostics - print some basic information about the driver.
 	*/
-	void				print_info();
-
-	int					collect();
+	void print_info();
 
 private:
 	uint8_t _rotation;
-	work_s						_work;
-	ringbuffer::RingBuffer		*_reports;
-	bool						_sensor_ok;
-	int							_measure_ticks;
-	int							_class_instance;
-	int							_orb_class_instance;
-
-	orb_advert_t				_optical_flow_pub;
+	work_s _work;
+	ringbuffer::RingBuffer *_reports;
+	bool _sensor_ok;
+	int _measure_ticks;
+	int _class_instance;
+	int _orb_class_instance;
 
-	perf_counter_t				_sample_perf;
-	perf_counter_t				_comms_errors;
+	orb_advert_t _optical_flow_pub;
 
-	uint64_t					_previous_collect_timestamp;
+	perf_counter_t _sample_perf;
+	perf_counter_t _comms_errors;
 
-	enum Rotation 				_sensor_rotation;
+	uint64_t _previous_collect_timestamp;
 
+	enum Rotation _sensor_rotation;
 
 
 	/**
@@ -151,25 +148,25 @@ private:
 	* @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();
 
 	/**
 	* Perform a poll cycle; collect from the previous measurement
 	* and start a new one.
 	*/
-	void				cycle();
-	// int					collect();
+	void cycle();
+	int	collect();
 
-	int 				readRegister(unsigned reg, uint8_t *data, unsigned count);
-	int         		writeRegister(unsigned reg, uint8_t data);
+	int readRegister(unsigned reg, uint8_t *data, unsigned count);
+	int writeRegister(unsigned reg, uint8_t data);
 
-	int 				sensorInit();
-	int        			readMotionCount(int16_t &deltaX, int16_t &deltaY);
+	int sensorInit();
+	int readMotionCount(int16_t &deltaX, int16_t &deltaY);
 
 	/**
 	* Static trampoline from the workq context; because we don't have a
@@ -177,14 +174,13 @@ private:
 	*
 	* @param arg		Instance pointer for the driver that is polling.
 	*/
-	static void			cycle_trampoline(void *arg);
+	static void cycle_trampoline(void *arg);
 
 
 };
 
 
 
-
 /*
  * Driver 'main' command.
  */
@@ -199,8 +195,8 @@ PMW3901::PMW3901(uint8_t rotation, int bus) :
 	_class_instance(-1),
 	_orb_class_instance(-1),
 	_optical_flow_pub(nullptr),
-	_sample_perf(perf_alloc(PC_ELAPSED, "tr1_read")),
-	_comms_errors(perf_alloc(PC_COUNT, "tr1_com_err")),
+	_sample_perf(perf_alloc(PC_ELAPSED, "pmw3901_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "pmw3901_com_err")),
 	_previous_collect_timestamp(0),
 	_sensor_rotation((enum Rotation)rotation)
 {
@@ -222,10 +218,6 @@ PMW3901::~PMW3901()
 		delete _reports;
 	}
 
-	// if (_class_instance != -1) {
-	// 	unregister_class_devname(PMW3901_DEVICE_PATH, _class_instance);
-	// }
-
 	// free perf counters
 	perf_free(_sample_perf);
 	perf_free(_comms_errors);
@@ -354,7 +346,7 @@ PMW3901::init()
 	/* For devices competing with NuttX SPI drivers on a bus (Crazyflie SD Card expansion board) */
 	SPI::set_lockmode(LOCK_THREADS);
 
-	/* do I2C init (and probe) first */
+	/* do SPI init (and probe) first */
 	if (SPI::init() != OK) {
 		goto out;
 	}
@@ -370,23 +362,6 @@ PMW3901::init()
 		goto out;
 	}
 
-	//_class_instance = register_class_devname(PMW3901_DEVICE_PATH);
-
-	//if (_class_instance == CLASS_DEVICE_PRIMARY) {               // change to optical flow topic
-	/* get a publish handle on the range finder topic */
-	// struct distance_sensor_s ds_report;
-
-	// _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_ok = true;
 	_previous_collect_timestamp = hrt_absolute_time();
@@ -414,7 +389,7 @@ PMW3901::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 					bool want_start = (_measure_ticks == 0);
 
 					/* set interval for next measurement to minimum legal value */
-					_measure_ticks = USEC2TICK(PMW3901_CONVERSION_INTERVAL);
+					_measure_ticks = USEC2TICK(PMW3901_SAMPLE_RATE);
 
 					/* if we need to start the poll state machine, do it */
 					if (want_start) {
@@ -440,7 +415,7 @@ PMW3901::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 					unsigned ticks = USEC2TICK(1000000 / arg);
 
 					/* check against maximum rate */
-					if (ticks < USEC2TICK(PMW3901_CONVERSION_INTERVAL)) {
+					if (ticks < USEC2TICK(PMW3901_SAMPLE_RATE)) {
 						return -EINVAL;
 					}
 
@@ -567,7 +542,7 @@ PMW3901::writeRegister(unsigned reg, uint8_t data)
 int
 PMW3901::collect()
 {
-	int ret;
+	int ret = OK;
 	int16_t delta_x_raw, delta_y_raw;
 	float delta_x, delta_y;
 
@@ -610,9 +585,8 @@ PMW3901::collect()
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
-	ret = OK;
-
 	perf_end(_sample_perf);
+
 	return ret;
 
 }
@@ -652,7 +626,7 @@ PMW3901::start()
 	_reports->flush();
 
 	/* schedule a cycle to start things */
-	work_queue(LPWORK, &_work, (worker_t)&PMW3901::cycle_trampoline, this, 1000);
+	work_queue(LPWORK, &_work, (worker_t)&PMW3901::cycle_trampoline, this, USEC2TICK(PMW3901_MS));
 
 	/* notify about state change */
 	struct subsystem_info_s info = {};
@@ -796,65 +770,26 @@ void
 test()
 {
 
-	if (g_dev != nullptr) {
-		errx(1, "already started");
-	}
-
-	/* create the driver */
-	g_dev = new PMW3901(0, PMW3901_BUS);
-
-	if (g_dev == nullptr) {
-		delete g_dev;
-		g_dev = nullptr;
-	}
-
-	if (OK != g_dev->init()) {
-		delete g_dev;
-		g_dev = nullptr;
-
-	}
+	struct optical_flow_s report;
+	ssize_t sz;
 
 	int fd = open(PMW3901_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0) {
-		err(1, "%s open failed (try 'vl53lxx start' if the driver is not running", PMW3901_DEVICE_PATH);
-	}
-
-	/* start the sensor polling at 2Hz */
-	// if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
-	// 	errx(1, "failed to set 2Hz poll rate");
-	// }
-
-	for (int i = 0; i < 10000; i++) {
-		g_dev->collect();
-		usleep(10000);
+		err(1, "%s open failed (try 'pmw3901 start' if the driver is not running)", PMW3901_DEVICE_PATH);
 	}
 
-	errx(0, "PASS");
-}
-
-
-/**
- * Reset the driver.
- */
-void
-reset()
-{
-	int fd = open(PMW3901_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0) {
-		err(1, "failed ");
-	}
+	/* do a simple demand read */
+	sz = read(fd, &report, sizeof(report));
 
-	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
-		err(1, "driver reset failed");
+	if (sz != sizeof(report)) {
+		PX4_ERR("ret: %d, expected: %d", sz, sizeof(report));
+		err(1, "immediate acc read failed");
 	}
 
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
-		err(1, "driver poll restart failed");
-	}
+	print_message(report);
 
-	exit(0);
+	errx(0, "PASS");
 }
 
 
@@ -880,28 +815,13 @@ info()
 int
 pmw3901_main(int argc, char *argv[])
 {
-	int ch;
 	int myoptind = 1;
-
-	const char *myoptarg = nullptr;
 	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
 
 	if (argc < 2) {
 		goto out;
 	}
 
-	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.
 	 */
@@ -923,22 +843,15 @@ pmw3901_main(int argc, char *argv[])
 		pmw3901::test();
 	}
 
-	/*
-	 * Reset the driver.
-	 */
-	if (!strcmp(argv[myoptind], "reset")) {
-		pmw3901::reset();
-	}
-
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[1], "status")) {
+	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
 		pmw3901::info();
 	}
 
 out:
 
-	PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
+	PX4_ERR("unrecognized command, try 'start', 'test', or 'info'");
 	return PX4_ERROR;
 }