mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
mpu9250 move to px4 work queue
This commit is contained in:
Daniel Agar
parent
e9ca5d5aba
commit
ea734f634d
@ -45,5 +45,5 @@ px4_add_module(
|
||||
mag.cpp
|
||||
mag_i2c.cpp
|
||||
DEPENDS
|
||||
px4_work_queue
|
||||
)
|
||||
|
||||
|
||||
@ -93,6 +93,7 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const
|
||||
const char *path_gyro, const char *path_mag,
|
||||
enum Rotation rotation,
|
||||
bool magnetometer_only) :
|
||||
ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())),
|
||||
_interface(interface),
|
||||
_whoami(0),
|
||||
_accel(magnetometer_only ? nullptr : new MPU9250_accel(this, path_accel)),
|
||||
@ -100,13 +101,6 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const
|
||||
_mag(new MPU9250_mag(this, mag_interface, path_mag)),
|
||||
_selected_bank(0xFF), // invalid/improbable bank value, will be set on first read/write
|
||||
_magnetometer_only(magnetometer_only),
|
||||
#if defined(USE_I2C)
|
||||
_work {},
|
||||
_use_hrt(false),
|
||||
#else
|
||||
_use_hrt(true),
|
||||
#endif
|
||||
_call {},
|
||||
_call_interval(0),
|
||||
_accel_reports(nullptr),
|
||||
_accel_scale{},
|
||||
@ -232,16 +226,14 @@ MPU9250::init()
|
||||
{
|
||||
irqstate_t state;
|
||||
|
||||
#if defined(USE_I2C)
|
||||
use_i2c(_interface->get_device_bus_type() == device::Device::DeviceBusType_I2C);
|
||||
#endif
|
||||
|
||||
/*
|
||||
* If the MPU is using I2C we should reduce the sample rate to 200Hz and
|
||||
* make the integration autoreset faster so that we integrate just one
|
||||
* sample since the sampling rate is already low.
|
||||
*/
|
||||
if (is_i2c() && !_magnetometer_only) {
|
||||
const bool is_i2c = (_interface->get_device_bus_type() == device::Device::DeviceBusType_I2C);
|
||||
|
||||
if (is_i2c && !_magnetometer_only) {
|
||||
_sample_rate = 200;
|
||||
_accel_int.set_autoreset_interval(1000000 / 1000);
|
||||
_gyro_int.set_autoreset_interval(1000000 / 1000);
|
||||
@ -250,7 +242,7 @@ MPU9250::init()
|
||||
int ret = probe();
|
||||
|
||||
if (ret != OK) {
|
||||
PX4_DEBUG("MPU9250 probe failed");
|
||||
PX4_DEBUG("probe failed");
|
||||
return ret;
|
||||
}
|
||||
|
||||
@ -453,7 +445,8 @@ int MPU9250::reset_mpu()
|
||||
}
|
||||
|
||||
// Enable I2C bus or Disable I2C bus (recommended on data sheet)
|
||||
write_checked_reg(MPUREG_USER_CTRL, is_i2c() ? 0 : BIT_I2C_IF_DIS);
|
||||
const bool is_i2c = (_interface->get_device_bus_type() == device::Device::DeviceBusType_I2C);
|
||||
write_checked_reg(MPUREG_USER_CTRL, is_i2c ? 0 : BIT_I2C_IF_DIS);
|
||||
|
||||
// SAMPLE RATE
|
||||
_set_sample_rate(_sample_rate);
|
||||
@ -599,16 +592,16 @@ MPU9250::_set_pollrate(unsigned long rate)
|
||||
bool want_start = (_call_interval == 0);
|
||||
|
||||
/* convert hz to hrt interval via microseconds */
|
||||
unsigned ticks = 1000000 / rate;
|
||||
unsigned interval = 1000000 / rate;
|
||||
|
||||
/* 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);
|
||||
@ -621,15 +614,7 @@ MPU9250::_set_pollrate(unsigned long rate)
|
||||
|
||||
/* update interval for next measurement */
|
||||
/* XXX this is a bit shady, but no other way to adjust... */
|
||||
_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 mpu9250 clock
|
||||
*/
|
||||
_call.period = _call_interval - MPU9250_TIMER_REDUCTION;
|
||||
_call_interval = interval;
|
||||
|
||||
/* if we need to start the poll state machine, do it */
|
||||
if (want_start) {
|
||||
@ -826,78 +811,20 @@ MPU9250::start()
|
||||
|
||||
_mag->_mag_reports->flush();
|
||||
|
||||
if (_use_hrt) {
|
||||
/* start polling at the specified rate */
|
||||
hrt_call_every(&_call,
|
||||
1000,
|
||||
_call_interval - MPU9250_TIMER_REDUCTION,
|
||||
(hrt_callout)&MPU9250::measure_trampoline, this);
|
||||
|
||||
} else {
|
||||
#ifdef USE_I2C
|
||||
/* schedule a cycle to start things */
|
||||
work_queue(HPWORK, &_work, (worker_t)&MPU9250::cycle_trampoline, this, 1);
|
||||
#endif
|
||||
}
|
||||
|
||||
ScheduleOnInterval(_call_interval - MPU9250_TIMER_REDUCTION, 1000);
|
||||
}
|
||||
|
||||
void
|
||||
MPU9250::stop()
|
||||
{
|
||||
if (_use_hrt) {
|
||||
hrt_cancel(&_call);
|
||||
|
||||
} else {
|
||||
#ifdef USE_I2C
|
||||
work_cancel(HPWORK, &_work);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#if defined(USE_I2C)
|
||||
void
|
||||
MPU9250::cycle_trampoline(void *arg)
|
||||
{
|
||||
MPU9250 *dev = (MPU9250 *)arg;
|
||||
|
||||
dev->cycle();
|
||||
ScheduleClear();
|
||||
}
|
||||
|
||||
void
|
||||
MPU9250::cycle()
|
||||
MPU9250::Run()
|
||||
{
|
||||
|
||||
// int ret = measure();
|
||||
|
||||
measure();
|
||||
|
||||
// if (ret != OK) {
|
||||
// /* issue a reset command to the sensor */
|
||||
// reset();
|
||||
// start();
|
||||
// return;
|
||||
// }
|
||||
|
||||
if (_call_interval != 0) {
|
||||
work_queue(HPWORK,
|
||||
&_work,
|
||||
(worker_t)&MPU9250::cycle_trampoline,
|
||||
this,
|
||||
USEC2TICK(_call_interval - MPU9250_TIMER_REDUCTION));
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
void
|
||||
MPU9250::measure_trampoline(void *arg)
|
||||
{
|
||||
MPU9250 *dev = reinterpret_cast<MPU9250 *>(arg);
|
||||
|
||||
/* make another measurement */
|
||||
dev->measure();
|
||||
measure();
|
||||
}
|
||||
|
||||
void
|
||||
|
||||
@ -36,8 +36,6 @@
|
||||
#include <perf/perf_counter.h>
|
||||
#include <systemlib/conversions.h>
|
||||
|
||||
#include <nuttx/wqueue.h>
|
||||
|
||||
#include <board_config.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
@ -49,9 +47,9 @@
|
||||
#include <mathlib/math/filter/LowPassFilter2p.hpp>
|
||||
#include <lib/conversion/rotation.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <px4_work_queue/ScheduledWorkItem.hpp>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/debug_key_value.h>
|
||||
|
||||
#include "mag.h"
|
||||
#include "accel.h"
|
||||
@ -253,7 +251,7 @@ class MPU9250_mag;
|
||||
class MPU9250_accel;
|
||||
class MPU9250_gyro;
|
||||
|
||||
class MPU9250
|
||||
class MPU9250 : public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
MPU9250(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro,
|
||||
@ -281,6 +279,8 @@ protected:
|
||||
friend class MPU9250_mag;
|
||||
friend class MPU9250_gyro;
|
||||
|
||||
void Run() override;
|
||||
|
||||
private:
|
||||
MPU9250_accel *_accel;
|
||||
MPU9250_gyro *_gyro;
|
||||
@ -289,16 +289,6 @@ private:
|
||||
bool
|
||||
_magnetometer_only; /* To disable accel and gyro reporting if only magnetometer is used (e.g. as external magnetometer) */
|
||||
|
||||
#if defined(USE_I2C)
|
||||
/*
|
||||
* SPI bus based device use hrt
|
||||
* I2C bus needs to use work queue
|
||||
*/
|
||||
work_s _work{};
|
||||
#endif
|
||||
bool _use_hrt;
|
||||
|
||||
struct hrt_call _call {};
|
||||
unsigned _call_interval;
|
||||
|
||||
ringbuffer::RingBuffer *_accel_reports;
|
||||
@ -392,52 +382,6 @@ private:
|
||||
*/
|
||||
int reset_mpu();
|
||||
|
||||
|
||||
#if defined(USE_I2C)
|
||||
/**
|
||||
* When the I2C interfase is on
|
||||
* Perform a poll cycle; collect from the previous measurement
|
||||
* and start a new one.
|
||||
*
|
||||
* This is the heart of the measurement state machine. This function
|
||||
* alternately starts a measurement, or collects the data from the
|
||||
* previous measurement.
|
||||
*
|
||||
* When the interval between measurements is greater than the minimum
|
||||
* measurement interval, a gap is inserted between collection
|
||||
* and measurement to provide the most recent measurement possible
|
||||
* at the next interval.
|
||||
*/
|
||||
void cycle();
|
||||
|
||||
/**
|
||||
* Static trampoline from the workq context; because we don't have a
|
||||
* generic workq wrapper yet.
|
||||
*
|
||||
* @param arg Instance pointer for the driver that is polling.
|
||||
*/
|
||||
static void cycle_trampoline(void *arg);
|
||||
|
||||
void use_i2c(bool on_true) { _use_hrt = !on_true; }
|
||||
|
||||
#endif
|
||||
|
||||
bool is_i2c(void) { return !_use_hrt; }
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* 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);
|
||||
|
||||
/**
|
||||
* Fetch measurements from the sensor and update the report buffers.
|
||||
*/
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user