From f4f112424fbf194562043866bf162b7df0481527 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Thu, 16 Aug 2018 13:59:27 +0200 Subject: [PATCH] pmw3901: add bus option and minor improvements - remove unused distance sensor rotation - add possibility to define SPI bus for other boards - use SENS_FLOW_ROT param to rotate sensor --- src/drivers/pmw3901/pmw3901.cpp | 80 ++++++++++++++++++++++++++------- 1 file changed, 63 insertions(+), 17 deletions(-) diff --git a/src/drivers/pmw3901/pmw3901.cpp b/src/drivers/pmw3901/pmw3901.cpp index 9974a2c656..8b58bac7f3 100644 --- a/src/drivers/pmw3901/pmw3901.cpp +++ b/src/drivers/pmw3901/pmw3901.cpp @@ -62,6 +62,7 @@ #include #include +#include #include #include @@ -108,7 +109,7 @@ class PMW3901 : public device::SPI { public: - PMW3901(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, int bus = PMW3901_BUS); + PMW3901(int bus = PMW3901_BUS, enum Rotation yaw_rotation = (enum Rotation)0); virtual ~PMW3901(); @@ -127,7 +128,6 @@ protected: virtual int probe(); private: - uint8_t _rotation; work_s _work; ringbuffer::RingBuffer *_reports; bool _sensor_ok; @@ -143,7 +143,7 @@ private: uint64_t _previous_collect_timestamp; - enum Rotation _sensor_rotation; + enum Rotation _yaw_rotation; int _flow_sum_x = 0; int _flow_sum_y = 0; uint64_t _flow_dt_sum_usec = 0; @@ -193,9 +193,8 @@ private: */ extern "C" __EXPORT int pmw3901_main(int argc, char *argv[]); -PMW3901::PMW3901(uint8_t rotation, int bus) : +PMW3901::PMW3901(int bus, enum Rotation yaw_rotation) : SPI("PMW3901", PMW3901_DEVICE_PATH, bus, PMW3901_SPIDEV, SPIDEV_MODE0, PMW3901_SPI_BUS_SPEED), - _rotation(rotation), _reports(nullptr), _sensor_ok(false), _measure_ticks(0), @@ -206,7 +205,7 @@ PMW3901::PMW3901(uint8_t rotation, int bus) : _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) + _yaw_rotation(yaw_rotation) { // enable debug() calls @@ -341,6 +340,16 @@ PMW3901::init() { int ret = PX4_ERROR; + // get yaw rotation from sensor frame to body frame + param_t rot = param_find("SENS_FLOW_ROT"); + + if (rot != PARAM_INVALID) { + int32_t val = 0; + param_get(rot, &val); + + _yaw_rotation = (enum Rotation)val; + } + /* For devices competing with NuttX SPI drivers on a bus (Crazyflie SD Card expansion board) */ SPI::set_lockmode(LOCK_THREADS); @@ -583,6 +592,11 @@ PMW3901::collect() report.pixel_flow_x_integral = static_cast(delta_x); report.pixel_flow_y_integral = static_cast(delta_y); + // rotate measurements in yaw from sensor frame to body frame according to parameter SENS_FLOW_ROT + float zeroval = 0.0f; + rotate_3f(_yaw_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval); + rotate_3f(_yaw_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral); + report.frame_count_since_last_readout = 4; //microseconds report.integration_timespan = _flow_dt_sum_usec; //microseconds @@ -719,18 +733,19 @@ namespace pmw3901 PMW3901 *g_dev; -void start(uint8_t rotation); +void start(int spi_bus); void stop(); void test(); void reset(); void info(); +void usage(); /** * Start the driver. */ void -start(uint8_t rotation) +start(int spi_bus) { int fd; @@ -739,7 +754,7 @@ start(uint8_t rotation) } /* create the driver */ - g_dev = new PMW3901(rotation, PMW3901_BUS); + g_dev = new PMW3901(spi_bus, (enum Rotation)0); if (g_dev == nullptr) { goto fail; @@ -837,24 +852,57 @@ info() exit(0); } +/** + * Print a little info about how to start/stop/use the driver + */ +void usage() +{ + PX4_INFO("usage: pmw3901 {start|test|reset|info'}"); + PX4_INFO(" [-b SPI_BUS]"); +} + } // namespace pmw3901 int pmw3901_main(int argc, char *argv[]) { - int myoptind = 1; - uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; - if (argc < 2) { - goto out; + pmw3901::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 = PMW3901_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) { + pmw3901::usage(); + return PX4_ERROR; } /* * Start/load the driver. */ if (!strcmp(argv[myoptind], "start")) { - pmw3901::start(rotation); + pmw3901::start(spi_bus); } /* @@ -878,8 +926,6 @@ pmw3901_main(int argc, char *argv[]) pmw3901::info(); } -out: - - PX4_ERR("unrecognized command, try 'start', 'test', or 'info'"); + pmw3901::usage(); return PX4_ERROR; }