diff --git a/src/drivers/differential_pressure/ets/ets_airspeed.cpp b/src/drivers/differential_pressure/ets/ets_airspeed.cpp index 4a529f41d3..f6da316144 100644 --- a/src/drivers/differential_pressure/ets/ets_airspeed.cpp +++ b/src/drivers/differential_pressure/ets/ets_airspeed.cpp @@ -84,9 +84,9 @@ protected: * Perform a poll cycle; collect from the previous measurement * and start a new one. */ - virtual void cycle(); - virtual int measure(); - virtual int collect(); + void Run() override; + int measure() override; + int collect() override; }; @@ -174,7 +174,7 @@ ETSAirspeed::collect() } void -ETSAirspeed::cycle() +ETSAirspeed::Run() { int ret; @@ -198,14 +198,10 @@ ETSAirspeed::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)&Airspeed::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(CONVERSION_INTERVAL)); + ScheduleDelayed(_measure_interval - CONVERSION_INTERVAL); return; } @@ -224,11 +220,7 @@ ETSAirspeed::cycle() _collect_phase = true; /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&Airspeed::cycle_trampoline, - this, - USEC2TICK(CONVERSION_INTERVAL)); + ScheduleDelayed(CONVERSION_INTERVAL); } /** diff --git a/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp b/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp index 9a11a4df6d..7b08cd6290 100644 --- a/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp +++ b/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp @@ -99,9 +99,9 @@ protected: * Perform a poll cycle; collect from the previous measurement * and start a new one. */ - virtual void cycle(); - virtual int measure(); - virtual int collect(); + void Run() override; + int measure() override; + int collect() override; math::LowPassFilter2p _filter{MEAS_RATE, MEAS_DRIVER_FILTER_FREQ}; @@ -238,7 +238,7 @@ MEASAirspeed::collect() } void -MEASAirspeed::cycle() +MEASAirspeed::Run() { int ret; @@ -261,14 +261,10 @@ MEASAirspeed::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)&Airspeed::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(CONVERSION_INTERVAL)); + ScheduleDelayed(_measure_interval - CONVERSION_INTERVAL); return; } @@ -287,11 +283,7 @@ MEASAirspeed::cycle() _collect_phase = true; /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&Airspeed::cycle_trampoline, - this, - USEC2TICK(CONVERSION_INTERVAL)); + ScheduleDelayed(CONVERSION_INTERVAL); } /** diff --git a/src/drivers/differential_pressure/ms5525/MS5525.cpp b/src/drivers/differential_pressure/ms5525/MS5525.cpp index 4f8e077209..1c7642411b 100644 --- a/src/drivers/differential_pressure/ms5525/MS5525.cpp +++ b/src/drivers/differential_pressure/ms5525/MS5525.cpp @@ -272,7 +272,7 @@ MS5525::collect() } void -MS5525::cycle() +MS5525::Run() { int ret = PX4_ERROR; @@ -292,11 +292,10 @@ MS5525::cycle() _collect_phase = false; // 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)&Airspeed::cycle_trampoline, this, - _measure_ticks - USEC2TICK(CONVERSION_INTERVAL)); + ScheduleDelayed(_measure_interval - CONVERSION_INTERVAL); return; } @@ -315,5 +314,5 @@ MS5525::cycle() _collect_phase = true; // schedule a fresh cycle call when the measurement is done - work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, USEC2TICK(CONVERSION_INTERVAL)); + ScheduleDelayed(CONVERSION_INTERVAL); } diff --git a/src/drivers/differential_pressure/ms5525/MS5525.hpp b/src/drivers/differential_pressure/ms5525/MS5525.hpp index c915b791f0..ee9e146008 100644 --- a/src/drivers/differential_pressure/ms5525/MS5525.hpp +++ b/src/drivers/differential_pressure/ms5525/MS5525.hpp @@ -71,7 +71,7 @@ private: * Perform a poll cycle; collect from the previous measurement * and start a new one. */ - void cycle() override; + void Run() override; int measure() override; int collect() override; diff --git a/src/drivers/differential_pressure/sdp3x/SDP3X.cpp b/src/drivers/differential_pressure/sdp3x/SDP3X.cpp index cf632b4db3..c5ce6e08f9 100644 --- a/src/drivers/differential_pressure/sdp3x/SDP3X.cpp +++ b/src/drivers/differential_pressure/sdp3x/SDP3X.cpp @@ -168,7 +168,7 @@ SDP3X::collect() } void -SDP3X::cycle() +SDP3X::Run() { int ret = PX4_ERROR; @@ -181,7 +181,7 @@ SDP3X::cycle() } // schedule a fresh cycle call when the measurement is done - work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, USEC2TICK(CONVERSION_INTERVAL)); + ScheduleDelayed(CONVERSION_INTERVAL); } bool SDP3X::crc(const uint8_t data[], unsigned size, uint8_t checksum) diff --git a/src/drivers/differential_pressure/sdp3x/SDP3X.hpp b/src/drivers/differential_pressure/sdp3x/SDP3X.hpp index aeafc04e8e..942932b462 100644 --- a/src/drivers/differential_pressure/sdp3x/SDP3X.hpp +++ b/src/drivers/differential_pressure/sdp3x/SDP3X.hpp @@ -87,7 +87,7 @@ private: * Perform a poll cycle; collect from the previous measurement * and start a new one. */ - void cycle() override; + void Run() override; int measure() override { return 0; } int collect() override; int probe() override; diff --git a/src/lib/drivers/airspeed/airspeed.cpp b/src/lib/drivers/airspeed/airspeed.cpp index 5f63f76d87..0997d931ee 100644 --- a/src/lib/drivers/airspeed/airspeed.cpp +++ b/src/lib/drivers/airspeed/airspeed.cpp @@ -58,8 +58,9 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char *path) : I2C("Airspeed", path, bus, address, 100000), + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), _sensor_ok(false), - _measure_ticks(0), + _measure_interval(0), _collect_phase(false), _diff_pres_offset(0.0f), _airspeed_pub(nullptr), @@ -69,11 +70,6 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const cha _sample_perf(perf_alloc(PC_ELAPSED, "aspd_read")), _comms_errors(perf_alloc(PC_COUNT, "aspd_com_err")) { - // enable debug() calls - _debug_enabled = false; - - // work_cancel in the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); } Airspeed::~Airspeed() @@ -148,10 +144,10 @@ Airspeed::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 = USEC2TICK(_conversion_interval); /* if we need to start the poll state machine, do it */ if (want_start) { @@ -164,18 +160,18 @@ Airspeed::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 */ - unsigned ticks = USEC2TICK(1000000 / arg); + unsigned 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) { @@ -207,18 +203,11 @@ Airspeed::start() _collect_phase = false; /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1); + ScheduleNow(); } void Airspeed::stop() { - work_cancel(HPWORK, &_work); -} - -void -Airspeed::cycle_trampoline(void *arg) -{ - Airspeed *dev = (Airspeed *)arg; - dev->cycle(); + ScheduleClear(); } diff --git a/src/lib/drivers/airspeed/airspeed.h b/src/lib/drivers/airspeed/airspeed.h index 031f493d2a..f374267548 100644 --- a/src/lib/drivers/airspeed/airspeed.h +++ b/src/lib/drivers/airspeed/airspeed.h @@ -39,15 +39,15 @@ #include #include #include -#include #include #include #include +#include /* Default I2C bus */ static constexpr uint8_t PX4_I2C_BUS_DEFAULT = PX4_I2C_BUS_EXPANSION; -class __EXPORT Airspeed : public device::I2C +class __EXPORT Airspeed : public device::I2C, public px4::ScheduledWorkItem { public: Airspeed(int bus, int address, unsigned conversion_interval, const char *path); @@ -69,13 +69,12 @@ protected: * Perform a poll cycle; collect from the previous measurement * and start a new one. */ - virtual void cycle() = 0; + virtual void Run() = 0; virtual int measure() = 0; virtual int collect() = 0; - work_s _work; bool _sensor_ok; - int _measure_ticks; + int _measure_interval; bool _collect_phase; float _diff_pres_offset; @@ -102,14 +101,6 @@ protected: */ void stop(); - /** - * 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); - /** * add a new report to the reports queue *