Browse Source

bmi055 move to px4 work queue

sbg
Daniel Agar 6 years ago
parent
commit
e27a8e1010
  1. 2
      src/drivers/imu/bmi055/BMI055.hpp
  2. 33
      src/drivers/imu/bmi055/BMI055_accel.cpp
  3. 13
      src/drivers/imu/bmi055/BMI055_accel.hpp
  4. 33
      src/drivers/imu/bmi055/BMI055_gyro.cpp
  5. 13
      src/drivers/imu/bmi055/BMI055_gyro.hpp
  6. 1
      src/drivers/imu/bmi055/bmi055_main.cpp

2
src/drivers/imu/bmi055/BMI055.hpp

@ -41,6 +41,7 @@ @@ -41,6 +41,7 @@
#include <px4_config.h>
#include <systemlib/conversions.h>
#include <systemlib/err.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>
#define DIR_READ 0x80
#define DIR_WRITE 0x00
@ -59,7 +60,6 @@ protected: @@ -59,7 +60,6 @@ protected:
uint8_t _whoami; /** whoami result */
struct hrt_call _call;
unsigned _call_interval;
uint8_t _register_wait;

33
src/drivers/imu/bmi055/BMI055_accel.cpp

@ -48,6 +48,7 @@ const uint8_t BMI055_accel::_checked_registers[BMI055_ACCEL_NUM_CHECKED_REGISTER @@ -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 @@ -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) @@ -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() @@ -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<BMI055_accel *>(arg);
/* make another measurement */
dev->measure();
measure();
}
void

13
src/drivers/imu/bmi055/BMI055_accel.hpp

@ -148,7 +148,7 @@ @@ -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: @@ -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.

33
src/drivers/imu/bmi055/BMI055_gyro.cpp

@ -49,6 +49,7 @@ const uint8_t BMI055_gyro::_checked_registers[BMI055_GYRO_NUM_CHECKED_REGISTERS] @@ -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 @@ -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) @@ -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() @@ -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<BMI055_gyro *>(arg);
/* make another measurement */
dev->measure();
measure();
}
void

13
src/drivers/imu/bmi055/BMI055_gyro.hpp

@ -140,7 +140,7 @@ @@ -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: @@ -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.

1
src/drivers/imu/bmi055/bmi055_main.cpp

@ -446,7 +446,6 @@ BMI055::BMI055(const char *name, const char *devname, int bus, uint32_t device, @@ -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),

Loading…
Cancel
Save