From 8f1b4f693bc6b1aedf1fc8f0fabd904f643cd8bc Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 22 Feb 2019 10:51:18 -0500 Subject: [PATCH] rm3100 move to px4 work queue --- src/drivers/magnetometer/rm3100/rm3100.cpp | 58 ++++++++-------------- src/drivers/magnetometer/rm3100/rm3100.h | 19 ++----- 2 files changed, 24 insertions(+), 53 deletions(-) diff --git a/src/drivers/magnetometer/rm3100/rm3100.cpp b/src/drivers/magnetometer/rm3100/rm3100.cpp index a95156415c..8a39f4a07e 100644 --- a/src/drivers/magnetometer/rm3100/rm3100.cpp +++ b/src/drivers/magnetometer/rm3100/rm3100.cpp @@ -43,8 +43,8 @@ RM3100::RM3100(device::Device *interface, const char *path, enum Rotation rotation) : CDev("RM3100", path), + ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())), _interface(interface), - _work{}, _reports(nullptr), _scale{}, _last_report{}, @@ -57,7 +57,7 @@ RM3100::RM3100(device::Device *interface, const char *path, enum Rotation rotati _continuous_mode_set(false), _mode(SINGLE), _rotation(rotation), - _measure_ticks(0), + _measure_interval(0), _class_instance(-1), _orb_class_instance(-1), _range_scale(1.0f / (RM3100_SENSITIVITY * UTESLA_TO_GAUSS)), @@ -69,9 +69,6 @@ RM3100::RM3100(device::Device *interface, const char *path, enum Rotation rotati _device_id.devid_s.address = _interface->get_device_address(); _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_RM3100; - // enable debug() calls - _debug_enabled = false; - // default scaling _scale.x_offset = 0; _scale.x_scale = 1.0f; @@ -79,9 +76,6 @@ RM3100::RM3100(device::Device *interface, const char *path, enum Rotation rotati _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)); } RM3100::~RM3100() @@ -293,10 +287,10 @@ RM3100::convert_signed(int32_t *n) } void -RM3100::cycle() +RM3100::Run() { - /* _measure_ticks == 0 is used as _task_should_exit */ - if (_measure_ticks == 0) { + /* _measure_interval == 0 is used as _task_should_exit */ + if (_measure_interval == 0) { return; } @@ -313,24 +307,12 @@ RM3100::cycle() DEVICE_DEBUG("measure error"); } - if (_measure_ticks > 0) { + if (_measure_interval > 0) { /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&RM3100::cycle_trampoline, - this, - _measure_ticks); + ScheduleDelayed(_measure_interval); } } -void -RM3100::cycle_trampoline(void *arg) -{ - RM3100 *dev = (RM3100 *)arg; - - dev->cycle(); -} - int RM3100::init() { @@ -379,10 +361,10 @@ RM3100::ioctl(struct file *file_pointer, int cmd, unsigned long arg) case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ - bool not_started = (_measure_ticks == 0); + bool not_started = (_measure_interval == 0); /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(RM3100_CONVERSION_INTERVAL); + _measure_interval = (RM3100_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ if (not_started) { @@ -395,18 +377,18 @@ RM3100::ioctl(struct file *file_pointer, int cmd, unsigned long arg) /* Uses arg (hz) for a custom poll rate */ default: { /* do we need to start internal polling? */ - bool not_started = (_measure_ticks == 0); + bool not_started = (_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(RM3100_CONVERSION_INTERVAL)) { + if (interval < RM3100_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 (not_started) { @@ -496,7 +478,7 @@ RM3100::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); - PX4_INFO("poll interval: %u ticks", _measure_ticks); + PX4_INFO("poll interval: %u", _measure_interval); print_message(_last_report); _reports->print_info("report queue"); } @@ -528,7 +510,7 @@ RM3100::read(struct file *file_pointer, char *buffer, size_t buffer_len) } /* 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; @@ -609,18 +591,18 @@ RM3100::start() _reports->flush(); set_default_register_values(); - _measure_ticks = USEC2TICK(RM3100_CONVERSION_INTERVAL); + _measure_interval = (RM3100_CONVERSION_INTERVAL); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&RM3100::cycle_trampoline, this, 1); + ScheduleNow(); } void RM3100::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(); } } diff --git a/src/drivers/magnetometer/rm3100/rm3100.h b/src/drivers/magnetometer/rm3100/rm3100.h index 872fce4297..1d7ffc24ad 100644 --- a/src/drivers/magnetometer/rm3100/rm3100.h +++ b/src/drivers/magnetometer/rm3100/rm3100.h @@ -52,9 +52,7 @@ #include #include -#ifndef CONFIG_SCHED_WORKQUEUE -# error This requires CONFIG_SCHED_WORKQUEUE. -#endif +#include /** * RM3100 internal constants and data structures. @@ -117,7 +115,7 @@ enum OPERATING_MODE { }; -class RM3100 : public device::CDev +class RM3100 : public device::CDev, public px4::ScheduledWorkItem { public: RM3100(device::Device *interface, const char *path, enum Rotation rotation); @@ -149,7 +147,6 @@ protected: Device *_interface; private: - work_s _work; ringbuffer::RingBuffer *_reports; @@ -171,7 +168,7 @@ private: enum OPERATING_MODE _mode; enum Rotation _rotation; - unsigned int _measure_ticks; + unsigned int _measure_interval; int _class_instance; int _orb_class_instance; @@ -217,15 +214,7 @@ private: * and measurement to provide the most recent measurement possible * at the next interval. */ - void cycle(); - - /** - * @brief 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; /** * Issue a measurement command.