diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 4dd6e3b607..80341aea41 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -128,6 +128,7 @@ #define DRV_ACC_DEVTYPE_BMI088 0x6a #define DRV_OSD_DEVTYPE_ATXXXX 0x6b #define DRV_FLOW_DEVTYPE_PMW3901 0x6c +#define DRV_FLOW_DEVTYPE_PAW3902 0x6d #define DRV_DIST_DEVTYPE_LL40LS 0x70 #define DRV_DIST_DEVTYPE_MAPPYDOT 0x71 diff --git a/src/drivers/optical_flow/paw3902/PAW3902.cpp b/src/drivers/optical_flow/paw3902/PAW3902.cpp index cf44ed0b0b..cae13d028b 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.cpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.cpp @@ -33,9 +33,10 @@ #include "PAW3902.hpp" -PAW3902::PAW3902(int bus, enum Rotation yaw_rotation) : - SPI("PAW3902", nullptr, bus, PAW3902_SPIDEV, SPIDEV_MODE0, PAW3902_SPI_BUS_SPEED), - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), +PAW3902::PAW3902(I2CSPIBusOption bus_option, int bus, int devid, enum Rotation yaw_rotation, int bus_frequency, + spi_mode_e spi_mode) : + SPI("PAW3902", nullptr, 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, "paw3902: read")), _comms_errors(perf_alloc(PC_COUNT, "paw3902: com_err")), _dupe_count_perf(perf_alloc(PC_COUNT, "paw3902: duplicate reading")), @@ -45,9 +46,6 @@ PAW3902::PAW3902(int bus, enum Rotation yaw_rotation) : PAW3902::~PAW3902() { - // make sure we are truly inactive - stop(); - // free perf counters perf_free(_sample_perf); perf_free(_comms_errors); @@ -534,7 +532,7 @@ PAW3902::registerWrite(uint8_t reg, uint8_t data) } void -PAW3902::Run() +PAW3902::RunImpl() { perf_begin(_sample_perf); @@ -686,14 +684,9 @@ PAW3902::start() } void -PAW3902::stop() -{ - ScheduleClear(); -} - -void -PAW3902::print_info() +PAW3902::print_status() { + I2CSPIDriverBase::print_status(); perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); perf_print_counter(_dupe_count_perf); diff --git a/src/drivers/optical_flow/paw3902/PAW3902.hpp b/src/drivers/optical_flow/paw3902/PAW3902.hpp index 4441cc537f..278a3d5b11 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.hpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.hpp @@ -44,7 +44,7 @@ #include #include #include -#include +#include #include #include #include @@ -56,26 +56,6 @@ /* 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)) @@ -86,26 +66,30 @@ using namespace PixArt_PAW3902JF; // PAW3902JF-TXQT is PixArt Imaging -class PAW3902 : public device::SPI, public px4::ScheduledWorkItem +class PAW3902 : public device::SPI, public I2CSPIDriver { public: - PAW3902(int bus = PAW3902_BUS, enum Rotation yaw_rotation = ROTATION_NONE); + PAW3902(I2CSPIBusOption bus_option, int bus, int devid, enum Rotation yaw_rotation, int bus_frequency, + spi_mode_e spi_mode); virtual ~PAW3902(); - virtual int init(); + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); - void print_info(); + int init() override; + + void print_status() override; + + void RunImpl(); void start(); - void stop(); protected: - virtual int probe(); + int probe() override; private: - void Run() override; - uint8_t registerRead(uint8_t reg); void registerWrite(uint8_t reg, uint8_t data); diff --git a/src/drivers/optical_flow/paw3902/paw3902_main.cpp b/src/drivers/optical_flow/paw3902/paw3902_main.cpp index ce8e1c075e..37633d275c 100644 --- a/src/drivers/optical_flow/paw3902/paw3902_main.cpp +++ b/src/drivers/optical_flow/paw3902/paw3902_main.cpp @@ -32,159 +32,77 @@ ****************************************************************************/ #include "PAW3902.hpp" +#include -/** - * 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[]); +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_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); + + if (!instance) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (OK != instance->init()) { + delete instance; + return nullptr; + } + + return instance; +} + 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; + using ThisDriver = PAW3902; + BusCLIArguments cli{false, true}; + cli.spi_mode = SPIDEV_MODE0; + cli.default_spi_frequency = PAW3902_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) { - pmw3902::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")) { - pmw3902::start(spi_bus); + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_FLOW_DEVTYPE_PAW3902); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); } - /* - * Stop the driver - */ - if (!strcmp(argv[myoptind], "stop")) { - pmw3902::stop(); + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); } - /* - * Print driver information. - */ - if (!strcmp(argv[myoptind], "status")) { - pmw3902::info(); + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); } - pmw3902::usage(); - return PX4_ERROR; + ThisDriver::print_usage(); + return -1; }