From fc7f1ca598a7bfa0ba05043c88522c3a28fecaf3 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 11 Feb 2019 11:50:18 -0500 Subject: [PATCH] fxos8701cq move to px4 work queue --- src/drivers/imu/fxos8701cq/fxos8701cq.cpp | 88 +++++++---------------- 1 file changed, 27 insertions(+), 61 deletions(-) diff --git a/src/drivers/imu/fxos8701cq/fxos8701cq.cpp b/src/drivers/imu/fxos8701cq/fxos8701cq.cpp index cad790633f5c..d7b6cb4cd6fc 100644 --- a/src/drivers/imu/fxos8701cq/fxos8701cq.cpp +++ b/src/drivers/imu/fxos8701cq/fxos8701cq.cpp @@ -78,6 +78,7 @@ #include #include #include +#include /* SPI protocol address bits */ #define DIR_READ(a) ((a) & 0x7f) @@ -159,7 +160,7 @@ extern "C" { __EXPORT int fxos8701cq_main(int argc, char *argv[]); } class FXOS8701CQ_mag; #endif -class FXOS8701CQ : public device::SPI +class FXOS8701CQ : public device::SPI, public px4::ScheduledWorkItem { public: FXOS8701CQ(int bus, const char *path, uint32_t device, enum Rotation rotation); @@ -197,9 +198,10 @@ class FXOS8701CQ : public device::SPI private: + void Run() override; + #if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) FXOS8701CQ_mag *_mag; - struct hrt_call _mag_call; unsigned _call_mag_interval; ringbuffer::RingBuffer *_mag_reports; @@ -212,9 +214,9 @@ class FXOS8701CQ : public device::SPI int16_t _last_raw_mag_x; int16_t _last_raw_mag_y; int16_t _last_raw_mag_z; -#endif - struct hrt_call _accel_call; + hrt_abstime _mag_last_measure{0}; +#endif unsigned _call_accel_interval; @@ -285,24 +287,6 @@ class FXOS8701CQ : public device::SPI */ void disable_i2c(); - /** - * 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); - - /** - * Static trampoline for the mag because it runs at a lower rate - * - * @param arg Instance pointer for the driver that is polling. - */ - static void mag_measure_trampoline(void *arg); - /** * check key registers for correct values */ @@ -456,8 +440,6 @@ class FXOS8701CQ_mag : public device::CDev void measure(); - void measure_trampoline(void *arg); - /* this class does not allow copying due to ptr data members */ FXOS8701CQ_mag(const FXOS8701CQ_mag &); FXOS8701CQ_mag operator=(const FXOS8701CQ_mag &); @@ -467,9 +449,9 @@ class FXOS8701CQ_mag : public device::CDev FXOS8701CQ::FXOS8701CQ(int bus, const char *path, uint32_t device, enum Rotation rotation) : SPI("FXOS8701CQ", path, bus, device, SPIDEV_MODE0, 1 * 1000 * 1000), + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), #if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) _mag(new FXOS8701CQ_mag(this)), - _mag_call{}, _call_mag_interval(0), _mag_reports(nullptr), _mag_scale{}, @@ -482,7 +464,6 @@ FXOS8701CQ::FXOS8701CQ(int bus, const char *path, uint32_t device, enum Rotation _last_raw_mag_y(0), _last_raw_mag_z(0), #endif - _accel_call {}, _call_accel_interval(0), _accel_reports(nullptr), _accel_scale{}, @@ -819,10 +800,10 @@ FXOS8701CQ::ioctl(struct file *filp, int cmd, unsigned long arg) bool want_start = (_call_accel_interval == 0); /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; + unsigned interval = 1000000 / arg; /* check against maximum sane rate */ - if (ticks < 500) { + if (interval < 500) { return -EINVAL; } @@ -831,9 +812,7 @@ FXOS8701CQ::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_accel_interval = ticks; - - _accel_call.period = _call_accel_interval - FXOS8701C_TIMER_REDUCTION; + _call_accel_interval = interval; /* if we need to start the poll state machine, do it */ if (want_start) { @@ -893,16 +872,16 @@ FXOS8701CQ::mag_ioctl(struct file *filp, int cmd, unsigned long arg) bool want_start = (_call_mag_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; } /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ - _mag_call.period = _call_mag_interval = ticks; + _call_mag_interval = interval; /* if we need to start the poll state machine, do it */ if (want_start) { @@ -1141,22 +1120,14 @@ FXOS8701CQ::start() #endif /* start polling at the specified rate */ - hrt_call_every(&_accel_call, - 1000, - _call_accel_interval - FXOS8701C_TIMER_REDUCTION, - (hrt_callout)&FXOS8701CQ::measure_trampoline, this); -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&FXOS8701CQ::mag_measure_trampoline, this); -#endif + ScheduleOnInterval(_call_accel_interval - FXOS8701C_TIMER_REDUCTION, 10000); } void FXOS8701CQ::stop() { - hrt_cancel(&_accel_call); -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - hrt_cancel(&_mag_call); -#endif + ScheduleClear(); + /* reset internal states */ memset(_last_accel, 0, sizeof(_last_accel)); @@ -1168,21 +1139,18 @@ FXOS8701CQ::stop() } void -FXOS8701CQ::measure_trampoline(void *arg) +FXOS8701CQ::Run() { - FXOS8701CQ *dev = (FXOS8701CQ *)arg; - /* make another measurement */ - dev->measure(); -} + measure(); -void -FXOS8701CQ::mag_measure_trampoline(void *arg) -{ - FXOS8701CQ *dev = (FXOS8701CQ *)arg; +#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - /* make another measurement */ - dev->mag_measure(); + if (hrt_elapsed_time(&_mag_last_measure) >= _call_mag_interval) { + mag_measure(); + } + +#endif } void @@ -1413,6 +1381,9 @@ FXOS8701CQ::mag_measure() */ mag_report.timestamp = hrt_absolute_time(); + + _mag_last_measure = mag_report.timestamp; + mag_report.is_external = external(); mag_report.x_raw = _last_raw_mag_x; @@ -1580,11 +1551,6 @@ FXOS8701CQ_mag::measure() _parent->mag_measure(); } -void -FXOS8701CQ_mag::measure_trampoline(void *arg) -{ - _parent->mag_measure_trampoline(arg); -} #endif /**