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

imu/invensense/icm20689: minor improvements and potential fixes 

- perform full sensor signal path reset and wait for max time (100 ms)
 - issue full sensor reset on any error
 - always read FIFO count before transfersj
 - only allocate drdy perf counter if GPIO is available
sbg
Daniel Agar 5 years ago
parent
ff3b040d3c
commit
01f4486b32
4 changed files with 181 additions and 209 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
      src/drivers/imu/invensense/icm20689/CMakeLists.txt
  2. 290
      src/drivers/imu/invensense/icm20689/ICM20689.cpp
  3. 45
      src/drivers/imu/invensense/icm20689/ICM20689.hpp
  4. 53
      src/drivers/imu/invensense/icm20689/InvenSense_ICM20689_registers.hpp

2
src/drivers/imu/invensense/icm20689/CMakeLists.txt
Unescape View File

@ -35,11 +35,11 @@ px4_add_module( @@ -35,11 +35,11 @@ px4_add_module(
MODULE drivers__imu__invensense__icm20689
MAIN icm20689
COMPILE_FLAGS
-Wno-error
SRCS
ICM20689.cpp
ICM20689.hpp
icm20689_main.cpp
InvenSense_ICM20689_registers.hpp
DEPENDS
drivers_accelerometer
drivers_gyroscope

290
src/drivers/imu/invensense/icm20689/ICM20689.cpp
Unescape View File

@ -48,6 +48,10 @@ ICM20689::ICM20689(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Ro @@ -48,6 +48,10 @@ ICM20689::ICM20689(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Ro
_px4_accel(get_device_id(), ORB_PRIO_HIGH, rotation),
_px4_gyro(get_device_id(), ORB_PRIO_HIGH, rotation)
{
if (drdy_gpio != 0) {
_drdy_interval_perf = perf_alloc(PC_INTERVAL, MODULE_NAME": DRDY interval");
}
ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
}
@ -77,6 +81,7 @@ int ICM20689::init() @@ -77,6 +81,7 @@ int ICM20689::init()
bool ICM20689::Reset()
{
_state = STATE::RESET;
DataReadyInterruptDisable();
ScheduleClear();
ScheduleNow();
return true;
@ -91,8 +96,8 @@ void ICM20689::exit_and_cleanup() @@ -91,8 +96,8 @@ void ICM20689::exit_and_cleanup()
void ICM20689::print_status()
{
I2CSPIDriverBase::print_status();
PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us,
static_cast<double>(1000000 / _fifo_empty_interval_us));
PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
perf_print_counter(_transfer_perf);
perf_print_counter(_bad_register_perf);
@ -120,13 +125,17 @@ int ICM20689::probe() @@ -120,13 +125,17 @@ int ICM20689::probe()
void ICM20689::RunImpl()
{
const hrt_abstime now = hrt_absolute_time();
switch (_state) {
case STATE::RESET:
// PWR_MGMT_1: Device Reset
RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET);
_reset_timestamp = hrt_absolute_time();
_reset_timestamp = now;
_consecutive_failures = 0;
_total_failures = 0;
_state = STATE::WAIT_FOR_RESET;
ScheduleDelayed(1_ms);
ScheduleDelayed(100_ms);
break;
case STATE::WAIT_FOR_RESET:
@ -136,13 +145,18 @@ void ICM20689::RunImpl() @@ -136,13 +145,18 @@ void ICM20689::RunImpl()
if ((RegisterRead(Register::WHO_AM_I) == WHOAMI)
&& (RegisterRead(Register::PWR_MGMT_1) == 0x40)) {
// Wakeup and reset digital signal path
RegisterWrite(Register::PWR_MGMT_1, 0);
RegisterWrite(Register::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::ACCEL_RST | SIGNAL_PATH_RESET_BIT::TEMP_RST);
RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::SIG_COND_RST, USER_CTRL_BIT::I2C_IF_DIS);
// if reset succeeded then configure
_state = STATE::CONFIGURE;
ScheduleNow();
ScheduleDelayed(35_ms); // max 35 ms start-up time from sleep
} else {
// RESET not complete
if (hrt_elapsed_time(&_reset_timestamp) > 100_ms) {
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Reset failed, retrying");
_state = STATE::RESET;
ScheduleDelayed(100_ms);
@ -164,7 +178,7 @@ void ICM20689::RunImpl() @@ -164,7 +178,7 @@ void ICM20689::RunImpl()
_data_ready_interrupt_enabled = true;
// backup schedule as a watchdog timeout
ScheduleDelayed(10_ms);
ScheduleDelayed(100_ms);
} else {
_data_ready_interrupt_enabled = false;
@ -174,82 +188,88 @@ void ICM20689::RunImpl() @@ -174,82 +188,88 @@ void ICM20689::RunImpl()
FIFOReset();
} else {
PX4_DEBUG("Configure failed, retrying");
// try again in 10 ms
// CONFIGURE not complete
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Configure failed, resetting");
_state = STATE::RESET;
} else {
PX4_DEBUG("Configure failed, retrying");
}
ScheduleDelayed(10_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
if (!_force_fifo_count_check) {
samples = _fifo_read_samples.load();
} else {
const uint16_t fifo_count = FIFOReadCount();
samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) * SAMPLES_PER_TRANSFER; // round down to nearest
// scheduled from interrupt if _drdy_fifo_read_samples was set
if (_drdy_fifo_read_samples.fetch_and(0) == _fifo_gyro_samples) {
perf_count_interval(_drdy_interval_perf, now);
}
timestamp_sample = _fifo_watermark_interrupt_timestamp;
// push backup schedule back
ScheduleDelayed(_fifo_empty_interval_us * 2);
}
bool failure = false;
// always check current FIFO count
bool success = false;
const uint16_t fifo_count = FIFOReadCount();
if (fifo_count >= FIFO::SIZE) {
FIFOReset();
perf_count(_fifo_overflow_perf);
// 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))) {
} else if (fifo_count == 0) {
perf_count(_fifo_empty_perf);
// 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();
samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) * SAMPLES_PER_TRANSFER; // round down to nearest
} else {
// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) *
SAMPLES_PER_TRANSFER; // round down to nearest
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
FIFOReset();
perf_count(_fifo_overflow_perf);
} else if (samples >= 1) {
if (FIFORead(now, samples)) {
success = true;
_consecutive_failures = 0;
}
}
}
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();
if (!success) {
_consecutive_failures++;
_total_failures++;
} else if (samples >= SAMPLES_PER_TRANSFER) {
// require at least SAMPLES_PER_TRANSFER (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();
// full reset if things are failing consistently
if (_consecutive_failures > 100 || _total_failures > 1000) {
Reset();
return;
}
} else if (samples == 0) {
failure = true;
perf_count(_fifo_empty_perf);
}
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;
if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 10_ms) {
// check configuration registers periodically or immediately following any failure
if (RegisterCheck(_register_cfg[_checked_register])) {
_last_config_check_timestamp = now;
_checked_register = (_checked_register + 1) % size_register_cfg;
} else {
// register check failed, force reconfigure
PX4_DEBUG("Health check failed, reconfiguring");
_state = STATE::CONFIGURE;
ScheduleNow();
// register check failed, force reset
perf_count(_bad_register_perf);
Reset();
}
} else {
// periodically update temperature (1 Hz)
if (hrt_elapsed_time(&_temperature_update_timestamp) > 1_s) {
// periodically update temperature (~1 Hz)
if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
UpdateTemperature();
_temperature_update_timestamp = timestamp_sample;
_temperature_update_timestamp = now;
}
}
}
@ -319,23 +339,27 @@ void ICM20689::ConfigureSampleRate(int sample_rate) @@ -319,23 +339,27 @@ void ICM20689::ConfigureSampleRate(int sample_rate)
}
// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
const float min_interval = SAMPLES_PER_TRANSFER * FIFO_SAMPLE_DT;
const float min_interval = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
_fifo_gyro_samples = math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES);
_fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES));
// recompute FIFO empty interval (us) with actual gyro sample limit
_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
_fifo_accel_samples = math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES);
}
bool ICM20689::Configure()
{
// first set and clear all configured register bits
for (const auto &reg_cfg : _register_cfg) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
}
// now check that all are configured
bool success = true;
for (const auto &reg : _register_cfg) {
if (!RegisterCheck(reg)) {
for (const auto &reg_cfg : _register_cfg) {
if (!RegisterCheck(reg_cfg)) {
success = false;
}
}
@ -354,12 +378,13 @@ int ICM20689::DataReadyInterruptCallback(int irq, void *context, void *arg) @@ -354,12 +378,13 @@ int ICM20689::DataReadyInterruptCallback(int irq, void *context, void *arg)
void ICM20689::DataReady()
{
perf_count(_drdy_interval_perf);
const uint8_t count = _drdy_count.fetch_add(1) + 1;
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);
uint8_t expected = 0;
// at least the required number of samples in the FIFO
if ((count >= _fifo_gyro_samples) && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
_drdy_count.store(0);
ScheduleNow();
}
}
@ -383,7 +408,7 @@ bool ICM20689::DataReadyInterruptDisable() @@ -383,7 +408,7 @@ bool ICM20689::DataReadyInterruptDisable()
return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
}
bool ICM20689::RegisterCheck(const register_config_t &reg_cfg, bool notify)
bool ICM20689::RegisterCheck(const register_config_t &reg_cfg)
{
bool success = true;
@ -399,16 +424,6 @@ bool ICM20689::RegisterCheck(const register_config_t &reg_cfg, bool notify) @@ -399,16 +424,6 @@ bool ICM20689::RegisterCheck(const register_config_t &reg_cfg, bool notify)
success = false;
}
if (!success) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
if (notify) {
perf_count(_bad_register_perf);
_px4_accel.increase_error_count();
_px4_gyro.increase_error_count();
}
}
return success;
}
@ -429,17 +444,12 @@ void ICM20689::RegisterWrite(Register reg, uint8_t value) @@ -429,17 +444,12 @@ void ICM20689::RegisterWrite(Register reg, uint8_t value)
void ICM20689::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
{
const uint8_t orig_val = RegisterRead(reg);
uint8_t val = orig_val;
if (setbits) {
val |= setbits;
}
uint8_t val = (orig_val & ~clearbits) | setbits;
if (clearbits) {
val &= ~clearbits;
if (orig_val != val) {
RegisterWrite(reg, val);
}
RegisterWrite(reg, val);
}
uint16_t ICM20689::FIFOReadCount()
@ -456,11 +466,11 @@ uint16_t ICM20689::FIFOReadCount() @@ -456,11 +466,11 @@ uint16_t ICM20689::FIFOReadCount()
return combine(fifo_count_buf[1], fifo_count_buf[2]);
}
bool ICM20689::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples)
bool ICM20689::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
{
perf_begin(_transfer_perf);
FIFOTransferBuffer buffer{};
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 3, FIFO::SIZE);
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
perf_end(_transfer_perf);
@ -470,47 +480,8 @@ bool ICM20689::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples) @@ -470,47 +480,8 @@ bool ICM20689::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples)
perf_end(_transfer_perf);
const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
const uint16_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
if (fifo_count_samples == 0) {
perf_count(_fifo_empty_perf);
return false;
}
if (fifo_count_bytes >= FIFO::SIZE) {
perf_count(_fifo_overflow_perf);
FIFOReset();
return false;
}
const uint16_t valid_samples = math::min(samples, fifo_count_samples);
if (fifo_count_samples < samples) {
// force check if there is somehow fewer samples actually in the FIFO (potentially a serious error)
_force_fifo_count_check = true;
} else if (fifo_count_samples >= samples + 2) {
// if we're more than a couple samples behind force FIFO_COUNT check
_force_fifo_count_check = true;
} else {
// skip earlier FIFO_COUNT and trust DRDY count if we're in sync
_force_fifo_count_check = false;
}
if (valid_samples > 0) {
ProcessGyro(timestamp_sample, buffer, valid_samples);
if (ProcessAccel(timestamp_sample, buffer, valid_samples)) {
return true;
}
}
// force FIFO count check if there was any other error
_force_fifo_count_check = true;
return false;
ProcessGyro(timestamp_sample, buffer.f, samples);
return ProcessAccel(timestamp_sample, buffer.f, samples);
}
void ICM20689::FIFOReset()
@ -524,9 +495,8 @@ void ICM20689::FIFOReset() @@ -524,9 +495,8 @@ void ICM20689::FIFOReset()
RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_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);
_drdy_count.store(0);
_drdy_fifo_read_samples.store(0);
// FIFO_EN: enable both gyro and accel
// USER_CTRL: re-enable FIFO
@ -542,12 +512,12 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1) @@ -542,12 +512,12 @@ 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 ICM20689::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer,
const uint8_t samples)
bool ICM20689::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
{
PX4Accelerometer::FIFOSample accel;
accel.timestamp_sample = timestamp_sample;
accel.dt = _fifo_empty_interval_us / _fifo_accel_samples;
accel.samples = 0;
accel.dt = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
bool bad_data = false;
@ -555,58 +525,57 @@ bool ICM20689::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFOTrans @@ -555,58 +525,57 @@ bool ICM20689::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFOTrans
int accel_first_sample = 1;
if (samples >= 4) {
if (fifo_accel_equal(buffer.f[0], buffer.f[1]) && fifo_accel_equal(buffer.f[2], buffer.f[3])) {
if (fifo_accel_equal(fifo[0], fifo[1]) && fifo_accel_equal(fifo[2], fifo[3])) {
// [A0, A1, A2, A3]
// A0==A1, A2==A3
accel_first_sample = 1;
} else if (fifo_accel_equal(buffer.f[1], buffer.f[2])) {
} else if (fifo_accel_equal(fifo[1], fifo[2])) {
// [A0, A1, A2, A3]
// A0, A1==A2, A3
accel_first_sample = 0;
} else {
perf_count(_bad_transfer_perf);
// no matching accel samples is an error
bad_data = true;
perf_count(_bad_transfer_perf);
}
}
int accel_samples = 0;
for (int i = accel_first_sample; i < samples; i = i + 2) {
const FIFO::DATA &fifo_sample = buffer.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);
for (int i = accel_first_sample; i < samples; i = i + SAMPLES_PER_TRANSFER) {
int16_t accel_x = combine(fifo[i].ACCEL_XOUT_H, fifo[i].ACCEL_XOUT_L);
int16_t accel_y = combine(fifo[i].ACCEL_YOUT_H, fifo[i].ACCEL_YOUT_L);
int16_t accel_z = combine(fifo[i].ACCEL_ZOUT_H, fifo[i].ACCEL_ZOUT_L);
// 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;
accel_samples++;
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;
_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
_px4_accel.updateFIFO(accel);
if (accel.samples > 0) {
_px4_accel.updateFIFO(accel);
}
return !bad_data;
}
void ICM20689::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples)
void ICM20689::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
{
PX4Gyroscope::FIFOSample gyro;
gyro.timestamp_sample = timestamp_sample;
gyro.samples = samples;
gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples;
gyro.dt = FIFO_SAMPLE_DT;
for (int i = 0; i < samples; i++) {
const FIFO::DATA &fifo_sample = buffer.f[i];
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);
const int16_t gyro_x = combine(fifo[i].GYRO_XOUT_H, fifo[i].GYRO_XOUT_L);
const int16_t gyro_y = combine(fifo[i].GYRO_YOUT_H, fifo[i].GYRO_YOUT_L);
const int16_t gyro_z = combine(fifo[i].GYRO_ZOUT_H, fifo[i].GYRO_ZOUT_L);
// 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)
@ -615,6 +584,9 @@ void ICM20689::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFOTransf @@ -615,6 +584,9 @@ void ICM20689::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFOTransf
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
}
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
_px4_gyro.updateFIFO(gyro);
}

