diff --git a/src/drivers/imu/adis16477/ADIS16477.cpp b/src/drivers/imu/adis16477/ADIS16477.cpp index 7f5e7b997a..87d8551200 100644 --- a/src/drivers/imu/adis16477/ADIS16477.cpp +++ b/src/drivers/imu/adis16477/ADIS16477.cpp @@ -79,6 +79,7 @@ using namespace time_literals; ADIS16477::ADIS16477(int bus, const char *path_accel, const char *path_gyro, uint32_t device, enum Rotation rotation) : SPI("ADIS16477", path_accel, bus, device, SPIDEV_MODE3, 1000000), + ScheduledWorkItem(px4::device_bus_to_wq(this->get_device_id())), _gyro(new ADIS16477_gyro(this, path_gyro)), _sample_perf(perf_alloc(PC_ELAPSED, "adis16477_read")), _bad_transfers(perf_alloc(PC_COUNT, "adis16477_bad_transfers")), @@ -358,16 +359,16 @@ ADIS16477::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); @@ -379,7 +380,7 @@ ADIS16477::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.period = _call_interval = ticks; + _call_interval = interval; /* if we need to start the poll state machine, do it */ if (want_start) { @@ -478,22 +479,20 @@ ADIS16477::start() _call_interval = last_call_interval; /* start polling at the specified rate */ - hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&ADIS16477::measure_trampoline, this); + ScheduleOnInterval(_call_interval, 10000); } void ADIS16477::stop() { - hrt_cancel(&_call); + ScheduleClear(); } void -ADIS16477::measure_trampoline(void *arg) +ADIS16477::Run() { - ADIS16477 *dev = reinterpret_cast(arg); - /* make another measurement */ - dev->measure(); + measure(); } int diff --git a/src/drivers/imu/adis16477/ADIS16477.hpp b/src/drivers/imu/adis16477/ADIS16477.hpp index 0c5f875224..ab7b0edb9e 100644 --- a/src/drivers/imu/adis16477/ADIS16477.hpp +++ b/src/drivers/imu/adis16477/ADIS16477.hpp @@ -41,7 +41,6 @@ #include #include -#include #include #include #include @@ -50,6 +49,7 @@ #include #include #include +#include #define ADIS16477_GYRO_DEFAULT_RATE 250 #define ADIS16477_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 @@ -62,7 +62,7 @@ class ADIS16477_gyro; -class ADIS16477 : public device::SPI +class ADIS16477 : public device::SPI, public px4::ScheduledWorkItem { public: ADIS16477(int bus, const char *path_accel, const char *path_gyro, uint32_t device, enum Rotation rotation); @@ -86,7 +86,6 @@ private: uint16_t _product{0}; /** product code */ - struct hrt_call _call {}; unsigned _call_interval{0}; struct gyro_calibration_s _gyro_scale {}; @@ -163,16 +162,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.