diff --git a/boards/nxp/fmuk66-v3/init/rc.board_sensors b/boards/nxp/fmuk66-v3/init/rc.board_sensors index 9bb57737ed..082b60b6ec 100644 --- a/boards/nxp/fmuk66-v3/init/rc.board_sensors +++ b/boards/nxp/fmuk66-v3/init/rc.board_sensors @@ -19,7 +19,7 @@ bmp280 -I start mpl3115a2 -I start # Internal SPI (accel + mag) -fxos8701cq start +fxos8701cq -s start # Internal SPI (gyro) fxas21002c -s start diff --git a/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp b/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp index c4d54ac9dc..83a23df432 100644 --- a/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp +++ b/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp @@ -53,9 +53,10 @@ const uint8_t FXOS8701CQ::_checked_registers[FXOS8701C_NUM_CHECKED_REGISTERS] = FXOS8701CQ_M_CTRL_REG2, }; -FXOS8701CQ::FXOS8701CQ(int bus, uint32_t device, enum Rotation rotation) : - SPI("FXOS8701CQ", nullptr, bus, device, SPIDEV_MODE0, 1 * 1000 * 1000), - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), +FXOS8701CQ::FXOS8701CQ(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode) : + SPI("FXOS8701CQ", nullptr, bus, device, spi_mode, bus_frequency), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), _px4_accel(get_device_id(), ORB_PRIO_LOW, rotation), #if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) _px4_mag(get_device_id(), ORB_PRIO_LOW, rotation), @@ -77,9 +78,6 @@ FXOS8701CQ::FXOS8701CQ(int bus, uint32_t device, enum Rotation rotation) : FXOS8701CQ::~FXOS8701CQ() { - // make sure we are truly inactive - stop(); - #if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) perf_free(_mag_sample_perf); #endif @@ -284,19 +282,10 @@ FXOS8701CQ::accel_set_samplerate(unsigned frequency) void FXOS8701CQ::start() { - // make sure we are stopped first - stop(); - // start polling at the specified rate ScheduleOnInterval(1000000 / (FXOS8701C_ACCEL_DEFAULT_RATE) - FXOS8701C_TIMER_REDUCTION, 10000); } -void -FXOS8701CQ::stop() -{ - ScheduleClear(); -} - void FXOS8701CQ::check_registers(void) { @@ -328,7 +317,7 @@ FXOS8701CQ::check_registers(void) } void -FXOS8701CQ::Run() +FXOS8701CQ::RunImpl() { // start the performance counter perf_begin(_accel_sample_perf); @@ -407,8 +396,9 @@ FXOS8701CQ::Run() } void -FXOS8701CQ::print_info() +FXOS8701CQ::print_status() { + I2CSPIDriverBase::print_status(); perf_print_counter(_accel_sample_perf); #if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) diff --git a/src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp b/src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp index a5528d918f..7793af77ce 100644 --- a/src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp +++ b/src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp @@ -43,7 +43,7 @@ #include #include #include -#include +#include #if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) #include @@ -111,27 +111,32 @@ */ #define FXOS8701C_TIMER_REDUCTION 240 -class FXOS8701CQ : public device::SPI, public px4::ScheduledWorkItem +class FXOS8701CQ : public device::SPI, public I2CSPIDriver { public: - FXOS8701CQ(int bus, uint32_t device, enum Rotation rotation); + FXOS8701CQ(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode); virtual ~FXOS8701CQ(); - virtual int init(); + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + + int init() override; + + void print_status() override; + + void RunImpl(); - void print_info(); void print_registers(); void test_error(); - protected: - virtual int probe(); + int probe() override; + void custom_method(const BusCLIArguments &cli) override; private: - void Run() override; - void start(); - void stop(); void reset(); /** diff --git a/src/drivers/imu/fxos8701cq/fxos8701cq_main.cpp b/src/drivers/imu/fxos8701cq/fxos8701cq_main.cpp index 1c4a91e8a2..f72afb21e3 100644 --- a/src/drivers/imu/fxos8701cq/fxos8701cq_main.cpp +++ b/src/drivers/imu/fxos8701cq/fxos8701cq_main.cpp @@ -40,191 +40,97 @@ #include "FXOS8701CQ.hpp" #include +#include -/** - * Local functions in support of the shell command. - */ -namespace fxos8701cq +void +FXOS8701CQ::print_usage() { - -FXOS8701CQ *g_dev{nullptr}; - -int start(bool external_bus, enum Rotation rotation); -int info(); -int stop(); -int regdump(); -int usage(); -int test_error(); - -/** - * Start the driver. - * - * This function call only returns once the driver is - * up and running or failed to detect the sensor. - */ -int -start(bool external_bus, enum Rotation rotation) -{ - if (g_dev != nullptr) { - PX4_INFO("already started"); - return 0; - } - - /* create the driver */ - if (external_bus) { -#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_ACCEL_MAG) - g_dev = new FXOS8701CQ(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_ACCEL_MAG, rotation); -#else - PX4_ERR("External SPI not available"); - return 0; -#endif - - } else { - g_dev = new FXOS8701CQ(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_ACCEL_MAG, rotation); - } - - if (g_dev == nullptr) { - PX4_ERR("failed instantiating FXOS8701C obj"); - goto fail; - } - - if (OK != g_dev->init()) { - goto fail; - } - - return PX4_OK; -fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - PX4_ERR("driver start failed"); - return PX4_ERROR; + PRINT_MODULE_USAGE_NAME("fxos8701cq", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("imu"); + 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_COMMAND("regdump"); + PRINT_MODULE_USAGE_COMMAND("testerror"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -/** - * Print a little info about the driver. - */ -int -info() +I2CSPIDriverBase *FXOS8701CQ::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { - if (g_dev == nullptr) { - PX4_ERR("driver not running\n"); - return 1; + FXOS8701CQ *instance = new FXOS8701CQ(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, + cli.bus_frequency, cli.spi_mode); + + if (!instance) { + PX4_ERR("alloc failed"); + return nullptr; } - g_dev->print_info(); - - return 0; -} - -int -stop() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running\n"); - return 1; + if (OK != instance->init()) { + delete instance; + return nullptr; } - delete g_dev; - g_dev = nullptr; - - return 0; + return instance; } -/** - * dump registers from device - */ -int -regdump() +void FXOS8701CQ::custom_method(const BusCLIArguments &cli) { - if (g_dev == nullptr) { - PX4_ERR("driver not running\n"); - return 1; + switch (cli.custom1) { + case 0: print_registers(); break; + + case 1: test_error(); break; } - printf("regdump @ %p\n", g_dev); - g_dev->print_registers(); - - return 0; } -/** - * trigger an error - */ -int -test_error() +extern "C" int fxos8701cq_main(int argc, char *argv[]) { - if (g_dev == nullptr) { - PX4_ERR("driver not running\n"); - return 1; - } - - g_dev->test_error(); - - return 0; -} - -int -usage() -{ - PX4_INFO("missing command: try 'start', 'info', 'stop', 'testerror' or 'regdump'"); - PX4_INFO("options:"); - PX4_INFO(" -X (external bus)"); - PX4_INFO(" -R rotation"); - - return 0; -} - -} // namespace - -extern "C" { __EXPORT int fxos8701cq_main(int argc, char *argv[]); } - -int fxos8701cq_main(int argc, char *argv[]) -{ - bool external_bus = false; int ch; - enum Rotation rotation = ROTATION_NONE; + using ThisDriver = FXOS8701CQ; + BusCLIArguments cli{false, true}; + cli.default_spi_frequency = 1 * 1000 * 1000; + cli.spi_mode = SPIDEV_MODE0; - int myoptind = 1; - const char *myoptarg = NULL; - - while ((ch = px4_getopt(argc, argv, "XR:a:", &myoptind, &myoptarg)) != EOF) { + while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { switch (ch) { - case 'X': - external_bus = true; - break; - case 'R': - rotation = (enum Rotation)atoi(myoptarg); + cli.rotation = (enum Rotation)atoi(cli.optarg()); break; - - default: - fxos8701cq::usage(); - exit(0); } } - const char *verb = argv[myoptind]; + const char *verb = cli.optarg(); - if (!strcmp(verb, "start")) { - return fxos8701cq::start(external_bus, rotation); - - } else if (!strcmp(verb, "stop")) { - return fxos8701cq::stop(); - - } else if (!strcmp(verb, "info")) { - return fxos8701cq::info(); - - } else if (!strcmp(verb, "regdump")) { - return fxos8701cq::regdump(); - - } else if (!strcmp(verb, "testerror")) { - return fxos8701cq::test_error(); + if (!verb) { + ThisDriver::print_usage(); + return -1; } - PX4_ERR("unrecognized command, try 'start', 'stop', 'info', 'testerror' or 'regdump'"); - return PX4_ERROR; + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_ACC_DEVTYPE_FXOS8701C); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + } + + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + if (!strcmp(verb, "regdump")) { + cli.custom1 = 0; + return ThisDriver::module_custom_method(cli, iterator); + } + + if (!strcmp(verb, "testerror")) { + cli.custom1 = 1; + return ThisDriver::module_custom_method(cli, iterator); + } + + ThisDriver::print_usage(); + return -1; }