PixArt PAW3902 optical motion tracking driver (#12576)

src/drivers/optical_flow/CMakeLists.txt

@@ -31,5 +31,7 @@
 #
 ############################################################################
 
-add_subdirectory(px4flow)
+add_subdirectory(paw3902)
 add_subdirectory(pmw3901)
+add_subdirectory(px4flow)
+

src/drivers/optical_flow/paw3902/CMakeLists.txt

@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2019 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__optical_flow__paw3902
+	MAIN paw3902
+	STACK_MAIN 1500
+	SRCS
+		paw3902_main.cpp
+		PAW3902.cpp
+	)

src/drivers/optical_flow/paw3902/PAW3902.hpp

@@ -0,0 +1,140 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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 paw3902.cpp
+ *
+ * Driver for the Pixart PAW3902 optical flow sensor connected via SPI.
+ */
+
+#pragma once
+
+#include "PixArt_PAW3902JF_Registers.hpp"
+
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_getopt.h>
+#include <px4_work_queue/ScheduledWorkItem.hpp>
+#include <drivers/device/spi.h>
+#include <conversion/rotation.h>
+#include <lib/perf/perf_counter.h>
+#include <lib/parameters/param.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_range_finder.h>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/optical_flow.h>
+
+/* Configuration Constants */
+
+#if defined PX4_SPI_BUS_EXPANSION   // crazyflie
+# define PAW3902_BUS PX4_SPI_BUS_EXPANSION
+#elif defined PX4_SPI_BUS_EXTERNAL1   // fmu-v5
+# define PAW3902_BUS PX4_SPI_BUS_EXTERNAL1
+#elif defined PX4_SPI_BUS_EXTERNAL    // fmu-v4 extspi
+# define PAW3902_BUS PX4_SPI_BUS_EXTERNAL
+#else
+# error "add the required spi bus from board_config.h here"
+#endif
+
+#if defined PX4_SPIDEV_EXPANSION_2    // crazyflie flow deck
+# define PAW3902_SPIDEV PX4_SPIDEV_EXPANSION_2
+#elif defined PX4_SPIDEV_EXTERNAL1_1    // fmu-v5 ext CS1
+# define PAW3902_SPIDEV PX4_SPIDEV_EXTERNAL1_1
+#elif defined PX4_SPIDEV_EXTERNAL   // fmu-v4 extspi
+# define PAW3902_SPIDEV PX4_SPIDEV_EXTERNAL
+#else
+# error "add the required spi dev from board_config.h here"
+#endif
+
+#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
+
+class PAW3902 : public device::SPI, public px4::ScheduledWorkItem
+{
+public:
+	PAW3902(int bus = PAW3902_BUS, enum Rotation yaw_rotation = ROTATION_NONE);
+	virtual ~PAW3902();
+
+	virtual int init();
+
+	void print_info();
+
+	void start();
+	void stop();
+
+protected:
+	virtual int probe();
+
+private:
+
+	void Run() override;
+
+	uint8_t	registerRead(uint8_t reg);
+	void	registerWrite(uint8_t reg, uint8_t data);
+
+	bool reset();
+
+	bool modeBright();
+	bool modeLowLight();
+	bool modeSuperLowLight();
+
+	bool changeMode(Mode newMode);
+
+	uORB::Publication<optical_flow_s> _optical_flow_pub{ORB_ID(optical_flow)};
+
+	perf_counter_t	_sample_perf;
+	perf_counter_t	_interval_perf;
+	perf_counter_t	_comms_errors;
+	perf_counter_t	_dupe_count_perf;
+
+	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};
+
+	enum Rotation	_yaw_rotation {ROTATION_NONE};
+
+	int		_flow_sum_x{0};
+	int		_flow_sum_y{0};
+
+	Mode		_mode{Mode::LowLight};
+
+};

src/drivers/optical_flow/paw3902/PixArt_PAW3902JF_Registers.hpp

