mpu9250 move to px4 work queue

src/drivers/imu/mpu9250/CMakeLists.txt

@@ -45,5 +45,5 @@ px4_add_module(
 		mag.cpp
 		mag_i2c.cpp
 	DEPENDS
+		px4_work_queue
 	)
-

src/drivers/imu/mpu9250/mpu9250.cpp

@@ -93,6 +93,7 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const
 		 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 MPU9250_accel(this, path_accel)),
@@ -100,13 +101,6 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const
 	_mag(new MPU9250_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{},
@@ -232,16 +226,14 @@ MPU9250::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);
@@ -250,7 +242,7 @@ MPU9250::init()
 	int ret = probe();
 
 	if (ret != OK) {
-		PX4_DEBUG("MPU9250 probe failed");
+		PX4_DEBUG("probe failed");
 		return ret;
 	}
 
@@ -453,7 +445,8 @@ int MPU9250::reset_mpu()
 	}
 
 	// Enable I2C bus or Disable I2C bus (recommended on data sheet)
-	write_checked_reg(MPUREG_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(MPUREG_USER_CTRL, is_i2c ? 0 : BIT_I2C_IF_DIS);
 
 	// SAMPLE RATE
 	_set_sample_rate(_sample_rate);
@@ -599,16 +592,16 @@ MPU9250::_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);
@@ -621,15 +614,7 @@ MPU9250::_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;
+		_call_interval = interval;
-
-		/*
-		  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;
 
 		/* if we need to start the poll state machine, do it */
 		if (want_start) {
@@ -826,78 +811,20 @@ MPU9250::start()
 
 	_mag->_mag_reports->flush();
 
-	if (_use_hrt) {
+	ScheduleOnInterval(_call_interval - MPU9250_TIMER_REDUCTION, 1000);
-		/* start polling at the specified rate */
-		hrt_call_every(&_call,
-			       1000,
-			       _call_interval - MPU9250_TIMER_REDUCTION,
-			       (hrt_callout)&MPU9250::measure_trampoline, this);
-
-	} else {
-#ifdef USE_I2C
-		/* schedule a cycle to start things */
-		work_queue(HPWORK, &_work, (worker_t)&MPU9250::cycle_trampoline, this, 1);
-#endif
-	}
-
 }
 
 void
 MPU9250::stop()
 {
-	if (_use_hrt) {
+	ScheduleClear();
-		hrt_cancel(&_call);
-
-	} else {
-#ifdef USE_I2C
-		work_cancel(HPWORK, &_work);
-#endif
-	}
-}
-
-
-#if defined(USE_I2C)
-void
-MPU9250::cycle_trampoline(void *arg)
-{
-	MPU9250 *dev = (MPU9250 *)arg;
-
-	dev->cycle();
 }
 
 void
-MPU9250::cycle()
+MPU9250::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)&MPU9250::cycle_trampoline,
-			   this,
-			   USEC2TICK(_call_interval - MPU9250_TIMER_REDUCTION));
-	}
-}
-#endif
-
-
-void
-MPU9250::measure_trampoline(void *arg)
-{
-	MPU9250 *dev = reinterpret_cast<MPU9250 *>(arg);
-
 	/* make another measurement */
-	dev->measure();
+	measure();
 }
 
 void

src/drivers/imu/mpu9250/mpu9250.h

@@ -36,8 +36,6 @@
 #include <perf/perf_counter.h>
 #include <systemlib/conversions.h>
 
-#include <nuttx/wqueue.h>
-
 #include <board_config.h>
 #include <drivers/drv_hrt.h>
 
@@ -49,9 +47,9 @@
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
 #include <lib/conversion/rotation.h>
 #include <systemlib/err.h>
+#include <px4_work_queue/ScheduledWorkItem.hpp>
 
 #include <uORB/uORB.h>
-#include <uORB/topics/debug_key_value.h>
 
 #include "mag.h"
 #include "accel.h"
@@ -253,7 +251,7 @@ class MPU9250_mag;
 class MPU9250_accel;
 class MPU9250_gyro;
 
-class MPU9250
+class MPU9250 : public px4::ScheduledWorkItem
 {
 public:
 	MPU9250(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro,
@@ -281,6 +279,8 @@ protected:
 	friend class MPU9250_mag;
 	friend class MPU9250_gyro;
 
+	void Run() override;
+
 private:
 	MPU9250_accel   *_accel;
 	MPU9250_gyro	*_gyro;
@@ -289,16 +289,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;
@@ -392,52 +382,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.
 	 */