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

mpu6000 move to px4 work queue

This commit is contained in:
Daniel Agar 2019-01-18 21:04:59 -05:00
parent 70947920b8
commit 16b26c1927
2 changed files with 12 additions and 127 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
src/drivers/imu/mpu6000/MPU6000.cpp
View File

@ -41,13 +41,9 @@ constexpr uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS];
MPU6000::MPU6000(device::Device *interface, const char *path, enum Rotation rotation, int device_type) :
CDev(path),
ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())),
_interface(interface),
_device_type(device_type),
#if defined(USE_I2C)
_use_hrt(false),
#else
_use_hrt(true),
#endif
_px4_accel(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH), rotation),
_px4_gyro(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH), rotation),
_sample_perf(perf_alloc(PC_ELAPSED, "mpu6k_read")),
@ -98,11 +94,6 @@ MPU6000::~MPU6000()
int
MPU6000::init()
{
#if defined(USE_I2C)
use_i2c(_interface->get_device_bus_type() == Device::DeviceBusType_I2C);
#endif
/* probe again to get our settings that are based on the device type */
int ret = probe();
@ -153,7 +144,8 @@ int MPU6000::reset()
up_udelay(1000);
// 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);
px4_leave_critical_section(state);
@ -654,79 +646,23 @@ MPU6000::start()
stop();
_call_interval = last_call_interval;
if (!is_i2c()) {
_call.period = _call_interval - MPU6000_TIMER_REDUCTION;
/* start polling at the specified rate */
hrt_call_every(&_call,
1000,
_call_interval - MPU6000_TIMER_REDUCTION,
(hrt_callout)&MPU6000::measure_trampoline, this);
} else {
#ifdef USE_I2C
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&MPU6000::cycle_trampoline, this, 1);
#endif
}
ScheduleOnInterval(_call_interval - MPU6000_TIMER_REDUCTION, 1000);
}
void
MPU6000::stop()
{
if (!is_i2c()) {
hrt_cancel(&_call);
} else {
#ifdef USE_I2C
_call_interval = 0;
work_cancel(HPWORK, &_work);
#endif
}
ScheduleClear();
/* reset internal states */
memset(_last_accel, 0, sizeof(_last_accel));
}
#if defined(USE_I2C)
void
MPU6000::cycle_trampoline(void *arg)
MPU6000::Run()
{
MPU6000 *dev = (MPU6000 *)arg;
dev->cycle();
}
void
MPU6000::cycle()
{
int ret = measure();
if (ret != OK) {
/* issue a reset command to the sensor */
reset();
start();
return;
}
if (_call_interval != 0) {
work_queue(HPWORK,
&_work,
(worker_t)&MPU6000::cycle_trampoline,
this,
USEC2TICK(_call_interval - MPU6000_TIMER_REDUCTION));
}
}
#endif
void
MPU6000::measure_trampoline(void *arg)
{
MPU6000 *dev = reinterpret_cast<MPU6000 *>(arg);
/* make another measurement */
dev->measure();
measure();
}
void

61
src/drivers/imu/mpu6000/MPU6000.hpp
View File

@ -66,7 +66,7 @@
#include <px4_config.h>
#include <px4_getopt.h>
#include <px4_time.h>
#include <px4_workqueue.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>
#include <systemlib/conversions.h>
#include <systemlib/px4_macros.h>
@ -302,7 +302,7 @@ enum MPU6000_BUS {
MPU6000_BUS_SPI_EXTERNAL2
};
class MPU6000 : public cdev::CDev
class MPU6000 : public cdev::CDev, public px4::ScheduledWorkItem
{
public:
MPU6000(device::Device *interface, const char *path, enum Rotation rotation, int device_type);
@ -346,19 +346,12 @@ protected:
virtual int probe();
private:
void Run() override;
int _device_type;
uint8_t _product{0}; /** product code */
#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{1000};
PX4Accelerometer _px4_accel;
@ -420,50 +413,6 @@ private:
*/
bool is_mpu_device() { return _device_type == MPU_DEVICE_TYPE_MPU6000; }
#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.
*/