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

invensense icm20602 improvements 

- refactor Run() into simple state machine
 - perform reset and configuration in sensor bus thread
 - when using data ready interrupt skip checking FIFO count
sbg
Daniel Agar 5 years ago
parent
0c6d558c50
commit
afc59f843c
7 changed files with 409 additions and 263 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. 2
      boards/px4/fmu-v5/init/rc.board_sensors
  2. 1
      src/drivers/imu/invensense/icm20602/CMakeLists.txt
  3. 587
      src/drivers/imu/invensense/icm20602/ICM20602.cpp
  4. 73
      src/drivers/imu/invensense/icm20602/ICM20602.hpp
  5. 3
      src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp
  6. 3
      src/lib/drivers/accelerometer/PX4Accelerometer.hpp
  7. 3
      src/lib/drivers/gyroscope/PX4Gyroscope.hpp

2
boards/px4/fmu-v5/init/rc.board_sensors
Unescape View File

@ -7,9 +7,11 @@ adc start @@ -7,9 +7,11 @@ adc start
# Internal SPI bus ICM-20602
mpu6000 -R 8 -s -T 20602 start
#icm20602
# Internal SPI bus ICM-20689
mpu6000 -R 8 -z -T 20689 start
#icm20689 start
# Internal SPI bus BMI055 accel
bmi055 -A -R 10 start

1
src/drivers/imu/invensense/icm20602/CMakeLists.txt
Unescape View File