@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+namespace PixArt_PAW3902JF
+{
+
+static constexpr uint8_t PRODUCT_ID = 0x49;	// shared with the PMW3901
+static constexpr uint8_t REVISION_ID = 0x01;
+
+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
+
+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,
+
+	Observation	= 0x15,
+	Motion_Burst	= 0x16,
+
+	Power_Up_Reset	= 0x3A,
+
+	Resolution	= 0x4E,
+
+};
+
+enum Product_ID_Bit : uint8_t {
+	Reset = 0x5A,
+};
+
+
+enum class Mode {
+	Bright = 0,
+	LowLight = 1,
+	SuperLowLight = 2,
+};
+
+struct BURST_TRANSFER {
+	uint8_t Motion;
+	uint8_t Observation;
+	uint8_t Delta_X_L;
+	uint8_t Delta_X_H;
+	uint8_t Delta_Y_L;
+	uint8_t Delta_Y_H;
+	uint8_t SQUAL;
+	uint8_t RawData_Sum;
+	uint8_t Maximum_RawData;
+	uint8_t Minimum_RawData;
+	uint8_t Shutter_Upper;
+	uint8_t Shutter_Lower;
+};
+
+}

src/drivers/optical_flow/paw3902/paw3902_main.cpp

@@ -0,0 +1,190 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 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.
+ *
+ ****************************************************************************/
+
+#include "PAW3902.hpp"
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace pmw3902
+{
+
+PAW3902	*g_dev;
+
+void	start(int spi_bus);
+void	stop();
+void	test();
+void	reset();
+void	info();
+void	usage();
+
+
+/**
+ * Start the driver.
+ */
+void
+start(int spi_bus)
+{
+	if (g_dev != nullptr) {
+		errx(1, "already started");
+	}
+
+	/* create the driver */
+	g_dev = new PAW3902(spi_bus, (enum Rotation)0);
+
+	if (g_dev == nullptr) {
+		goto fail;
+	}
+
+	if (OK != g_dev->init()) {
+		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);
+}
+
+/**
+ * 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);
+}
+
+/**
+ * Print a little info about how to start/stop/use the driver
+ */
+void usage()
+{
+	PX4_INFO("usage: pmw3902 {start|test|reset|info'}");
+	PX4_INFO("    [-b SPI_BUS]");
+}
+
+} // namespace pmw3902
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int paw3902_main(int argc, char *argv[]);
+
+int
+paw3902_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		pmw3902::usage();
+		return PX4_ERROR;
+	}
+
+	// don't exit from getopt loop to leave getopt global variables in consistent state,
+	// set error flag instead
+	bool err_flag = false;
+	int ch;
+	int myoptind = 1;
+	const char *myoptarg = nullptr;
+	int spi_bus = PAW3902_BUS;
+
+	while ((ch = px4_getopt(argc, argv, "b:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'b':
+			spi_bus = (uint8_t)atoi(myoptarg);
+
+			break;
+
+		default:
+			err_flag = true;
+			break;
+		}
+	}
+
+	if (err_flag) {
+		pmw3902::usage();
+		return PX4_ERROR;
+	}
+
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[myoptind], "start")) {
+		pmw3902::start(spi_bus);
+	}
+
+	/*
+	 * Stop the driver
+	 */
+	if (!strcmp(argv[myoptind], "stop")) {
+		pmw3902::stop();
+	}
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[myoptind], "status")) {
+		pmw3902::info();
+	}
+
+	pmw3902::usage();
+	return PX4_ERROR;
+}

src/drivers/optical_flow/pmw3901/CMakeLists.txt

@@ -32,7 +32,7 @@
 ############################################################################
 
 px4_add_module(
-	MODULE drivers__pmw3901
+	MODULE drivers__optical_flow__pmw3901
 	MAIN pmw3901
 	COMPILE_FLAGS
 		-Wno-cast-align # TODO: fix and enable