From 8176fe1b6ec1af619c1a14e64510681fd2286a88 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 22 Feb 2019 10:51:02 -0500 Subject: [PATCH] lsm303agr move to px4 work queue --- .../magnetometer/lsm303agr/LSM303AGR.cpp | 45 +++++++------------ .../magnetometer/lsm303agr/LSM303AGR.hpp | 17 ++----- 2 files changed, 21 insertions(+), 41 deletions(-) diff --git a/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp b/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp index 1da84874bb..86cf6524aa 100644 --- a/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp +++ b/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp @@ -63,6 +63,7 @@ static constexpr uint8_t LSM303AGR_WHO_AM_I_M = 0x40; LSM303AGR::LSM303AGR(int bus, const char *path, uint32_t device, enum Rotation rotation) : SPI("LSM303AGR", path, bus, device, SPIDEV_MODE3, 8 * 1000 * 1000), + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), _mag_sample_perf(perf_alloc(PC_ELAPSED, "LSM303AGR_mag_read")), _bad_registers(perf_alloc(PC_COUNT, "LSM303AGR_bad_reg")), _bad_values(perf_alloc(PC_COUNT, "LSM303AGR_bad_val")), @@ -252,10 +253,10 @@ LSM303AGR::ioctl(struct file *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) { @@ -268,18 +269,18 @@ LSM303AGR::ioctl(struct file *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) { @@ -345,31 +346,23 @@ LSM303AGR::start() _collect_phase = false; /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&LSM303AGR::cycle_trampoline, this, 1); + ScheduleNow(); } void LSM303AGR::stop() { - if (_measure_ticks > 0) { + if (_measure_interval > 0) { /* ensure no new items are queued while we cancel this one */ - _measure_ticks = 0; - work_cancel(HPWORK, &_work); + _measure_interval = 0; + ScheduleClear(); } } void -LSM303AGR::cycle_trampoline(void *arg) +LSM303AGR::Run() { - LSM303AGR *dev = (LSM303AGR *)arg; - - dev->cycle(); -} - -void -LSM303AGR::cycle() -{ - if (_measure_ticks == 0) { + if (_measure_interval == 0) { return; } @@ -390,14 +383,10 @@ LSM303AGR::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)&LSM303AGR::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(CONVERSION_INTERVAL)); + ScheduleDelayed(_measure_interval - CONVERSION_INTERVAL); return; } @@ -409,9 +398,9 @@ LSM303AGR::cycle() /* next phase is collection */ _collect_phase = true; - if (_measure_ticks > 0) { + if (_measure_interval > 0) { /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, &_work, (worker_t)&LSM303AGR::cycle_trampoline, this, USEC2TICK(CONVERSION_INTERVAL)); + ScheduleDelayed(CONVERSION_INTERVAL); } } diff --git a/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp b/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp index 3a7d61f5b5..fcf96ce2aa 100644 --- a/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp +++ b/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp @@ -42,7 +42,7 @@ #include #include #include -#include +#include #include // Register mapping @@ -88,7 +88,7 @@ static constexpr uint8_t OUTY_H_REG_M = 0x6B; static constexpr uint8_t OUTZ_L_REG_M = 0x6C; static constexpr uint8_t OUTZ_H_REG_M = 0x6D; -class LSM303AGR : public device::SPI +class LSM303AGR : public device::SPI, public px4::ScheduledWorkItem { public: LSM303AGR(int bus, const char *path, uint32_t device, enum Rotation rotation); @@ -108,10 +108,9 @@ protected: private: - work_s _work{}; bool _collect_phase{false}; - unsigned _measure_ticks{0}; + unsigned _measure_interval{0}; unsigned _call_mag_interval{0}; @@ -178,15 +177,7 @@ private: * and measurement to provide the most recent measurement possible * at the next interval. */ - void cycle(); - - /** - * 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); + void Run() override; /** * Read a register from the LSM303AGR