@ -39,6 +39,7 @@ px4_add_module( @@ -39,6 +39,7 @@ px4_add_module(
ICM20602.cpp
ICM20602.hpp
icm20602_main.cpp
InvenSense_ICM20602_registers.hpp
DEPENDS
drivers_accelerometer
drivers_gyroscope

587
src/drivers/imu/invensense/icm20602/ICM20602.cpp
Unescape View File

@ -42,11 +42,6 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) @@ -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);
}
ICM20602::ICM20602(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 @@ ICM20602::ICM20602(int bus, uint32_t device, enum Rotation rotation) : @@ -54,8 +49,11 @@ ICM20602::ICM20602(int bus, uint32_t device, enum Rotation rotation) :
_px4_gyro(get_device_id(), ORB_PRIO_VERY_HIGH, rotation)
{
set_device_type(DRV_ACC_DEVTYPE_ICM20602);
_px4_accel.set_device_type(DRV_ACC_DEVTYPE_ICM20602);
_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ICM20602);
ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
}
ICM20602::~ICM20602()
@ -75,36 +73,57 @@ ICM20602::~ICM20602() @@ -75,36 +73,57 @@ ICM20602::~ICM20602()
perf_free(_drdy_interval_perf);
}
void ICM20602::ConfigureSampleRate(int sample_rate)
bool ICM20602::Init()
{
if (sample_rate == 0) {
sample_rate = 1000; // default to 1 kHz
if (SPI::init() != PX4_OK) {
PX4_ERR("SPI::init failed");
return false;
}
sample_rate = math::constrain(sample_rate, 250, 2000); // limit 250 - 2000 Hz
// allocate DMA capable buffer
_dma_data_buffer = (uint8_t *)board_dma_alloc(FIFO::SIZE);
_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);
if (_dma_data_buffer == nullptr) {
PX4_ERR("DMA alloc failed");
return false;
}
// recompute FIFO empty interval (us) with actual gyro sample limit
_fifo_empty_interval_us = _fifo_gyro_samples * (1000000 / GYRO_RATE);
return Reset();
}
_fifo_accel_samples = math::min(_fifo_empty_interval_us / (1000000 / ACCEL_RATE), FIFO_MAX_SAMPLES);
void ICM20602::Stop()
{
// wait until stopped
while (_state.load() != STATE::STOPPED) {
_state.store(STATE::REQUEST_STOP);
ScheduleNow();
px4_usleep(10);
}
}
_px4_accel.set_update_rate(1000000 / _fifo_empty_interval_us);
_px4_gyro.set_update_rate(1000000 / _fifo_empty_interval_us);
bool ICM20602::Reset()
{
_state.store(STATE::RESET);
ScheduleClear();
ScheduleNow();
return true;
}
// FIFO watermark threshold in number of bytes
const uint16_t fifo_watermark_threshold = _fifo_gyro_samples * sizeof(FIFO::DATA);
void ICM20602::PrintInfo()
{
PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us,
static_cast<double>(1000000 / _fifo_empty_interval_us));
for (auto &r : _register_cfg) {
if (r.reg == Register::FIFO_WM_TH1) {
r.set_bits = (fifo_watermark_threshold >> 8) & 0b00000011;
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);
} else if (r.reg == Register::FIFO_WM_TH2) {
r.set_bits = fifo_watermark_threshold & 0xFF;
}
}
_px4_accel.print_status();
_px4_gyro.print_status();
}
int ICM20602::probe()
@ -119,47 +138,143 @@ int ICM20602::probe() @@ -119,47 +138,143 @@ int ICM20602::probe()
return PX4_OK;
}
bool ICM20602::Init()
void ICM20602::Run()
{
if (SPI::init() != PX4_OK) {
PX4_ERR("SPI::init failed");
return false;
}
// allocate DMA capable buffer
_dma_data_buffer = (uint8_t *)board_dma_alloc(FIFO::SIZE);
if (_dma_data_buffer == nullptr) {
PX4_ERR("DMA alloc failed");
return false;
}
if (!Reset()) {
PX4_ERR("reset failed");
return false;
}
Start();
return true;
}
switch (_state.load()) {
case STATE::RESET:
// PWR_MGMT_1: Device Reset
RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET);
_reset_timestamp = hrt_absolute_time();
_state.store(STATE::WAIT_FOR_RESET);
ScheduleDelayed(100);
break;
bool ICM20602::Reset()
{
// PWR_MGMT_1: Device Reset
RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET);
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: DS-000176 Page 31 of 57
if ((RegisterRead(Register::WHO_AM_I) == WHOAMI)
&& (RegisterRead(Register::PWR_MGMT_1) == 0x41)
&& (RegisterRead(Register::CONFIG) == 0x80)) {
return true;
// 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);
}
}
}
return false;
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(&timestamp_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();
}
}
}
break;
case STATE::REQUEST_STOP:
DataReadyInterruptDisable();
ScheduleClear();
_state.store(STATE::STOPPED);
break;
case STATE::STOPPED:
// DO NOTHING
break;
}
}
void ICM20602::ConfigureAccel()
@ -216,54 +331,126 @@ void ICM20602::ConfigureGyro() @@ -216,54 +331,126 @@ void ICM20602::ConfigureGyro()
}
}
void ICM20602::ResetFIFO()
void ICM20602::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);
// FIFO_EN: enable both gyro and accel
RegisterSetBits(Register::FIFO_EN, FIFO_EN_BIT::GYRO_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN);
// recompute FIFO empty interval (us) with actual gyro sample limit
_fifo_empty_interval_us = _fifo_gyro_samples * (1000000 / GYRO_RATE);
// 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);
_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);
// FIFO watermark threshold in number of bytes
const uint16_t fifo_watermark_threshold = _fifo_gyro_samples * sizeof(FIFO::DATA);
for (auto &r : _register_cfg) {
if (r.reg == Register::FIFO_WM_TH1) {
r.set_bits = (fifo_watermark_threshold >> 8) & 0b00000011;
} else if (r.reg == Register::FIFO_WM_TH2) {
r.set_bits = fifo_watermark_threshold & 0xFF;
}
}
}
bool ICM20602::Configure(bool notify)
bool ICM20602::Configure()
{
bool success = true;
for (const auto &reg : _register_cfg) {
if (!CheckRegister(reg, notify)) {
if (!RegisterCheck(reg)) {
success = false;
}
}
ConfigureAccel();
ConfigureGyro();
return success;
}
bool ICM20602::CheckRegister(const register_config_t &reg_cfg, bool notify)
int ICM20602::DataReadyInterruptCallback(int irq, void *context, void *arg)
{
static_cast<ICM20602 *>(arg)->DataReady();
return 0;
}
void ICM20602::DataReady()
{
_fifo_watermark_interrupt_timestamp = hrt_absolute_time();
_fifo_read_samples.store(_fifo_gyro_samples);
ScheduleNow();
perf_count(_drdy_interval_perf);
}
bool ICM20602::DataReadyInterruptConfigure()
{
int ret_setevent = -1;
// Setup data ready on rising edge
// TODO: cleanup horrible DRDY define mess
#if defined(GPIO_DRDY_PORTC_PIN14)
ret_setevent = px4_arch_gpiosetevent(GPIO_DRDY_PORTC_PIN14, true, false, true, &ICM20602::DataReadyInterruptCallback,
this);
#elif defined(GPIO_SPI1_DRDY1_ICM20602)
ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback,
this);
#elif defined(GPIO_SPI1_DRDY4_ICM20602)
ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY4_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback,
this);
#elif defined(GPIO_SPI1_DRDY1_ICM20602)
ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback,
this);
#elif defined(GPIO_DRDY_ICM_2060X)
ret_setevent = px4_arch_gpiosetevent(GPIO_DRDY_ICM_2060X, true, false, true, &ICM20602::DataReadyInterruptCallback,
this);
#endif
return (ret_setevent == 0);
}
bool ICM20602::DataReadyInterruptDisable()
{
int ret_setevent = -1;
// Disable data ready callback
// TODO: cleanup horrible DRDY define mess
#if defined(GPIO_DRDY_PORTC_PIN14)
ret_setevent = px4_arch_gpiosetevent(GPIO_DRDY_PORTC_PIN14, false, false, false, nullptr, nullptr);
#elif defined(GPIO_SPI1_DRDY1_ICM20602)
ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, false, false, false, nullptr, nullptr);
#elif defined(GPIO_SPI1_DRDY4_ICM20602)
ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY4_ICM20602, false, false, false, nullptr, nullptr);
#elif defined(GPIO_SPI1_DRDY1_ICM20602)
ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, false, false, false, nullptr, nullptr);
#elif defined(GPIO_DRDY_ICM_2060X)
ret_setevent = px4_arch_gpiosetevent(GPIO_DRDY_ICM_2060X, false, false, false, nullptr, nullptr);
#endif
return (ret_setevent == 0);
}
bool ICM20602::RegisterCheck(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);
}
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;
}
@ -279,6 +466,8 @@ bool ICM20602::CheckRegister(const register_config_t &reg_cfg, bool notify) @@ -279,6 +466,8 @@ bool ICM20602::CheckRegister(const register_config_t &reg_cfg, bool notify)
if (notify) {
perf_count(_bad_register_perf);
_px4_accel.increase_error_count();
_px4_gyro.increase_error_count();
}
}
@ -325,170 +514,96 @@ void ICM20602::RegisterClearBits(Register reg, uint8_t clearbits) @@ -325,170 +514,96 @@ void ICM20602::RegisterClearBits(Register reg, uint8_t clearbits)
RegisterSetAndClearBits(reg, 0, clearbits);
}
int ICM20602::DataReadyInterruptCallback(int irq, void *context, void *arg)
{
ICM20602 *dev = reinterpret_cast<ICM20602 *>(arg);
dev->DataReady();
return 0;
}
void ICM20602::DataReady()
{
perf_count(_drdy_interval_perf);
// make another measurement
ScheduleNow();
}
void ICM20602::Start()
uint16_t ICM20602::FIFOReadCount()
{
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_PORTC_PIN14)
_using_data_ready_interrupt_enabled = true;
// Setup data ready on rising edge
px4_arch_gpiosetevent(GPIO_DRDY_PORTC_PIN14, true, false, true, &ICM20602::DataReadyInterruptCallback, this);
#elif defined(GPIO_SPI1_DRDY1_ICM20602)
_using_data_ready_interrupt_enabled = true;
// Setup data ready on rising edge
px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, this);
#elif defined(GPIO_SPI1_DRDY4_ICM20602)
_using_data_ready_interrupt_enabled = true;
// Setup data ready on rising edge
px4_arch_gpiosetevent(GPIO_SPI1_DRDY4_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, this);
#elif defined(GPIO_SPI1_DRDY1_ICM20602)
_using_data_ready_interrupt_enabled = true;
// Setup data ready on rising edge
px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, this);
#elif defined(GPIO_DRDY_ICM_2060X)
_using_data_ready_interrupt_enabled = true;
// Setup data ready on rising edge
px4_arch_gpiosetevent(GPIO_DRDY_ICM_2060X, true, false, true, &ICM20602::DataReadyInterruptCallback, this);
#else
_using_data_ready_interrupt_enabled = false;
ScheduleOnInterval(FIFO_EMPTY_INTERVAL_US, FIFO_EMPTY_INTERVAL_US);
#endif
ResetFIFO();
// read FIFO count
uint8_t fifo_count_buf[3] {};
fifo_count_buf[0] = static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ;
// schedule as watchdog
if (_using_data_ready_interrupt_enabled) {
ScheduleDelayed(100_ms);
if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
perf_count(_bad_transfer_perf);
return 0;
}
}
void ICM20602::Stop()
{
Reset();
// TODO: cleanup horrible DRDY define mess
#if defined(GPIO_DRDY_PORTC_PIN14)
// Disable data ready callback
px4_arch_gpiosetevent(GPIO_DRDY_PORTC_PIN14, false, false, false, nullptr, nullptr);
#elif defined(GPIO_SPI1_DRDY1_ICM20602)
// Disable data ready callback
px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, false, false, false, nullptr, nullptr);
#elif defined(GPIO_SPI1_DRDY4_ICM20602)
// Disable data ready callback
px4_arch_gpiosetevent(GPIO_SPI1_DRDY4_ICM20602, false, false, false, nullptr, nullptr);
#elif defined(GPIO_SPI1_DRDY1_ICM20602)
// Disable data ready callback
px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, false, false, false, nullptr, nullptr);
#elif defined(GPIO_DRDY_ICM_2060X)
// Disable data ready callback
px4_arch_gpiosetevent(GPIO_DRDY_ICM_2060X, false, false, false, nullptr, nullptr);
#endif
ScheduleClear();
return combine(fifo_count_buf[1], fifo_count_buf[2]);
}
void ICM20602::Run()
bool ICM20602::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples)
{
// use the time now roughly corresponding with the last sample we'll pull from the FIFO
const hrt_abstime timestamp_sample = hrt_absolute_time();
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;
// read FIFO count
uint8_t fifo_count_buf[3] {};
fifo_count_buf[0] = static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ;
perf_begin(_transfer_perf);
if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) {
perf_end(_transfer_perf);
perf_count(_bad_transfer_perf);
return false;
}
if (_using_data_ready_interrupt_enabled) {
// re-schedule as watchdog
ScheduleDelayed(100_ms);
}
perf_end(_transfer_perf);
// check registers
if (hrt_elapsed_time(&_last_config_check) > 100_ms) {
_checked_register = (_checked_register + 1) % size_register_cfg;
bool bad_data = false;
if (CheckRegister(_register_cfg[_checked_register])) {
// delay next register check if current succeeded
_last_config_check = hrt_absolute_time();
ProcessGyro(timestamp_sample, report, samples);
} else {
// if register check failed reconfigure all
Configure();
ResetFIFO();
return;
}
if (!ProcessAccel(timestamp_sample, report, samples)) {
bad_data = true;
}
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
// limit temperature updates to 1 Hz
if (hrt_elapsed_time(&_temperature_update_timestamp) > 1_s) {
_temperature_update_timestamp = timestamp_sample;
if (samples < 2) {
perf_count(_fifo_empty_perf);
return;
if (!ProcessTemperature(report, samples)) {
bad_data = true;
}
}
} 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 !bad_data;
}
return;
}
void ICM20602::FIFOReset()
{
perf_count(_fifo_reset_perf);
// 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)));
// FIFO_EN: disable FIFO
RegisterWrite(Register::FIFO_EN, 0);
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;
// 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);
perf_begin(_transfer_perf);
// reset while FIFO is disabled
_fifo_watermark_interrupt_timestamp = 0;
_fifo_read_samples.store(0);
if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) {
perf_end(_transfer_perf);
perf_count(_bad_transfer_perf);
return;
// 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);
}
}
}
perf_end(_transfer_perf);
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 ICM20602::ProcessAccel(const hrt_abstime &timestamp_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])) {
@ -503,7 +618,7 @@ void ICM20602::Run() @@ -503,7 +618,7 @@ void ICM20602::Run()
} else {
perf_count(_bad_transfer_perf);
return;
bad_data = true;
}
}
@ -515,7 +630,8 @@ void ICM20602::Run() @@ -515,7 +630,8 @@ void ICM20602::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;
@ -524,30 +640,44 @@ void ICM20602::Run() @@ -524,30 +640,44 @@ void ICM20602::Run()
accel.samples = accel_samples;
_px4_accel.updateFIFO(accel);
return !bad_data;
}
void ICM20602::ProcessGyro(const hrt_abstime &timestamp_sample, const TransferBuffer *const report, uint8_t samples)
{
PX4Gyroscope::FIFOSample gyro;
gyro.timestamp_sample = timestamp_sample;
gyro.samples = samples;
gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples;
int16_t temperature[samples];
for (int i = 0; i < samples; i++) {
const FIFO::DATA &fifo_sample = report->f[i];
temperature[i] = combine(fifo_sample.TEMP_OUT_H, fifo_sample.TEMP_OUT_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);
// 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
_px4_gyro.updateFIFO(gyro);
}
bool ICM20602::ProcessTemperature(const TransferBuffer *const report, uint8_t samples)
{
int16_t temperature[samples];
for (int i = 0; i < samples; i++) {
const FIFO::DATA &fifo_sample = report->f[i];
temperature[i] = combine(fifo_sample.TEMP_OUT_H, fifo_sample.TEMP_OUT_L);
}
int32_t temperature_sum{0};
for (auto t : temperature) {
@ -560,7 +690,7 @@ void ICM20602::Run() @@ -560,7 +690,7 @@ void ICM20602::Run()
// temperature changing wildly is an indication of a transfer error
if (fabsf(t - temperature_avg) > 1000) {
perf_count(_bad_transfer_perf);
return;
return false;
}
}
@ -569,24 +699,5 @@ void ICM20602::Run() @@ -569,24 +699,5 @@ void ICM20602::Run()
_px4_accel.set_temperature(temperature_C);
_px4_gyro.set_temperature(temperature_C);
_px4_gyro.updateFIFO(gyro);
_px4_accel.updateFIFO(accel);
}
void ICM20602::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();
return true;
}

