mpu6000 move to px4 work queue

src/drivers/imu/mpu6000/MPU6000.cpp

@@ -41,13 +41,9 @@ constexpr uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS];
 
 MPU6000::MPU6000(device::Device *interface, const char *path, enum Rotation rotation, int device_type) :
 	CDev(path),
+	ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())),
 	_interface(interface),
 	_device_type(device_type),
-#if defined(USE_I2C)
-	_use_hrt(false),
-#else
-	_use_hrt(true),
-#endif
 	_px4_accel(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH), rotation),
 	_px4_gyro(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH), rotation),
 	_sample_perf(perf_alloc(PC_ELAPSED, "mpu6k_read")),
@@ -98,11 +94,6 @@ MPU6000::~MPU6000()
 int
 MPU6000::init()
 {
-
-#if defined(USE_I2C)
-	use_i2c(_interface->get_device_bus_type() == Device::DeviceBusType_I2C);
-#endif
-
 	/* probe again to get our settings that are based on the device type */
 	int ret = probe();
 
@@ -153,7 +144,8 @@ int MPU6000::reset()
 		up_udelay(1000);
 
 		// 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);
 
 		px4_leave_critical_section(state);
 
@@ -654,79 +646,23 @@ MPU6000::start()
 	stop();
 	_call_interval = last_call_interval;
 
-	if (!is_i2c()) {
-
-		_call.period = _call_interval - MPU6000_TIMER_REDUCTION;
-
-		/* start polling at the specified rate */
-		hrt_call_every(&_call,
-			       1000,
-			       _call_interval - MPU6000_TIMER_REDUCTION,
-			       (hrt_callout)&MPU6000::measure_trampoline, this);
-
-	} else {
-#ifdef USE_I2C
-		/* schedule a cycle to start things */
-		work_queue(HPWORK, &_work, (worker_t)&MPU6000::cycle_trampoline, this, 1);
-#endif
-	}
+	ScheduleOnInterval(_call_interval - MPU6000_TIMER_REDUCTION, 1000);
 }
 
 void
 MPU6000::stop()
 {
-	if (!is_i2c()) {
-		hrt_cancel(&_call);
-
-	} else {
-#ifdef USE_I2C
-		_call_interval = 0;
-		work_cancel(HPWORK, &_work);
-#endif
-	}
+	ScheduleClear();
 
 	/* reset internal states */
 	memset(_last_accel, 0, sizeof(_last_accel));
 }
 
-#if defined(USE_I2C)
-void
-MPU6000::cycle_trampoline(void *arg)
-{
-	MPU6000 *dev = (MPU6000 *)arg;
-
-	dev->cycle();
-}
-
-void
-MPU6000::cycle()
-{
-	int ret = measure();
-
-	if (ret != OK) {
-		/* issue a reset command to the sensor */
-		reset();
-		start();
-		return;
-	}
-
-	if (_call_interval != 0) {
-		work_queue(HPWORK,
-			   &_work,
-			   (worker_t)&MPU6000::cycle_trampoline,
-			   this,
-			   USEC2TICK(_call_interval - MPU6000_TIMER_REDUCTION));
-	}
-}
-#endif
-
 void
-MPU6000::measure_trampoline(void *arg)
+MPU6000::Run()
 {
-	MPU6000 *dev = reinterpret_cast<MPU6000 *>(arg);
-
 	/* make another measurement */
-	dev->measure();
+	measure();
 }
 
 void

src/drivers/imu/mpu6000/MPU6000.hpp

@@ -66,7 +66,7 @@
 #include <px4_config.h>
 #include <px4_getopt.h>
 #include <px4_time.h>
-#include <px4_workqueue.h>
+#include <px4_work_queue/ScheduledWorkItem.hpp>
 #include <systemlib/conversions.h>
 #include <systemlib/px4_macros.h>
 
@@ -302,7 +302,7 @@ enum MPU6000_BUS {
 	MPU6000_BUS_SPI_EXTERNAL2
 };
 
-class MPU6000 : public cdev::CDev
+class MPU6000 : public cdev::CDev, public px4::ScheduledWorkItem
 {
 public:
 	MPU6000(device::Device *interface, const char *path, enum Rotation rotation, int device_type);
@@ -346,19 +346,12 @@ class MPU6000 : public cdev::CDev
 	virtual int		probe();
 
 private:
+
+	void Run() override;
+
 	int 			_device_type;
 	uint8_t			_product{0};	/** product code */
 
-#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{1000};
 
 	PX4Accelerometer	_px4_accel;
@@ -420,50 +413,6 @@ class MPU6000 : public cdev::CDev
 	 */
 	bool 		is_mpu_device() { return _device_type == MPU_DEVICE_TYPE_MPU6000; }
 
-
-#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.
 	 */