From ec0d1d6f62d989c722f6c3981704a35ce2e0f946 Mon Sep 17 00:00:00 2001 From: Daniel Agar <daniel@agar.ca> Date: Mon, 11 Feb 2019 11:42:31 -0500 Subject: [PATCH] lsm303d move to px4 work queue --- src/drivers/imu/lsm303d/lsm303d.cpp | 82 ++++++++--------------------- 1 file changed, 23 insertions(+), 59 deletions(-) diff --git a/src/drivers/imu/lsm303d/lsm303d.cpp b/src/drivers/imu/lsm303d/lsm303d.cpp index 531a68cc9bc9..3fa7b10f16aa 100644 --- a/src/drivers/imu/lsm303d/lsm303d.cpp +++ b/src/drivers/imu/lsm303d/lsm303d.cpp @@ -72,6 +72,7 @@ #include <board_config.h> #include <mathlib/math/filter/LowPassFilter2p.hpp> #include <lib/conversion/rotation.h> +#include <px4_work_queue/ScheduledWorkItem.hpp> /* SPI protocol address bits */ #define DIR_READ (1<<7) @@ -220,7 +221,7 @@ extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } class LSM303D_mag; -class LSM303D : public device::SPI +class LSM303D : public device::SPI, public px4::ScheduledWorkItem { public: LSM303D(int bus, const char *path, uint32_t device, enum Rotation rotation); @@ -256,10 +257,9 @@ class LSM303D : public device::SPI private: - LSM303D_mag *_mag; + void Run() override; - struct hrt_call _accel_call; - struct hrt_call _mag_call; + LSM303D_mag *_mag; unsigned _call_accel_interval; unsigned _call_mag_interval; @@ -308,6 +308,8 @@ class LSM303D : public device::SPI // last temperature value float _last_temperature; + hrt_abstime _mag_last_measure{0}; + // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset @@ -338,24 +340,6 @@ class LSM303D : public device::SPI */ void disable_i2c(); - /** - * Static trampoline from the hrt_call context; because we don't have a - * generic hrt wrapper yet. - * - * Called by the HRT in interrupt context at the specified rate if - * automatic polling is enabled. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void measure_trampoline(void *arg); - - /** - * Static trampoline for the mag because it runs at a lower rate - * - * @param arg Instance pointer for the driver that is polling. - */ - static void mag_measure_trampoline(void *arg); - /** * check key registers for correct values */ @@ -508,8 +492,6 @@ class LSM303D_mag : public device::CDev void measure(); - void measure_trampoline(void *arg); - /* this class does not allow copying due to ptr data members */ LSM303D_mag(const LSM303D_mag &); LSM303D_mag operator=(const LSM303D_mag &); @@ -519,9 +501,8 @@ class LSM303D_mag : public device::CDev LSM303D::LSM303D(int bus, const char *path, uint32_t device, enum Rotation rotation) : SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */), + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), _mag(new LSM303D_mag(this)), - _accel_call{}, - _mag_call{}, _call_accel_interval(0), _call_mag_interval(0), _accel_reports(nullptr), @@ -830,10 +811,10 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) bool want_start = (_call_accel_interval == 0); /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; + unsigned interval = 1000000 / arg; /* check against maximum sane rate */ - if (ticks < 500) { + if (interval < 500) { return -EINVAL; } @@ -842,9 +823,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ - _call_accel_interval = ticks; - - _accel_call.period = _call_accel_interval - LSM303D_TIMER_REDUCTION; + _call_accel_interval = interval; /* if we need to start the poll state machine, do it */ if (want_start) { @@ -903,16 +882,16 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) bool want_start = (_call_mag_interval == 0); /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; + unsigned interval = 1000000 / arg; /* check against maximum sane rate */ - if (ticks < 1000) { + if (interval < 1000) { return -EINVAL; } /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ - _mag_call.period = _call_mag_interval = ticks; + _call_mag_interval = interval; /* if we need to start the poll state machine, do it */ if (want_start) { @@ -1214,18 +1193,13 @@ LSM303D::start() _mag_reports->flush(); /* start polling at the specified rate */ - hrt_call_every(&_accel_call, - 1000, - _call_accel_interval - LSM303D_TIMER_REDUCTION, - (hrt_callout)&LSM303D::measure_trampoline, this); - hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this); + ScheduleOnInterval(_call_accel_interval - LSM303D_TIMER_REDUCTION, 10000); } void LSM303D::stop() { - hrt_cancel(&_accel_call); - hrt_cancel(&_mag_call); + ScheduleClear(); /* reset internal states */ memset(_last_accel, 0, sizeof(_last_accel)); @@ -1236,21 +1210,14 @@ LSM303D::stop() } void -LSM303D::measure_trampoline(void *arg) +LSM303D::Run() { - LSM303D *dev = (LSM303D *)arg; - /* make another measurement */ - dev->measure(); -} - -void -LSM303D::mag_measure_trampoline(void *arg) -{ - LSM303D *dev = (LSM303D *)arg; + measure(); - /* make another measurement */ - dev->mag_measure(); + if (hrt_elapsed_time(&_mag_last_measure) >= _call_mag_interval) { + mag_measure(); + } } void @@ -1473,6 +1440,9 @@ LSM303D::mag_measure() */ mag_report.timestamp = hrt_absolute_time(); + + _mag_last_measure = mag_report.timestamp; + mag_report.is_external = external(); mag_report.x_raw = raw_mag_report.x; @@ -1672,12 +1642,6 @@ LSM303D_mag::measure() _parent->mag_measure(); } -void -LSM303D_mag::measure_trampoline(void *arg) -{ - _parent->mag_measure_trampoline(arg); -} - /** * Local functions in support of the shell command. */