diff --git a/src/drivers/imu/mpu9250/CMakeLists.txt b/src/drivers/imu/mpu9250/CMakeLists.txt index bd40db5da7..833d2e8e58 100644 --- a/src/drivers/imu/mpu9250/CMakeLists.txt +++ b/src/drivers/imu/mpu9250/CMakeLists.txt @@ -45,5 +45,5 @@ px4_add_module( mag.cpp mag_i2c.cpp DEPENDS + px4_work_queue ) - diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp index 2d29eb6bb9..913ce7d547 100644 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ b/src/drivers/imu/mpu9250/mpu9250.cpp @@ -93,6 +93,7 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const const char *path_gyro, const char *path_mag, enum Rotation rotation, bool magnetometer_only) : + ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())), _interface(interface), _whoami(0), _accel(magnetometer_only ? nullptr : new MPU9250_accel(this, path_accel)), @@ -100,13 +101,6 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const _mag(new MPU9250_mag(this, mag_interface, path_mag)), _selected_bank(0xFF), // invalid/improbable bank value, will be set on first read/write _magnetometer_only(magnetometer_only), -#if defined(USE_I2C) - _work {}, - _use_hrt(false), -#else - _use_hrt(true), -#endif - _call {}, _call_interval(0), _accel_reports(nullptr), _accel_scale{}, @@ -232,16 +226,14 @@ MPU9250::init() { irqstate_t state; -#if defined(USE_I2C) - use_i2c(_interface->get_device_bus_type() == device::Device::DeviceBusType_I2C); -#endif - /* * If the MPU is using I2C we should reduce the sample rate to 200Hz and * make the integration autoreset faster so that we integrate just one * sample since the sampling rate is already low. */ - if (is_i2c() && !_magnetometer_only) { + const bool is_i2c = (_interface->get_device_bus_type() == device::Device::DeviceBusType_I2C); + + if (is_i2c && !_magnetometer_only) { _sample_rate = 200; _accel_int.set_autoreset_interval(1000000 / 1000); _gyro_int.set_autoreset_interval(1000000 / 1000); @@ -250,7 +242,7 @@ MPU9250::init() int ret = probe(); if (ret != OK) { - PX4_DEBUG("MPU9250 probe failed"); + PX4_DEBUG("probe failed"); return ret; } @@ -453,7 +445,8 @@ int MPU9250::reset_mpu() } // Enable I2C bus or Disable I2C bus (recommended on data sheet) - write_checked_reg(MPUREG_USER_CTRL, is_i2c() ? 0 : BIT_I2C_IF_DIS); + const bool is_i2c = (_interface->get_device_bus_type() == device::Device::DeviceBusType_I2C); + write_checked_reg(MPUREG_USER_CTRL, is_i2c ? 0 : BIT_I2C_IF_DIS); // SAMPLE RATE _set_sample_rate(_sample_rate); @@ -599,16 +592,16 @@ MPU9250::_set_pollrate(unsigned long rate) bool want_start = (_call_interval == 0); /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / rate; + unsigned interval = 1000000 / rate; /* check against maximum sane rate */ - if (ticks < 1000) { + if (interval < 1000) { return -EINVAL; } // adjust filters float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); - float sample_rate = 1.0e6f / ticks; + float sample_rate = 1.0e6f / interval; _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); @@ -621,15 +614,7 @@ MPU9250::_set_pollrate(unsigned long rate) /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ - _call_interval = ticks; - - /* - set call interval faster than the sample time. We - then detect when we have duplicate samples and reject - them. This prevents aliasing due to a beat between the - stm32 clock and the mpu9250 clock - */ - _call.period = _call_interval - MPU9250_TIMER_REDUCTION; + _call_interval = interval; /* if we need to start the poll state machine, do it */ if (want_start) { @@ -826,78 +811,20 @@ MPU9250::start() _mag->_mag_reports->flush(); - if (_use_hrt) { - /* start polling at the specified rate */ - hrt_call_every(&_call, - 1000, - _call_interval - MPU9250_TIMER_REDUCTION, - (hrt_callout)&MPU9250::measure_trampoline, this); - - } else { -#ifdef USE_I2C - /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&MPU9250::cycle_trampoline, this, 1); -#endif - } - + ScheduleOnInterval(_call_interval - MPU9250_TIMER_REDUCTION, 1000); } void MPU9250::stop() { - if (_use_hrt) { - hrt_cancel(&_call); - - } else { -#ifdef USE_I2C - work_cancel(HPWORK, &_work); -#endif - } -} - - -#if defined(USE_I2C) -void -MPU9250::cycle_trampoline(void *arg) -{ - MPU9250 *dev = (MPU9250 *)arg; - - dev->cycle(); + ScheduleClear(); } void -MPU9250::cycle() +MPU9250::Run() { - -// int ret = measure(); - - measure(); - -// if (ret != OK) { -// /* issue a reset command to the sensor */ -// reset(); -// start(); -// return; -// } - - if (_call_interval != 0) { - work_queue(HPWORK, - &_work, - (worker_t)&MPU9250::cycle_trampoline, - this, - USEC2TICK(_call_interval - MPU9250_TIMER_REDUCTION)); - } -} -#endif - - -void -MPU9250::measure_trampoline(void *arg) -{ - MPU9250 *dev = reinterpret_cast(arg); - /* make another measurement */ - dev->measure(); + measure(); } void diff --git a/src/drivers/imu/mpu9250/mpu9250.h b/src/drivers/imu/mpu9250/mpu9250.h index 3d25e52879..9987464c8b 100644 --- a/src/drivers/imu/mpu9250/mpu9250.h +++ b/src/drivers/imu/mpu9250/mpu9250.h @@ -36,8 +36,6 @@ #include #include -#include - #include #include @@ -49,9 +47,9 @@ #include #include #include +#include #include -#include #include "mag.h" #include "accel.h" @@ -253,7 +251,7 @@ class MPU9250_mag; class MPU9250_accel; class MPU9250_gyro; -class MPU9250 +class MPU9250 : public px4::ScheduledWorkItem { public: MPU9250(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro, @@ -281,6 +279,8 @@ protected: friend class MPU9250_mag; friend class MPU9250_gyro; + void Run() override; + private: MPU9250_accel *_accel; MPU9250_gyro *_gyro; @@ -289,16 +289,6 @@ private: bool _magnetometer_only; /* To disable accel and gyro reporting if only magnetometer is used (e.g. as external magnetometer) */ -#if defined(USE_I2C) - /* - * SPI bus based device use hrt - * I2C bus needs to use work queue - */ - work_s _work{}; -#endif - bool _use_hrt; - - struct hrt_call _call {}; unsigned _call_interval; ringbuffer::RingBuffer *_accel_reports; @@ -392,52 +382,6 @@ private: */ int reset_mpu(); - -#if defined(USE_I2C) - /** - * When the I2C interfase is on - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - * - * This is the heart of the measurement state machine. This function - * alternately starts a measurement, or collects the data from the - * previous measurement. - * - * When the interval between measurements is greater than the minimum - * measurement interval, a gap is inserted between collection - * and measurement to provide the most recent measurement possible - * at the next interval. - */ - void cycle(); - - /** - * Static trampoline from the workq context; because we don't have a - * generic workq wrapper yet. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void cycle_trampoline(void *arg); - - void use_i2c(bool on_true) { _use_hrt = !on_true; } - -#endif - - bool is_i2c(void) { return !_use_hrt; } - - - - - /** - * Static trampoline from the hrt_call context; because we don't have a - * generic hrt wrapper yet. - * - * Called by the HRT in interrupt context at the specified rate if - * automatic polling is enabled. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void measure_trampoline(void *arg); - /** * Fetch measurements from the sensor and update the report buffers. */