45
src/drivers/imu/invensense/icm20689/ICM20689.hpp
Unescape View File

@ -73,22 +73,20 @@ private: @@ -73,22 +73,20 @@ private:
void exit_and_cleanup() override;
// Sensor Configuration
static constexpr float FIFO_SAMPLE_DT{125.f};
static constexpr uint32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 8 kHz gyro
static constexpr float ACCEL_RATE{GYRO_RATE / 2.f}; // 4 kHz accel
static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
static constexpr uint32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 8000 Hz gyro
static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))};
// Transfer data
struct FIFOTransferBuffer {
uint8_t cmd{static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ};
uint8_t FIFO_COUNTH{0};
uint8_t FIFO_COUNTL{0};
uint8_t cmd{static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ};
FIFO::DATA f[FIFO_MAX_SAMPLES] {};
};
// ensure no struct padding
static_assert(sizeof(FIFOTransferBuffer) == (3 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
struct register_config_t {
Register reg;
@ -110,20 +108,18 @@ private: @@ -110,20 +108,18 @@ private:
bool DataReadyInterruptConfigure();
bool DataReadyInterruptDisable();
bool RegisterCheck(const register_config_t &reg_cfg, bool notify = false);
bool RegisterCheck(const register_config_t &reg_cfg);
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) { RegisterSetAndClearBits(reg, setbits, 0); }
void RegisterClearBits(Register reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); }
uint16_t FIFOReadCount();
bool FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples);
bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
void FIFOReset();
bool ProcessAccel(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples);
void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples);
bool ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
void UpdateTemperature();
const spi_drdy_gpio_t _drdy_gpio;
@ -137,17 +133,17 @@ private: @@ -137,17 +133,17 @@ private:
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 _drdy_interval_perf{nullptr};
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};
unsigned _consecutive_failures{0};
unsigned _total_failures{0};
px4::atomic<uint8_t> _data_ready_count{0};
px4::atomic<uint8_t> _fifo_read_samples{0};
px4::atomic<uint8_t> _drdy_fifo_read_samples{0};
px4::atomic<uint8_t> _drdy_count{0};
bool _data_ready_interrupt_enabled{false};
bool _force_fifo_count_check{true};
enum class STATE : uint8_t {
RESET,
@ -160,20 +156,19 @@ private: @@ -160,20 +156,19 @@ private:
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 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))};
uint8_t _checked_register{0};
static constexpr uint8_t size_register_cfg{9};
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::DEVICE_RESET | PWR_MGMT_1_BIT::SLEEP },
{ Register::CONFIG, CONFIG_BIT::FIFO_MODE | CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ, 0 },
{ Register::GYRO_CONFIG, GYRO_CONFIG_BIT::FS_SEL_2000_DPS, GYRO_CONFIG_BIT::FCHOICE_B_8KHZ_BYPASS_DLPF },
{ Register::ACCEL_CONFIG, ACCEL_CONFIG_BIT::ACCEL_FS_SEL_16G, 0 },
{ Register::ACCEL_CONFIG2, ACCEL_CONFIG2_BIT::ACCEL_FCHOICE_B, ACCEL_CONFIG2_BIT::FIFO_SIZE },
{ Register::GYRO_CONFIG, GYRO_CONFIG_BIT::FS_SEL_2000_DPS, GYRO_CONFIG_BIT::FCHOICE_B_8KHZ_BYPASS_DLPF },
{ Register::CONFIG, CONFIG_BIT::FIFO_MODE | CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ, 0 },
{ Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::I2C_IF_DIS, USER_CTRL_BIT::FIFO_RST | USER_CTRL_BIT::SIG_COND_RST },
{ Register::FIFO_EN, FIFO_EN_BIT::XG_FIFO_EN | FIFO_EN_BIT::YG_FIFO_EN | FIFO_EN_BIT::ZG_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN, FIFO_EN_BIT::TEMP_FIFO_EN },
{ Register::INT_PIN_CFG, INT_PIN_CFG_BIT::INT_LEVEL, 0 },
{ Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN, 0 }
{ Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN, 0 },
{ Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::I2C_IF_DIS, 0 },
{ Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::DEVICE_RESET | PWR_MGMT_1_BIT::SLEEP },
};
};

