zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

invensense mpu9250 improvements 

- checked register mechanism and simple watchdog
    - driver checks for errors gradually and can reconfigure itself
 - respect IMU_GYRO_RATEMAX at the driver level
 - fixed sensor INT16_MIN and INT16_MAX handling (y & z axis are flipped before publishing)
sbg
Daniel Agar 5 years ago
parent
22499effb9
commit
89d4d6e4a9
3 changed files with 348 additions and 129 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 31
      src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp
  2. 395
      src/drivers/imu/invensense/mpu9250/MPU9250.cpp
  3. 51
      src/drivers/imu/invensense/mpu9250/MPU9250.hpp

31
src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp
Unescape View File

@ -40,6 +40,8 @@ @@ -40,6 +40,8 @@
#pragma once
#include <cstdint>
// TODO: move to a central header
static constexpr uint8_t Bit0 = (1 << 0);
static constexpr uint8_t Bit1 = (1 << 1);
@ -57,6 +59,9 @@ static constexpr uint8_t DIR_READ = 0x80; @@ -57,6 +59,9 @@ static constexpr uint8_t DIR_READ = 0x80;
static constexpr uint8_t WHOAMI = 0x71;
static constexpr float TEMPERATURE_SENSITIVITY = 326.8f; // LSB/C
static constexpr float ROOM_TEMPERATURE_OFFSET = 25.f; // C
enum class Register : uint8_t {
CONFIG = 0x1A,
GYRO_CONFIG = 0x1B,
@ -65,8 +70,6 @@ enum class Register : uint8_t { @@ -65,8 +70,6 @@ enum class Register : uint8_t {
FIFO_EN = 0x23,
INT_STATUS = 0x3A,
INT_ENABLE = 0x38,
TEMP_OUT_H = 0x41,
@ -75,6 +78,8 @@ enum class Register : uint8_t { @@ -75,6 +78,8 @@ enum class Register : uint8_t {
USER_CTRL = 0x6A,
PWR_MGMT_1 = 0x6B,
I2C_IF = 0x70,
FIFO_COUNTH = 0x72,
FIFO_COUNTL = 0x73,
FIFO_R_W = 0x74,
@ -97,7 +102,7 @@ enum GYRO_CONFIG_BIT : uint8_t { @@ -97,7 +102,7 @@ enum GYRO_CONFIG_BIT : uint8_t {
GYRO_FS_SEL_2000_DPS = Bit4 | Bit3, // 0b11000
// FCHOICE_B [1:0]
FCHOICE_B_8KHZ_BYPASS_DLPF = Bit1 | Bit0, // 0b10 - 3-dB BW: 3281 Noise BW (Hz): 3451.0 8 kHz
FCHOICE_B_8KHZ_BYPASS_DLPF = Bit1 | Bit0, // 0b00 - 3-dB BW: 3281 Noise BW (Hz): 3451.0 8 kHz
};
// ACCEL_CONFIG
@ -125,25 +130,25 @@ enum FIFO_EN_BIT : uint8_t { @@ -125,25 +130,25 @@ enum FIFO_EN_BIT : uint8_t {
// INT_ENABLE
enum INT_ENABLE_BIT : uint8_t {
FIFO_OFLOW_EN = Bit4,
DATA_RDY_INT_EN = Bit0
};
// INT_STATUS
enum INT_STATUS_BIT : uint8_t {
FIFO_OFLOW_INT = Bit4,
DATA_RDY_INT = Bit0,
FIFO_OFLOW_EN = Bit4,
RAW_RDY_EN = Bit0
};
// USER_CTRL
enum USER_CTRL_BIT : uint8_t {
FIFO_EN = Bit6,
FIFO_RST = Bit2,
FIFO_EN = Bit6,
I2C_MST_EN = Bit5,
I2C_IF_DIS = Bit4,
FIFO_RST = Bit2,
I2C_MST_RST = Bit1,
SIG_COND_RST = Bit0,
};
// PWR_MGMT_1
enum PWR_MGMT_1_BIT : uint8_t {
H_RESET = Bit7,
SLEEP = Bit6,
CLKSEL_2 = Bit2,
CLKSEL_1 = Bit1,

395
src/drivers/imu/invensense/mpu9250/MPU9250.cpp
Unescape View File

@ -36,17 +36,16 @@ @@ -36,17 +36,16 @@
#include <px4_platform/board_dma_alloc.h>
using namespace time_literals;
using namespace InvenSense_MPU9250;
static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; }
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_INTERVAL{1000}; // 1000 us / 1000 Hz interval
static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
{
return (msb << 8u) | lsb;
}
static constexpr uint32_t FIFO_GYRO_SAMPLES{FIFO_INTERVAL / (1000000 / GYRO_RATE)};
static constexpr uint32_t FIFO_ACCEL_SAMPLES{FIFO_INTERVAL / (1000000 / ACCEL_RATE)};
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),
@ -57,9 +56,6 @@ MPU9250::MPU9250(int bus, uint32_t device, enum Rotation rotation) : @@ -57,9 +56,6 @@ MPU9250::MPU9250(int bus, uint32_t device, enum Rotation 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);
_px4_accel.set_update_rate(1000000 / FIFO_INTERVAL);
_px4_gyro.set_update_rate(1000000 / FIFO_INTERVAL);
}
MPU9250::~MPU9250()
@ -71,12 +67,34 @@ MPU9250::~MPU9250() @@ -71,12 +67,34 @@ MPU9250::~MPU9250()
}
perf_free(_transfer_perf);
perf_free(_bad_register_perf);
perf_free(_bad_transfer_perf);
perf_free(_fifo_empty_perf);
perf_free(_fifo_overflow_perf);
perf_free(_fifo_reset_perf);
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);
@ -96,11 +114,6 @@ bool MPU9250::Init() @@ -96,11 +114,6 @@ bool MPU9250::Init()
return false;
}
if (!Reset()) {
PX4_ERR("reset failed");
return false;
}
// allocate DMA capable buffer
_dma_data_buffer = (uint8_t *)board_dma_alloc(FIFO::SIZE);
@ -109,6 +122,11 @@ bool MPU9250::Init() @@ -109,6 +122,11 @@ bool MPU9250::Init()
return false;
}
if (!Reset()) {
PX4_ERR("reset failed");
return false;
}
Start();
return true;
@ -117,60 +135,144 @@ bool MPU9250::Init() @@ -117,60 +135,144 @@ bool MPU9250::Init()
bool MPU9250::Reset()
{
// PWR_MGMT_1: Device Reset
// CLKSEL[2:0] must be set to 001 to achieve full gyroscope performance.
RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::H_RESET);
usleep(1000);
// PWR_MGMT_1: CLKSEL[2:0] must be set to 001 to achieve full gyroscope performance.
RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0);
usleep(1000);
// ACCEL_CONFIG: Accel 16 G range
RegisterSetBits(Register::ACCEL_CONFIG, ACCEL_CONFIG_BIT::ACCEL_FS_SEL_16G);
_px4_accel.set_scale(CONSTANTS_ONE_G / 2048);
_px4_accel.set_range(16.0f * CONSTANTS_ONE_G);
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;
}
}
// GYRO_CONFIG: Gyro 2000 degrees/second
RegisterSetBits(Register::GYRO_CONFIG, GYRO_CONFIG_BIT::GYRO_FS_SEL_2000_DPS);
_px4_gyro.set_scale(math::radians(1.0f / 16.4f));
_px4_gyro.set_range(math::radians(2000.0f));
return false;
}
// reset done once data is ready
const bool reset_done = !(RegisterRead(Register::PWR_MGMT_1) & PWR_MGMT_1_BIT::H_RESET);
const bool clksel_done = (RegisterRead(Register::PWR_MGMT_1) & PWR_MGMT_1_BIT::CLKSEL_0);
const bool data_ready = (RegisterRead(Register::INT_STATUS) & INT_STATUS_BIT::DATA_RDY_INT);
void MPU9250::ConfigureAccel()
{
const uint8_t ACCEL_FS_SEL = RegisterRead(Register::ACCEL_CONFIG) & (Bit4 | Bit3); // [4:3] ACCEL_FS_SEL[1:0]
switch (ACCEL_FS_SEL) {
case ACCEL_FS_SEL_2G:
_px4_accel.set_scale(CONSTANTS_ONE_G / 16384);
_px4_accel.set_range(2 * CONSTANTS_ONE_G);
break;
case ACCEL_FS_SEL_4G:
_px4_accel.set_scale(CONSTANTS_ONE_G / 8192);
_px4_accel.set_range(4 * CONSTANTS_ONE_G);
break;
case ACCEL_FS_SEL_8G:
_px4_accel.set_scale(CONSTANTS_ONE_G / 4096);
_px4_accel.set_range(8 * CONSTANTS_ONE_G);
break;
case ACCEL_FS_SEL_16G:
_px4_accel.set_scale(CONSTANTS_ONE_G / 2048);
_px4_accel.set_range(16 * CONSTANTS_ONE_G);
break;
}
}
return reset_done && clksel_done && data_ready;
void MPU9250::ConfigureGyro()
{
const uint8_t GYRO_FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] GYRO_FS_SEL[1:0]
switch (GYRO_FS_SEL) {
case GYRO_FS_SEL_250_DPS:
_px4_gyro.set_scale(math::radians(1.0f / 131.f));
_px4_gyro.set_range(math::radians(250.f));
break;
case GYRO_FS_SEL_500_DPS:
_px4_gyro.set_scale(math::radians(1.0f / 65.5f));
_px4_gyro.set_range(math::radians(500.f));
break;
case GYRO_FS_SEL_1000_DPS:
_px4_gyro.set_scale(math::radians(1.0f / 32.8f));
_px4_gyro.set_range(math::radians(1000.0f));
break;
case GYRO_FS_SEL_2000_DPS:
_px4_gyro.set_scale(math::radians(1.0f / 16.4f));
_px4_gyro.set_range(math::radians(2000.0f));
break;
}
}
void MPU9250::ResetFIFO()
{
perf_count(_fifo_reset_perf);
// ACCEL_CONFIG2: Accel DLPF disabled for full rate (4 kHz)
RegisterSetBits(Register::ACCEL_CONFIG2, ACCEL_CONFIG2_BIT::ACCEL_FCHOICE_B_BYPASS_DLPF);
// GYRO_CONFIG: Gyro DLPF disabled for full rate (8 kHz)
RegisterClearBits(Register::GYRO_CONFIG, GYRO_CONFIG_BIT::FCHOICE_B_8KHZ_BYPASS_DLPF);
// 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_EN: disable FIFO
RegisterWrite(Register::FIFO_EN, 0);
RegisterClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::FIFO_RST);
// USER_CTRL: reset FIFO then re-enable
RegisterSetBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST);
up_udelay(1); // bit auto clears after one clock cycle of the internal 20 MHz clock
RegisterSetBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN);
// CONFIG: should ensure that bit 7 of register 0x1A is set to 0 before using FIFO watermark feature
RegisterSetBits(Register::CONFIG, CONFIG_BIT::FIFO_MODE);
RegisterSetBits(Register::CONFIG, CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ);
_data_ready_count.store(0);
// FIFO_EN: enable both gyro and accel
_data_ready_count = 0;
RegisterWrite(Register::FIFO_EN, FIFO_EN_BIT::GYRO_XOUT | FIFO_EN_BIT::GYRO_YOUT | FIFO_EN_BIT::GYRO_ZOUT |
FIFO_EN_BIT::ACCEL);
up_udelay(10);
// 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);
}
bool MPU9250::Configure(bool notify)
{
bool success = true;
for (const auto &reg : _register_cfg) {
if (!CheckRegister(reg, notify)) {
success = false;
}
}
return success;
}
bool MPU9250::CheckRegister(const register_config_t &reg_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);
}
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);
}
success = false;
}
if (!success) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
if (reg_cfg.reg == Register::ACCEL_CONFIG) {
ConfigureAccel();
} else if (reg_cfg.reg == Register::GYRO_CONFIG) {
ConfigureGyro();
}
if (notify) {
perf_count(_bad_register_perf);
}
}
return success;
}
uint8_t MPU9250::RegisterRead(Register reg)
@ -187,24 +289,30 @@ void MPU9250::RegisterWrite(Register reg, uint8_t value) @@ -187,24 +289,30 @@ void MPU9250::RegisterWrite(Register reg, uint8_t value)
transfer(cmd, cmd, sizeof(cmd));
}
void MPU9250::RegisterSetBits(Register reg, uint8_t setbits)
void MPU9250::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
{
uint8_t val = RegisterRead(reg);
const uint8_t orig_val = RegisterRead(reg);
uint8_t val = orig_val;
if (!(val & setbits)) {
if (setbits) {
val |= setbits;
RegisterWrite(reg, val);
}
if (clearbits) {
val &= ~clearbits;
}
RegisterWrite(reg, val);
}
void MPU9250::RegisterClearBits(Register reg, uint8_t clearbits)
void MPU9250::RegisterSetBits(Register reg, uint8_t setbits)
{
uint8_t val = RegisterRead(reg);
RegisterSetAndClearBits(reg, setbits, 0);
}
if (val & clearbits) {
val &= !clearbits;
RegisterWrite(reg, val);
}
void MPU9250::RegisterClearBits(Register reg, uint8_t clearbits)
{
RegisterSetAndClearBits(reg, 0, clearbits);
}
int MPU9250::DataReadyInterruptCallback(int irq, void *context, void *arg)
@ -218,81 +326,111 @@ void MPU9250::DataReady() @@ -218,81 +326,111 @@ void MPU9250::DataReady()
{
perf_count(_drdy_interval_perf);
_data_ready_count++;
if (_data_ready_count >= 8) {
_time_data_ready = hrt_absolute_time();
_data_ready_count = 0;
if (_data_ready_count.fetch_add(1) >= (_fifo_gyro_samples - 1)) {
// make another measurement
ScheduleNow();
_data_ready_count.store(0);
}
}
void MPU9250::Start()
{
Stop();
ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
ResetFIFO();
// 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);
RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN);
#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);
RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN);
#else
ScheduleClear();
#endif
ScheduleClear();
}
void MPU9250::Run()
{
// use timestamp from the data ready interrupt if available,
// otherwise use the time now roughly corresponding with the last sample we'll pull from the FIFO
const hrt_abstime timestamp_sample = (hrt_elapsed_time(&_time_data_ready) < FIFO_INTERVAL) ? _time_data_ready :
hrt_absolute_time();
// 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) {
return;
perf_count(_bad_transfer_perf);
}
if (_using_data_ready_interrupt_enabled) {
// re-schedule as watchdog
ScheduleDelayed(100_ms);
}
const size_t fifo_count = combine(fifo_count_buf[1], fifo_count_buf[2]);
const int samples = (fifo_count / sizeof(FIFO::DATA) / 2) * 2; // round down to nearest 2
// 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 > 16) {
// not technically an overflow, but more samples than we expected
} 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[16]; // max 16 samples
FIFO::DATA f[FIFO_MAX_SAMPLES];
};
static_assert(sizeof(TransferBuffer) == (sizeof(uint8_t) + 16 * sizeof(FIFO::DATA))); // ensure no struct padding
// ensure no struct padding
static_assert(sizeof(TransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES * sizeof(FIFO::DATA)));
TransferBuffer *report = (TransferBuffer *)_dma_data_buffer;
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
@ -303,43 +441,73 @@ void MPU9250::Run() @@ -303,43 +441,73 @@ void MPU9250::Run()
if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) {
perf_end(_transfer_perf);
perf_count(_bad_transfer_perf);
return;
}
perf_end(_transfer_perf);
PX4Accelerometer::FIFOSample accel;
accel.timestamp_sample = timestamp_sample;
accel.dt = FIFO_INTERVAL / FIFO_ACCEL_SAMPLES;
accel.dt = _fifo_empty_interval_us / _fifo_accel_samples;
// accel data is doubled in FIFO, but might be shifted
int accel_first_sample = 0;
if (samples >= 3) {
if (fifo_accel_equal(report->f[0], report->f[1])) {
// [A0, A1, A2, A3]
// A0==A1, A2==A3
accel_first_sample = 1;
} else if (fifo_accel_equal(report->f[1], report->f[2])) {
// [A0, A1, A2, A3]
// A0, A1==A2, A3
accel_first_sample = 0;
} else {
perf_count(_bad_transfer_perf);
return;
}
}
int accel_samples = 0;
for (int i = accel_first_sample; i < samples; i = i + 2) {
const FIFO::DATA &fifo_sample = report->f[i];
int16_t accel_x = combine(fifo_sample.ACCEL_XOUT_H, fifo_sample.ACCEL_XOUT_L);
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)
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;
accel_samples++;
}
accel.samples = accel_samples;
PX4Gyroscope::FIFOSample gyro;
gyro.timestamp_sample = timestamp_sample;
gyro.samples = samples;
gyro.dt = FIFO_INTERVAL / FIFO_GYRO_SAMPLES;
int accel_samples = 0;
gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples;
for (int i = 0; i < samples; i++) {
const FIFO::DATA &fifo_sample = report->f[i];
// accel data is doubled
if (i % 2) {
// coordinate convention (x forward, y right, z down)
accel.x[accel_samples] = combine(fifo_sample.ACCEL_XOUT_H, fifo_sample.ACCEL_XOUT_L);
accel.y[accel_samples] = -combine(fifo_sample.ACCEL_YOUT_H, fifo_sample.ACCEL_YOUT_L);
accel.z[accel_samples] = -combine(fifo_sample.ACCEL_ZOUT_H, fifo_sample.ACCEL_ZOUT_L);
const int16_t gyro_x = combine(fifo_sample.GYRO_XOUT_H, fifo_sample.GYRO_XOUT_L);
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);
accel_samples++;
}
// coordinate convention (x forward, y right, z down)
gyro.x[i] = combine(fifo_sample.GYRO_XOUT_H, fifo_sample.GYRO_XOUT_L);
gyro.y[i] = -combine(fifo_sample.GYRO_YOUT_H, fifo_sample.GYRO_YOUT_L);
gyro.z[i] = -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)
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;
}
accel.samples = accel_samples;
// Temperature
if (hrt_elapsed_time(&_time_last_temperature_update) > 1_s) {
// read current temperature
@ -351,11 +519,7 @@ void MPU9250::Run() @@ -351,11 +519,7 @@ void MPU9250::Run()
}
const int16_t TEMP_OUT = combine(temperature_buf[1], temperature_buf[2]);
static constexpr float RoomTemp_Offset = 25.0f; // Room Temperature Offset 25°C
static constexpr float Temp_Sensitivity = 326.8f; // Sensitivity 326.8 LSB/°C
const float TEMP_degC = ((TEMP_OUT - RoomTemp_Offset) / Temp_Sensitivity) + 25.0f;
const float TEMP_degC = ((TEMP_OUT - ROOM_TEMPERATURE_OFFSET) / TEMPERATURE_SENSITIVITY) + ROOM_TEMPERATURE_OFFSET;
_px4_accel.set_temperature(TEMP_degC);
_px4_gyro.set_temperature(TEMP_degC);
@ -367,7 +531,12 @@ void MPU9250::Run() @@ -367,7 +531,12 @@ void MPU9250::Run()
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);

