diff --git a/src/drivers/distance_sensor/mb12xx/mb12xx.cpp b/src/drivers/distance_sensor/mb12xx/mb12xx.cpp index dcfc75dc05..4765e8571a 100644 --- a/src/drivers/distance_sensor/mb12xx/mb12xx.cpp +++ b/src/drivers/distance_sensor/mb12xx/mb12xx.cpp @@ -41,7 +41,7 @@ #include #include -#include +#include #include #include @@ -85,23 +85,19 @@ #define MB12XX_MAX_DISTANCE (7.65f) #define MB12XX_CONVERSION_INTERVAL 100000 /* 60ms for one sonar */ -#define TICKS_BETWEEN_SUCCESIVE_FIRES 100000 /* 30ms between each sonar measurement (watch out for interference!) */ +#define MB12XX_INTERVAL_BETWEEN_SUCCESIVE_FIRES 100000 /* 30ms between each sonar measurement (watch out for interference!) */ -#ifndef CONFIG_SCHED_WORKQUEUE -# error This requires CONFIG_SCHED_WORKQUEUE. -#endif - -class MB12XX : public device::I2C +class MB12XX : public device::I2C, public px4::ScheduledWorkItem { public: MB12XX(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, int bus = MB12XX_BUS_DEFAULT, int address = MB12XX_BASEADDR); virtual ~MB12XX(); - 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. @@ -109,16 +105,15 @@ public: void print_info(); protected: - virtual int probe(); + virtual int probe() override; private: uint8_t _rotation; float _min_distance; float _max_distance; - work_s _work{}; ringbuffer::RingBuffer *_reports; bool _sensor_ok; - int _measure_ticks; + int _measure_interval; bool _collect_phase; int _class_instance; int _orb_class_instance; @@ -169,17 +164,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); - }; @@ -190,12 +177,13 @@ extern "C" __EXPORT int mb12xx_main(int argc, char *argv[]); MB12XX::MB12XX(uint8_t rotation, int bus, int address) : I2C("MB12xx", MB12XX_DEVICE_PATH, bus, address, 100000), + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), _rotation(rotation), _min_distance(MB12XX_MIN_DISTANCE), _max_distance(MB12XX_MAX_DISTANCE), _reports(nullptr), _sensor_ok(false), - _measure_ticks(0), + _measure_interval(0), _collect_phase(false), _class_instance(-1), _orb_class_instance(-1), @@ -205,7 +193,6 @@ MB12XX::MB12XX(uint8_t rotation, int bus, int address) : _cycle_counter(0), /* initialising counter for cycling function to zero */ _cycling_rate(0), /* initialising cycling rate (which can differ depending on one sonar or multiple) */ _index_counter(0) /* initialising temp sonar i2c address to zero */ - { } @@ -285,7 +272,7 @@ MB12XX::init() _cycling_rate = MB12XX_CONVERSION_INTERVAL; } else { - _cycling_rate = TICKS_BETWEEN_SUCCESIVE_FIRES; + _cycling_rate = MB12XX_INTERVAL_BETWEEN_SUCCESIVE_FIRES; } /* show the connected sonars in terminal */ @@ -347,10 +334,10 @@ MB12XX::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(_cycling_rate); + _measure_interval = _cycling_rate; /* if we need to start the poll state machine, do it */ if (want_start) { @@ -364,18 +351,18 @@ MB12XX::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(_cycling_rate)) { + if (interval < _cycling_rate) { 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) { @@ -407,7 +394,7 @@ MB12XX::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. @@ -537,27 +524,17 @@ MB12XX::start() _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 5); + ScheduleDelayed(5); } void MB12XX::stop() { - work_cancel(HPWORK, &_work); + ScheduleClear(); } void -MB12XX::cycle_trampoline(void *arg) -{ - - MB12XX *dev = (MB12XX *)arg; - - dev->cycle(); - -} - -void -MB12XX::cycle() +MB12XX::Run() { if (_collect_phase) { _index_counter = addr_ind[_cycle_counter]; /*sonar from previous iteration collect is now read out */ @@ -584,14 +561,11 @@ MB12XX::cycle() /* Is there a collect->measure gap? Yes, and the timing is set equal to the cycling_rate Otherwise the next sonar would fire without the first one having received its reflected sonar pulse */ - if (_measure_ticks > USEC2TICK(_cycling_rate)) { + if (_measure_interval > _cycling_rate) { /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&MB12XX::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(_cycling_rate)); + ScheduleDelayed(_measure_interval - _cycling_rate); + return; } } @@ -611,12 +585,7 @@ MB12XX::cycle() _collect_phase = true; /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&MB12XX::cycle_trampoline, - this, - USEC2TICK(_cycling_rate)); - + ScheduleDelayed(_cycling_rate); } void @@ -624,7 +593,7 @@ MB12XX::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"); }