fxos8701cq move to px4 work queue

src/drivers/imu/fxos8701cq/fxos8701cq.cpp

@@ -78,6 +78,7 @@
 #include <lib/conversion/rotation.h>
 #include <px4_getopt.h>
 #include <systemlib/err.h>
+#include <px4_work_queue/ScheduledWorkItem.hpp>
 
 /* 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 @@ protected:
 
 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 @@ private:
 	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 @@ private:
 	 */
 	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 @@ private:
 
 	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 @@ private:
 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
 
 /**