51
src/drivers/imu/invensense/mpu9250/MPU9250.hpp
Unescape View File

@ -48,9 +48,10 @@ @@ -48,9 +48,10 @@
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/ecl/geo/geo.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
using InvenSense_MPU9250::Register;
using namespace InvenSense_MPU9250;
class MPU9250 : public device::SPI, public px4::ScheduledWorkItem
{
@ -65,6 +66,13 @@ public: @@ -65,6 +66,13 @@ public:
void PrintInfo();
private:
struct register_config_t {
Register reg;
uint8_t set_bits{0};
uint8_t clear_bits{0};
};
int probe() override;
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
@ -72,8 +80,16 @@ private: @@ -72,8 +80,16 @@ private:
void Run() override;
void ConfigureSampleRate(int sample_rate);
bool CheckRegister(const register_config_t &reg_cfg, bool notify = true);
bool Configure(bool notify = true);
void ConfigureAccel();
void ConfigureGyro();
uint8_t RegisterRead(Register reg);
void RegisterWrite(Register reg, uint8_t value);
void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
void RegisterSetBits(Register reg, uint8_t setbits);
void RegisterClearBits(Register reg, uint8_t clearbits);
@ -85,12 +101,41 @@ private: @@ -85,12 +101,41 @@ private:
PX4Gyroscope _px4_gyro;
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")};
hrt_abstime _time_data_ready{0};
hrt_abstime _last_config_check{0};
hrt_abstime _time_last_temperature_update{0};
int _data_ready_count{0};
px4::atomic<int> _data_ready_count{0};
uint8_t _checked_register{0};
bool _using_data_ready_interrupt_enabled{false};
// 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]))};
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))};
uint8_t _fifo_accel_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))};
static constexpr uint8_t size_register_cfg{11};
register_config_t _register_cfg[size_register_cfg] {
// Register | Set bits, Clear bits
{ Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::H_RESET | PWR_MGMT_1_BIT::SLEEP },
{ Register::ACCEL_CONFIG, ACCEL_CONFIG_BIT::ACCEL_FS_SEL_16G, 0 },
{ Register::ACCEL_CONFIG2, ACCEL_CONFIG2_BIT::ACCEL_FCHOICE_B_BYPASS_DLPF, 0 },
{ Register::GYRO_CONFIG, GYRO_CONFIG_BIT::GYRO_FS_SEL_2000_DPS, GYRO_CONFIG_BIT::FCHOICE_B_8KHZ_BYPASS_DLPF },
{ 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 }
};
};

Write Preview
Loading…
Cancel
Save