diff --git a/boards/bitcraze/crazyflie/init/rc.board_sensors b/boards/bitcraze/crazyflie/init/rc.board_sensors index 12d5625e4e..287c217a66 100644 --- a/boards/bitcraze/crazyflie/init/rc.board_sensors +++ b/boards/bitcraze/crazyflie/init/rc.board_sensors @@ -6,7 +6,7 @@ adc start # Onboard I2C -mpu9250 -R 12 start +mpu9250 -I -R 12 start # I2C bypass of mpu lps25h -I start diff --git a/boards/mro/x21-777/init/rc.board_sensors b/boards/mro/x21-777/init/rc.board_sensors index 85f6735ed7..4a43aef12c 100644 --- a/boards/mro/x21-777/init/rc.board_sensors +++ b/boards/mro/x21-777/init/rc.board_sensors @@ -16,7 +16,7 @@ mpu6000 -s -R 2 -T 20608 start mpu6000 -s -R 2 -T 20602 start # Internal SPI bus mpu9250 is rotated 90 deg yaw -mpu9250 -R 2 start +mpu9250 -s -R 2 start # Possible external compasses ist8310 -X start diff --git a/boards/mro/x21/init/rc.board_sensors b/boards/mro/x21/init/rc.board_sensors index 23c091eca3..5fcee1c1c6 100644 --- a/boards/mro/x21/init/rc.board_sensors +++ b/boards/mro/x21/init/rc.board_sensors @@ -18,7 +18,7 @@ mpu6000 -s -R 2 -T 20608 start mpu6000 -s -R 2 -T 20602 start # Internal SPI bus mpu9250 is rotated 90 deg yaw -mpu9250 -R 2 start +mpu9250 -s -R 2 start # Internal SPI ms5611 -s start diff --git a/boards/px4/fmu-v2/init/rc.board_sensors b/boards/px4/fmu-v2/init/rc.board_sensors index 0b5317f3a2..b7d3afca17 100644 --- a/boards/px4/fmu-v2/init/rc.board_sensors +++ b/boards/px4/fmu-v2/init/rc.board_sensors @@ -37,8 +37,8 @@ then set BOARD_FMUV3 20 else # Check for Pixhawk 2.1 cube - # external MPU9250 is rotated 180 degrees yaw - if mpu9250 -S -R 4 start + # (external) MPU9250 is rotated 180 degrees yaw + if mpu9250 -s -R 4 start then set BOARD_FMUV3 21 fi @@ -57,8 +57,8 @@ then set BOARD_FMUV3 20 else # Check for Pixhack 3.1 - # external MPU9250 is rotated 180 degrees yaw - if mpu9250 -S -R 4 start + # (external) MPU9250 is rotated 180 degrees yaw + if mpu9250 -s -R 4 start then set BOARD_FMUV3 21 fi @@ -101,7 +101,7 @@ else # V3 build hwtypecmp supports V2|V2M|V30 if ! ver hwtypecmp V2M then - mpu9250 start + mpu9250 -s start # else: On the PixhawkMini the mpu9250 has been disabled due to HW errata fi diff --git a/boards/px4/fmu-v3/init/rc.board_sensors b/boards/px4/fmu-v3/init/rc.board_sensors index 99ca3f4b59..f634af508f 100644 --- a/boards/px4/fmu-v3/init/rc.board_sensors +++ b/boards/px4/fmu-v3/init/rc.board_sensors @@ -38,8 +38,8 @@ then set BOARD_FMUV3 20 else # Check for Pixhawk 2.1 cube - # external MPU9250 is rotated 180 degrees yaw - if mpu9250 -S -R 4 start + # (external) MPU9250 is rotated 180 degrees yaw + if mpu9250 -s -R 4 start then set BOARD_FMUV3 21 fi @@ -58,8 +58,8 @@ then set BOARD_FMUV3 20 else # Check for Pixhack 3.1 - # external MPU9250 is rotated 180 degrees yaw - if mpu9250 -S -R 4 start + # (external) MPU9250 is rotated 180 degrees yaw + if mpu9250 -s -R 4 start then set BOARD_FMUV3 21 fi @@ -109,7 +109,7 @@ else # V3 build hwtypecmp supports V2|V2M|V30 if ! ver hwtypecmp V2M then - mpu9250 start + mpu9250 -s start # else: On the PixhawkMini the mpu9250 has been disabled due to HW errata fi diff --git a/boards/px4/fmu-v4/init/rc.board_sensors b/boards/px4/fmu-v4/init/rc.board_sensors index 8d9f9f027d..55ce160c66 100644 --- a/boards/px4/fmu-v4/init/rc.board_sensors +++ b/boards/px4/fmu-v4/init/rc.board_sensors @@ -24,13 +24,6 @@ rm3100 start # Internal SPI ms5611 -s start -# For Teal One airframe -if param compare SYS_AUTOSTART 4250 -then - mpu9250 -s -R 14 start - mpu9250 -t -R 14 start -fi - # hmc5883 internal SPI bus is rotated 90 deg yaw if ! hmc5883 -T -S -R 2 start then @@ -49,5 +42,11 @@ then icm20608g -R 8 start fi -# mpu9250 internal SPI bus mpu9250 is rotated 90 deg yaw -mpu9250 -R 2 start +# For Teal One airframe +if param compare SYS_AUTOSTART 4250 +then + mpu9250 -s -R 14 start +else + # mpu9250 internal SPI bus mpu9250 is rotated 90 deg yaw + mpu9250 -s -R 2 start +fi diff --git a/boards/px4/fmu-v4pro/init/rc.board_sensors b/boards/px4/fmu-v4pro/init/rc.board_sensors index da5b8b268b..f02d67d3c2 100644 --- a/boards/px4/fmu-v4pro/init/rc.board_sensors +++ b/boards/px4/fmu-v4pro/init/rc.board_sensors @@ -14,7 +14,7 @@ icm20608g -R 8 start icm20602 -s -R 8 start # Internal SPI bus mpu9250 -mpu9250 -R 2 start +mpu9250 -s -R 2 start # Internal SPI bus lis3mdl -R 0 start diff --git a/boards/uvify/core/init/rc.board_sensors b/boards/uvify/core/init/rc.board_sensors index 1b8919cc01..58c2b92065 100644 --- a/boards/uvify/core/init/rc.board_sensors +++ b/boards/uvify/core/init/rc.board_sensors @@ -18,7 +18,7 @@ then rgbled_ncp5623c start -X -a 0x38 #mpu6000 -s -R 4 -T 20608 start - mpu9250 -R 4 start + mpu9250 -s -R 4 start # Default GNSS with LIS3MDL magnetometer with external i2c. lis3mdl -R 2 -X start @@ -27,7 +27,7 @@ fi # Draco if param compare SYS_AUTOSTART 4072 then - mpu9250 -R 2 start + mpu9250 -s -R 2 start fi # IFO @@ -39,6 +39,6 @@ then # IFO rgb LED pca9685 start - mpu9250 -R 2 start + mpu9250 -s -R 2 start lis3mdl -R 2 -X start fi diff --git a/posix-configs/bbblue/px4.config b/posix-configs/bbblue/px4.config index cf83992c1a..08275729b5 100644 --- a/posix-configs/bbblue/px4.config +++ b/posix-configs/bbblue/px4.config @@ -47,7 +47,7 @@ dataman start bmp280 -I start -mpu9250 start +mpu9250 -I start # options: -R rotation gps start -d /dev/ttyS2 -s -p ubx diff --git a/posix-configs/bbblue/px4_fw.config b/posix-configs/bbblue/px4_fw.config index 2a50222b6f..03be2746ef 100644 --- a/posix-configs/bbblue/px4_fw.config +++ b/posix-configs/bbblue/px4_fw.config @@ -47,7 +47,7 @@ dataman start bmp280 -I start -mpu9250 start +mpu9250 -I start # options: -R rotation gps start -d /dev/ttyS2 -s -p ubx diff --git a/posix-configs/eagle/200qx/px4.config b/posix-configs/eagle/200qx/px4.config index 8ea74aab74..b67e90d99f 100644 --- a/posix-configs/eagle/200qx/px4.config +++ b/posix-configs/eagle/200qx/px4.config @@ -18,7 +18,7 @@ param set SENS_BOARD_ROT 0 sleep 1 -mpu9250 start +mpu9250 -s start bmp280 -I start gps start -d /dev/tty-4 rc_update start diff --git a/posix-configs/eagle/210qc/px4.config b/posix-configs/eagle/210qc/px4.config index 2adeb36f8f..f129ddfbd4 100644 --- a/posix-configs/eagle/210qc/px4.config +++ b/posix-configs/eagle/210qc/px4.config @@ -18,7 +18,7 @@ param set ATT_W_MAG 0.00 param set SENS_BOARD_ROT 0 sleep 1 -mpu9250 start +mpu9250 -s start bmp280 -I start gps start -d /dev/tty-4 rc_update start diff --git a/posix-configs/excelsior/px4.config b/posix-configs/excelsior/px4.config index f7de529cf5..7f66c02ae4 100644 --- a/posix-configs/excelsior/px4.config +++ b/posix-configs/excelsior/px4.config @@ -17,7 +17,7 @@ param set MC_ROLLRATE_D 0.001 param set SENS_BOARD_ROT 4 sleep 1 -mpu9250 start +mpu9250 -s start bmp280 -I start rc_update start sensors start diff --git a/posix-configs/ocpoc/px4.config b/posix-configs/ocpoc/px4.config index 5c91ca04f5..e8058f0db2 100644 --- a/posix-configs/ocpoc/px4.config +++ b/posix-configs/ocpoc/px4.config @@ -15,7 +15,7 @@ param set MAV_BROADCAST 1 param set MAV_TYPE 2 param set MAV_SYS_ID 1 -mpu9250 -R 10 start +mpu9250 -s -R 10 start hmc5883 -I start ms5611 -s start diff --git a/posix-configs/rpi/px4.config b/posix-configs/rpi/px4.config index e14cb29fb4..0792152875 100644 --- a/posix-configs/rpi/px4.config +++ b/posix-configs/rpi/px4.config @@ -17,7 +17,7 @@ param set MAV_TYPE 2 dataman start -mpu9250 -R 8 start +mpu9250 -s -R 8 start lsm9ds1 -s -R 4 start lsm9ds1_mag -s -R 4 start ms5611 -X start diff --git a/posix-configs/rpi/px4_fw.config b/posix-configs/rpi/px4_fw.config index 3bdad55e8b..52dd94f3dc 100644 --- a/posix-configs/rpi/px4_fw.config +++ b/posix-configs/rpi/px4_fw.config @@ -17,7 +17,7 @@ param set MAV_TYPE 1 dataman start -mpu9250 -R 8 start +mpu9250 -s -R 8 start lsm9ds1 -s -R 4 start lsm9ds1_mag -s -R 4 start ms5611 -X start diff --git a/posix-configs/rpi/px4_test.config b/posix-configs/rpi/px4_test.config index 1d896db9a2..c48a4a048a 100644 --- a/posix-configs/rpi/px4_test.config +++ b/posix-configs/rpi/px4_test.config @@ -17,7 +17,7 @@ param set MAV_TYPE 2 dataman start -mpu9250 -R 8 start +mpu9250 -s -R 8 start lsm9ds1 -s -R 4 start lsm9ds1_mag -s -R 4 start ms5611 -X start diff --git a/src/drivers/imu/mpu9250/AK8963_I2C.cpp b/src/drivers/imu/mpu9250/AK8963_I2C.cpp index 877f8d6629..fe5cfb5f22 100644 --- a/src/drivers/imu/mpu9250/AK8963_I2C.cpp +++ b/src/drivers/imu/mpu9250/AK8963_I2C.cpp @@ -46,12 +46,12 @@ #ifdef USE_I2C -device::Device *AK8963_I2C_interface(int bus); +device::Device *AK8963_I2C_interface(int bus, int bus_frequency); class AK8963_I2C : public device::I2C { public: - AK8963_I2C(int bus); + AK8963_I2C(int bus, int bus_frequency); ~AK8963_I2C() override = default; int read(unsigned address, void *data, unsigned count) override; @@ -63,12 +63,12 @@ protected: }; device::Device * -AK8963_I2C_interface(int bus) +AK8963_I2C_interface(int bus, int bus_frequency) { - return new AK8963_I2C(bus); + return new AK8963_I2C(bus, bus_frequency); } -AK8963_I2C::AK8963_I2C(int bus) : I2C("AK8963_I2C", nullptr, bus, AK8963_I2C_ADDR, 400000) +AK8963_I2C::AK8963_I2C(int bus, int bus_frequency) : I2C("AK8963_I2C", nullptr, bus, AK8963_I2C_ADDR, bus_frequency) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_MPU9250; } diff --git a/src/drivers/imu/mpu9250/MPU9250_mag.h b/src/drivers/imu/mpu9250/MPU9250_mag.h index 383ea24021..5a504d4868 100644 --- a/src/drivers/imu/mpu9250/MPU9250_mag.h +++ b/src/drivers/imu/mpu9250/MPU9250_mag.h @@ -105,9 +105,7 @@ struct ak8963_regs { }; #pragma pack(pop) -extern device::Device *AK8963_I2C_interface(int bus); - -typedef device::Device *(*MPU9250_mag_constructor)(int, bool); +extern device::Device *AK8963_I2C_interface(int bus, int bus_frequency); /** * Helper class implementing the magnetometer driver node. diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp index 336db75f86..c2916bafab 100644 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ b/src/drivers/imu/mpu9250/mpu9250.cpp @@ -70,8 +70,9 @@ const uint16_t MPU9250::_mpu9250_checked_registers[MPU9250_NUM_CHECKED_REGISTERS MPUREG_INT_PIN_CFG }; -MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, enum Rotation rotation) : - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), +MPU9250::MPU9250(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_MAX : ORB_PRIO_HIGH), rotation), _px4_gyro(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH), rotation), @@ -87,9 +88,6 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, enum MPU9250::~MPU9250() { - // make sure we are truly inactive - stop(); - // delete the perf counter perf_free(_sample_perf); perf_free(_bad_registers); @@ -473,20 +471,11 @@ MPU9250::set_accel_range(unsigned max_g_in) void MPU9250::start() { - /* make sure we are stopped first */ - stop(); - ScheduleOnInterval(_call_interval - MPU9250_TIMER_REDUCTION, 1000); } void -MPU9250::stop() -{ - ScheduleClear(); -} - -void -MPU9250::Run() +MPU9250::RunImpl() { /* make another measurement */ measure(); @@ -676,8 +665,9 @@ MPU9250::measure() } void -MPU9250::print_info() +MPU9250::print_status() { + I2CSPIDriverBase::print_status(); perf_print_counter(_sample_perf); perf_print_counter(_bad_registers); perf_print_counter(_duplicates); diff --git a/src/drivers/imu/mpu9250/mpu9250.h b/src/drivers/imu/mpu9250/mpu9250.h index 30508bcbba..ece19ad6c3 100644 --- a/src/drivers/imu/mpu9250/mpu9250.h +++ b/src/drivers/imu/mpu9250/mpu9250.h @@ -37,13 +37,13 @@ #include #include #include -#include +#include #include #include #include "MPU9250_mag.h" -#if defined(PX4_I2C_OBDEV_MPU9250) || defined(PX4_I2C_BUS_EXPANSION) +#if defined(PX4_I2C_OBDEV_MPU9250) # define USE_I2C #endif @@ -224,40 +224,42 @@ struct MPUReport { static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; } /* interface factories */ -extern device::Device *MPU9250_SPI_interface(int bus, uint32_t cs); -extern device::Device *MPU9250_I2C_interface(int bus, uint32_t address); -extern int MPU9250_probe(device::Device *dev); - -typedef device::Device *(*MPU9250_constructor)(int, uint32_t); +extern device::Device *MPU9250_SPI_interface(int bus, uint32_t cs, int bus_frequency, spi_mode_e spi_mode); +extern device::Device *MPU9250_I2C_interface(int bus, uint32_t address, int bus_frequency); class MPU9250_mag; -class MPU9250 : public px4::ScheduledWorkItem +class MPU9250 : public I2CSPIDriver { public: - MPU9250(device::Device *interface, device::Device *mag_interface, enum Rotation rotation); + MPU9250(device::Device *interface, device::Device *mag_interface, enum Rotation rotation, I2CSPIBusOption bus_option, + int bus); virtual ~MPU9250(); - virtual int init(); + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + + int init(); uint8_t get_whoami() { return _whoami; } /** * Diagnostics - print some basic information about the driver. */ - void print_info(); + void print_status() override; + + void RunImpl(); protected: device::Device *_interface; uint8_t _whoami{0}; /** whoami result */ - virtual int probe(); + int probe(); friend class MPU9250_mag; private: - void Run() override; - PX4Accelerometer _px4_accel; PX4Gyroscope _px4_gyro; @@ -300,7 +302,6 @@ private: bool _got_duplicate{false}; void start(); - void stop(); int reset(); /** diff --git a/src/drivers/imu/mpu9250/mpu9250_i2c.cpp b/src/drivers/imu/mpu9250/mpu9250_i2c.cpp index 7704838fa1..adb3016128 100644 --- a/src/drivers/imu/mpu9250/mpu9250_i2c.cpp +++ b/src/drivers/imu/mpu9250/mpu9250_i2c.cpp @@ -48,7 +48,7 @@ device::Device *MPU9250_I2C_interface(int bus, uint32_t address); class MPU9250_I2C : public device::I2C { public: - MPU9250_I2C(int bus, uint32_t address); + MPU9250_I2C(int bus, uint32_t address, int bus_frequency); ~MPU9250_I2C() override = default; int read(unsigned address, void *data, unsigned count) override; @@ -62,13 +62,13 @@ private: }; device::Device * -MPU9250_I2C_interface(int bus, uint32_t address) +MPU9250_I2C_interface(int bus, uint32_t address, int bus_frequency) { - return new MPU9250_I2C(bus, address); + return new MPU9250_I2C(bus, address, bus_frequency); } -MPU9250_I2C::MPU9250_I2C(int bus, uint32_t address) : - I2C("MPU9250_I2C", nullptr, bus, address, 400000) +MPU9250_I2C::MPU9250_I2C(int bus, uint32_t address, int bus_frequency) : + I2C("MPU9250_I2C", nullptr, bus, address, bus_frequency) { set_device_type(DRV_IMU_DEVTYPE_MPU9250); } diff --git a/src/drivers/imu/mpu9250/mpu9250_main.cpp b/src/drivers/imu/mpu9250/mpu9250_main.cpp index 71deffe5be..11fe2a0e7b 100644 --- a/src/drivers/imu/mpu9250/mpu9250_main.cpp +++ b/src/drivers/imu/mpu9250/mpu9250_main.cpp @@ -31,234 +31,106 @@ * ****************************************************************************/ -#include -#include +#include +#include #include "mpu9250.h" -enum class MPU9250_BUS { - ALL = 0, - I2C_INTERNAL, - I2C_EXTERNAL, - SPI_INTERNAL, - SPI_INTERNAL2, - SPI_EXTERNAL -}; - -namespace mpu9250 +void +MPU9250::print_usage() { - -// list of supported bus configurations -struct mpu9250_bus_option { - MPU9250_BUS busid; - MPU9250_constructor interface_constructor; - bool magpassthrough; - uint8_t busnum; - uint32_t address; - MPU9250 *dev; -} bus_options[] = { -#if defined(USE_I2C) -# if defined(PX4_I2C_BUS_ONBOARD) && defined(PX4_I2C_OBDEV_MPU9250) - { MPU9250_BUS::I2C_INTERNAL, &MPU9250_I2C_interface, false, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_MPU9250, nullptr }, -# endif -# if defined(PX4_I2C_BUS_EXPANSION) && defined(PX4_I2C_OBDEV_MPU9250) - { MPU9250_BUS::I2C_EXTERNAL, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION, PX4_I2C_OBDEV_MPU9250, nullptr }, -# endif -# if defined(PX4_I2C_BUS_EXPANSION1) && defined(PX4_I2C_OBDEV_MPU9250) - { MPU9250_BUS::I2C_EXTERNAL, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION1, PX4_I2C_OBDEV_MPU9250, nullptr }, -# endif -# if defined(PX4_I2C_BUS_EXPANSION2) && defined(PX4_I2C_OBDEV_MPU9250) - { MPU9250_BUS::I2C_EXTERNAL, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION2, PX4_I2C_OBDEV_MPU9250, nullptr }, -# endif -#endif -#if defined(PX4_SPI_BUS_SENSORS) && defined(PX4_SPIDEV_MPU) - { MPU9250_BUS::SPI_INTERNAL, &MPU9250_SPI_interface, true, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_MPU, nullptr }, -#endif -#if defined(PX4_SPI_BUS_SENSORS) && defined(PX4_SPIDEV_MPU2) - { MPU9250_BUS::SPI_INTERNAL2, &MPU9250_SPI_interface, true, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_MPU2, nullptr }, -#endif -#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_MPU) - { MPU9250_BUS::SPI_EXTERNAL, &MPU9250_SPI_interface, true, PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_MPU, nullptr }, -#endif -}; - -// find a bus structure for a busid -static mpu9250_bus_option *find_bus(MPU9250_BUS busid) -{ - for (mpu9250_bus_option &bus_option : bus_options) { - if ((busid == MPU9250_BUS::ALL || - busid == bus_option.busid) && bus_option.dev != nullptr) { - - return &bus_option; - } - } - - return nullptr; + PRINT_MODULE_USAGE_NAME("mpu9250", "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(); } -static bool start_bus(mpu9250_bus_option &bus, enum Rotation rotation) +I2CSPIDriverBase *MPU9250::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { - device::Device *interface = bus.interface_constructor(bus.busnum, bus.address); - - if ((interface == nullptr) || (interface->init() != PX4_OK)) { - PX4_WARN("no device on bus %u", (unsigned)bus.busid); - delete interface; - return false; - } - + device::Device *interface = nullptr; device::Device *mag_interface = nullptr; + if (iterator.busType() == BOARD_I2C_BUS) { #ifdef USE_I2C - - // For i2c interfaces, connect to the magnetomer directly - if ((bus.busid == MPU9250_BUS::I2C_INTERNAL) || (bus.busid == MPU9250_BUS::I2C_EXTERNAL)) { - mag_interface = AK8963_I2C_interface(bus.busnum); - } - + interface = MPU9250_I2C_interface(iterator.bus(), PX4_I2C_OBDEV_MPU9250, cli.bus_frequency); + // For i2c interfaces, connect to the magnetomer directly + mag_interface = AK8963_I2C_interface(iterator.bus(), cli.bus_frequency); #endif // USE_I2C - MPU9250 *dev = new MPU9250(interface, mag_interface, rotation); + } else if (iterator.busType() == BOARD_SPI_BUS) { + interface = MPU9250_SPI_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode); + } - if (dev == nullptr) { + if (interface == nullptr) { PX4_ERR("alloc failed"); delete mag_interface; - return false; + return nullptr; } - if (dev->init() != PX4_OK) { - PX4_ERR("driver start failed"); - delete dev; + if (interface->init() != OK) { + delete interface; delete mag_interface; - return false; + PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + return nullptr; } - bus.dev = dev; - - return true; -} - -static int start(MPU9250_BUS busid, enum Rotation rotation) -{ - for (mpu9250_bus_option &bus_option : bus_options) { - if (bus_option.dev != nullptr) { - // this device is already started - PX4_WARN("already started"); - continue; - } - - if (busid != MPU9250_BUS::ALL && bus_option.busid != busid) { - // not the one that is asked for - continue; - } - - if (start_bus(bus_option, rotation)) { - return 0; - } - } - - return -1; -} - -static int stop(MPU9250_BUS busid) -{ - mpu9250_bus_option *bus = find_bus(busid); + MPU9250 *dev = new MPU9250(interface, mag_interface, cli.rotation, iterator.configuredBusOption(), iterator.bus()); - if (bus != nullptr && bus->dev != nullptr) { - delete bus->dev; - bus->dev = nullptr; - - } else { - PX4_WARN("driver not running"); - return -1; + if (dev == nullptr) { + delete interface; + delete mag_interface; + return nullptr; } - return 0; -} - -static int status(MPU9250_BUS busid) -{ - mpu9250_bus_option *bus = find_bus(busid); - - if (bus != nullptr && bus->dev != nullptr) { - bus->dev->print_info(); - return 0; + if (OK != dev->init()) { + delete dev; + return nullptr; } - PX4_WARN("driver not running"); - return -1; -} - -static int usage() -{ - PX4_INFO("missing command: try 'start', 'stop', 'status'"); - 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 0; + return dev; } -} // namespace - -extern "C" int mpu9250_main(int argc, char *argv[]) +extern "C" int +mpu9250_main(int argc, char *argv[]) { - int myoptind = 1; int ch; - const char *myoptarg = nullptr; - - MPU9250_BUS busid = MPU9250_BUS::ALL; - enum Rotation rotation = ROTATION_NONE; + using ThisDriver = MPU9250; + BusCLIArguments cli{true, true}; + cli.default_spi_frequency = 1000 * 1000; // low speed bus frequency + cli.default_i2c_frequency = 400000; - while ((ch = px4_getopt(argc, argv, "XISstR:", &myoptind, &myoptarg)) != EOF) { + while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { switch (ch) { - case 'X': - busid = MPU9250_BUS::I2C_EXTERNAL; - break; - - case 'I': - busid = MPU9250_BUS::I2C_INTERNAL; - break; - - case 'S': - busid = MPU9250_BUS::SPI_EXTERNAL; - break; - - case 's': - busid = MPU9250_BUS::SPI_INTERNAL; - break; - - case 't': - busid = MPU9250_BUS::SPI_INTERNAL2; - break; - case 'R': - rotation = (enum Rotation)atoi(myoptarg); + cli.rotation = (enum Rotation)atoi(cli.optarg()); break; - - default: - return mpu9250::usage(); } } - if (myoptind >= argc) { - return mpu9250::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_MPU9250); if (!strcmp(verb, "start")) { - return mpu9250::start(busid, rotation); + return ThisDriver::module_start(cli, iterator); + } - } else if (!strcmp(verb, "stop")) { - return mpu9250::stop(busid); + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } - } else if (!strcmp(verb, "status")) { - return mpu9250::status(busid); + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); } - return mpu9250::usage(); + ThisDriver::print_usage(); + return -1; } diff --git a/src/drivers/imu/mpu9250/mpu9250_spi.cpp b/src/drivers/imu/mpu9250/mpu9250_spi.cpp index 2badcf0f05..0c3c65d7f7 100644 --- a/src/drivers/imu/mpu9250/mpu9250_spi.cpp +++ b/src/drivers/imu/mpu9250/mpu9250_spi.cpp @@ -59,12 +59,12 @@ #define MPU9250_LOW_SPI_BUS_SPEED 1000*1000 #define MPU9250_HIGH_SPI_BUS_SPEED 20*1000*1000 -device::Device *MPU9250_SPI_interface(int bus, uint32_t cs); +device::Device *MPU9250_SPI_interface(int bus, uint32_t cs, int bus_frequency, spi_mode_e spi_mode); class MPU9250_SPI : public device::SPI { public: - MPU9250_SPI(int bus, uint32_t device); + MPU9250_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode); ~MPU9250_SPI() override = default; int read(unsigned address, void *data, unsigned count) override; @@ -80,13 +80,13 @@ private: }; device::Device * -MPU9250_SPI_interface(int bus, uint32_t cs) +MPU9250_SPI_interface(int bus, uint32_t cs, int bus_frequency, spi_mode_e spi_mode) { - return new MPU9250_SPI(bus, cs); + return new MPU9250_SPI(bus, cs, bus_frequency, spi_mode); } -MPU9250_SPI::MPU9250_SPI(int bus, uint32_t device) : - SPI("MPU9250", nullptr, bus, device, SPIDEV_MODE3, MPU9250_LOW_SPI_BUS_SPEED) +MPU9250_SPI::MPU9250_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) : + SPI("MPU9250", nullptr, bus, device, spi_mode, bus_frequency) { set_device_type(DRV_IMU_DEVTYPE_MPU9250); }