From e27a8e10105d75ae67836fde81af569e96d12c66 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 27 Jan 2019 20:49:24 -0500 Subject: [PATCH] bmi055 move to px4 work queue --- src/drivers/imu/bmi055/BMI055.hpp | 2 +- src/drivers/imu/bmi055/BMI055_accel.cpp | 33 ++++++++----------------- src/drivers/imu/bmi055/BMI055_accel.hpp | 13 ++-------- src/drivers/imu/bmi055/BMI055_gyro.cpp | 33 ++++++++----------------- src/drivers/imu/bmi055/BMI055_gyro.hpp | 13 ++-------- src/drivers/imu/bmi055/bmi055_main.cpp | 1 - 6 files changed, 25 insertions(+), 70 deletions(-) diff --git a/src/drivers/imu/bmi055/BMI055.hpp b/src/drivers/imu/bmi055/BMI055.hpp index a341ce288c..d8ea4c009e 100644 --- a/src/drivers/imu/bmi055/BMI055.hpp +++ b/src/drivers/imu/bmi055/BMI055.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #define DIR_READ 0x80 #define DIR_WRITE 0x00 @@ -59,7 +60,6 @@ protected: uint8_t _whoami; /** whoami result */ - struct hrt_call _call; unsigned _call_interval; uint8_t _register_wait; diff --git a/src/drivers/imu/bmi055/BMI055_accel.cpp b/src/drivers/imu/bmi055/BMI055_accel.cpp index bc3e2c1beb..ae823e7a66 100644 --- a/src/drivers/imu/bmi055/BMI055_accel.cpp +++ b/src/drivers/imu/bmi055/BMI055_accel.cpp @@ -48,6 +48,7 @@ const uint8_t BMI055_accel::_checked_registers[BMI055_ACCEL_NUM_CHECKED_REGISTER BMI055_accel::BMI055_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation) : BMI055("BMI055_ACCEL", path_accel, bus, device, SPIDEV_MODE3, BMI055_BUS_SPEED, rotation), + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), _sample_perf(perf_alloc(PC_ELAPSED, "bmi055_accel_read")), _measure_interval(perf_alloc(PC_INTERVAL, "bmi055_accel_measure_interval")), _bad_transfers(perf_alloc(PC_COUNT, "bmi055_accel_bad_transfers")), @@ -76,8 +77,6 @@ BMI055_accel::BMI055_accel(int bus, const char *path_accel, uint32_t device, enu _accel_scale.y_scale = 1.0f; _accel_scale.z_offset = 0; _accel_scale.z_scale = 1.0f; - - memset(&_call, 0, sizeof(_call)); } BMI055_accel::~BMI055_accel() @@ -306,31 +305,23 @@ BMI055_accel::ioctl(struct file *filp, int cmd, unsigned long arg) bool want_start = (_call_interval == 0); /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; + unsigned interval = 1000000 / arg; /* check against maximum rate */ - if (ticks < 1000) { + if (interval < 1000) { return -EINVAL; } // adjust filters float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); - float sample_rate = 1.0e6f / ticks; + float sample_rate = 1.0e6f / interval; _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); /* update interval for next measurement */ - _call_interval = ticks; - - /* - set call interval faster than the sample time. We - then detect when we have duplicate samples and reject - them. This prevents aliasing due to a beat between the - stm32 clock and the bmi055 clock - */ - _call.period = _call_interval - BMI055_TIMER_REDUCTION; + _call_interval = interval; /* if we need to start the poll state machine, do it */ if (want_start) { @@ -440,26 +431,22 @@ BMI055_accel::start() _accel_reports->flush(); /* start polling at the specified rate */ - hrt_call_every(&_call, - 1000, - _call_interval - BMI055_TIMER_REDUCTION, - (hrt_callout)&BMI055_accel::measure_trampoline, this); + ScheduleOnInterval(_call_interval - BMI055_TIMER_REDUCTION, 1000); + reset(); } void BMI055_accel::stop() { - hrt_cancel(&_call); + ScheduleClear(); } void -BMI055_accel::measure_trampoline(void *arg) +BMI055_accel::Run() { - BMI055_accel *dev = reinterpret_cast(arg); - /* make another measurement */ - dev->measure(); + measure(); } void diff --git a/src/drivers/imu/bmi055/BMI055_accel.hpp b/src/drivers/imu/bmi055/BMI055_accel.hpp index 773f3fad51..b82a92923b 100644 --- a/src/drivers/imu/bmi055/BMI055_accel.hpp +++ b/src/drivers/imu/bmi055/BMI055_accel.hpp @@ -148,7 +148,7 @@ /* Mask definitions for ACCD_X_LSB, ACCD_Y_LSB and ACCD_Z_LSB Register */ #define BMI055_NEW_DATA_MASK 0x01 -class BMI055_accel : public BMI055 +class BMI055_accel : public BMI055, public px4::ScheduledWorkItem { public: BMI055_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation); @@ -227,16 +227,7 @@ private: */ int reset(); - /** - * 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); + void Run() override; /** * Fetch measurements from the sensor and update the report buffers. diff --git a/src/drivers/imu/bmi055/BMI055_gyro.cpp b/src/drivers/imu/bmi055/BMI055_gyro.cpp index aecb0ff6cd..cd6406ce0b 100644 --- a/src/drivers/imu/bmi055/BMI055_gyro.cpp +++ b/src/drivers/imu/bmi055/BMI055_gyro.cpp @@ -49,6 +49,7 @@ const uint8_t BMI055_gyro::_checked_registers[BMI055_GYRO_NUM_CHECKED_REGISTERS] BMI055_gyro::BMI055_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation) : BMI055("BMI055_GYRO", path_gyro, bus, device, SPIDEV_MODE3, BMI055_BUS_SPEED, rotation), + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), _sample_perf(perf_alloc(PC_ELAPSED, "bmi055_gyro_read")), _measure_interval(perf_alloc(PC_INTERVAL, "bmi055_gyro_measure_interval")), _bad_transfers(perf_alloc(PC_COUNT, "bmi055_gyro_bad_transfers")), @@ -76,8 +77,6 @@ BMI055_gyro::BMI055_gyro(int bus, const char *path_gyro, uint32_t device, enum R _gyro_scale.y_scale = 1.0f; _gyro_scale.z_offset = 0; _gyro_scale.z_scale = 1.0f; - - memset(&_call, 0, sizeof(_call)); } BMI055_gyro::~BMI055_gyro() @@ -334,30 +333,22 @@ BMI055_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) bool want_start = (_call_interval == 0); /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; + unsigned interval = 1000000 / arg; /* check against maximum rate */ - if (ticks < 1000) { + if (interval < 1000) { return -EINVAL; } // adjust filters float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq(); - float sample_rate = 1.0e6f / ticks; + float sample_rate = 1.0e6f / interval; _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); /* update interval for next measurement */ - _call_interval = ticks; - - /* - set call interval faster than the sample time. We - then detect when we have duplicate samples and reject - them. This prevents aliasing due to a beat between the - stm32 clock and the bmi055 clock - */ - _call.period = _call_interval - BMI055_TIMER_REDUCTION; + _call_interval = interval; /* if we need to start the poll state machine, do it */ if (want_start) { @@ -463,26 +454,22 @@ BMI055_gyro::start() _gyro_reports->flush(); /* start polling at the specified rate */ - hrt_call_every(&_call, - 1000, - _call_interval - BMI055_TIMER_REDUCTION, - (hrt_callout)&BMI055_gyro::measure_trampoline, this); + ScheduleOnInterval(_call_interval - BMI055_TIMER_REDUCTION, 1000); + reset(); } void BMI055_gyro::stop() { - hrt_cancel(&_call); + ScheduleClear(); } void -BMI055_gyro::measure_trampoline(void *arg) +BMI055_gyro::Run() { - BMI055_gyro *dev = reinterpret_cast(arg); - /* make another measurement */ - dev->measure(); + measure(); } void diff --git a/src/drivers/imu/bmi055/BMI055_gyro.hpp b/src/drivers/imu/bmi055/BMI055_gyro.hpp index cc1eb42ce6..458e54202f 100644 --- a/src/drivers/imu/bmi055/BMI055_gyro.hpp +++ b/src/drivers/imu/bmi055/BMI055_gyro.hpp @@ -140,7 +140,7 @@ #define BMI055_ACC_TEMP 0x08 -class BMI055_gyro : public BMI055 +class BMI055_gyro : public BMI055, public px4::ScheduledWorkItem { public: BMI055_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation); @@ -218,16 +218,7 @@ private: */ int reset(); - /** - * 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); + void Run() override; /** * Fetch measurements from the sensor and update the report buffers. diff --git a/src/drivers/imu/bmi055/bmi055_main.cpp b/src/drivers/imu/bmi055/bmi055_main.cpp index 44db028cc9..cd9320b02f 100644 --- a/src/drivers/imu/bmi055/bmi055_main.cpp +++ b/src/drivers/imu/bmi055/bmi055_main.cpp @@ -446,7 +446,6 @@ BMI055::BMI055(const char *name, const char *devname, int bus, uint32_t device, uint32_t frequency, enum Rotation rotation): SPI(name, devname, bus, device, mode, frequency), _whoami(0), - _call{}, _call_interval(0), _register_wait(0), _reset_wait(0),