diff --git a/src/drivers/imu/bmi160/bmi160.cpp b/src/drivers/imu/bmi160/bmi160.cpp index 62d3778794..0cf90dffa2 100644 --- a/src/drivers/imu/bmi160/bmi160.cpp +++ b/src/drivers/imu/bmi160/bmi160.cpp @@ -20,9 +20,9 @@ const uint8_t BMI160::_checked_registers[BMI160_NUM_CHECKED_REGISTERS] = { BM BMI160::BMI160(int bus, const char *path_accel, const char *path_gyro, uint32_t device, enum Rotation rotation) : SPI("BMI160", path_accel, bus, device, SPIDEV_MODE3, BMI160_BUS_SPEED), + ScheduledWorkItem(px4::device_bus_to_wq(this->get_device_id())), _gyro(new BMI160_gyro(this, path_gyro)), _whoami(0), - _call{}, _call_interval(0), _accel_reports(nullptr), _accel_scale{}, @@ -86,8 +86,6 @@ BMI160::BMI160(int bus, const char *path_accel, const char *path_gyro, uint32_t _gyro_scale.y_scale = 1.0f; _gyro_scale.z_offset = 0; _gyro_scale.z_scale = 1.0f; - - memset(&_call, 0, sizeof(_call)); } @@ -575,16 +573,16 @@ BMI160::ioctl(struct file *filp, int cmd, unsigned long arg) bool want_start = (_call_interval == 0); /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; + unsigned interval = 1000000 / arg; /* 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); @@ -597,15 +595,7 @@ BMI160::ioctl(struct file *filp, int cmd, unsigned long arg) /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ - _call_interval = ticks; - - /* - set call interval faster then 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 bmi160 clock - */ - _call.period = _call_interval - BMI160_TIMER_REDUCTION; + _call_interval = interval; /* if we need to start the poll state machine, do it */ if (want_start) { @@ -817,26 +807,22 @@ BMI160::start() _gyro_reports->flush(); /* start polling at the specified rate */ - hrt_call_every(&_call, - 1000, - _call_interval - BMI160_TIMER_REDUCTION, - (hrt_callout)&BMI160::measure_trampoline, this); + ScheduleOnInterval(_call_interval - BMI160_TIMER_REDUCTION, 1000); + reset(); } void BMI160::stop() { - hrt_cancel(&_call); + ScheduleClear(); } void -BMI160::measure_trampoline(void *arg) +BMI160::Run() { - BMI160 *dev = reinterpret_cast(arg); - /* make another measurement */ - dev->measure(); + measure(); } void diff --git a/src/drivers/imu/bmi160/bmi160.hpp b/src/drivers/imu/bmi160/bmi160.hpp index 8f0d9f25bf..c3b68ebf85 100644 --- a/src/drivers/imu/bmi160/bmi160.hpp +++ b/src/drivers/imu/bmi160/bmi160.hpp @@ -35,6 +35,7 @@ #include #include #include +#include #define DIR_READ 0x80 #define DIR_WRITE 0x00 @@ -243,7 +244,7 @@ class BMI160_gyro; -class BMI160 : public device::SPI +class BMI160 : public device::SPI, public px4::ScheduledWorkItem { public: BMI160(int bus, const char *path_accel, const char *path_gyro, uint32_t device, enum Rotation rotation); @@ -276,7 +277,6 @@ private: BMI160_gyro *_gyro; uint8_t _whoami; /** whoami result */ - struct hrt_call _call; unsigned _call_interval; ringbuffer::RingBuffer *_accel_reports; @@ -355,16 +355,7 @@ private: */ int reset(); - /** - * 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); + void Run() override; /** * Fetch measurements from the sensor and update the report buffers.