bmi055 move to px4 work queue

src/drivers/imu/bmi055/BMI055.hpp

@@ -41,6 +41,7 @@
 #include <px4_config.h>
 #include <systemlib/conversions.h>
 #include <systemlib/err.h>
+#include <px4_work_queue/ScheduledWorkItem.hpp>
 
 #define DIR_READ                0x80
 #define DIR_WRITE               0x00
@@ -59,7 +60,6 @@ class BMI055 : public device::SPI
 
 	uint8_t         _whoami;    /** whoami result */
 
-	struct hrt_call     _call;
 	unsigned        _call_interval;
 
 	uint8_t         _register_wait;

src/drivers/imu/bmi055/BMI055_accel.cpp

@@ -48,6 +48,7 @@ const uint8_t BMI055_accel::_checked_registers[BMI055_ACCEL_NUM_CHECKED_REGISTER
 
 BMI055_accel::BMI055_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation) :
 	BMI055("BMI055_ACCEL", path_accel, bus, device, SPIDEV_MODE3, BMI055_BUS_SPEED, rotation),
+	ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())),
 	_sample_perf(perf_alloc(PC_ELAPSED, "bmi055_accel_read")),
 	_measure_interval(perf_alloc(PC_INTERVAL, "bmi055_accel_measure_interval")),
 	_bad_transfers(perf_alloc(PC_COUNT, "bmi055_accel_bad_transfers")),
@@ -76,8 +77,6 @@ BMI055_accel::BMI055_accel(int bus, const char *path_accel, uint32_t device, enu
 	_accel_scale.y_scale  = 1.0f;
 	_accel_scale.z_offset = 0;
 	_accel_scale.z_scale  = 1.0f;
-
-	memset(&_call, 0, sizeof(_call));
 }
 
 BMI055_accel::~BMI055_accel()
@@ -306,31 +305,23 @@ BMI055_accel::ioctl(struct file *filp, int cmd, unsigned long arg)
 					bool want_start = (_call_interval == 0);
 
 					/* convert hz to hrt interval via microseconds */
-					unsigned ticks = 1000000 / arg;
+					unsigned interval = 1000000 / arg;
 
 					/* check against maximum 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);
 
 					/* update interval for next measurement */
-					_call_interval = ticks;
-
-					/*
-					  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 bmi055 clock
-					 */
-					_call.period = _call_interval - BMI055_TIMER_REDUCTION;
+					_call_interval = interval;
 
 					/* if we need to start the poll state machine, do it */
 					if (want_start) {
@@ -440,26 +431,22 @@ BMI055_accel::start()
 	_accel_reports->flush();
 
 	/* start polling at the specified rate */
-	hrt_call_every(&_call,
-		       1000,
-		       _call_interval - BMI055_TIMER_REDUCTION,
-		       (hrt_callout)&BMI055_accel::measure_trampoline, this);
+	ScheduleOnInterval(_call_interval - BMI055_TIMER_REDUCTION, 1000);
+
 	reset();
 }
 
 void
 BMI055_accel::stop()
 {
-	hrt_cancel(&_call);
+	ScheduleClear();
 }
 
 void
-BMI055_accel::measure_trampoline(void *arg)
+BMI055_accel::Run()
 {
-	BMI055_accel *dev = reinterpret_cast<BMI055_accel *>(arg);
-
 	/* make another measurement */
-	dev->measure();
+	measure();
 }
 
 void

src/drivers/imu/bmi055/BMI055_accel.hpp

@@ -148,7 +148,7 @@
 /* Mask definitions for ACCD_X_LSB, ACCD_Y_LSB and ACCD_Z_LSB Register */
 #define BMI055_NEW_DATA_MASK                 0x01
 
-class BMI055_accel : public BMI055
+class BMI055_accel : public BMI055, public px4::ScheduledWorkItem
 {
 public:
 	BMI055_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation);
@@ -227,16 +227,7 @@ class BMI055_accel : public BMI055
 	 */
 	int         reset();
 
