diff --git a/src/drivers/magnetometer/bmm150/bmm150.cpp b/src/drivers/magnetometer/bmm150/bmm150.cpp index 21c0fb90e1..cbf8ab57ef 100644 --- a/src/drivers/magnetometer/bmm150/bmm150.cpp +++ b/src/drivers/magnetometer/bmm150/bmm150.cpp @@ -249,8 +249,9 @@ usage() } // namespace bmm150 -BMM150 :: BMM150(int bus, const char *path, enum Rotation rotation) : +BMM150::BMM150(int bus, const char *path, enum Rotation rotation) : I2C("BMM150", path, bus, BMM150_SLAVE_ADDRESS, BMM150_BUS_SPEED), + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), _running(false), _call_interval(0), _reports(nullptr), @@ -285,9 +286,6 @@ BMM150 :: BMM150(int bus, const char *path, enum Rotation rotation) : { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_BMM150; - // work_cancel in the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); - // default scaling _scale.x_offset = 0; _scale.x_scale = 1.0f; @@ -415,16 +413,14 @@ BMM150::start() _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&BMM150::cycle_trampoline, this, 1); - + ScheduleNow(); } void BMM150::stop() { _running = false; - work_cancel(HPWORK, &_work); - + ScheduleClear(); } ssize_t @@ -488,26 +484,17 @@ BMM150::read(struct file *filp, char *buffer, size_t buflen) } - void -BMM150::cycle_trampoline(void *arg) -{ - BMM150 *dev = reinterpret_cast(arg); - - /* make measurement */ - dev->cycle(); -} - -void -BMM150::cycle() +BMM150::Run() { if (_collect_phase) { collect(); - unsigned wait_gap = _call_interval - USEC2TICK(BMM150_CONVERSION_INTERVAL); + unsigned wait_gap = _call_interval - BMM150_CONVERSION_INTERVAL; if ((wait_gap != 0) && (_running)) { - work_queue(HPWORK, &_work, (worker_t)&BMM150::cycle_trampoline, this, - wait_gap); //need to wait some time before new measurement + // need to wait some time before new measurement + ScheduleDelayed(wait_gap); + return; } @@ -517,11 +504,7 @@ BMM150::cycle() if ((_running)) { /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&BMM150::cycle_trampoline, - this, - USEC2TICK(BMM150_CONVERSION_INTERVAL)); + ScheduleDelayed(BMM150_CONVERSION_INTERVAL); } @@ -743,15 +726,15 @@ BMM150::ioctl(struct file *filp, int cmd, unsigned long arg) bool want_start = (_call_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(BMM150_CONVERSION_INTERVAL)) { + if (interval < BMM150_CONVERSION_INTERVAL) { return -EINVAL; } /* update interval for next measurement */ - _call_interval = ticks; + _call_interval = interval; /* if we need to start the poll state machine, do it */ if (want_start) { diff --git a/src/drivers/magnetometer/bmm150/bmm150.hpp b/src/drivers/magnetometer/bmm150/bmm150.hpp index 6683a74d87..78411ae804 100644 --- a/src/drivers/magnetometer/bmm150/bmm150.hpp +++ b/src/drivers/magnetometer/bmm150/bmm150.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include @@ -191,7 +191,7 @@ struct bmm150_data { }; -class BMM150 : public device::I2C +class BMM150 : public device::I2C, public px4::ScheduledWorkItem { public: BMM150(int bus, const char *path, enum Rotation rotation); @@ -217,7 +217,6 @@ protected: virtual int probe(); private: - work_s _work{}; bool _running; @@ -278,8 +277,7 @@ private: int measure(); //start measure int collect(); //get results and publish - static void cycle_trampoline(void *arg); - void cycle(); //main execution + void Run() override; /** * Read the specified number of bytes from BMM150.