73
src/drivers/imu/invensense/icm20602/ICM20602.hpp
Unescape View File

@ -48,6 +48,7 @@ @@ -48,6 +48,7 @@
#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 namespace InvenSense_ICM20602;
@ -66,6 +67,19 @@ public: @@ -66,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};
@ -74,17 +88,19 @@ private: @@ -74,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 &reg_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 &reg_cfg, bool notify = false);
uint8_t RegisterRead(Register reg);
void RegisterWrite(Register reg, uint8_t value);
@ -92,7 +108,13 @@ private: @@ -92,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 &timestamp_sample, uint16_t samples);
void FIFOReset();
bool ProcessAccel(const hrt_abstime &timestamp_sample, const TransferBuffer *const buffer, uint8_t samples);
void ProcessGyro(const hrt_abstime &timestamp_sample, const TransferBuffer *const buffer, uint8_t samples);
bool ProcessTemperature(const TransferBuffer *const report, uint8_t samples);
uint8_t *_dma_data_buffer{nullptr};
@ -102,21 +124,30 @@ private: @@ -102,21 +124,30 @@ 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")};
hrt_abstime _last_config_check{0};
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 _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> _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))};
@ -135,6 +166,6 @@ private: @@ -135,6 +166,6 @@ private:
{ Register::FIFO_WM_TH2, 0, 0 }, // FIFO_WM_TH[7:0]
{ Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN, 0 },
{ Register::FIFO_EN, FIFO_EN_BIT::GYRO_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN, 0 },
{ Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN, INT_ENABLE_BIT::DATA_RDY_INT_EN }
{ Register::INT_ENABLE, 0, INT_ENABLE_BIT::DATA_RDY_INT_EN }
};
};