53
src/drivers/imu/invensense/icm20689/InvenSense_ICM20689_registers.hpp
Unescape View File

@ -63,26 +63,28 @@ static constexpr float TEMPERATURE_SENSITIVITY = 326.8f; // LSB/C @@ -63,26 +63,28 @@ static constexpr float TEMPERATURE_SENSITIVITY = 326.8f; // LSB/C
static constexpr float TEMPERATURE_OFFSET = 25.f; // C
enum class Register : uint8_t {
CONFIG = 0x1A,
GYRO_CONFIG = 0x1B,
ACCEL_CONFIG = 0x1C,
ACCEL_CONFIG2 = 0x1D,
CONFIG = 0x1A,
GYRO_CONFIG = 0x1B,
ACCEL_CONFIG = 0x1C,
ACCEL_CONFIG2 = 0x1D,
FIFO_EN = 0x23,
FIFO_EN = 0x23,
INT_PIN_CFG = 0x37,
INT_ENABLE = 0x38,
INT_PIN_CFG = 0x37,
INT_ENABLE = 0x38,
TEMP_OUT_H = 0x41,
TEMP_OUT_L = 0x42,
TEMP_OUT_H = 0x41,
TEMP_OUT_L = 0x42,
USER_CTRL = 0x6A,
PWR_MGMT_1 = 0x6B,
SIGNAL_PATH_RESET = 0x68,
FIFO_COUNTH = 0x72,
FIFO_COUNTL = 0x73,
FIFO_R_W = 0x74,
WHO_AM_I = 0x75,
USER_CTRL = 0x6A,
PWR_MGMT_1 = 0x6B,
FIFO_COUNTH = 0x72,
FIFO_COUNTL = 0x73,
FIFO_R_W = 0x74,
WHO_AM_I = 0x75,
};
// CONFIG
@ -115,8 +117,8 @@ enum ACCEL_CONFIG_BIT : uint8_t { @@ -115,8 +117,8 @@ enum ACCEL_CONFIG_BIT : uint8_t {
// ACCEL_CONFIG2
enum ACCEL_CONFIG2_BIT : uint8_t {
FIFO_SIZE = Bit7 | Bit6, // 0=512bytes,
ACCEL_FCHOICE_B = Bit3, // Used to bypass DLPF as shown in the table below. (DS-000114 Page 40 of 53)
FIFO_SIZE = Bit7 | Bit6, // 0=512bytes,
ACCEL_FCHOICE_B = Bit3, // Used to bypass DLPF (DS-000114 Page 40 of 53)
};
// FIFO_EN
@ -130,14 +132,18 @@ enum FIFO_EN_BIT : uint8_t { @@ -130,14 +132,18 @@ enum FIFO_EN_BIT : uint8_t {
// INT_PIN_CFG
enum INT_PIN_CFG_BIT : uint8_t {
INT_LEVEL = Bit7,
INT_RD_CLEAR = Bit4,
INT_LEVEL = Bit7,
};
// INT_ENABLE
enum INT_ENABLE_BIT : uint8_t {
FIFO_OFLOW_EN = Bit4,
DATA_RDY_INT_EN = Bit0
DATA_RDY_INT_EN = Bit0,
};
// SIGNAL_PATH_RESET
enum SIGNAL_PATH_RESET_BIT : uint8_t {
ACCEL_RST = Bit1,
TEMP_RST = Bit0,
};
// USER_CTRL
@ -153,9 +159,8 @@ enum PWR_MGMT_1_BIT : uint8_t { @@ -153,9 +159,8 @@ enum PWR_MGMT_1_BIT : uint8_t {
DEVICE_RESET = Bit7,
SLEEP = Bit6,
CLKSEL_2 = Bit2,
CLKSEL_1 = Bit1,
CLKSEL_0 = Bit0,
// CLKSEL[2:0]
CLKSEL_0 = Bit0, // It is required that CLKSEL[2:0] be set to 001 to achieve full gyroscope performance.
};
namespace FIFO

Write Preview
Loading…
Cancel
Save