diff --git a/src/drivers/distance_sensor/sf1xx/sf1xx.cpp b/src/drivers/distance_sensor/sf1xx/sf1xx.cpp index d0674354abd9..9ab43c864522 100644 --- a/src/drivers/distance_sensor/sf1xx/sf1xx.cpp +++ b/src/drivers/distance_sensor/sf1xx/sf1xx.cpp @@ -44,7 +44,7 @@ #include #include #include -#include +#include #include #include @@ -78,21 +78,18 @@ #define SF1XX_DEVICE_PATH "/dev/sf1xx" -#ifndef CONFIG_SCHED_WORKQUEUE -# error This requires CONFIG_SCHED_WORKQUEUE. -#endif - -class SF1XX : public device::I2C +class SF1XX : public device::I2C, public px4::ScheduledWorkItem { public: SF1XX(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, int bus = SF1XX_BUS_DEFAULT, int address = SF1XX_BASEADDR); - virtual ~SF1XX(); - virtual int init(); + virtual ~SF1XX() override; + + int init() override; - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override; + int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; /** * Diagnostics - print some basic information about the driver. @@ -100,7 +97,7 @@ class SF1XX : public device::I2C void print_info(); protected: - virtual int probe(); + int probe() override; private: /** @@ -139,23 +136,15 @@ class SF1XX : public device::I2C * Perform a poll cycle; collect from the previous measurement * and start a new one. */ - void cycle(); + void Run() override; int measure(); int collect(); - /** - * Static trampoline from the workq context; because we don't have a - * generic workqueue wrapper yet. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void cycle_trampoline(void *arg); - bool _sensor_ok{false}; int _class_instance{-1}; int _conversion_interval{-1}; - int _measure_ticks{0}; + int _measure_interval{0}; int _orb_class_instance{-1}; float _max_distance{-1.0f}; @@ -163,8 +152,6 @@ class SF1XX : public device::I2C uint8_t _rotation{0}; - work_s _work{}; - ringbuffer::RingBuffer *_reports{nullptr}; orb_advert_t _distance_sensor_topic{nullptr}; @@ -180,6 +167,7 @@ extern "C" __EXPORT int sf1xx_main(int argc, char *argv[]); SF1XX::SF1XX(uint8_t rotation, int bus, int address) : I2C("SF1XX", SF1XX_DEVICE_PATH, bus, address, 400000), + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), _rotation(rotation) { } @@ -340,10 +328,10 @@ SF1XX::ioctl(device::file_t *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(_conversion_interval); + _measure_interval = (_conversion_interval); /* if we need to start the poll state machine, do it */ if (want_start) { @@ -357,18 +345,18 @@ SF1XX::ioctl(device::file_t *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 */ - int ticks = USEC2TICK(1000000 / arg); + int interval = (1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(_conversion_interval)) { + if (interval < _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) { @@ -399,7 +387,7 @@ SF1XX::read(device::file_t *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. @@ -528,25 +516,17 @@ SF1XX::start() measure(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&SF1XX::cycle_trampoline, this, USEC2TICK(_conversion_interval)); + ScheduleDelayed(_conversion_interval); } void SF1XX::stop() { - work_cancel(HPWORK, &_work); + ScheduleClear(); } void -SF1XX::cycle_trampoline(void *arg) -{ - SF1XX *dev = (SF1XX *)arg; - - dev->cycle(); -} - -void -SF1XX::cycle() +SF1XX::Run() { /* Collect results */ if (OK != collect()) { @@ -557,12 +537,7 @@ SF1XX::cycle() } /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&SF1XX::cycle_trampoline, - this, - USEC2TICK(_conversion_interval)); - + ScheduleDelayed(_conversion_interval); } void @@ -570,7 +545,7 @@ SF1XX::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); - printf("poll interval: %u ticks\n", _measure_ticks); + printf("poll interval: %u\n", _measure_interval); _reports->print_info("report queue"); }