diff --git a/boards/nxp/fmurt1062-v1/init/rc.board_sensors b/boards/nxp/fmurt1062-v1/init/rc.board_sensors index 41d114650e9..375270ae310 100644 --- a/boards/nxp/fmurt1062-v1/init/rc.board_sensors +++ b/boards/nxp/fmurt1062-v1/init/rc.board_sensors @@ -36,7 +36,7 @@ qmc5883 -X start lis3mdl -X start # ICM20948 as external magnetometer on I2C (e.g. Here GPS) -if ! icm20948 -X -M -R 6 start +if ! icm20948 -X -R 6 start then # external emulated AK09916 (Here2) is rotated 270 degrees yaw ak09916 -X -R 6 start diff --git a/boards/px4/fmu-v3/init/rc.board_sensors b/boards/px4/fmu-v3/init/rc.board_sensors index 7c7c06361cb..6dadd699400 100644 --- a/boards/px4/fmu-v3/init/rc.board_sensors +++ b/boards/px4/fmu-v3/init/rc.board_sensors @@ -72,7 +72,7 @@ then param set SENS_EN_THERMAL 0 # ICM20948 as external magnetometer on I2C (e.g. Here GPS) - if ! icm20948 -X -M -R 6 start + if ! icm20948 -X -R 6 start then # external emulated AK09916 (Here2) is rotated 270 degrees yaw ak09916 -X -R 6 start diff --git a/boards/px4/fmu-v5/init/rc.board_sensors b/boards/px4/fmu-v5/init/rc.board_sensors index 5165d4d9d58..8977ed23303 100644 --- a/boards/px4/fmu-v5/init/rc.board_sensors +++ b/boards/px4/fmu-v5/init/rc.board_sensors @@ -30,7 +30,7 @@ qmc5883 -X start lis3mdl -X start # ICM20948 as external magnetometer on I2C (e.g. Here GPS) -if ! icm20948 -X -M -R 6 start +if ! icm20948 -X -R 6 start then # external emulated AK09916 (Here2) is rotated 270 degrees yaw ak09916 -X -R 6 start diff --git a/src/drivers/imu/icm20948/ICM20948_mag.h b/src/drivers/imu/icm20948/ICM20948_mag.h index 7b3cdc9b1e6..542c3a08bd1 100644 --- a/src/drivers/imu/icm20948/ICM20948_mag.h +++ b/src/drivers/imu/icm20948/ICM20948_mag.h @@ -101,9 +101,7 @@ struct ak09916_regs { }; #pragma pack(pop) -extern device::Device *AK09916_I2C_interface(int bus); - -typedef device::Device *(*ICM20948_mag_constructor)(int, bool); +extern device::Device *AK09916_I2C_interface(int bus, int bus_frequency); /** * Helper class implementing the magnetometer driver node. diff --git a/src/drivers/imu/icm20948/icm20948.cpp b/src/drivers/imu/icm20948/icm20948.cpp index 7011770d71b..a0f11fc27cb 100644 --- a/src/drivers/imu/icm20948/icm20948.cpp +++ b/src/drivers/imu/icm20948/icm20948.cpp @@ -84,8 +84,10 @@ const uint16_t ICM20948::_icm20948_checked_registers[ICM20948_NUM_CHECKED_REGIST ICMREG_20948_ACCEL_CONFIG_2 }; -ICM20948::ICM20948(device::Device *interface, device::Device *mag_interface, enum Rotation rotation) : - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), +ICM20948::ICM20948(device::Device *interface, device::Device *mag_interface, enum Rotation rotation, + I2CSPIBusOption bus_option, + int bus) : + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus), _interface(interface), _px4_accel(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_DEFAULT : ORB_PRIO_HIGH), rotation), _px4_gyro(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_DEFAULT : ORB_PRIO_HIGH), rotation), @@ -106,10 +108,6 @@ ICM20948::ICM20948(device::Device *interface, device::Device *mag_interface, enu ICM20948::~ICM20948() { - // make sure we are truly inactive - stop(); - - // delete the perf counter perf_free(_sample_perf); perf_free(_interval_perf); perf_free(_bad_transfers); @@ -148,15 +146,12 @@ ICM20948::init() /* Magnetometer setup */ -#ifdef USE_I2C px4_usleep(100); if (!_mag.is_passthrough() && _mag._interface->init() != PX4_OK) { PX4_ERR("failed to setup ak09916 interface"); } -#endif /* USE_I2C */ - ret = _mag.ak09916_reset(); if (ret != OK) { @@ -235,11 +230,7 @@ ICM20948::reset_mpu() // INT CFG => Interrupt on Data Ready write_checked_reg(ICMREG_20948_INT_ENABLE_1, BIT_RAW_RDY_EN); // INT: Raw data ready -#ifdef USE_I2C bool bypass = !_mag.is_passthrough(); -#else - bool bypass = false; -#endif /* INT: Clear on any read. * If this instance is for a device is on I2C bus the Mag will have an i2c interface @@ -602,25 +593,9 @@ ICM20948::set_accel_range(unsigned max_g_in) void ICM20948::start() { - /* make sure we are stopped first */ - stop(); - ScheduleOnInterval(_call_interval - ICM20948_TIMER_REDUCTION, 1000); } -void -ICM20948::stop() -{ - ScheduleClear(); -} - -void -ICM20948::Run() -{ - /* make another measurement */ - measure(); -} - void ICM20948::check_registers(void) { @@ -728,7 +703,7 @@ ICM20948::check_duplicate(uint8_t *accel_data) } void -ICM20948::measure() +ICM20948::RunImpl() { perf_begin(_sample_perf); perf_count(_interval_perf); @@ -777,21 +752,13 @@ ICM20948::measure() * try to read a magnetometer report. */ -# ifdef USE_I2C - if (_mag.is_passthrough()) { -# endif - _mag._measure(timestamp_sample, mpu_report.mag); -# ifdef USE_I2C - } else { _mag.measure(); } -# endif - // Continue evaluating gyro and accelerometer results if (_register_wait == 0) { // Convert from big to little endian @@ -854,8 +821,9 @@ ICM20948::measure() } void -ICM20948::print_info() +ICM20948::print_status() { + I2CSPIDriverBase::print_status(); perf_print_counter(_sample_perf); perf_print_counter(_bad_transfers); perf_print_counter(_bad_registers); diff --git a/src/drivers/imu/icm20948/icm20948.h b/src/drivers/imu/icm20948/icm20948.h index afbe3d2eef6..372b5924172 100644 --- a/src/drivers/imu/icm20948/icm20948.h +++ b/src/drivers/imu/icm20948/icm20948.h @@ -37,16 +37,12 @@ #include #include #include -#include +#include #include #include #include "ICM20948_mag.h" -#if defined(PX4_I2C_OBDEV_ICM20948) || defined(PX4_I2C_BUS_EXPANSION) -# define USE_I2C -#endif - // ICM20948 registers #define MPUREG_WHOAMI 0x75 @@ -339,40 +335,38 @@ struct MPUReport { # define ICM20948_LOW_SPEED_OP(r) ((r) &~ICM20948_HIGH_BUS_SPEED) /* interface factories */ -extern device::Device *ICM20948_SPI_interface(int bus, uint32_t cs); -extern device::Device *ICM20948_I2C_interface(int bus, uint32_t address); -extern int ICM20948_probe(device::Device *dev); - -typedef device::Device *(*ICM20948_constructor)(int, uint32_t); +extern device::Device *ICM20948_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode); +extern device::Device *ICM20948_I2C_interface(int bus, uint32_t address, int bus_frequency); class ICM20948_mag; -class ICM20948 : public px4::ScheduledWorkItem +class ICM20948 : public I2CSPIDriver { public: - ICM20948(device::Device *interface, device::Device *mag_interface, enum Rotation rotation); + ICM20948(device::Device *interface, device::Device *mag_interface, enum Rotation rotation, I2CSPIBusOption bus_option, + int bus); virtual ~ICM20948(); - virtual int init(); - uint8_t get_whoami() { return _whoami; } + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); + int init(); + + void print_status() override; + + void RunImpl(); protected: device::Device *_interface; uint8_t _whoami{0}; /** whoami result */ - virtual int probe(); + int probe(); friend class ICM20948_mag; private: - void Run() override; - PX4Accelerometer _px4_accel; PX4Gyroscope _px4_gyro; @@ -423,7 +417,6 @@ private: bool _got_duplicate{false}; void start(); - void stop(); int reset(); /** @@ -431,11 +424,6 @@ private: */ int reset_mpu(); - /** - * Fetch measurements from the sensor and update the report buffers. - */ - void measure(); - /** * Select a register bank in ICM20948 * diff --git a/src/drivers/imu/icm20948/icm20948_i2c.cpp b/src/drivers/imu/icm20948/icm20948_i2c.cpp index 3e18e2a34d2..e01dc39f673 100644 --- a/src/drivers/imu/icm20948/icm20948_i2c.cpp +++ b/src/drivers/imu/icm20948/icm20948_i2c.cpp @@ -43,14 +43,12 @@ #include "icm20948.h" -#ifdef USE_I2C - -device::Device *ICM20948_I2C_interface(int bus, uint32_t address); +device::Device *ICM20948_I2C_interface(int bus, uint32_t address, int bus_frequency); class ICM20948_I2C : public device::I2C { public: - ICM20948_I2C(int bus, uint32_t address); + ICM20948_I2C(int bus, uint32_t address, int bus_frequency); ~ICM20948_I2C() override = default; int read(unsigned address, void *data, unsigned count) override; @@ -64,13 +62,13 @@ private: }; device::Device * -ICM20948_I2C_interface(int bus, uint32_t address) +ICM20948_I2C_interface(int bus, uint32_t address, int bus_frequency) { - return new ICM20948_I2C(bus, address); + return new ICM20948_I2C(bus, address, bus_frequency); } -ICM20948_I2C::ICM20948_I2C(int bus, uint32_t address) : - I2C("ICM20948_I2C", nullptr, bus, address, 400000) +ICM20948_I2C::ICM20948_I2C(int bus, uint32_t address, int bus_frequency) : + I2C("ICM20948_I2C", nullptr, bus, address, bus_frequency) { _device_id.devid_s.devtype = DRV_IMU_DEVTYPE_ICM20948; } @@ -126,5 +124,3 @@ ICM20948_I2C::probe() return -ENODEV; } - -#endif /* USE_I2C */ diff --git a/src/drivers/imu/icm20948/icm20948_main.cpp b/src/drivers/imu/icm20948/icm20948_main.cpp index 6963bde7855..5ddb41049c9 100644 --- a/src/drivers/imu/icm20948/icm20948_main.cpp +++ b/src/drivers/imu/icm20948/icm20948_main.cpp @@ -35,284 +35,106 @@ * @file main.cpp * * Driver for the Invensense icm20948 connected via I2C or SPI. - * - * based on the icm20948 driver */ -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include #include "icm20948.h" -/** driver 'main' command */ -extern "C" { __EXPORT int icm20948_main(int argc, char *argv[]); } - -enum ICM20948_BUS { - ICM20948_BUS_ALL = 0, - ICM20948_BUS_I2C_INTERNAL, - ICM20948_BUS_I2C_EXTERNAL, - ICM20948_BUS_SPI_INTERNAL, - ICM20948_BUS_SPI_EXTERNAL -}; - -/** - * Local functions in support of the shell command. - */ -namespace icm20948 +void +ICM20948::print_usage() { - -// list of supported bus configurations -struct icm20948_bus_option { - enum ICM20948_BUS busid; - ICM20948_constructor interface_constructor; - bool magpassthrough; - uint8_t busnum; - uint32_t address; - ICM20948 *dev; -} bus_options[] = { - -#if defined(PX4_SPIDEV_ICM_20948) && defined(PX4_SPI_BUS_1) - { ICM20948_BUS_SPI_INTERNAL, &ICM20948_SPI_interface, true, PX4_SPI_BUS_1, PX4_SPIDEV_ICM_20948, nullptr }, -#endif - -#if defined (USE_I2C) -# if defined(PX4_I2C_BUS_EXPANSION) - { ICM20948_BUS_I2C_EXTERNAL, &ICM20948_I2C_interface, false, PX4_I2C_BUS_EXPANSION, PX4_I2C_EXT_ICM20948_1, nullptr }, -# endif -#endif -}; - -#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) - - -bool start_bus(icm20948_bus_option &bus, enum Rotation rotation); -icm20948_bus_option *find_bus(enum ICM20948_BUS busid); - -int start(enum ICM20948_BUS busid, enum Rotation rotation); -int stop(enum ICM20948_BUS busid); -int info(enum ICM20948_BUS busid); -int usage(); - -/** - * find a bus structure for a busid - */ -struct icm20948_bus_option *find_bus(enum ICM20948_BUS busid) -{ - for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { - if ((busid == ICM20948_BUS_ALL || - busid == bus_options[i].busid) && bus_options[i].dev != nullptr) { - return &bus_options[i]; - } - } - - PX4_ERR("bus %u not started", (unsigned)busid); - return nullptr; + PRINT_MODULE_USAGE_NAME("icm20948", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("imu"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true); + PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -/** - * start driver for a specific bus option - */ -bool -start_bus(icm20948_bus_option &bus, enum Rotation rotation) +I2CSPIDriverBase *ICM20948::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { - PX4_INFO("Bus probed: %d", bus.busid); + device::Device *interface = nullptr; + device::Device *mag_interface = nullptr; - if (bus.dev != nullptr) { - PX4_ERR("SPI %d not available", bus.busid); - return false; + if (iterator.busType() == BOARD_I2C_BUS) { + interface = ICM20948_I2C_interface(iterator.bus(), PX4_I2C_EXT_ICM20948_1, cli.bus_frequency); + // For i2c interfaces, connect to the magnetometer directly + mag_interface = AK09916_I2C_interface(iterator.bus(), cli.bus_frequency); + + } else if (iterator.busType() == BOARD_SPI_BUS) { + interface = ICM20948_SPI_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode); } - device::Device *interface = bus.interface_constructor(bus.busnum, bus.address); - if (interface == nullptr) { - PX4_WARN("no device on bus %u", (unsigned)bus.busid); - return false; + PX4_ERR("alloc failed"); + delete mag_interface; + return nullptr; } if (interface->init() != OK) { delete interface; - PX4_WARN("no device on bus %u", (unsigned)bus.busid); - return false; + delete mag_interface; + PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + return nullptr; } - device::Device *mag_interface = nullptr; + ICM20948 *dev = new ICM20948(interface, mag_interface, cli.rotation, iterator.configuredBusOption(), iterator.bus()); -#ifdef USE_I2C - /* For i2c interfaces, connect to the magnetomer directly */ - const bool is_i2c = bus.busid == ICM20948_BUS_I2C_INTERNAL || bus.busid == ICM20948_BUS_I2C_EXTERNAL; - - if (is_i2c) { - mag_interface = AK09916_I2C_interface(bus.busnum); - } - -#endif - - bus.dev = new ICM20948(interface, mag_interface, rotation); - - if (bus.dev == nullptr) { + if (dev == nullptr) { delete interface; - - if (mag_interface != nullptr) { - delete mag_interface; - } - - return false; + delete mag_interface; + return nullptr; } - if (OK != bus.dev->init()) { - goto fail; + if (OK != dev->init()) { + delete dev; + return nullptr; } - return true; - -fail: - - if (bus.dev != nullptr) { - delete (bus.dev); - bus.dev = nullptr; - } - - PX4_ERR("driver start failed"); - - return false; + return dev; } -/** - * Start the driver. - * - * This function only returns if the driver is up and running - * or failed to detect the sensor. - */ -int -start(enum ICM20948_BUS busid, enum Rotation rotation) -{ - bool started = false; - - for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { - if (bus_options[i].dev != nullptr) { - // this device is already started - continue; - } - - if (busid != ICM20948_BUS_ALL && bus_options[i].busid != busid) { - // not the one that is asked for - continue; - } - - started |= start_bus(bus_options[i], rotation); - - if (started) { break; } - } - - return PX4_OK; -} - -int -stop(enum ICM20948_BUS busid) -{ - icm20948_bus_option *bus = find_bus(busid); - - if (bus != nullptr && bus->dev != nullptr) { - delete bus->dev; - bus->dev = nullptr; - - } else { - /* warn, but not an error */ - PX4_WARN("already stopped."); - } - - return PX4_OK; -} - -/** - * Print a little info about the driver. - */ -int -info(enum ICM20948_BUS busid) -{ - icm20948_bus_option *bus = find_bus(busid); - - if (bus != nullptr && bus->dev != nullptr) { - PX4_WARN("driver not running"); - return PX4_ERROR; - } - - bus->dev->print_info(); - - return PX4_OK; -} - -int -usage() -{ - PX4_INFO("missing command: try 'start', 'stop', 'info'"); - PX4_INFO("options:"); - PX4_INFO(" -X (i2c external bus)"); - PX4_INFO(" -I (i2c internal bus)"); - PX4_INFO(" -s (spi internal bus)"); - PX4_INFO(" -S (spi external bus)"); - PX4_INFO(" -t (spi internal bus, 2nd instance)"); - PX4_INFO(" -R rotation"); - - return PX4_OK; -} - -} // namespace icm20948 - -int +extern "C" int icm20948_main(int argc, char *argv[]) { - int myoptind = 1; int ch; - const char *myoptarg = nullptr; + using ThisDriver = ICM20948; + BusCLIArguments cli{true, true}; + cli.default_spi_frequency = 1000 * 1000; // low speed freq + cli.default_i2c_frequency = 400000; - enum ICM20948_BUS busid = ICM20948_BUS_ALL; - enum Rotation rotation = ROTATION_NONE; - - while ((ch = px4_getopt(argc, argv, "XISstMR:", &myoptind, &myoptarg)) != EOF) { + while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { switch (ch) { - case 'X': - busid = ICM20948_BUS_I2C_EXTERNAL; - break; - case 'R': - rotation = (enum Rotation)atoi(myoptarg); + cli.rotation = (enum Rotation)atoi(cli.optarg()); break; - - default: - return icm20948::usage(); } } - if (myoptind >= argc) { - return icm20948::usage(); + const char *verb = cli.optarg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; } - const char *verb = argv[myoptind]; + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ICM20948); - /* - * Start/load the driver. - */ if (!strcmp(verb, "start")) { - return icm20948::start(busid, rotation); + return ThisDriver::module_start(cli, iterator); } if (!strcmp(verb, "stop")) { - return icm20948::stop(busid); + return ThisDriver::module_stop(iterator); } - /* - * Print driver information. - */ - if (!strcmp(verb, "info")) { - return icm20948::info(busid); + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); } - return icm20948::usage(); + ThisDriver::print_usage(); + return -1; } diff --git a/src/drivers/imu/icm20948/icm20948_spi.cpp b/src/drivers/imu/icm20948/icm20948_spi.cpp index 0b8619e29f7..d086fddff78 100644 --- a/src/drivers/imu/icm20948/icm20948_spi.cpp +++ b/src/drivers/imu/icm20948/icm20948_spi.cpp @@ -59,12 +59,12 @@ #define ICM20948_LOW_SPI_BUS_SPEED 1000*1000 #define ICM20948_HIGH_SPI_BUS_SPEED 20*1000*1000 -device::Device *ICM20948_SPI_interface(int bus, uint32_t cs); +device::Device *ICM20948_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode); class ICM20948_SPI : public device::SPI { public: - ICM20948_SPI(int bus, uint32_t device); + ICM20948_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode); ~ICM20948_SPI() override = default; int read(unsigned address, void *data, unsigned count) override; @@ -80,17 +80,13 @@ private: }; device::Device * -ICM20948_SPI_interface(int bus, uint32_t cs) +ICM20948_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode) { - device::Device *interface = nullptr; - - interface = new ICM20948_SPI(bus, cs); - - return interface; + return new ICM20948_SPI(bus, devid, bus_frequency, spi_mode); } -ICM20948_SPI::ICM20948_SPI(int bus, uint32_t device) : - SPI("ICM20948", nullptr, bus, device, SPIDEV_MODE3, ICM20948_LOW_SPI_BUS_SPEED) +ICM20948_SPI::ICM20948_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) : + SPI("ICM20948", nullptr, bus, device, spi_mode, bus_frequency) { _device_id.devid_s.devtype = DRV_IMU_DEVTYPE_ICM20948; } diff --git a/src/drivers/imu/icm20948/mag_i2c.cpp b/src/drivers/imu/icm20948/mag_i2c.cpp index 3ac0eb077ad..57c6c11f0b6 100644 --- a/src/drivers/imu/icm20948/mag_i2c.cpp +++ b/src/drivers/imu/icm20948/mag_i2c.cpp @@ -42,14 +42,12 @@ #include -#ifdef USE_I2C - -device::Device *AK09916_I2C_interface(int bus); +device::Device *AK09916_I2C_interface(int bus, int bus_frequency); class AK09916_I2C : public device::I2C { public: - AK09916_I2C(int bus); + AK09916_I2C(int bus, int bus_frequency); ~AK09916_I2C() override = default; int read(unsigned address, void *data, unsigned count) override; @@ -61,13 +59,13 @@ protected: }; device::Device * -AK09916_I2C_interface(int bus) +AK09916_I2C_interface(int bus, int bus_frequency) { - return new AK09916_I2C(bus); + return new AK09916_I2C(bus, bus_frequency); } -AK09916_I2C::AK09916_I2C(int bus) : - I2C("AK09916_I2C", nullptr, bus, AK09916_I2C_ADDR, 400000) +AK09916_I2C::AK09916_I2C(int bus, int bus_frequency) : + I2C("AK09916_I2C", nullptr, bus, AK09916_I2C_ADDR, bus_frequency) { _device_id.devid_s.devtype = DRV_IMU_DEVTYPE_ICM20948; } @@ -109,5 +107,3 @@ AK09916_I2C::probe() return OK; } - -#endif // USE_I2C