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

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
This commit is contained in:
Daniel Agar 2020-02-22 01:53:00 -05:00
parent 0c6d558c50
commit afc59f843c
7 changed files with 416 additions and 270 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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
View File

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

605
src/drivers/imu/invensense/icm20602/ICM20602.cpp
View File

@ -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) :
_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,50 +73,6 @@ ICM20602::~ICM20602()
perf_free(_drdy_interval_perf);
}
void ICM20602::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);
// 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;
}
}
}
int ICM20602::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 ICM20602::Init()
{
if (SPI::init() != PX4_OK) {
@ -134,32 +88,193 @@ bool ICM20602::Init()
return false;
}
if (!Reset()) {
PX4_ERR("reset failed");
return false;
return Reset();
}
void ICM20602::Stop()
{
// wait until stopped
while (_state.load() != STATE::STOPPED) {
_state.store(STATE::REQUEST_STOP);
ScheduleNow();
px4_usleep(10);
}
Start();
return true;
}
bool ICM20602::Reset()
{
// PWR_MGMT_1: Device Reset
RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET);
_state.store(STATE::RESET);
ScheduleClear();
ScheduleNow();
return true;
}
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();
}
int ICM20602::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 ICM20602::Run()
{
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;
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;
}
}
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(&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()
}
}
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)
if (notify) {
perf_count(_bad_register_perf);
_px4_accel.increase_error_count();
_px4_gyro.increase_error_count();
}
}
@ -325,148 +514,22 @@ void ICM20602::RegisterClearBits(Register reg, uint8_t clearbits)
RegisterSetAndClearBits(reg, 0, clearbits);
}
int ICM20602::DataReadyInterruptCallback(int irq, void *context, void *arg)
uint16_t ICM20602::FIFOReadCount()
{
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()
{
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();
// schedule as watchdog
if (_using_data_ready_interrupt_enabled) {
ScheduleDelayed(100_ms);
}
}
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();
}
void ICM20602::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 ICM20602::FIFORead(const hrt_abstime &timestamp_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);
@ -477,18 +540,70 @@ void ICM20602::Run()
if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) {
perf_end(_transfer_perf);
perf_count(_bad_transfer_perf);
return;
return false;
}
perf_end(_transfer_perf);
bool bad_data = false;
ProcessGyro(timestamp_sample, report, samples);
if (!ProcessAccel(timestamp_sample, report, samples)) {
bad_data = true;
}
// limit temperature updates to 1 Hz
if (hrt_elapsed_time(&_temperature_update_timestamp) > 1_s) {
_temperature_update_timestamp = timestamp_sample;
if (!ProcessTemperature(report, samples)) {
bad_data = true;
}
}
return !bad_data;
}
void ICM20602::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
_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 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()
} else {
perf_count(_bad_transfer_perf);
return;
bad_data = true;
}
}
@ -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()
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()
// 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()
_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;
}

69
src/drivers/imu/invensense/icm20602/ICM20602.hpp
View File

@ -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:
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:
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:
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:
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 _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:
{ 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
View File

@ -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
View File

@ -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
View File

@ -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; }