forked from Archive/PX4-Autopilot
invensense mpu9250 improvements and fixes
- refactor Run() into simple state machine - perform reset and configuration in sensor bus thread - when using data ready interrupt skip checking FIFO count - fix periodic temperature sampling (rate limit to 1 Hz)
This commit is contained in:
Daniel Agar
parent
4bd665d169
commit
5ab4cf3d62
|
|
@ -54,7 +54,8 @@ static constexpr uint8_t Bit7 = (1 << 7);
|
|||
|
||||
namespace InvenSense_MPU9250
|
||||
{
|
||||
static constexpr uint32_t SPI_SPEED = 20 * 1000 * 1000;
|
||||
static constexpr uint32_t SPI_SPEED = 1 * 1000 * 1000;
|
||||
static constexpr uint32_t SPI_SPEED_SENSOR = 20 * 1000 * 1000; // 20MHz for reading sensor and interrupt registers
|
||||
static constexpr uint8_t DIR_READ = 0x80;
|
||||
|
||||
static constexpr uint8_t WHOAMI = 0x71;
|
||||
|
|
|
|||
|
|
@ -42,11 +42,6 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
|
|||
return (msb << 8u) | lsb;
|
||||
}
|
||||
|
||||
static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
|
||||
{
|
||||
return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0);
|
||||
}
|
||||
|
||||
MPU9250::MPU9250(int bus, uint32_t device, enum Rotation rotation) :
|
||||
SPI(MODULE_NAME, nullptr, bus, device, SPIDEV_MODE3, SPI_SPEED),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
|
||||
|
|
@ -54,8 +49,11 @@ MPU9250::MPU9250(int bus, uint32_t device, enum Rotation rotation) :
|
|||
_px4_gyro(get_device_id(), ORB_PRIO_VERY_HIGH, rotation)
|
||||
{
|
||||
set_device_type(DRV_ACC_DEVTYPE_MPU9250);
|
||||
|
||||
_px4_accel.set_device_type(DRV_ACC_DEVTYPE_MPU9250);
|
||||
_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_MPU9250);
|
||||
|
||||
ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
|
||||
}
|
||||
|
||||
MPU9250::~MPU9250()
|
||||
|
|
@ -75,38 +73,6 @@ MPU9250::~MPU9250()
|
|||
perf_free(_drdy_interval_perf);
|
||||
}
|
||||
|
||||
void MPU9250::ConfigureSampleRate(int sample_rate)
|
||||
{
|
||||
if (sample_rate == 0) {
|
||||
sample_rate = 1000; // default to 1 kHz
|
||||
}
|
||||
|
||||
sample_rate = math::constrain(sample_rate, 250, 2000); // limit 250 - 2000 Hz
|
||||
|
||||
_fifo_empty_interval_us = math::max(((1000000 / sample_rate) / 250) * 250, 500); // round down to nearest 250 us
|
||||
_fifo_gyro_samples = math::min(_fifo_empty_interval_us / (1000000 / GYRO_RATE), FIFO_MAX_SAMPLES);
|
||||
|
||||
// recompute FIFO empty interval (us) with actual gyro sample limit
|
||||
_fifo_empty_interval_us = _fifo_gyro_samples * (1000000 / GYRO_RATE);
|
||||
|
||||
_fifo_accel_samples = math::min(_fifo_empty_interval_us / (1000000 / ACCEL_RATE), FIFO_MAX_SAMPLES);
|
||||
|
||||
_px4_accel.set_update_rate(1000000 / _fifo_empty_interval_us);
|
||||
_px4_gyro.set_update_rate(1000000 / _fifo_empty_interval_us);
|
||||
}
|
||||
|
||||
int MPU9250::probe()
|
||||
{
|
||||
const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
|
||||
|
||||
if (whoami != WHOAMI) {
|
||||
PX4_WARN("unexpected WHO_AM_I 0x%02x", whoami);
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
bool MPU9250::Init()
|
||||
{
|
||||
if (SPI::init() != PX4_OK) {
|
||||
|
|
@ -122,31 +88,199 @@ bool MPU9250::Init()
|
|||
return false;
|
||||
}
|
||||
|
||||
if (!Reset()) {
|
||||
PX4_ERR("reset failed");
|
||||
return false;
|
||||
return Reset();
|
||||
}
|
||||
|
||||
void MPU9250::Stop()
|
||||
{
|
||||
// wait until stopped
|
||||
while (_state.load() != STATE::STOPPED) {
|
||||
_state.store(STATE::REQUEST_STOP);
|
||||
ScheduleNow();
|
||||
px4_usleep(10);
|
||||
}
|
||||
|
||||
Start();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool MPU9250::Reset()
|
||||
{
|
||||
// PWR_MGMT_1: Device Reset
|
||||
RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::H_RESET);
|
||||
_state.store(STATE::RESET);
|
||||
ScheduleClear();
|
||||
ScheduleNow();
|
||||
return true;
|
||||
}
|
||||
|
||||
void MPU9250::PrintInfo()
|
||||
{
|
||||
PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us,
|
||||
static_cast<double>(1000000 / _fifo_empty_interval_us));
|
||||
|
||||
perf_print_counter(_transfer_perf);
|
||||
perf_print_counter(_bad_register_perf);
|
||||
perf_print_counter(_bad_transfer_perf);
|
||||
perf_print_counter(_fifo_empty_perf);
|
||||
perf_print_counter(_fifo_overflow_perf);
|
||||
perf_print_counter(_fifo_reset_perf);
|
||||
perf_print_counter(_drdy_interval_perf);
|
||||
|
||||
_px4_accel.print_status();
|
||||
_px4_gyro.print_status();
|
||||
}
|
||||
|
||||
int MPU9250::probe()
|
||||
{
|
||||
const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
|
||||
|
||||
if (whoami != WHOAMI) {
|
||||
PX4_WARN("unexpected WHO_AM_I 0x%02x", whoami);
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
void MPU9250::Run()
|
||||
{
|
||||
switch (_state.load()) {
|
||||
case STATE::RESET:
|
||||
// PWR_MGMT_1: Device Reset
|
||||
RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::H_RESET);
|
||||
_reset_timestamp = hrt_absolute_time();
|
||||
_state.store(STATE::WAIT_FOR_RESET);
|
||||
ScheduleDelayed(100);
|
||||
break;
|
||||
|
||||
case STATE::WAIT_FOR_RESET:
|
||||
|
||||
for (int i = 0; i < 100; i++) {
|
||||
// The reset value is 0x00 for all registers other than the registers below
|
||||
// Document Number: RM-MPU-9250A-00 Page 9 of 55
|
||||
if ((RegisterRead(Register::WHO_AM_I) == WHOAMI)
|
||||
&& (RegisterRead(Register::PWR_MGMT_1) == 0x01)) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
// if reset succeeded then configure
|
||||
_state.store(STATE::CONFIGURE);
|
||||
ScheduleNow();
|
||||
|
||||
} else {
|
||||
// RESET not complete
|
||||
if (hrt_elapsed_time(&_reset_timestamp) > 10_ms) {
|
||||
PX4_ERR("Reset failed, retrying");
|
||||
_state.store(STATE::RESET);
|
||||
ScheduleDelayed(10_ms);
|
||||
|
||||
} else {
|
||||
PX4_DEBUG("Reset not complete, check again in 1 ms");
|
||||
ScheduleDelayed(1_ms);
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case STATE::CONFIGURE:
|
||||
if (Configure()) {
|
||||
// if configure succeeded then start reading from FIFO
|
||||
_state.store(STATE::FIFO_READ);
|
||||
|
||||
if (DataReadyInterruptConfigure()) {
|
||||
_data_ready_interrupt_enabled = true;
|
||||
|
||||
// backup schedule as a watchdog timeout
|
||||
ScheduleDelayed(10_ms);
|
||||
|
||||
} else {
|
||||
_data_ready_interrupt_enabled = false;
|
||||
ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
|
||||
}
|
||||
|
||||
FIFOReset();
|
||||
|
||||
} else {
|
||||
PX4_DEBUG("Configure failed, retrying");
|
||||
// try again in 1 ms
|
||||
ScheduleDelayed(1_ms);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case STATE::FIFO_READ: {
|
||||
hrt_abstime timestamp_sample = 0;
|
||||
uint8_t samples = 0;
|
||||
|
||||
if (_data_ready_interrupt_enabled) {
|
||||
// re-schedule as watchdog timeout
|
||||
ScheduleDelayed(10_ms);
|
||||
|
||||
// timestamp set in data ready interrupt
|
||||
samples = _fifo_read_samples.load();
|
||||
timestamp_sample = _fifo_watermark_interrupt_timestamp;
|
||||
}
|
||||
|
||||
bool failure = false;
|
||||
|
||||
// manually check FIFO count if no samples from DRDY or timestamp looks bogus
|
||||
if (!_data_ready_interrupt_enabled || (samples == 0)
|
||||
|| (hrt_elapsed_time(×tamp_sample) > (_fifo_empty_interval_us / 2))) {
|
||||
|
||||
// use the time now roughly corresponding with the last sample we'll pull from the FIFO
|
||||
timestamp_sample = hrt_absolute_time();
|
||||
const uint16_t fifo_count = FIFOReadCount();
|
||||
|
||||
if (fifo_count == 0) {
|
||||
failure = true;
|
||||
perf_count(_fifo_empty_perf);
|
||||
}
|
||||
|
||||
samples = (fifo_count / sizeof(FIFO::DATA) / 2) * 2; // round down to nearest 2
|
||||
}
|
||||
|
||||
if (samples > FIFO_MAX_SAMPLES) {
|
||||
// not technically an overflow, but more samples than we expected or can publish
|
||||
perf_count(_fifo_overflow_perf);
|
||||
failure = true;
|
||||
FIFOReset();
|
||||
|
||||
} else if (samples >= 2) {
|
||||
// require at least 2 samples (we want at least 1 new accel sample per transfer)
|
||||
if (!FIFORead(timestamp_sample, samples)) {
|
||||
failure = true;
|
||||
_px4_accel.increase_error_count();
|
||||
_px4_gyro.increase_error_count();
|
||||
}
|
||||
}
|
||||
|
||||
if (failure || hrt_elapsed_time(&_last_config_check_timestamp) > 10_ms) {
|
||||
// check registers incrementally
|
||||
if (RegisterCheck(_register_cfg[_checked_register], true)) {
|
||||
_last_config_check_timestamp = timestamp_sample;
|
||||
_checked_register = (_checked_register + 1) % size_register_cfg;
|
||||
|
||||
} else {
|
||||
// register check failed, force reconfigure
|
||||
PX4_DEBUG("Health check failed, reconfiguring");
|
||||
_state.store(STATE::CONFIGURE);
|
||||
ScheduleNow();
|
||||
}
|
||||
|
||||
} else {
|
||||
// periodically update temperature (1 Hz)
|
||||
if (hrt_elapsed_time(&_temperature_update_timestamp) > 1_s) {
|
||||
UpdateTemperature();
|
||||
_temperature_update_timestamp = timestamp_sample;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case STATE::REQUEST_STOP:
|
||||
DataReadyInterruptDisable();
|
||||
ScheduleClear();
|
||||
_state.store(STATE::STOPPED);
|
||||
break;
|
||||
|
||||
case STATE::STOPPED:
|
||||
// DO NOTHING
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void MPU9250::ConfigureAccel()
|
||||
|
|
@ -203,57 +337,98 @@ void MPU9250::ConfigureGyro()
|
|||
}
|
||||
}
|
||||
|
||||
void MPU9250::ResetFIFO()
|
||||
void MPU9250::ConfigureSampleRate(int sample_rate)
|
||||
{
|
||||
perf_count(_fifo_reset_perf);
|
||||
if (sample_rate == 0) {
|
||||
sample_rate = 1000; // default to 1 kHz
|
||||
}
|
||||
|
||||
// USER_CTRL: disable FIFO and reset all signal paths
|
||||
RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST | USER_CTRL_BIT::SIG_COND_RST,
|
||||
USER_CTRL_BIT::FIFO_EN);
|
||||
_fifo_empty_interval_us = math::max(((1000000 / sample_rate) / 250) * 250, 250); // round down to nearest 250 us
|
||||
_fifo_gyro_samples = math::min(_fifo_empty_interval_us / (1000000 / GYRO_RATE), FIFO_MAX_SAMPLES);
|
||||
|
||||
_data_ready_count.store(0);
|
||||
// recompute FIFO empty interval (us) with actual gyro sample limit
|
||||
_fifo_empty_interval_us = _fifo_gyro_samples * (1000000 / GYRO_RATE);
|
||||
|
||||
// FIFO_EN: enable both gyro and accel
|
||||
RegisterWrite(Register::FIFO_EN, FIFO_EN_BIT::GYRO_XOUT | FIFO_EN_BIT::GYRO_YOUT | FIFO_EN_BIT::GYRO_ZOUT |
|
||||
FIFO_EN_BIT::ACCEL);
|
||||
_fifo_accel_samples = math::min(_fifo_empty_interval_us / (1000000 / ACCEL_RATE), FIFO_MAX_SAMPLES);
|
||||
|
||||
// USER_CTRL: re-enable FIFO
|
||||
RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN,
|
||||
USER_CTRL_BIT::FIFO_RST | USER_CTRL_BIT::SIG_COND_RST);
|
||||
_px4_accel.set_update_rate(1000000 / _fifo_empty_interval_us);
|
||||
_px4_gyro.set_update_rate(1000000 / _fifo_empty_interval_us);
|
||||
}
|
||||
|
||||
bool MPU9250::Configure(bool notify)
|
||||
bool MPU9250::Configure()
|
||||
{
|
||||
bool success = true;
|
||||
|
||||
for (const auto ® : _register_cfg) {
|
||||
if (!CheckRegister(reg, notify)) {
|
||||
if (!RegisterCheck(reg)) {
|
||||
success = false;
|
||||
}
|
||||
}
|
||||
|
||||
ConfigureAccel();
|
||||
ConfigureGyro();
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
bool MPU9250::CheckRegister(const register_config_t ®_cfg, bool notify)
|
||||
int MPU9250::DataReadyInterruptCallback(int irq, void *context, void *arg)
|
||||
{
|
||||
static_cast<MPU9250 *>(arg)->DataReady();
|
||||
return 0;
|
||||
}
|
||||
|
||||
void MPU9250::DataReady()
|
||||
{
|
||||
if (_data_ready_count.fetch_add(1) >= (_fifo_gyro_samples - 1)) {
|
||||
_data_ready_count.store(0);
|
||||
_fifo_watermark_interrupt_timestamp = hrt_absolute_time();
|
||||
_fifo_read_samples.store(_fifo_gyro_samples);
|
||||
ScheduleNow();
|
||||
}
|
||||
|
||||
perf_count(_drdy_interval_perf);
|
||||
}
|
||||
|
||||
bool MPU9250::DataReadyInterruptConfigure()
|
||||
{
|
||||
int ret_setevent = -1;
|
||||
|
||||
// Setup data ready on rising edge
|
||||
// TODO: cleanup horrible DRDY define mess
|
||||
#if defined(GPIO_DRDY_PORTD_PIN15)
|
||||
ret_setevent = px4_arch_gpiosetevent(GPIO_DRDY_PORTD_PIN15, true, false, true, &MPU9250::DataReadyInterruptCallback,
|
||||
this);
|
||||
#endif
|
||||
|
||||
return (ret_setevent == 0);
|
||||
}
|
||||
|
||||
bool MPU9250::DataReadyInterruptDisable()
|
||||
{
|
||||
int ret_setevent = -1;
|
||||
|
||||
// Disable data ready callback
|
||||
// TODO: cleanup horrible DRDY define mess
|
||||
#if defined(GPIO_DRDY_PORTD_PIN15)
|
||||
ret_setevent = px4_arch_gpiosetevent(GPIO_DRDY_PORTD_PIN15, false, false, false, nullptr, nullptr);
|
||||
#endif
|
||||
|
||||
return (ret_setevent == 0);
|
||||
}
|
||||
|
||||
bool MPU9250::RegisterCheck(const register_config_t ®_cfg, bool notify)
|
||||
{
|
||||
bool success = true;
|
||||
|
||||
const uint8_t reg_value = RegisterRead(reg_cfg.reg);
|
||||
|
||||
if (reg_cfg.set_bits && !(reg_value & reg_cfg.set_bits)) {
|
||||
if (notify) {
|
||||
PX4_ERR("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
|
||||
}
|
||||
|
||||
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
|
||||
success = false;
|
||||
}
|
||||
|
||||
if (reg_cfg.clear_bits && (reg_value & reg_cfg.clear_bits)) {
|
||||
if (notify) {
|
||||
PX4_ERR("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
|
||||
}
|
||||
|
||||
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
|
||||
success = false;
|
||||
}
|
||||
|
||||
|
|
@ -269,6 +444,8 @@ bool MPU9250::CheckRegister(const register_config_t ®_cfg, bool notify)
|
|||
|
||||
if (notify) {
|
||||
perf_count(_bad_register_perf);
|
||||
_px4_accel.increase_error_count();
|
||||
_px4_gyro.increase_error_count();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -315,145 +492,84 @@ void MPU9250::RegisterClearBits(Register reg, uint8_t clearbits)
|
|||
RegisterSetAndClearBits(reg, 0, clearbits);
|
||||
}
|
||||
|
||||
int MPU9250::DataReadyInterruptCallback(int irq, void *context, void *arg)
|
||||
uint16_t MPU9250::FIFOReadCount()
|
||||
{
|
||||
MPU9250 *dev = reinterpret_cast<MPU9250 *>(arg);
|
||||
dev->DataReady();
|
||||
return 0;
|
||||
}
|
||||
|
||||
void MPU9250::DataReady()
|
||||
{
|
||||
perf_count(_drdy_interval_perf);
|
||||
|
||||
if (_data_ready_count.fetch_add(1) >= (_fifo_gyro_samples - 1)) {
|
||||
// make another measurement
|
||||
ScheduleNow();
|
||||
_data_ready_count.store(0);
|
||||
}
|
||||
}
|
||||
|
||||
void MPU9250::Start()
|
||||
{
|
||||
ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
|
||||
|
||||
// attempt to configure 3 times
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (Configure(false)) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// TODO: cleanup horrible DRDY define mess
|
||||
#if defined(GPIO_DRDY_PORTD_PIN15)
|
||||
_using_data_ready_interrupt_enabled = true;
|
||||
// Setup data ready on rising edge
|
||||
px4_arch_gpiosetevent(GPIO_DRDY_PORTD_PIN15, true, false, true, &MPU9250::DataReadyInterruptCallback, this);
|
||||
#else
|
||||
_using_data_ready_interrupt_enabled = false;
|
||||
ScheduleOnInterval(FIFO_INTERVAL, FIFO_INTERVAL);
|
||||
#endif
|
||||
|
||||
ResetFIFO();
|
||||
|
||||
// schedule as watchdog
|
||||
if (_using_data_ready_interrupt_enabled) {
|
||||
ScheduleDelayed(100_ms);
|
||||
}
|
||||
}
|
||||
|
||||
void MPU9250::Stop()
|
||||
{
|
||||
Reset();
|
||||
|
||||
// TODO: cleanup horrible DRDY define mess
|
||||
#if defined(GPIO_DRDY_PORTD_PIN15)
|
||||
// Disable data ready callback
|
||||
px4_arch_gpiosetevent(GPIO_DRDY_PORTD_PIN15, false, false, false, nullptr, nullptr);
|
||||
#endif
|
||||
|
||||
ScheduleClear();
|
||||
}
|
||||
|
||||
void MPU9250::Run()
|
||||
{
|
||||
// use the time now roughly corresponding with the last sample we'll pull from the FIFO
|
||||
const hrt_abstime timestamp_sample = hrt_absolute_time();
|
||||
|
||||
// read FIFO count
|
||||
uint8_t fifo_count_buf[3] {};
|
||||
fifo_count_buf[0] = static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ;
|
||||
|
||||
if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
|
||||
perf_count(_bad_transfer_perf);
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (_using_data_ready_interrupt_enabled) {
|
||||
// re-schedule as watchdog
|
||||
ScheduleDelayed(100_ms);
|
||||
}
|
||||
|
||||
// check registers
|
||||
if (hrt_elapsed_time(&_last_config_check) > 100_ms) {
|
||||
_checked_register = (_checked_register + 1) % size_register_cfg;
|
||||
|
||||
if (CheckRegister(_register_cfg[_checked_register])) {
|
||||
// delay next register check if current succeeded
|
||||
_last_config_check = hrt_absolute_time();
|
||||
|
||||
} else {
|
||||
// if register check failed reconfigure all
|
||||
Configure();
|
||||
ResetFIFO();
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
const uint16_t fifo_count = combine(fifo_count_buf[1], fifo_count_buf[2]);
|
||||
const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / 2) * 2; // round down to nearest 2
|
||||
|
||||
if (samples < 2) {
|
||||
perf_count(_fifo_empty_perf);
|
||||
return;
|
||||
|
||||
} else if (samples > FIFO_MAX_SAMPLES) {
|
||||
// not technically an overflow, but more samples than we expected or can publish
|
||||
perf_count(_fifo_overflow_perf);
|
||||
ResetFIFO();
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
// Transfer data
|
||||
struct TransferBuffer {
|
||||
uint8_t cmd;
|
||||
FIFO::DATA f[FIFO_MAX_SAMPLES];
|
||||
};
|
||||
// ensure no struct padding
|
||||
static_assert(sizeof(TransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES * sizeof(FIFO::DATA)));
|
||||
return combine(fifo_count_buf[1], fifo_count_buf[2]);
|
||||
}
|
||||
|
||||
bool MPU9250::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples)
|
||||
{
|
||||
TransferBuffer *report = (TransferBuffer *)_dma_data_buffer;
|
||||
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
|
||||
memset(report, 0, transfer_size);
|
||||
report->cmd = static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ;
|
||||
|
||||
perf_begin(_transfer_perf);
|
||||
set_frequency(SPI_SPEED_SENSOR);
|
||||
|
||||
if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) {
|
||||
set_frequency(SPI_SPEED); // restore normal speed
|
||||
perf_end(_transfer_perf);
|
||||
perf_count(_bad_transfer_perf);
|
||||
return;
|
||||
return false;
|
||||
}
|
||||
|
||||
set_frequency(SPI_SPEED); // restore normal speed
|
||||
perf_end(_transfer_perf);
|
||||
|
||||
ProcessGyro(timestamp_sample, report, samples);
|
||||
return ProcessAccel(timestamp_sample, report, samples);
|
||||
}
|
||||
|
||||
void MPU9250::FIFOReset()
|
||||
{
|
||||
perf_count(_fifo_reset_perf);
|
||||
|
||||
// FIFO_EN: disable FIFO
|
||||
RegisterWrite(Register::FIFO_EN, 0);
|
||||
|
||||
// USER_CTRL: disable FIFO and reset all signal paths
|
||||
RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST | USER_CTRL_BIT::SIG_COND_RST,
|
||||
USER_CTRL_BIT::FIFO_EN);
|
||||
|
||||
// reset while FIFO is disabled
|
||||
_data_ready_count.store(0);
|
||||
_fifo_watermark_interrupt_timestamp = 0;
|
||||
_fifo_read_samples.store(0);
|
||||
|
||||
// FIFO_EN: enable both gyro and accel
|
||||
// USER_CTRL: re-enable FIFO
|
||||
for (const auto &r : _register_cfg) {
|
||||
if ((r.reg == Register::FIFO_EN) || (r.reg == Register::USER_CTRL)) {
|
||||
RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
|
||||
{
|
||||
return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0);
|
||||
}
|
||||
|
||||
bool MPU9250::ProcessAccel(const hrt_abstime ×tamp_sample, const TransferBuffer *const report, uint8_t samples)
|
||||
{
|
||||
PX4Accelerometer::FIFOSample accel;
|
||||
accel.timestamp_sample = timestamp_sample;
|
||||
accel.dt = _fifo_empty_interval_us / _fifo_accel_samples;
|
||||
|
||||
bool bad_data = false;
|
||||
|
||||
// accel data is doubled in FIFO, but might be shifted
|
||||
int accel_first_sample = 0;
|
||||
int accel_first_sample = 1;
|
||||
|
||||
if (samples >= 3) {
|
||||
if (fifo_accel_equal(report->f[0], report->f[1])) {
|
||||
|
|
@ -468,7 +584,7 @@ void MPU9250::Run()
|
|||
|
||||
} else {
|
||||
perf_count(_bad_transfer_perf);
|
||||
return;
|
||||
bad_data = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -480,7 +596,8 @@ void MPU9250::Run()
|
|||
int16_t accel_y = combine(fifo_sample.ACCEL_YOUT_H, fifo_sample.ACCEL_YOUT_L);
|
||||
int16_t accel_z = combine(fifo_sample.ACCEL_ZOUT_H, fifo_sample.ACCEL_ZOUT_L);
|
||||
|
||||
// sensor's frame is +x forward, +y left, +z up, flip y & z to publish right handed (x forward, y right, z down)
|
||||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
accel.x[accel_samples] = accel_x;
|
||||
accel.y[accel_samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
|
||||
accel.z[accel_samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
|
||||
|
|
@ -489,7 +606,13 @@ void MPU9250::Run()
|
|||
|
||||
accel.samples = accel_samples;
|
||||
|
||||
_px4_accel.updateFIFO(accel);
|
||||
|
||||
return !bad_data;
|
||||
}
|
||||
|
||||
void MPU9250::ProcessGyro(const hrt_abstime ×tamp_sample, const TransferBuffer *const report, uint8_t samples)
|
||||
{
|
||||
PX4Gyroscope::FIFOSample gyro;
|
||||
gyro.timestamp_sample = timestamp_sample;
|
||||
gyro.samples = samples;
|
||||
|
|
@ -502,46 +625,32 @@ void MPU9250::Run()
|
|||
const int16_t gyro_y = combine(fifo_sample.GYRO_YOUT_H, fifo_sample.GYRO_YOUT_L);
|
||||
const int16_t gyro_z = combine(fifo_sample.GYRO_ZOUT_H, fifo_sample.GYRO_ZOUT_L);
|
||||
|
||||
// sensor's frame is +x forward, +y left, +z up, flip y & z to publish right handed (x forward, y right, z down)
|
||||
// sensor's frame is +x forward, +y left, +z up
|
||||
// flip y & z to publish right handed with z down (x forward, y right, z down)
|
||||
gyro.x[i] = gyro_x;
|
||||
gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
|
||||
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
|
||||
}
|
||||
|
||||
// Temperature
|
||||
if (hrt_elapsed_time(&_time_last_temperature_update) > 1_s) {
|
||||
// read current temperature
|
||||
uint8_t temperature_buf[3] {};
|
||||
temperature_buf[0] = static_cast<uint8_t>(Register::TEMP_OUT_H) | DIR_READ;
|
||||
_px4_gyro.updateFIFO(gyro);
|
||||
}
|
||||
|
||||
if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
|
||||
return;
|
||||
}
|
||||
void MPU9250::UpdateTemperature()
|
||||
{
|
||||
// read current temperature
|
||||
uint8_t temperature_buf[3] {};
|
||||
temperature_buf[0] = static_cast<uint8_t>(Register::TEMP_OUT_H) | DIR_READ;
|
||||
|
||||
const int16_t TEMP_OUT = combine(temperature_buf[1], temperature_buf[2]);
|
||||
const float TEMP_degC = ((TEMP_OUT - ROOM_TEMPERATURE_OFFSET) / TEMPERATURE_SENSITIVITY) + ROOM_TEMPERATURE_OFFSET;
|
||||
if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
|
||||
perf_count(_bad_transfer_perf);
|
||||
return;
|
||||
}
|
||||
|
||||
const int16_t TEMP_OUT = combine(temperature_buf[1], temperature_buf[2]);
|
||||
const float TEMP_degC = ((TEMP_OUT - ROOM_TEMPERATURE_OFFSET) / TEMPERATURE_SENSITIVITY) + ROOM_TEMPERATURE_OFFSET;
|
||||
|
||||
if (PX4_ISFINITE(TEMP_degC)) {
|
||||
_px4_accel.set_temperature(TEMP_degC);
|
||||
_px4_gyro.set_temperature(TEMP_degC);
|
||||
}
|
||||
|
||||
_px4_gyro.updateFIFO(gyro);
|
||||
_px4_accel.updateFIFO(accel);
|
||||
}
|
||||
|
||||
void MPU9250::PrintInfo()
|
||||
{
|
||||
PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us,
|
||||
static_cast<double>(1000000 / _fifo_empty_interval_us));
|
||||
|
||||
perf_print_counter(_transfer_perf);
|
||||
perf_print_counter(_bad_register_perf);
|
||||
perf_print_counter(_bad_transfer_perf);
|
||||
perf_print_counter(_fifo_empty_perf);
|
||||
perf_print_counter(_fifo_overflow_perf);
|
||||
perf_print_counter(_fifo_reset_perf);
|
||||
perf_print_counter(_drdy_interval_perf);
|
||||
|
||||
_px4_accel.print_status();
|
||||
_px4_gyro.print_status();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -67,6 +67,19 @@ public:
|
|||
|
||||
private:
|
||||
|
||||
// Sensor Configuration
|
||||
static constexpr uint32_t GYRO_RATE{8000}; // 8 kHz gyro
|
||||
static constexpr uint32_t ACCEL_RATE{4000}; // 4 kHz accel
|
||||
static constexpr uint32_t FIFO_MAX_SAMPLES{ math::min(FIFO::SIZE / sizeof(FIFO::DATA) + 1, sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))};
|
||||
|
||||
// Transfer data
|
||||
struct TransferBuffer {
|
||||
uint8_t cmd;
|
||||
FIFO::DATA f[FIFO_MAX_SAMPLES];
|
||||
};
|
||||
// ensure no struct padding
|
||||
static_assert(sizeof(TransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
|
||||
|
||||
struct register_config_t {
|
||||
Register reg;
|
||||
uint8_t set_bits{0};
|
||||
|
|
@ -75,17 +88,19 @@ private:
|
|||
|
||||
int probe() override;
|
||||
|
||||
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
|
||||
void DataReady();
|
||||
|
||||
void Run() override;
|
||||
|
||||
void ConfigureSampleRate(int sample_rate);
|
||||
bool CheckRegister(const register_config_t ®_cfg, bool notify = true);
|
||||
bool Configure(bool notify = true);
|
||||
|
||||
bool Configure();
|
||||
void ConfigureAccel();
|
||||
void ConfigureGyro();
|
||||
void ConfigureSampleRate(int sample_rate);
|
||||
|
||||
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
|
||||
void DataReady();
|
||||
bool DataReadyInterruptConfigure();
|
||||
bool DataReadyInterruptDisable();
|
||||
|
||||
bool RegisterCheck(const register_config_t ®_cfg, bool notify = false);
|
||||
|
||||
uint8_t RegisterRead(Register reg);
|
||||
void RegisterWrite(Register reg, uint8_t value);
|
||||
|
|
@ -93,7 +108,13 @@ private:
|
|||
void RegisterSetBits(Register reg, uint8_t setbits);
|
||||
void RegisterClearBits(Register reg, uint8_t clearbits);
|
||||
|
||||
void ResetFIFO();
|
||||
uint16_t FIFOReadCount();
|
||||
bool FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples);
|
||||
void FIFOReset();
|
||||
|
||||
bool ProcessAccel(const hrt_abstime ×tamp_sample, const TransferBuffer *const buffer, uint8_t samples);
|
||||
void ProcessGyro(const hrt_abstime ×tamp_sample, const TransferBuffer *const buffer, uint8_t samples);
|
||||
void UpdateTemperature();
|
||||
|
||||
uint8_t *_dma_data_buffer{nullptr};
|
||||
|
||||
|
|
@ -103,24 +124,31 @@ private:
|
|||
perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")};
|
||||
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
|
||||
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
|
||||
perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo empty")};
|
||||
perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo overflow")};
|
||||
perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo reset")};
|
||||
perf_counter_t _drdy_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": drdy interval")};
|
||||
perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")};
|
||||
perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")};
|
||||
perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")};
|
||||
perf_counter_t _drdy_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": DRDY interval")};
|
||||
|
||||
hrt_abstime _last_config_check{0};
|
||||
hrt_abstime _time_last_temperature_update{0};
|
||||
|
||||
px4::atomic<int> _data_ready_count{0};
|
||||
hrt_abstime _reset_timestamp{0};
|
||||
hrt_abstime _last_config_check_timestamp{0};
|
||||
hrt_abstime _fifo_watermark_interrupt_timestamp{0};
|
||||
hrt_abstime _temperature_update_timestamp{0};
|
||||
|
||||
px4::atomic<uint8_t> _data_ready_count{0};
|
||||
px4::atomic<uint8_t> _fifo_read_samples{0};
|
||||
bool _data_ready_interrupt_enabled{false};
|
||||
uint8_t _checked_register{0};
|
||||
|
||||
bool _using_data_ready_interrupt_enabled{false};
|
||||
enum class STATE : uint8_t {
|
||||
RESET,
|
||||
WAIT_FOR_RESET,
|
||||
CONFIGURE,
|
||||
FIFO_READ,
|
||||
REQUEST_STOP,
|
||||
STOPPED,
|
||||
};
|
||||
|
||||
// Sensor Configuration
|
||||
static constexpr uint32_t GYRO_RATE{8000}; // 8 kHz gyro
|
||||
static constexpr uint32_t ACCEL_RATE{4000}; // 4 kHz accel
|
||||
static constexpr uint32_t FIFO_MAX_SAMPLES{ math::min(FIFO::SIZE / sizeof(FIFO::DATA) + 1, sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))};
|
||||
px4::atomic<STATE> _state{STATE::RESET};
|
||||
|
||||
uint16_t _fifo_empty_interval_us{1000}; // 1000 us / 1000 Hz transfer interval
|
||||
uint8_t _fifo_gyro_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
|
||||
|
|
@ -136,6 +164,6 @@ private:
|
|||
{ Register::CONFIG, CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ, Bit7 | CONFIG_BIT::FIFO_MODE },
|
||||
{ Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN, 0 },
|
||||
{ Register::FIFO_EN, FIFO_EN_BIT::GYRO_XOUT | FIFO_EN_BIT::GYRO_YOUT | FIFO_EN_BIT::GYRO_ZOUT | FIFO_EN_BIT::ACCEL, 0 },
|
||||
{ Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN | INT_ENABLE_BIT::RAW_RDY_EN }
|
||||
{ Register::INT_ENABLE, INT_ENABLE_BIT::RAW_RDY_EN, 0 }
|
||||
};
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in New Issue