optical_flow/paw3902: minor improvements

- verify register writes
 - cleanly reset on lighting mode changes as per the datasheet
 - perf counters for mode switches and other errors
 - cleanly reset on false motion detection
 - minimize sleep on reset
 - allowing setting custom rotation yaw angle directly on command line with -Y (ignoring SENS_FLOW_ROT)
 - enable LED output

src/drivers/optical_flow/paw3902/PAW3902.hpp

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file paw3902.cpp
+ * @file PAW3902.hpp
  *
  * Driver for the Pixart PAW3902 & PAW3903 optical flow sensors connected via SPI.
  */
@@ -50,27 +50,20 @@
 #include <lib/perf/perf_counter.h>
 #include <lib/parameters/param.h>
 #include <drivers/drv_hrt.h>
-#include <drivers/drv_range_finder.h>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/optical_flow.h>
 
-/* Configuration Constants */
-
-#define PAW3902_SPI_BUS_SPEED (2000000L) // 2MHz
-
-#define DIR_WRITE(a) ((a) | (1 << 7))
-#define DIR_READ(a) ((a) & 0x7f)
-
 using namespace time_literals;
 using namespace PixArt_PAW3902JF;
 
-// PAW3902JF-TXQT is PixArt Imaging
+#define DIR_WRITE(a) ((a) | (1 << 7))
+#define DIR_READ(a) ((a) & 0x7f)
 
 class PAW3902 : public device::SPI, public I2CSPIDriver<PAW3902>
 {
 public:
-	PAW3902(I2CSPIBusOption bus_option, int bus, int devid, enum Rotation yaw_rotation, int bus_frequency,
-		spi_mode_e spi_mode);
+	PAW3902(I2CSPIBusOption bus_option, int bus, int devid, int bus_frequency, spi_mode_e spi_mode,
+		float yaw_rotation_degrees = NAN);
 	virtual ~PAW3902();
 
 	static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
@@ -83,37 +76,38 @@ public:
 
 	void RunImpl();
 
-	void start();
-
-protected:
-	int probe() override;
-
 private:
+	int probe() override;
 
-	uint8_t	registerRead(uint8_t reg);
-	void	registerWrite(uint8_t reg, uint8_t data);
+	uint8_t	RegisterRead(uint8_t reg, int retries = 3);
+	void RegisterWrite(uint8_t reg, uint8_t data);
+	bool RegisterWriteVerified(uint8_t reg, uint8_t data, int retries = 5);
 
-	bool reset();
+	bool Reset();
 
-	bool modeBright();
-	bool modeLowLight();
-	bool modeSuperLowLight();
+	bool ModeBright();
+	bool ModeLowLight();
+	bool ModeSuperLowLight();
 
-	bool changeMode(Mode newMode);
+	bool ChangeMode(Mode newMode);
 
 	uORB::PublicationMulti<optical_flow_s> _optical_flow_pub{ORB_ID(optical_flow)};
 
-	perf_counter_t	_sample_perf;
-	perf_counter_t	_comms_errors;
-	perf_counter_t	_dupe_count_perf;
+	perf_counter_t	_sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
+	perf_counter_t	_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
+	perf_counter_t	_comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")};
+	perf_counter_t	_false_motion_perf{perf_alloc(PC_COUNT, MODULE_NAME": false motion report")};
+	perf_counter_t	_mode_change_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change")};
+	perf_counter_t	_register_write_fail_perf{perf_alloc(PC_COUNT, MODULE_NAME": verified register write failed")};
 
-	static constexpr uint64_t _collect_time{15000}; // 15 milliseconds, optical flow data publish rate
+	static constexpr uint64_t COLLECT_TIME{15000}; // 15 milliseconds, optical flow data publish rate
 
-	uint64_t	_previous_collect_timestamp{0};
-	uint64_t	_flow_dt_sum_usec{0};
-	unsigned	_frame_count_since_last{0};
+	uint64_t _previous_collect_timestamp{0};
+	uint64_t _flow_dt_sum_usec{0};
+	uint8_t _flow_sample_counter{0};
+	uint16_t _flow_quality_sum{0};
 
-	enum Rotation	_yaw_rotation {ROTATION_NONE};
+	matrix::Dcmf	_rotation;
 
 	int		_flow_sum_x{0};
 	int		_flow_sum_y{0};
@@ -123,5 +117,4 @@ private:
 	uint8_t 	_low_to_superlow_counter{0};
 	uint8_t 	_low_to_bright_counter{0};
 	uint8_t 	_superlow_to_low_counter{0};
-
 };

src/drivers/optical_flow/paw3902/PixArt_PAW3902JF_Registers.hpp

