icm20948 move to px4 work queue

src/drivers/imu/icm20948/icm20948.cpp

@@ -98,6 +98,7 @@ ICM20948::ICM20948(device::Device *interface, device::Device *mag_interface, con
 		   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 ICM20948_accel(this, path_accel)),
@@ -105,13 +106,6 @@ ICM20948::ICM20948(device::Device *interface, device::Device *mag_interface, con
 	_mag(new ICM20948_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{},
@@ -239,16 +233,14 @@ ICM20948::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);
@@ -257,7 +249,7 @@ ICM20948::init()
 	int ret = probe();
 
 	if (ret != OK) {
-		PX4_DEBUG("ICM20948 probe failed");
+		PX4_DEBUG("probe failed");
 		return ret;
 	}
 
@@ -457,10 +449,8 @@ int ICM20948::reset_mpu()
 	}
 
 	// Enable I2C bus or Disable I2C bus (recommended on data sheet)
-
+	const bool is_i2c = (_interface->get_device_bus_type() == device::Device::DeviceBusType_I2C);
-
+	write_checked_reg(ICMREG_20948_USER_CTRL, is_i2c ? 0 : BIT_I2C_IF_DIS);
-	write_checked_reg(MPU_OR_ICM(MPUREG_USER_CTRL, ICMREG_20948_USER_CTRL), is_i2c() ? 0 : BIT_I2C_IF_DIS);
-
 
 	// SAMPLE RATE
 	_set_sample_rate(_sample_rate);
@@ -617,16 +607,16 @@ ICM20948::_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);
@@ -639,15 +629,7 @@ ICM20948::_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) {
@@ -955,78 +937,20 @@ ICM20948::start()
 
 	_mag->_mag_reports->flush();
 
-	if (_use_hrt) {
+	ScheduleOnInterval(_call_interval - MPU9250_TIMER_REDUCTION, 10000);
-		/* start polling at the specified rate */
-		hrt_call_every(&_call,
-			       1000,
-			       _call_interval - MPU9250_TIMER_REDUCTION,
-			       (hrt_callout)&ICM20948::measure_trampoline, this);
-
-	} else {
-#ifdef USE_I2C
-		/* schedule a cycle to start things */
-		work_queue(HPWORK, &_work, (worker_t)&ICM20948::cycle_trampoline, this, 1);
-#endif
-	}
-
 }
 
 void
 ICM20948::stop()
 {
-	if (_use_hrt) {
+	ScheduleClear();
-		hrt_cancel(&_call);
-
-	} else {
-#ifdef USE_I2C
-		work_cancel(HPWORK, &_work);
-#endif
-	}
-}
-
-
-#if defined(USE_I2C)
-void
-ICM20948::cycle_trampoline(void *arg)
-{
-	ICM20948 *dev = (ICM20948 *)arg;
-
-	dev->cycle();
 }
 
 void
-ICM20948::cycle()
+ICM20948::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)&ICM20948::cycle_trampoline,
-			   this,
-			   USEC2TICK(_call_interval - MPU9250_TIMER_REDUCTION));
-	}
-}
-#endif
-
-
-void
-ICM20948::measure_trampoline(void *arg)
-{
-	ICM20948 *dev = reinterpret_cast<ICM20948 *>(arg);
-
 	/* make another measurement */
-	dev->measure();
+	measure();
 }
 
 void

src/drivers/imu/icm20948/icm20948.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,6 +47,7 @@
 #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>
@@ -374,7 +373,7 @@ class ICM20948_mag;
 class ICM20948_accel;
 class ICM20948_gyro;
 
-class ICM20948
+class ICM20948 : public px4::ScheduledWorkItem
 {
 public:
 	ICM20948(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro,
@@ -402,6 +401,8 @@ protected:
 	friend class ICM20948_mag;
 	friend class ICM20948_gyro;
 
+	void Run() override;
+
 private:
 	ICM20948_accel   *_accel;
 	ICM20948_gyro	*_gyro;
@@ -410,16 +411,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;
@@ -515,52 +506,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.
 	 */