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

lsm303d move to px4 work queue

Browse Source
This commit is contained in:
Daniel Agar
2019-05-24 12:58:55 -04:00
parent 8b4ecc6947
commit ec0d1d6f62
1 changed files with 23 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/lsm303d/lsm303d.cpp
+23 -59
View File
@@ -72,6 +72,7 @@
#include <board_config.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/conversion/rotation.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>
/* SPI protocol address bits */
#define DIR_READ (1<<7)
@@ -220,7 +221,7 @@ extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
class LSM303D_mag;
class LSM303D : public device::SPI
class LSM303D : public device::SPI, public px4::ScheduledWorkItem
{
public:
LSM303D(int bus, const char *path, uint32_t device, enum Rotation rotation);
@@ -256,10 +257,9 @@ protected:
private:
LSM303D_mag *_mag;
void Run() override;
struct hrt_call _accel_call;
struct hrt_call _mag_call;
LSM303D_mag *_mag;
unsigned _call_accel_interval;
unsigned _call_mag_interval;
@@ -308,6 +308,8 @@ private:
// last temperature value
float _last_temperature;
hrt_abstime _mag_last_measure{0};
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
// reset
@@ -338,24 +340,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
*/
@@ -508,8 +492,6 @@ private:
void measure();
void measure_trampoline(void *arg);
/* this class does not allow copying due to ptr data members */
LSM303D_mag(const LSM303D_mag &);
LSM303D_mag operator=(const LSM303D_mag &);
@@ -519,9 +501,8 @@ private:
LSM303D::LSM303D(int bus, const char *path, uint32_t device, enum Rotation rotation) :
SPI("LSM303D", path, bus, device, SPIDEV_MODE3,
11 * 1000 * 1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */),
ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())),
_mag(new LSM303D_mag(this)),
_accel_call{},
_mag_call{},
_call_accel_interval(0),
_call_mag_interval(0),
_accel_reports(nullptr),
@@ -830,10 +811,10 @@ LSM303D::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;
}
@@ -842,9 +823,7 @@ LSM303D::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 - LSM303D_TIMER_REDUCTION;
_call_accel_interval = interval;
/* if we need to start the poll state machine, do it */
if (want_start) {
@@ -903,16 +882,16 @@ LSM303D::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) {
@@ -1214,18 +1193,13 @@ LSM303D::start()
_mag_reports->flush();
/* start polling at the specified rate */
hrt_call_every(&_accel_call,
1000,
_call_accel_interval - LSM303D_TIMER_REDUCTION,
(hrt_callout)&LSM303D::measure_trampoline, this);
hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this);
ScheduleOnInterval(_call_accel_interval - LSM303D_TIMER_REDUCTION, 10000);
}
void
LSM303D::stop()
{
hrt_cancel(&_accel_call);
hrt_cancel(&_mag_call);
ScheduleClear();
/* reset internal states */
memset(_last_accel, 0, sizeof(_last_accel));
@@ -1236,21 +1210,14 @@ LSM303D::stop()
}
void
LSM303D::measure_trampoline(void *arg)
LSM303D::Run()
{
LSM303D *dev = (LSM303D *)arg;
/* make another measurement */
dev->measure();
}
measure();
void
LSM303D::mag_measure_trampoline(void *arg)
{
LSM303D *dev = (LSM303D *)arg;
/* make another measurement */
dev->mag_measure();
if (hrt_elapsed_time(&_mag_last_measure) >= _call_mag_interval) {
mag_measure();
}
}
void
@@ -1473,6 +1440,9 @@ LSM303D::mag_measure()
*/
mag_report.timestamp = hrt_absolute_time();
_mag_last_measure = mag_report.timestamp;
mag_report.is_external = external();
mag_report.x_raw = raw_mag_report.x;
@@ -1672,12 +1642,6 @@ LSM303D_mag::measure()
_parent->mag_measure();
}
void
LSM303D_mag::measure_trampoline(void *arg)
{
_parent->mag_measure_trampoline(arg);
}
/**
* Local functions in support of the shell command.
*/