diff --git a/src/drivers/magnetometer/qmc5883/CMakeLists.txt b/src/drivers/magnetometer/qmc5883/CMakeLists.txt index 2ebe15551b7a..e674ba62758a 100644 --- a/src/drivers/magnetometer/qmc5883/CMakeLists.txt +++ b/src/drivers/magnetometer/qmc5883/CMakeLists.txt @@ -40,4 +40,6 @@ px4_add_module( qmc5883_i2c.cpp qmc5883_spi.cpp qmc5883.cpp + DEPENDS + px4_work_queue ) diff --git a/src/drivers/magnetometer/qmc5883/qmc5883.cpp b/src/drivers/magnetometer/qmc5883/qmc5883.cpp index 8f230de33d6e..42864a3d2acc 100644 --- a/src/drivers/magnetometer/qmc5883/qmc5883.cpp +++ b/src/drivers/magnetometer/qmc5883/qmc5883.cpp @@ -56,7 +56,7 @@ #include #include -#include +#include #include #include @@ -134,11 +134,7 @@ enum QMC5883_BUS { QMC5883_BUS_SPI }; -#ifndef CONFIG_SCHED_WORKQUEUE -# error This requires CONFIG_SCHED_WORKQUEUE. -#endif - -class QMC5883 : public device::CDev +class QMC5883 : public device::CDev, public px4::ScheduledWorkItem { public: QMC5883(device::Device *interface, const char *path, enum Rotation rotation); @@ -163,8 +159,7 @@ class QMC5883 : public device::CDev Device *_interface; private: - work_s _work{}; - unsigned _measure_ticks; + unsigned _measure_interval{0}; ringbuffer::RingBuffer *_reports; struct mag_calibration_s _scale; @@ -228,15 +223,7 @@ class QMC5883 : public device::CDev * 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 Run() override; /** * Write a register. @@ -274,8 +261,8 @@ extern "C" __EXPORT int qmc5883_main(int argc, char *argv[]); QMC5883::QMC5883(device::Device *interface, const char *path, enum Rotation rotation) : CDev("QMC5883", path), + ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())), _interface(interface), - _measure_ticks(0), _reports(nullptr), _scale{}, _range_scale(1.0f / 12000.0f), @@ -301,9 +288,6 @@ QMC5883::QMC5883(device::Device *interface, const char *path, enum Rotation rota _device_id.devid_s.address = _interface->get_device_address(); _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_QMC5883; - // enable debug() calls - _debug_enabled = false; - // default scaling _scale.x_offset = 0; _scale.x_scale = 1.0f; @@ -311,9 +295,6 @@ QMC5883::QMC5883(device::Device *interface, const char *path, enum Rotation rota _scale.y_scale = 1.0f; _scale.z_offset = 0; _scale.z_scale = 1.0f; - - // work_cancel in the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); } QMC5883::~QMC5883() @@ -407,7 +388,7 @@ QMC5883::read(struct file *filp, char *buffer, size_t buflen) } /* if automatic measurement is enabled */ - if (_measure_ticks > 0) { + if (_measure_interval > 0) { /* * While there is space in the caller's buffer, and reports, copy them. * Note that we may be pre-empted by the workq thread while we are doing this; @@ -462,10 +443,10 @@ QMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) /* set default polling rate */ case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + bool want_start = (_measure_interval == 0); /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(QMC5883_CONVERSION_INTERVAL); + _measure_interval = QMC5883_CONVERSION_INTERVAL; /* if we need to start the poll state machine, do it */ if (want_start) { @@ -478,18 +459,18 @@ QMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + bool want_start = (_measure_interval == 0); /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); + unsigned interval = (1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(QMC5883_CONVERSION_INTERVAL)) { + if (interval < QMC5883_CONVERSION_INTERVAL) { return -EINVAL; } /* update interval for next measurement */ - _measure_ticks = ticks; + _measure_interval = interval; /* if we need to start the poll state machine, do it */ if (want_start) { @@ -535,16 +516,16 @@ QMC5883::start() _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&QMC5883::cycle_trampoline, this, 1); + ScheduleNow(); } void QMC5883::stop() { - if (_measure_ticks > 0) { + if (_measure_interval > 0) { /* ensure no new items are queued while we cancel this one */ - _measure_ticks = 0; - work_cancel(HPWORK, &_work); + _measure_interval = 0; + ScheduleClear(); } } @@ -558,7 +539,6 @@ QMC5883::reset() /* software reset */ write_reg(QMC5883_ADDR_CONTROL_2, QMC5883_SOFT_RESET); - /* set reset period to 0x01 */ write_reg(QMC5883_ADDR_SET_RESET, QMC5883_SET_DEFAULT); @@ -578,17 +558,9 @@ QMC5883::reset() } void -QMC5883::cycle_trampoline(void *arg) +QMC5883::Run() { - QMC5883 *dev = (QMC5883 *)arg; - - dev->cycle(); -} - -void -QMC5883::cycle() -{ - if (_measure_ticks == 0) { + if (_measure_interval == 0) { return; } @@ -609,14 +581,9 @@ QMC5883::cycle() /* * Is there a collect->measure gap? */ - if (_measure_ticks > USEC2TICK(QMC5883_CONVERSION_INTERVAL)) { - + if (_measure_interval > QMC5883_CONVERSION_INTERVAL) { /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&QMC5883::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(QMC5883_CONVERSION_INTERVAL)); + ScheduleDelayed(_measure_interval - QMC5883_CONVERSION_INTERVAL); return; } @@ -625,13 +592,9 @@ QMC5883::cycle() /* next phase is collection */ _collect_phase = true; - if (_measure_ticks > 0) { + if (_measure_interval > 0) { /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&QMC5883::cycle_trampoline, - this, - USEC2TICK(QMC5883_CONVERSION_INTERVAL)); + ScheduleDelayed(QMC5883_CONVERSION_INTERVAL); } } @@ -829,7 +792,7 @@ QMC5883::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); - printf("poll interval: %u ticks\n", _measure_ticks); + printf("poll interval: %u us\n", _measure_interval); print_message(_last_report); _reports->print_info("report queue"); }