Skip to content

Commit

Permalink
icm20948 move to px4 work queue
Browse files Browse the repository at this point in the history
  • Loading branch information
@dagar
dagar committed May 24, 2019
1 parent fc7f1ca commit 8b4ecc6
Show file tree
Hide file tree
Showing 2 changed files with 19 additions and 150 deletions.
106 changes: 15 additions & 91 deletions src/drivers/imu/icm20948/icm20948.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,20 +98,14 @@ ICM20948::ICM20948(device::Device *interface, device::Device *mag_interface, con
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 ICM20948_accel(this, path_accel)),
_gyro(magnetometer_only ? nullptr : new ICM20948_gyro(this, path_gyro)),
_mag(new ICM20948_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{},
Expand Down Expand Up @@ -239,16 +233,14 @@ ICM20948::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);
Expand All @@ -257,7 +249,7 @@ ICM20948::init()
int ret = probe();

if (ret != OK) {
PX4_DEBUG("ICM20948 probe failed");
PX4_DEBUG("probe failed");
return ret;
}

Expand Down Expand Up @@ -457,10 +449,8 @@ int ICM20948::reset_mpu()
}

// Enable I2C bus or Disable I2C bus (recommended on data sheet)


write_checked_reg(MPU_OR_ICM(MPUREG_USER_CTRL, ICMREG_20948_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(ICMREG_20948_USER_CTRL, is_i2c ? 0 : BIT_I2C_IF_DIS);

// SAMPLE RATE
_set_sample_rate(_sample_rate);
Expand Down Expand Up @@ -617,16 +607,16 @@ ICM20948::_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);
Expand All @@ -639,15 +629,7 @@ ICM20948::_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) {
Expand Down Expand Up @@ -955,78 +937,20 @@ ICM20948::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)&ICM20948::measure_trampoline, this);

} else {
#ifdef USE_I2C
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&ICM20948::cycle_trampoline, this, 1);
#endif
}

ScheduleOnInterval(_call_interval - MPU9250_TIMER_REDUCTION, 10000);
}

void
ICM20948::stop()
{
if (_use_hrt) {
hrt_cancel(&_call);

} else {
#ifdef USE_I2C
work_cancel(HPWORK, &_work);
#endif
}
}


#if defined(USE_I2C)
void
ICM20948::cycle_trampoline(void *arg)
{
ICM20948 *dev = (ICM20948 *)arg;

dev->cycle();
}

void
ICM20948::cycle()
{

// 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)&ICM20948::cycle_trampoline,
this,
USEC2TICK(_call_interval - MPU9250_TIMER_REDUCTION));
}
ScheduleClear();
}
#endif


void
ICM20948::measure_trampoline(void *arg)
ICM20948::Run()
{
ICM20948 *dev = reinterpret_cast<ICM20948 *>(arg);

/* make another measurement */
dev->measure();
measure();
}

void
Expand Down
63 changes: 4 additions & 59 deletions src/drivers/imu/icm20948/icm20948.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>

Expand All @@ -49,6 +47,7 @@
#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>
Expand Down Expand Up @@ -374,7 +373,7 @@ class ICM20948_mag;
class ICM20948_accel;
class ICM20948_gyro;

class ICM20948
class ICM20948 : public px4::ScheduledWorkItem
{
public:
ICM20948(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro,
Expand Down Expand Up @@ -402,6 +401,8 @@ class ICM20948
friend class ICM20948_mag;
friend class ICM20948_gyro;

void Run() override;

private:
ICM20948_accel *_accel;
ICM20948_gyro *_gyro;
Expand All @@ -410,16 +411,6 @@ class ICM20948
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;
Expand Down Expand Up @@ -515,52 +506,6 @@ class ICM20948
*/
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.
*/
Expand Down

0 comments on commit 8b4ecc6

Please sign in to comment.