From 8b4ecc694751fe7e1ccba84af3e063e1e007ffb7 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 7 Feb 2019 10:50:00 -0500 Subject: [PATCH] icm20948 move to px4 work queue --- src/drivers/imu/icm20948/icm20948.cpp | 106 ++++---------------------- src/drivers/imu/icm20948/icm20948.h | 63 +-------------- 2 files changed, 19 insertions(+), 150 deletions(-) diff --git a/src/drivers/imu/icm20948/icm20948.cpp b/src/drivers/imu/icm20948/icm20948.cpp index 6467bc9039..5dda0970ac 100644 --- a/src/drivers/imu/icm20948/icm20948.cpp +++ b/src/drivers/imu/icm20948/icm20948.cpp @@ -98,6 +98,7 @@ ICM20948::ICM20948(device::Device *interface, device::Device *mag_interface, con 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 ICM20948_accel(this, path_accel)), @@ -105,13 +106,6 @@ ICM20948::ICM20948(device::Device *interface, device::Device *mag_interface, con _mag(new ICM20948_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{}, @@ -239,16 +233,14 @@ ICM20948::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); @@ -257,7 +249,7 @@ ICM20948::init() int ret = probe(); if (ret != OK) { - PX4_DEBUG("ICM20948 probe failed"); + PX4_DEBUG("probe failed"); return ret; } @@ -457,10 +449,8 @@ int ICM20948::reset_mpu() } // Enable I2C bus or Disable I2C bus (recommended on data sheet) - - - write_checked_reg(MPU_OR_ICM(MPUREG_USER_CTRL, ICMREG_20948_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(ICMREG_20948_USER_CTRL, is_i2c ? 0 : BIT_I2C_IF_DIS); // SAMPLE RATE _set_sample_rate(_sample_rate); @@ -617,16 +607,16 @@ ICM20948::_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); @@ -639,15 +629,7 @@ ICM20948::_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) { @@ -955,78 +937,20 @@ ICM20948::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)&ICM20948::measure_trampoline, this); - - } else { -#ifdef USE_I2C - /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&ICM20948::cycle_trampoline, this, 1); -#endif - } - + ScheduleOnInterval(_call_interval - MPU9250_TIMER_REDUCTION, 10000); } void ICM20948::stop() { - if (_use_hrt) { - hrt_cancel(&_call); - - } else { -#ifdef USE_I2C - work_cancel(HPWORK, &_work); -#endif - } -} - - -#if defined(USE_I2C) -void -ICM20948::cycle_trampoline(void *arg) -{ - ICM20948 *dev = (ICM20948 *)arg; - - dev->cycle(); + ScheduleClear(); } void -ICM20948::cycle() +ICM20948::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)&ICM20948::cycle_trampoline, - this, - USEC2TICK(_call_interval - MPU9250_TIMER_REDUCTION)); - } -} -#endif - - -void -ICM20948::measure_trampoline(void *arg) -{ - ICM20948 *dev = reinterpret_cast(arg); - /* make another measurement */ - dev->measure(); + measure(); } void diff --git a/src/drivers/imu/icm20948/icm20948.h b/src/drivers/imu/icm20948/icm20948.h index 35586514ae..7049beb10a 100644 --- a/src/drivers/imu/icm20948/icm20948.h +++ b/src/drivers/imu/icm20948/icm20948.h @@ -36,8 +36,6 @@ #include #include -#include - #include #include @@ -49,6 +47,7 @@ #include #include #include +#include #include #include @@ -374,7 +373,7 @@ class ICM20948_mag; class ICM20948_accel; class ICM20948_gyro; -class ICM20948 +class ICM20948 : public px4::ScheduledWorkItem { public: ICM20948(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro, @@ -402,6 +401,8 @@ protected: friend class ICM20948_mag; friend class ICM20948_gyro; + void Run() override; + private: ICM20948_accel *_accel; ICM20948_gyro *_gyro; @@ -410,16 +411,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; @@ -515,52 +506,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. */