diff --git a/src/drivers/distance_sensor/sf0x/sf0x.cpp b/src/drivers/distance_sensor/sf0x/sf0x.cpp index 3f7087c3ee..f454ea7cd3 100644 --- a/src/drivers/distance_sensor/sf0x/sf0x.cpp +++ b/src/drivers/distance_sensor/sf0x/sf0x.cpp @@ -41,7 +41,7 @@ #include #include -#include +#include #include #include @@ -71,25 +71,21 @@ /* Configuration Constants */ -#ifndef CONFIG_SCHED_WORKQUEUE -# error This requires CONFIG_SCHED_WORKQUEUE. -#endif - #define SF0X_TAKE_RANGE_REG 'd' // designated SERIAL4/5 on Pixhawk #define SF0X_DEFAULT_PORT "/dev/ttyS6" -class SF0X : public cdev::CDev +class SF0X : public cdev::CDev, public px4::ScheduledWorkItem { public: SF0X(const char *port = SF0X_DEFAULT_PORT, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); virtual ~SF0X(); - virtual int init(); + virtual 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); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override; + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; /** * Diagnostics - print some basic information about the driver. @@ -102,9 +98,8 @@ private: float _min_distance; float _max_distance; int _conversion_interval; - work_s _work{}; ringbuffer::RingBuffer *_reports; - int _measure_ticks; + int _measure_interval; bool _collect_phase; int _fd; char _linebuf[10]; @@ -149,17 +144,9 @@ private: * 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 workq wrapper yet. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void cycle_trampoline(void *arg); - }; @@ -170,12 +157,13 @@ extern "C" __EXPORT int sf0x_main(int argc, char *argv[]); SF0X::SF0X(const char *port, uint8_t rotation) : CDev(RANGE_FINDER0_DEVICE_PATH), + ScheduledWorkItem(px4::wq_configurations::hp_default), _rotation(rotation), _min_distance(0.30f), _max_distance(40.0f), _conversion_interval(83334), _reports(nullptr), - _measure_ticks(0), + _measure_interval(0), _collect_phase(false), _fd(-1), _linebuf_index(0), @@ -190,6 +178,7 @@ SF0X::SF0X(const char *port, uint8_t rotation) : { /* store port name */ strncpy(_port, port, sizeof(_port) - 1); + /* enforce null termination */ _port[sizeof(_port) - 1] = '\0'; @@ -332,10 +321,10 @@ SF0X::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) { @@ -349,18 +338,18 @@ SF0X::ioctl(device::file_t *filp, int cmd, unsigned long arg) 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) { @@ -391,7 +380,7 @@ SF0X::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. @@ -463,8 +452,6 @@ SF0X::measure() int SF0X::collect() { - int ret; - perf_begin(_sample_perf); /* clear buffer if last read was too long ago */ @@ -475,7 +462,7 @@ SF0X::collect() unsigned readlen = sizeof(readbuf) - 1; /* read from the sensor (uart buffer) */ - ret = ::read(_fd, &readbuf[0], readlen); + int ret = ::read(_fd, &readbuf[0], readlen); if (ret < 0) { PX4_DEBUG("read err: %d", ret); @@ -546,26 +533,17 @@ SF0X::start() _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&SF0X::cycle_trampoline, this, 1); - + ScheduleNow(); } void SF0X::stop() { - work_cancel(HPWORK, &_work); + ScheduleClear(); } void -SF0X::cycle_trampoline(void *arg) -{ - SF0X *dev = static_cast(arg); - - dev->cycle(); -} - -void -SF0X::cycle() +SF0X::Run() { /* fds initialized? */ if (_fd < 0) { @@ -626,11 +604,8 @@ SF0X::cycle() if (collect_ret == -EAGAIN) { /* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */ - work_queue(HPWORK, - &_work, - (worker_t)&SF0X::cycle_trampoline, - this, - USEC2TICK(1042 * 8)); + ScheduleDelayed(1042 * 8); + return; } @@ -658,14 +633,10 @@ SF0X::cycle() /* * Is there a collect->measure gap? */ - if (_measure_ticks > USEC2TICK(_conversion_interval)) { + if (_measure_interval > (_conversion_interval)) { /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&SF0X::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(_conversion_interval)); + ScheduleDelayed(_measure_interval - _conversion_interval); return; } @@ -680,11 +651,7 @@ SF0X::cycle() _collect_phase = true; /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&SF0X::cycle_trampoline, - this, - USEC2TICK(_conversion_interval)); + ScheduleDelayed(_conversion_interval); } void @@ -692,7 +659,7 @@ SF0X::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); - printf("poll interval: %d ticks\n", _measure_ticks); + printf("poll interval: %d\n", _measure_interval); _reports->print_info("report queue"); }