fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-29 02:34:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

bmi160 move to px4 work queue

Browse Source
This commit is contained in:
Daniel Agar 2019-01-27 20:56:42 -05:00
parent 370f0f73ce
commit 9fa865d490
2 changed files with 13 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
src/drivers/imu/bmi160/bmi160.cpp
View File

@ -20,9 +20,9 @@ const uint8_t BMI160::_checked_registers[BMI160_NUM_CHECKED_REGISTERS] = { BM
BMI160::BMI160(int bus, const char *path_accel, const char *path_gyro, uint32_t device, enum Rotation rotation) :
SPI("BMI160", path_accel, bus, device, SPIDEV_MODE3, BMI160_BUS_SPEED),
ScheduledWorkItem(px4::device_bus_to_wq(this->get_device_id())),
_gyro(new BMI160_gyro(this, path_gyro)),
_whoami(0),
_call{},
_call_interval(0),
_accel_reports(nullptr),
_accel_scale{},
@ -86,8 +86,6 @@ BMI160::BMI160(int bus, const char *path_accel, const char *path_gyro, uint32_t
_gyro_scale.y_scale = 1.0f;
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
memset(&_call, 0, sizeof(_call));
}
@ -575,16 +573,16 @@ BMI160::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 sane 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);
@ -597,15 +595,7 @@ BMI160::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_interval = ticks;
/*
set call interval faster then 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 bmi160 clock
*/
_call.period = _call_interval - BMI160_TIMER_REDUCTION;
_call_interval = interval;
/* if we need to start the poll state machine, do it */
if (want_start) {
@ -817,26 +807,22 @@ BMI160::start()
_gyro_reports->flush();
/* start polling at the specified rate */
hrt_call_every(&_call,
1000,
_call_interval - BMI160_TIMER_REDUCTION,
(hrt_callout)&BMI160::measure_trampoline, this);
ScheduleOnInterval(_call_interval - BMI160_TIMER_REDUCTION, 1000);
reset();
}
void
BMI160::stop()
{
hrt_cancel(&_call);
ScheduleClear();
}
void
BMI160::measure_trampoline(void *arg)
BMI160::Run()
{
BMI160 *dev = reinterpret_cast<BMI160 *>(arg);
/* make another measurement */
dev->measure();
measure();
}
void

15
src/drivers/imu/bmi160/bmi160.hpp
View File

@ -35,6 +35,7 @@
#include <drivers/drv_mag.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/conversion/rotation.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>
#define DIR_READ 0x80
#define DIR_WRITE 0x00
@ -243,7 +244,7 @@
class BMI160_gyro;
class BMI160 : public device::SPI
class BMI160 : public device::SPI, public px4::ScheduledWorkItem
{
public:
BMI160(int bus, const char *path_accel, const char *path_gyro, uint32_t device, enum Rotation rotation);
@ -276,7 +277,6 @@ private:
BMI160_gyro *_gyro;
uint8_t _whoami; /** whoami result */
struct hrt_call _call;
unsigned _call_interval;
ringbuffer::RingBuffer *_accel_reports;
@ -355,16 +355,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.