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

fxos8701cq move to px4 work queue

Browse Source
This commit is contained in:
Daniel Agar 2019-02-11 11:50:18 -05:00
parent d4ece2c7bb
commit fc7f1ca598
1 changed files with 27 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

88
src/drivers/imu/fxos8701cq/fxos8701cq.cpp
View File

@ -78,6 +78,7 @@
#include <lib/conversion/rotation.h>
#include <px4_getopt.h>
#include <systemlib/err.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>
/* SPI protocol address bits */
#define DIR_READ(a) ((a) & 0x7f)
@ -159,7 +160,7 @@ extern "C" { __EXPORT int fxos8701cq_main(int argc, char *argv[]); }
class FXOS8701CQ_mag;
#endif
class FXOS8701CQ : public device::SPI
class FXOS8701CQ : public device::SPI, public px4::ScheduledWorkItem
{
public:
FXOS8701CQ(int bus, const char *path, uint32_t device, enum Rotation rotation);
@ -197,9 +198,10 @@ protected:
private:
void Run() override;
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
FXOS8701CQ_mag *_mag;
struct hrt_call _mag_call;
unsigned _call_mag_interval;
ringbuffer::RingBuffer *_mag_reports;
@ -212,9 +214,9 @@ private:
int16_t _last_raw_mag_x;
int16_t _last_raw_mag_y;
int16_t _last_raw_mag_z;
#endif
struct hrt_call _accel_call;
hrt_abstime _mag_last_measure{0};
#endif
unsigned _call_accel_interval;
@ -285,24 +287,6 @@ private:
*/
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
*/
@ -456,8 +440,6 @@ private:
void measure();
void measure_trampoline(void *arg);
/* this class does not allow copying due to ptr data members */
FXOS8701CQ_mag(const FXOS8701CQ_mag &);
FXOS8701CQ_mag operator=(const FXOS8701CQ_mag &);
@ -467,9 +449,9 @@ private:
FXOS8701CQ::FXOS8701CQ(int bus, const char *path, uint32_t device, enum Rotation rotation) :
SPI("FXOS8701CQ", path, bus, device, SPIDEV_MODE0,
1 * 1000 * 1000),
ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())),
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
_mag(new FXOS8701CQ_mag(this)),
_mag_call{},
_call_mag_interval(0),
_mag_reports(nullptr),
_mag_scale{},
@ -482,7 +464,6 @@ FXOS8701CQ::FXOS8701CQ(int bus, const char *path, uint32_t device, enum Rotation
_last_raw_mag_y(0),
_last_raw_mag_z(0),
#endif
_accel_call {},
_call_accel_interval(0),
_accel_reports(nullptr),
_accel_scale{},
@ -819,10 +800,10 @@ FXOS8701CQ::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;
}
@ -831,9 +812,7 @@ FXOS8701CQ::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 - FXOS8701C_TIMER_REDUCTION;
_call_accel_interval = interval;
/* if we need to start the poll state machine, do it */
if (want_start) {
@ -893,16 +872,16 @@ FXOS8701CQ::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) {
@ -1141,22 +1120,14 @@ FXOS8701CQ::start()
#endif
/* start polling at the specified rate */
hrt_call_every(&_accel_call,
1000,
_call_accel_interval - FXOS8701C_TIMER_REDUCTION,
(hrt_callout)&FXOS8701CQ::measure_trampoline, this);
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&FXOS8701CQ::mag_measure_trampoline, this);
#endif
ScheduleOnInterval(_call_accel_interval - FXOS8701C_TIMER_REDUCTION, 10000);
}
void
FXOS8701CQ::stop()
{
hrt_cancel(&_accel_call);
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
hrt_cancel(&_mag_call);
#endif
ScheduleClear();
/* reset internal states */
memset(_last_accel, 0, sizeof(_last_accel));
@ -1168,21 +1139,18 @@ FXOS8701CQ::stop()
}
void
FXOS8701CQ::measure_trampoline(void *arg)
FXOS8701CQ::Run()
{
FXOS8701CQ *dev = (FXOS8701CQ *)arg;
/* make another measurement */
dev->measure();
}
measure();
void
FXOS8701CQ::mag_measure_trampoline(void *arg)
{
FXOS8701CQ *dev = (FXOS8701CQ *)arg;
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
/* make another measurement */
dev->mag_measure();
if (hrt_elapsed_time(&_mag_last_measure) >= _call_mag_interval) {
mag_measure();
}
#endif
}
void
@ -1413,6 +1381,9 @@ FXOS8701CQ::mag_measure()
*/
mag_report.timestamp = hrt_absolute_time();
_mag_last_measure = mag_report.timestamp;
mag_report.is_external = external();
mag_report.x_raw = _last_raw_mag_x;
@ -1580,11 +1551,6 @@ FXOS8701CQ_mag::measure()
_parent->mag_measure();
}
void
FXOS8701CQ_mag::measure_trampoline(void *arg)
{
_parent->mag_measure_trampoline(arg);
}
#endif
/**