-	/**
-	 * 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);
+	void     Run() override;
 
 	/**
 	 * Fetch measurements from the sensor and update the report buffers.

src/drivers/imu/bmi055/BMI055_gyro.cpp

@@ -49,6 +49,7 @@ const uint8_t BMI055_gyro::_checked_registers[BMI055_GYRO_NUM_CHECKED_REGISTERS]
 
 BMI055_gyro::BMI055_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation) :
 	BMI055("BMI055_GYRO", path_gyro, bus, device, SPIDEV_MODE3, BMI055_BUS_SPEED, rotation),
+	ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())),
 	_sample_perf(perf_alloc(PC_ELAPSED, "bmi055_gyro_read")),
 	_measure_interval(perf_alloc(PC_INTERVAL, "bmi055_gyro_measure_interval")),
 	_bad_transfers(perf_alloc(PC_COUNT, "bmi055_gyro_bad_transfers")),
@@ -76,8 +77,6 @@ BMI055_gyro::BMI055_gyro(int bus, const char *path_gyro, uint32_t device, enum R
 	_gyro_scale.y_scale  = 1.0f;
 	_gyro_scale.z_offset = 0;
 	_gyro_scale.z_scale  = 1.0f;
-
-	memset(&_call, 0, sizeof(_call));
 }
 
 BMI055_gyro::~BMI055_gyro()
@@ -334,30 +333,22 @@ BMI055_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
 					bool want_start = (_call_interval == 0);
 
 					/* convert hz to hrt interval via microseconds */
-					unsigned ticks = 1000000 / arg;
+					unsigned interval = 1000000 / arg;
 
 					/* check against maximum rate */
-					if (ticks < 1000) {
+					if (interval < 1000) {
 						return -EINVAL;
 					}
 
 					// adjust filters
 					float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq();
-					float sample_rate = 1.0e6f / ticks;
+					float sample_rate = 1.0e6f / interval;
 					_gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
 					_gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
 					_gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
 
 					/* update interval for next measurement */
-					_call_interval = ticks;
-
-					/*
-					  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 bmi055 clock
-					 */
-					_call.period = _call_interval - BMI055_TIMER_REDUCTION;
+					_call_interval = interval;
 
 					/* if we need to start the poll state machine, do it */
 					if (want_start) {
@@ -463,26 +454,22 @@ BMI055_gyro::start()
 	_gyro_reports->flush();
 
 	/* start polling at the specified rate */
-	hrt_call_every(&_call,
-		       1000,
-		       _call_interval - BMI055_TIMER_REDUCTION,
-		       (hrt_callout)&BMI055_gyro::measure_trampoline, this);
+	ScheduleOnInterval(_call_interval - BMI055_TIMER_REDUCTION, 1000);
+
 	reset();
 }
 
 void
 BMI055_gyro::stop()
 {
-	hrt_cancel(&_call);
+	ScheduleClear();
 }
 
 void
-BMI055_gyro::measure_trampoline(void *arg)
+BMI055_gyro::Run()
 {
-	BMI055_gyro *dev = reinterpret_cast<BMI055_gyro *>(arg);
-
 	/* make another measurement */
-	dev->measure();
+	measure();
 }
 
 void

src/drivers/imu/bmi055/BMI055_gyro.hpp

@@ -140,7 +140,7 @@
 
 #define BMI055_ACC_TEMP             0x08
 
-class BMI055_gyro : public BMI055
+class BMI055_gyro : public BMI055, public px4::ScheduledWorkItem
 {
 public:
 	BMI055_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation);
@@ -218,16 +218,7 @@ class BMI055_gyro : public BMI055
 	 */
 	int         reset();
 
-	/**
-	 * 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);
+	void     Run() override;
 
 	/**
 	 * Fetch measurements from the sensor and update the report buffers.

src/drivers/imu/bmi055/bmi055_main.cpp

@@ -446,7 +446,6 @@ BMI055::BMI055(const char *name, const char *devname, int bus, uint32_t device,
 	       uint32_t frequency, enum Rotation rotation):
 	SPI(name, devname, bus, device, mode, frequency),
 	_whoami(0),
-	_call{},
 	_call_interval(0),
 	_register_wait(0),
 	_reset_wait(0),