diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 599628c72c6..3937f8d493f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -106,10 +106,10 @@ then teraranger start -a fi -# Possible pmw3901 optical flow sensor +# Possible external pmw3901 optical flow sensor if param greater -s SENS_EN_PMW3901 0 then - pmw3901 start + pmw3901 -S start fi ############################################################################### diff --git a/boards/nxp/fmurt1062-v1/init/rc.board_sensors b/boards/nxp/fmurt1062-v1/init/rc.board_sensors index c281c1ebe11..9e4b6133c04 100644 --- a/boards/nxp/fmurt1062-v1/init/rc.board_sensors +++ b/boards/nxp/fmurt1062-v1/init/rc.board_sensors @@ -53,6 +53,6 @@ ms5611 -s start rm3100 start # Possible pmw3901 optical flow sensor -pmw3901 start +pmw3901 -S start px4flow start & diff --git a/src/drivers/optical_flow/pmw3901/PMW3901.cpp b/src/drivers/optical_flow/pmw3901/PMW3901.cpp index f282c962f9e..93b64de153d 100644 --- a/src/drivers/optical_flow/pmw3901/PMW3901.cpp +++ b/src/drivers/optical_flow/pmw3901/PMW3901.cpp @@ -35,9 +35,10 @@ static constexpr uint32_t TIME_us_TSWW = 11; // - actually 10.5us -PMW3901::PMW3901(int bus, enum Rotation yaw_rotation) : - SPI("PMW3901", PMW3901_DEVICE_PATH, bus, PMW3901_SPIDEV, SPIDEV_MODE0, PMW3901_SPI_BUS_SPEED), - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), +PMW3901::PMW3901(I2CSPIBusOption bus_option, int bus, int devid, enum Rotation yaw_rotation, int bus_frequency, + spi_mode_e spi_mode) : + SPI("PMW3901", PMW3901_DEVICE_PATH, bus, devid, spi_mode, bus_frequency), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), _sample_perf(perf_alloc(PC_ELAPSED, "pmw3901: read")), _comms_errors(perf_alloc(PC_COUNT, "pmw3901: com err")), _yaw_rotation(yaw_rotation) @@ -46,9 +47,6 @@ PMW3901::PMW3901(int bus, enum Rotation yaw_rotation) : PMW3901::~PMW3901() { - // make sure we are truly inactive - stop(); - // free perf counters perf_free(_sample_perf); perf_free(_comms_errors); @@ -296,7 +294,7 @@ PMW3901::writeRegister(unsigned reg, uint8_t data) } void -PMW3901::Run() +PMW3901::RunImpl() { perf_begin(_sample_perf); @@ -414,8 +412,9 @@ PMW3901::stop() } void -PMW3901::print_info() +PMW3901::print_status() { + I2CSPIDriverBase::print_status(); perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); } diff --git a/src/drivers/optical_flow/pmw3901/PMW3901.hpp b/src/drivers/optical_flow/pmw3901/PMW3901.hpp index 35d0009c379..3500b9f4911 100644 --- a/src/drivers/optical_flow/pmw3901/PMW3901.hpp +++ b/src/drivers/optical_flow/pmw3901/PMW3901.hpp @@ -49,29 +49,10 @@ #include #include #include +#include /* Configuration Constants */ -#if defined PX4_SPI_BUS_EXPANSION // crazyflie -# define PMW3901_BUS PX4_SPI_BUS_EXPANSION -#elif defined PX4_SPI_BUS_EXTERNAL1 // fmu-v5 -# define PMW3901_BUS PX4_SPI_BUS_EXTERNAL1 -#elif defined PX4_SPI_BUS_EXTERNAL // fmu-v4 extspi -# define PMW3901_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 PMW3901_SPIDEV PX4_SPIDEV_EXPANSION_2 -#elif defined PX4_SPIDEV_EXTERNAL1_1 // fmu-v5 ext CS1 -# define PMW3901_SPIDEV PX4_SPIDEV_EXTERNAL1_1 -#elif defined PX4_SPIDEV_EXTERNAL // fmu-v4 extspi -# define PMW3901_SPIDEV PX4_SPIDEV_EXTERNAL -#else -# error "add the required spi dev from board_config.h here" -#endif - #define PMW3901_SPI_BUS_SPEED (2000000L) // 2MHz #define DIR_WRITE(a) ((a) | (1 << 7)) @@ -84,19 +65,23 @@ #define PMW3901_SAMPLE_INTERVAL 10000 /* 10 ms */ -class PMW3901 : public device::SPI, public px4::ScheduledWorkItem +class PMW3901 : public device::SPI, public I2CSPIDriver { public: - PMW3901(int bus = PMW3901_BUS, enum Rotation yaw_rotation = (enum Rotation)0); + PMW3901(I2CSPIBusOption bus_option, int bus, int devid, enum Rotation yaw_rotation, int bus_frequency, + spi_mode_e spi_mode); virtual ~PMW3901(); + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + virtual int init(); - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); + void print_status(); + + void RunImpl(); protected: virtual int probe(); @@ -133,11 +118,6 @@ private: */ void stop(); - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - */ - void Run() override; int readRegister(unsigned reg, uint8_t *data, unsigned count); int writeRegister(unsigned reg, uint8_t data); diff --git a/src/drivers/optical_flow/pmw3901/pmw3901_main.cpp b/src/drivers/optical_flow/pmw3901/pmw3901_main.cpp index 3c59adc3b24..bc740950289 100644 --- a/src/drivers/optical_flow/pmw3901/pmw3901_main.cpp +++ b/src/drivers/optical_flow/pmw3901/pmw3901_main.cpp @@ -32,159 +32,77 @@ ****************************************************************************/ #include "PMW3901.hpp" +#include -/* - * Driver 'main' command. - */ extern "C" __EXPORT int pmw3901_main(int argc, char *argv[]); -/** - * Local functions in support of the shell command. - */ -namespace pmw3901 -{ - -PMW3901 *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) +PMW3901::print_usage() { - if (g_dev != nullptr) { - errx(1, "already started"); - } - - /* create the driver */ - g_dev = new PMW3901(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"); + PRINT_MODULE_USAGE_NAME("pmw3901", "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_DEFAULT_COMMANDS(); } -/** - * Stop the driver - */ -void stop() +I2CSPIDriverBase *PMW3901::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; + PMW3901 *instance = new PMW3901(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, + cli.bus_frequency, cli.spi_mode); - } else { - errx(1, "driver not running"); + if (!instance) { + PX4_ERR("alloc failed"); + return nullptr; } - exit(0); -} - -/** - * Print a little info about the driver. - */ -void -info() -{ - if (g_dev == nullptr) { - errx(1, "driver not running"); + if (OK != instance->init()) { + delete instance; + return nullptr; } - printf("state @ %p\n", g_dev); - g_dev->print_info(); - - exit(0); + return instance; } -/** - * 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[]) { - if (argc < 2) { - 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; + using ThisDriver = PMW3901; + BusCLIArguments cli{false, true}; + cli.spi_mode = SPIDEV_MODE0; + cli.default_spi_frequency = PMW3901_SPI_BUS_SPEED; - while ((ch = px4_getopt(argc, argv, "b:", &myoptind, &myoptarg)) != EOF) { + while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { switch (ch) { - case 'b': - spi_bus = (uint8_t)atoi(myoptarg); - - break; - - default: - err_flag = true; + case 'R': + cli.rotation = (enum Rotation)atoi(cli.optarg()); break; } } - if (err_flag) { - pmw3901::usage(); - return PX4_ERROR; + const char *verb = cli.optarg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; } - /* - * Start/load the driver. - */ - if (!strcmp(argv[myoptind], "start")) { - pmw3901::start(spi_bus); + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_FLOW_DEVTYPE_PMW3901); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); } - /* - * Stop the driver - */ - if (!strcmp(argv[myoptind], "stop")) { - pmw3901::stop(); + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); } - /* - * Print driver information. - */ - if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) { - pmw3901::info(); + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); } - pmw3901::usage(); - return PX4_ERROR; + ThisDriver::print_usage(); + return -1; }