@@ -36,45 +36,45 @@
 namespace PixArt_PAW3902JF
 {
 
-static constexpr uint8_t PRODUCT_ID = 0x49;	// shared with the PMW3901
+static constexpr uint8_t PRODUCT_ID = 0x49;
 static constexpr uint8_t REVISION_ID = 0x01;
+static constexpr uint8_t PRODUCT_ID_INVERSE = 0xB6;
 
 static constexpr uint32_t SAMPLE_INTERVAL_MODE_0{1000000 / 126};	// 126 fps
 static constexpr uint32_t SAMPLE_INTERVAL_MODE_1{1000000 / 126};	// 126 fps
 static constexpr uint32_t SAMPLE_INTERVAL_MODE_2{1000000 / 50};		// 50 fps
 
-static constexpr uint64_t T_SWW{11};	// 10.5 microseconds
-static constexpr uint64_t T_SRR{2};	// 1.5 microseconds
+static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2MHz SPI serial interface
 
 enum Register : uint8_t {
-	Product_ID	= 0x00,
-	Revision_ID	= 0x01,
-	Motion		= 0x02,
-	Delta_X_L	= 0x03,
-	Delta_X_H	= 0x04,
-	Delta_Y_L	= 0x05,
-	Delta_Y_H	= 0x06,
-	Squal		= 0x07,
-	RawData_Sum	= 0x08,
-	Maximum_RawData	= 0x09,
-	Minimum_RawData	= 0x0A,
-	Shutter_Lower	= 0x0B,
-	Shutter_Upper	= 0x0C,
+	Product_ID         = 0x00,
+	Revision_ID        = 0x01,
+	Motion             = 0x02,
+	Delta_X_L          = 0x03,
+	Delta_X_H          = 0x04,
+	Delta_Y_L          = 0x05,
+	Delta_Y_H          = 0x06,
+	Squal              = 0x07,
+	RawData_Sum        = 0x08,
+	Maximum_RawData    = 0x09,
+	Minimum_RawData    = 0x0A,
+	Shutter_Lower      = 0x0B,
+	Shutter_Upper      = 0x0C,
 
-	Observation	= 0x15,
-	Motion_Burst	= 0x16,
+	Observation        = 0x15,
+	Motion_Burst       = 0x16,
 
-	Power_Up_Reset	= 0x3A,
+	Power_Up_Reset     = 0x3A,
 
-	Resolution	= 0x4E,
+	Resolution	   = 0x4E,
 
+	Inverse_Product_ID = 0x5F,
 };
 
 enum Product_ID_Bit : uint8_t {
 	Reset = 0x5A,
 };
 
-
 enum class Mode {
 	Bright = 0,
 	LowLight = 1,

src/drivers/optical_flow/paw3902/paw3902_main.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2020 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
@@ -34,23 +34,20 @@
 #include "PAW3902.hpp"
 #include <px4_platform_common/module.h>
 
-extern "C" __EXPORT int paw3902_main(int argc, char *argv[]);
-
-void
-PAW3902::print_usage()
+void PAW3902::print_usage()
 {
 	PRINT_MODULE_USAGE_NAME("paw3902", "driver");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
-	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
+	PRINT_MODULE_USAGE_PARAM_INT('Y', 0, 0, 359, "custom yaw rotation (degrees)", true);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
 I2CSPIDriverBase *PAW3902::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
 				       int runtime_instance)
 {
-	PAW3902 *instance = new PAW3902(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
-					cli.bus_frequency, cli.spi_mode);
+	PAW3902 *instance = new PAW3902(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.bus_frequency,
+					cli.spi_mode, cli.custom1);
 
 	if (!instance) {
 		PX4_ERR("alloc failed");
@@ -65,19 +62,18 @@ I2CSPIDriverBase *PAW3902::instantiate(const BusCLIArguments &cli, const BusInst
 	return instance;
 }
 
-int
-paw3902_main(int argc, char *argv[])
+extern "C" __EXPORT int paw3902_main(int argc, char *argv[])
 {
-	int ch;
+	int ch = 0;
 	using ThisDriver = PAW3902;
 	BusCLIArguments cli{false, true};
 	cli.spi_mode = SPIDEV_MODE0;
-	cli.default_spi_frequency = PAW3902_SPI_BUS_SPEED;
+	cli.default_spi_frequency = SPI_SPEED;
 
-	while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
+	while ((ch = cli.getopt(argc, argv, "Y:")) != EOF) {
 		switch (ch) {
-		case 'R':
-			cli.rotation = (enum Rotation)atoi(cli.optarg());
+		case 'Y':
+			cli.custom1 = atoi(cli.optarg());
 			break;
 		}
 	}