3
src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp
Unescape View File

@ -54,8 +54,7 @@ static constexpr uint8_t Bit7 = (1 << 7); @@ -54,8 +54,7 @@ static constexpr uint8_t Bit7 = (1 << 7);
namespace InvenSense_ICM20602
{
static constexpr uint32_t SPI_SPEED = 10 * 1000 *
1000; // 10MHz SPI serial interface for communicating with all registers
static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10MHz SPI serial interface
static constexpr uint8_t DIR_READ = 0x80;
static constexpr uint8_t WHOAMI = 0x12;

3
src/lib/drivers/accelerometer/PX4Accelerometer.hpp
Unescape View File

@ -58,7 +58,8 @@ public: @@ -58,7 +58,8 @@ public:
void set_device_id(uint32_t device_id) { _device_id = device_id; }
void set_device_type(uint8_t devtype);
void set_error_count(uint64_t error_count) { _error_count += error_count; }
void set_error_count(uint64_t error_count) { _error_count = error_count; }
void increase_error_count() { _error_count++; }
void set_range(float range) { _range = range; UpdateClipLimit(); }
void set_scale(float scale) { _scale = scale; UpdateClipLimit(); }
void set_temperature(float temperature) { _temperature = temperature; }

3
src/lib/drivers/gyroscope/PX4Gyroscope.hpp
Unescape View File

@ -60,7 +60,8 @@ public: @@ -60,7 +60,8 @@ public:
void set_device_id(uint32_t device_id) { _device_id = device_id; }
void set_device_type(uint8_t devtype);
void set_error_count(uint64_t error_count) { _error_count += error_count; }
void set_error_count(uint64_t error_count) { _error_count = error_count; }
void increase_error_count() { _error_count++; }
void set_range(float range) { _range = range; UpdateClipLimit(); }
void set_scale(float scale) { _scale = scale; UpdateClipLimit(); }
void set_temperature(float temperature) { _temperature = temperature; }

Write Preview
Loading…
Cancel
Save