dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

adis16477 move to px4 work queue

This commit is contained in:
Daniel Agar 2019-01-27 20:36:20 -05:00
parent 6113caaa33
commit 427c49ac43
2 changed files with 12 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/drivers/imu/adis16477/ADIS16477.cpp
View File

@ -79,6 +79,7 @@ using namespace time_literals;
ADIS16477::ADIS16477(int bus, const char *path_accel, const char *path_gyro, uint32_t device, enum Rotation rotation) :
SPI("ADIS16477", path_accel, bus, device, SPIDEV_MODE3, 1000000),
ScheduledWorkItem(px4::device_bus_to_wq(this->get_device_id())),
_gyro(new ADIS16477_gyro(this, path_gyro)),
_sample_perf(perf_alloc(PC_ELAPSED, "adis16477_read")),
_bad_transfers(perf_alloc(PC_COUNT, "adis16477_bad_transfers")),
@ -358,16 +359,16 @@ ADIS16477::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);
@ -379,7 +380,7 @@ ADIS16477::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.period = _call_interval = ticks;
_call_interval = interval;
/* if we need to start the poll state machine, do it */
if (want_start) {
@ -478,22 +479,20 @@ ADIS16477::start()
_call_interval = last_call_interval;
/* start polling at the specified rate */
hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&ADIS16477::measure_trampoline, this);
ScheduleOnInterval(_call_interval, 10000);
}
void
ADIS16477::stop()
{
hrt_cancel(&_call);
ScheduleClear();
}
void
ADIS16477::measure_trampoline(void *arg)
ADIS16477::Run()
{
ADIS16477 *dev = reinterpret_cast<ADIS16477 *>(arg);
/* make another measurement */
dev->measure();
measure();
}
int

16
src/drivers/imu/adis16477/ADIS16477.hpp
View File

@ -41,7 +41,6 @@
#include <drivers/device/ringbuffer.h>
#include <drivers/device/spi.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
@ -50,6 +49,7 @@
#include <perf/perf_counter.h>
#include <ecl/geo/geo.h>
#include <systemlib/err.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>
#define ADIS16477_GYRO_DEFAULT_RATE 250
#define ADIS16477_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30
@ -62,7 +62,7 @@
class ADIS16477_gyro;
class ADIS16477 : public device::SPI
class ADIS16477 : public device::SPI, public px4::ScheduledWorkItem
{
public:
ADIS16477(int bus, const char *path_accel, const char *path_gyro, uint32_t device, enum Rotation rotation);
@ -86,7 +86,6 @@ private:
uint16_t _product{0}; /** product code */
struct hrt_call _call {};
unsigned _call_interval{0};
struct gyro_calibration_s _gyro_scale {};
@ -163,16 +162,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.