mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
drivers/imu/invensense/icm42670p: cleanup and small fixes
This commit is contained in:
Daniel Agar
David Sidrane
parent
3ea492b7a2
commit
83daf648ef
@ -1,6 +1,6 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
# Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@ -47,7 +47,7 @@ ICM42670P::ICM42670P(const I2CSPIDriverConfig &config):
|
||||
_px4_accel(get_device_id(), config.rotation),
|
||||
_px4_gyro(get_device_id(), config.rotation)
|
||||
{
|
||||
if (_drdy_gpio != 0) {
|
||||
if (config.drdy_gpio != 0) {
|
||||
_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
|
||||
}
|
||||
|
||||
@ -123,7 +123,7 @@ void ICM42670P::RunImpl()
|
||||
|
||||
switch (_state) {
|
||||
case STATE::RESET:
|
||||
// DEVICE_CONFIG: Software reset configuration
|
||||
// SIGNAL_PATH_RESET: Software Reset (auto clear bit)
|
||||
RegisterWrite(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::SOFT_RESET_DEVICE_CONFIG);
|
||||
_reset_timestamp = now;
|
||||
_failure_count = 0;
|
||||
@ -133,7 +133,7 @@ void ICM42670P::RunImpl()
|
||||
|
||||
case STATE::WAIT_FOR_RESET:
|
||||
if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI)
|
||||
&& (RegisterRead(Register::BANK_0::DEVICE_CONFIG) == 0x04)
|
||||
&& (RegisterRead(Register::BANK_0::SIGNAL_PATH_RESET) == 0x00)
|
||||
&& (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) {
|
||||
|
||||
// Wakeup accel and gyro and schedule remaining configuration
|
||||
@ -190,15 +190,19 @@ void ICM42670P::RunImpl()
|
||||
break;
|
||||
|
||||
case STATE::FIFO_READ: {
|
||||
uint32_t samples = 0;
|
||||
hrt_abstime timestamp_sample = now;
|
||||
uint8_t samples = 0;
|
||||
|
||||
if (_data_ready_interrupt_enabled) {
|
||||
// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
|
||||
if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
|
||||
perf_count(_drdy_missed_perf);
|
||||
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
|
||||
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
|
||||
|
||||
if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
|
||||
timestamp_sample = drdy_timestamp_sample;
|
||||
samples = _fifo_gyro_samples;
|
||||
|
||||
} else {
|
||||
samples = _fifo_gyro_samples;
|
||||
perf_count(_drdy_missed_perf);
|
||||
}
|
||||
|
||||
// push backup schedule back
|
||||
@ -209,8 +213,6 @@ void ICM42670P::RunImpl()
|
||||
// check current FIFO count
|
||||
const uint16_t fifo_count = FIFOReadCount();
|
||||
|
||||
// PX4_WARN("fifo_count = %d ",fifo_count);
|
||||
|
||||
if (fifo_count >= FIFO::SIZE) {
|
||||
FIFOReset();
|
||||
perf_count(_fifo_overflow_perf);
|
||||
@ -222,12 +224,17 @@ void ICM42670P::RunImpl()
|
||||
// FIFO count (size in bytes)
|
||||
samples = (fifo_count / sizeof(FIFO::DATA));
|
||||
|
||||
// tolerate minor jitter, leave sample to next iteration if behind by only 1
|
||||
if (samples == _fifo_gyro_samples + 1) {
|
||||
timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
|
||||
samples--;
|
||||
}
|
||||
|
||||
if (samples > FIFO_MAX_SAMPLES) {
|
||||
// not technically an overflow, but more samples than we expected or can publish
|
||||
FIFOReset();
|
||||
perf_count(_fifo_overflow_perf);
|
||||
samples = 0;
|
||||
// PX4_WARN("samples 1 = %d ",samples);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -235,12 +242,9 @@ void ICM42670P::RunImpl()
|
||||
bool success = false;
|
||||
|
||||
if (samples >= 1) {
|
||||
// PX4_WARN("samples 2 = %d ",samples);
|
||||
if (FIFORead(now, samples)) {
|
||||
if (FIFORead(timestamp_sample, samples)) {
|
||||
success = true;
|
||||
|
||||
// PX4_WARN("success = %d ",success);
|
||||
|
||||
if (_failure_count > 0) {
|
||||
_failure_count--;
|
||||
}
|
||||
@ -260,9 +264,11 @@ void ICM42670P::RunImpl()
|
||||
if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
|
||||
// check configuration registers periodically or immediately following any failure
|
||||
if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
|
||||
&& RegisterCheck(_register_mreg1_cfg[_checked_register_mreg1])
|
||||
) {
|
||||
_last_config_check_timestamp = now;
|
||||
_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
|
||||
_checked_register_mreg1 = (_checked_register_mreg1 + 1) % size_register_mreg1_cfg;
|
||||
|
||||
} else {
|
||||
// register check failed, force reset
|
||||
@ -283,68 +289,8 @@ void ICM42670P::RunImpl()
|
||||
}
|
||||
}
|
||||
|
||||
void ICM42670P::ConfigureAccel()
|
||||
{
|
||||
const uint8_t ACCEL_FS_SEL = RegisterRead(Register::BANK_0::ACCEL_CONFIG0) & (Bit7 | Bit6 | Bit5); // 7:5 ACCEL_FS_SEL
|
||||
|
||||
switch (ACCEL_FS_SEL) {
|
||||
case ACCEL_FS_SEL_2G:
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f);
|
||||
_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
|
||||
break;
|
||||
|
||||
case ACCEL_FS_SEL_4G:
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
|
||||
_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
|
||||
break;
|
||||
|
||||
case ACCEL_FS_SEL_8G:
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
|
||||
_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
|
||||
break;
|
||||
|
||||
case ACCEL_FS_SEL_16G:
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
|
||||
_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void ICM42670P::ConfigureGyro()
|
||||
{
|
||||
const uint8_t GYRO_FS_SEL = RegisterRead(Register::BANK_0::GYRO_CONFIG0) & (Bit7 | Bit6 | Bit5); // 7:5 GYRO_FS_SEL
|
||||
|
||||
float range_dps = 0.f;
|
||||
|
||||
switch (GYRO_FS_SEL) {
|
||||
|
||||
case GYRO_FS_SEL_250_DPS:
|
||||
range_dps = 250.f;
|
||||
break;
|
||||
|
||||
case GYRO_FS_SEL_500_DPS:
|
||||
range_dps = 500.f;
|
||||
break;
|
||||
|
||||
case GYRO_FS_SEL_1000_DPS:
|
||||
range_dps = 1000.f;
|
||||
break;
|
||||
|
||||
case GYRO_FS_SEL_2000_DPS:
|
||||
range_dps = 2000.f;
|
||||
break;
|
||||
}
|
||||
|
||||
_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
|
||||
_px4_gyro.set_range(math::radians(range_dps));
|
||||
}
|
||||
|
||||
void ICM42670P::ConfigureSampleRate(int sample_rate)
|
||||
{
|
||||
if (sample_rate == 0) {
|
||||
sample_rate = 800; // default to 800 Hz
|
||||
}
|
||||
|
||||
// round down to nearest FIFO sample dt
|
||||
const float min_interval = FIFO_SAMPLE_DT;
|
||||
_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
|
||||
@ -374,8 +320,6 @@ void ICM42670P::ConfigureFIFOWatermark(uint8_t samples)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
bool ICM42670P::Configure()
|
||||
{
|
||||
// first set and clear all configured register bits
|
||||
@ -383,7 +327,9 @@ bool ICM42670P::Configure()
|
||||
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
|
||||
}
|
||||
|
||||
Mreg1Config();
|
||||
for (const auto ®_cfg : _register_mreg1_cfg) {
|
||||
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
|
||||
}
|
||||
|
||||
// now check that all are configured
|
||||
bool success = true;
|
||||
@ -394,11 +340,17 @@ bool ICM42670P::Configure()
|
||||
}
|
||||
}
|
||||
|
||||
success = Mreg1Check();
|
||||
for (const auto ®_cfg : _register_mreg1_cfg) {
|
||||
if (!RegisterCheck(reg_cfg)) {
|
||||
success = false;
|
||||
}
|
||||
}
|
||||
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
|
||||
_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
|
||||
|
||||
ConfigureAccel();
|
||||
ConfigureGyro();
|
||||
_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
|
||||
_px4_gyro.set_range(math::radians(2000.f));
|
||||
|
||||
return success;
|
||||
}
|
||||
@ -411,11 +363,8 @@ int ICM42670P::DataReadyInterruptCallback(int irq, void *context, void *arg)
|
||||
|
||||
void ICM42670P::DataReady()
|
||||
{
|
||||
uint32_t expected = 0;
|
||||
|
||||
if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
|
||||
ScheduleNow();
|
||||
}
|
||||
_drdy_timestamp_sample.store(hrt_absolute_time());
|
||||
ScheduleNow();
|
||||
}
|
||||
|
||||
bool ICM42670P::DataReadyInterruptConfigure()
|
||||
@ -445,18 +394,19 @@ bool ICM42670P::RegisterCheck(const T ®_cfg)
|
||||
const uint8_t reg_value = RegisterRead(reg_cfg.reg);
|
||||
|
||||
if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != 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) != 0)) {
|
||||
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
|
||||
success = false;
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
uint8_t ICM42670P::RegisterRead(T reg)
|
||||
uint8_t ICM42670P::RegisterRead(Register::BANK_0 reg)
|
||||
{
|
||||
uint8_t cmd[2] {};
|
||||
cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
|
||||
@ -464,14 +414,50 @@ uint8_t ICM42670P::RegisterRead(T reg)
|
||||
return cmd[1];
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
void ICM42670P::RegisterWrite(T reg, uint8_t value)
|
||||
uint8_t ICM42670P::RegisterRead(Register::MREG1 reg)
|
||||
{
|
||||
// BLK_SEL_R must be set to 0
|
||||
RegisterWrite(Register::BANK_0::BLK_SEL_R, 0x00);
|
||||
|
||||
// MADDR_R must be set to 0x14 (address of the MREG1 register being accessed)
|
||||
RegisterWrite(Register::BANK_0::MADDR_R, (uint8_t)reg);
|
||||
|
||||
// Wait for 10 µs
|
||||
px4_udelay(10);
|
||||
|
||||
// Read register M_R to access the value in MREG1 register 0x14
|
||||
uint8_t value = RegisterRead(Register::BANK_0::M_R);
|
||||
|
||||
// Wait for 10 µs
|
||||
// Host must not access any other register for 10 µs once MREG1, MREG2 or MREG3 access is kicked off.
|
||||
px4_udelay(10);
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
void ICM42670P::RegisterWrite(Register::BANK_0 reg, uint8_t value)
|
||||
{
|
||||
uint8_t cmd[2] { (uint8_t)reg, value };
|
||||
|
||||
transfer(cmd, cmd, sizeof(cmd));
|
||||
}
|
||||
|
||||
void ICM42670P::RegisterWrite(Register::MREG1 reg, uint8_t value)
|
||||
{
|
||||
// BLK_SEL_W must be set to 0
|
||||
RegisterWrite(Register::BANK_0::BLK_SEL_W, 0x00);
|
||||
|
||||
// MADDR_W must be set to address of the MREG1 register being accessed
|
||||
RegisterWrite(Register::BANK_0::MADDR_W, (uint8_t)reg);
|
||||
|
||||
// M_W must be set to the desired value
|
||||
RegisterWrite(Register::BANK_0::M_W, value);
|
||||
|
||||
// Wait for 10 µs
|
||||
// Host must not access any other register for 10 µs once MREG1, MREG2 or MREG3 access is kicked off.
|
||||
px4_udelay(10);
|
||||
}
|
||||
|
||||
|
||||
template <typename T>
|
||||
void ICM42670P::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits)
|
||||
{
|
||||
@ -498,12 +484,10 @@ uint16_t ICM42670P::FIFOReadCount()
|
||||
return combine(fifo_count_buf[1], fifo_count_buf[2]);
|
||||
}
|
||||
|
||||
|
||||
|
||||
bool ICM42670P::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples)
|
||||
{
|
||||
FIFOTransferBuffer buffer{};
|
||||
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4 + 2, FIFO::SIZE);
|
||||
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 6, FIFO::SIZE);
|
||||
|
||||
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
|
||||
perf_count(_bad_transfer_perf);
|
||||
@ -518,7 +502,6 @@ bool ICM42670P::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples)
|
||||
|
||||
const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
|
||||
|
||||
|
||||
if (fifo_count_bytes >= FIFO::SIZE) {
|
||||
perf_count(_fifo_overflow_perf);
|
||||
FIFOReset();
|
||||
@ -552,6 +535,18 @@ bool ICM42670P::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples)
|
||||
} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO)) {
|
||||
// gyro bit not set
|
||||
valid = false;
|
||||
|
||||
} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_20) {
|
||||
// Packet does not contain a new and valid extended 20-bit data
|
||||
valid = false;
|
||||
|
||||
} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL) {
|
||||
// accel ODR changed
|
||||
valid = false;
|
||||
|
||||
} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO) {
|
||||
// gyro ODR changed
|
||||
valid = false;
|
||||
}
|
||||
|
||||
if (valid) {
|
||||
@ -563,7 +558,6 @@ bool ICM42670P::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples)
|
||||
}
|
||||
}
|
||||
|
||||
// PX4_WARN("valid_samples = %d ",valid_samples);
|
||||
if (valid_samples > 0) {
|
||||
ProcessGyro(timestamp_sample, buffer.f, valid_samples);
|
||||
ProcessAccel(timestamp_sample, buffer.f, valid_samples);
|
||||
@ -577,18 +571,30 @@ void ICM42670P::FIFOReset()
|
||||
{
|
||||
perf_count(_fifo_reset_perf);
|
||||
|
||||
// FIFO flush requires the following programming sequence:
|
||||
// Write FIFO_FLUSH = 1
|
||||
// Wait for 1.5 µs
|
||||
// Read FIFO_FLUSH, it should now be 0
|
||||
|
||||
// SIGNAL_PATH_RESET: FIFO flush
|
||||
RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
|
||||
px4_udelay(2); // Wait for 1.5 µs
|
||||
|
||||
const uint8_t SIGNAL_PATH_RESET = RegisterRead(Register::BANK_0::SIGNAL_PATH_RESET);
|
||||
|
||||
if ((SIGNAL_PATH_RESET & SIGNAL_PATH_RESET_BIT::FIFO_FLUSH) != 0) {
|
||||
PX4_DEBUG("SIGNAL_PATH_RESET FIFO_FLUSH failed");
|
||||
}
|
||||
|
||||
// reset while FIFO is disabled
|
||||
_drdy_fifo_read_samples.store(0);
|
||||
_drdy_timestamp_sample.store(0);
|
||||
}
|
||||
|
||||
void ICM42670P::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
|
||||
{
|
||||
sensor_accel_fifo_s accel{};
|
||||
accel.timestamp_sample = timestamp_sample;
|
||||
accel.samples = 0;
|
||||
accel.samples = samples;
|
||||
accel.dt = FIFO_SAMPLE_DT;
|
||||
|
||||
for (int i = 0; i < samples; i++) {
|
||||
@ -598,18 +604,15 @@ void ICM42670P::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DA
|
||||
|
||||
// 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[i] = accel_x;
|
||||
accel.y[i] = math::negate(accel_y);
|
||||
accel.z[i] = math::negate(accel_z);
|
||||
}
|
||||
|
||||
_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));
|
||||
|
||||
if (accel.samples > 0) {
|
||||
_px4_accel.updateFIFO(accel);
|
||||
}
|
||||
_px4_accel.updateFIFO(accel);
|
||||
}
|
||||
|
||||
void ICM42670P::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
|
||||
@ -627,8 +630,8 @@ void ICM42670P::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DAT
|
||||
// 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;
|
||||
gyro.y[i] = math::negate(gyro_y);
|
||||
gyro.z[i] = math::negate(gyro_z);
|
||||
}
|
||||
|
||||
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
@ -658,88 +661,3 @@ void ICM42670P::UpdateTemperature()
|
||||
_px4_gyro.set_temperature(TEMP_degC);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
uint8_t ICM42670P::RegisterReadBank1(uint8_t reg)
|
||||
{
|
||||
uint8_t value;
|
||||
|
||||
RegisterWrite((uint8_t)Register::BANK_0::BLK_SEL_R, 0x00);
|
||||
RegisterWrite((uint8_t)Register::BANK_0::MADDR_R, reg);
|
||||
ScheduleDelayed(10_us);
|
||||
value = RegisterRead((uint8_t)Register::BANK_0::M_R);
|
||||
ScheduleDelayed(10_us);
|
||||
RegisterWrite((uint8_t)Register::BANK_0::BLK_SEL_R, 0x00);
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
void ICM42670P::RegisterWriteBank1(uint8_t reg, uint8_t value)
|
||||
{
|
||||
RegisterWrite((uint8_t)Register::BANK_0::BLK_SEL_W, 0x00);
|
||||
RegisterWrite((uint8_t)Register::BANK_0::MADDR_W, reg);
|
||||
RegisterWrite((uint8_t)Register::BANK_0::M_W, value);
|
||||
ScheduleDelayed(10_us);
|
||||
RegisterWrite((uint8_t)Register::BANK_0::BLK_SEL_W, 0x00);
|
||||
}
|
||||
|
||||
void ICM42670P::Mreg1Config()
|
||||
{
|
||||
uint8_t data;
|
||||
uint8_t set_bits;
|
||||
uint8_t clear_bits;
|
||||
|
||||
clear_bits = Bit7 | Bit6 | Bit4 | Bit3;
|
||||
set_bits = FIFO_CONFIG5_BIT::FIFO_WM_GT_TH | FIFO_CONFIG5_BIT::FIFO_TMST_FSYNC_EN | FIFO_CONFIG5_BIT::FIFO_GYRO_EN |
|
||||
FIFO_CONFIG5_BIT::FIFO_ACCEL_EN;
|
||||
|
||||
data = RegisterReadBank1(0x01);
|
||||
data &= ~clear_bits;
|
||||
data |= set_bits;
|
||||
RegisterWriteBank1(0x01, data);
|
||||
|
||||
|
||||
clear_bits = Bit7 | Bit6 | Bit5 | Bit4 | Bit2 | Bit1 | Bit0;
|
||||
set_bits = INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ;
|
||||
data = RegisterReadBank1(0x04);
|
||||
data &= ~clear_bits;
|
||||
data |= set_bits;
|
||||
RegisterWriteBank1(0x04, data);
|
||||
}
|
||||
|
||||
bool ICM42670P::Mreg1Check()
|
||||
{
|
||||
uint8_t set_bits;
|
||||
uint8_t clear_bits;
|
||||
uint8_t reg_value;
|
||||
bool success;
|
||||
success = true;
|
||||
|
||||
reg_value = RegisterReadBank1(0x01);
|
||||
|
||||
clear_bits = Bit7 | Bit6 | Bit4 | Bit3;
|
||||
set_bits = FIFO_CONFIG5_BIT::FIFO_WM_GT_TH | FIFO_CONFIG5_BIT::FIFO_TMST_FSYNC_EN | FIFO_CONFIG5_BIT::FIFO_GYRO_EN |
|
||||
FIFO_CONFIG5_BIT::FIFO_ACCEL_EN;
|
||||
|
||||
if (set_bits && ((reg_value & set_bits) != set_bits)) {
|
||||
success = false;
|
||||
}
|
||||
|
||||
if (clear_bits && ((reg_value & clear_bits) != 0)) {
|
||||
success = false;
|
||||
}
|
||||
|
||||
reg_value = RegisterReadBank1(0x04);
|
||||
clear_bits = Bit7 | Bit6 | Bit5 | Bit4 | Bit2 | Bit1 | Bit0;
|
||||
set_bits = INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ;
|
||||
|
||||
if (set_bits && ((reg_value & set_bits) != set_bits)) {
|
||||
success = false;
|
||||
}
|
||||
|
||||
if (clear_bits && ((reg_value & clear_bits) != 0)) {
|
||||
success = false;
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@ -57,6 +57,7 @@ class ICM42670P : public device::SPI, public I2CSPIDriver<ICM42670P>
|
||||
public:
|
||||
ICM42670P(const I2CSPIDriverConfig &config);
|
||||
~ICM42670P() override;
|
||||
|
||||
static void print_usage();
|
||||
|
||||
void RunImpl();
|
||||
@ -68,12 +69,12 @@ private:
|
||||
void exit_and_cleanup() override;
|
||||
|
||||
// Sensor Configuration
|
||||
static constexpr float FIFO_SAMPLE_DT{1e6f / 800.f}; // 8000 Hz accel & gyro ODR configured
|
||||
static constexpr float FIFO_SAMPLE_DT{1e6f / 1600.f}; // 1600 Hz accel & gyro ODR configured
|
||||
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
|
||||
static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
|
||||
|
||||
// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
|
||||
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
|
||||
static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
|
||||
|
||||
// Transfer data
|
||||
struct FIFOTransferBuffer {
|
||||
@ -85,10 +86,8 @@ private:
|
||||
uint8_t FIFO_COUNTL{0};
|
||||
FIFO::DATA f[FIFO_MAX_SAMPLES] {};
|
||||
};
|
||||
|
||||
|
||||
// ensure no struct padding
|
||||
static_assert(sizeof(FIFOTransferBuffer) == (4 + 2 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
|
||||
static_assert(sizeof(FIFOTransferBuffer) == (6 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
|
||||
|
||||
struct register_bank0_config_t {
|
||||
Register::BANK_0 reg;
|
||||
@ -96,13 +95,17 @@ private:
|
||||
uint8_t clear_bits{0};
|
||||
};
|
||||
|
||||
struct register_mreg1_config_t {
|
||||
Register::MREG1 reg;
|
||||
uint8_t set_bits{0};
|
||||
uint8_t clear_bits{0};
|
||||
};
|
||||
|
||||
int probe() override;
|
||||
|
||||
bool Reset();
|
||||
|
||||
bool Configure();
|
||||
void ConfigureAccel();
|
||||
void ConfigureGyro();
|
||||
void ConfigureSampleRate(int sample_rate);
|
||||
void ConfigureFIFOWatermark(uint8_t samples);
|
||||
|
||||
@ -111,9 +114,13 @@ private:
|
||||
bool DataReadyInterruptConfigure();
|
||||
bool DataReadyInterruptDisable();
|
||||
|
||||
uint8_t RegisterRead(Register::BANK_0 reg);
|
||||
uint8_t RegisterRead(Register::MREG1 reg);
|
||||
|
||||
void RegisterWrite(Register::BANK_0 reg, uint8_t value);
|
||||
void RegisterWrite(Register::MREG1 reg, uint8_t value);
|
||||
|
||||
template <typename T> bool RegisterCheck(const T ®_cfg);
|
||||
template <typename T> uint8_t RegisterRead(T reg);
|
||||
template <typename T> void RegisterWrite(T reg, uint8_t value);
|
||||
template <typename T> void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits);
|
||||
template <typename T> void RegisterSetBits(T reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); }
|
||||
template <typename T> void RegisterClearBits(T reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); }
|
||||
@ -126,12 +133,6 @@ private:
|
||||
void ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
|
||||
void UpdateTemperature();
|
||||
|
||||
uint8_t RegisterReadBank1(uint8_t reg);
|
||||
void RegisterWriteBank1(uint8_t reg, uint8_t value);
|
||||
|
||||
void Mreg1Config();
|
||||
bool Mreg1Check();
|
||||
|
||||
const spi_drdy_gpio_t _drdy_gpio;
|
||||
|
||||
PX4Accelerometer _px4_accel;
|
||||
@ -149,9 +150,7 @@ private:
|
||||
hrt_abstime _temperature_update_timestamp{0};
|
||||
int _failure_count{0};
|
||||
|
||||
enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
|
||||
|
||||
px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
|
||||
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
|
||||
bool _data_ready_interrupt_enabled{false};
|
||||
|
||||
enum class STATE : uint8_t {
|
||||
@ -159,26 +158,32 @@ private:
|
||||
WAIT_FOR_RESET,
|
||||
CONFIGURE,
|
||||
FIFO_READ,
|
||||
};
|
||||
|
||||
STATE _state{STATE::RESET};
|
||||
} _state{STATE::RESET};
|
||||
|
||||
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
|
||||
uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
|
||||
int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
|
||||
|
||||
uint8_t _checked_register_bank0{0};
|
||||
static constexpr uint8_t size_register_bank0_cfg{10};
|
||||
register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] {
|
||||
// Register | Set bits, Clear bits
|
||||
{ Register::BANK_0::INT_CONFIG, INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY },
|
||||
{ Register::BANK_0::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_MODE_STOP_ON_FULL, Bit0 },
|
||||
{ Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 },
|
||||
{ Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_ODR_1600Hz | GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS, Bit7 | Bit3 | Bit0 },
|
||||
{ Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_ODR_1600Hz | ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_16G, Bit6 | Bit5 | Bit3 | Bit0 },
|
||||
{ Register::BANK_0::GYRO_CONFIG1, GYRO_CONFIG1_BIT::GYRO_UI_FILT_BW_34Hz, Bit1 },
|
||||
{ Register::BANK_0::ACCEL_CONFIG1, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_BW_34Hz, Bit1 },
|
||||
{ Register::BANK_0::FIFO_CONFIG2, 0, 0 }, // FIFO_WM[7:0] set at runtime
|
||||
{ Register::BANK_0::FIFO_CONFIG3, 0, 0 }, // FIFO_WM[11:8] set at runtime
|
||||
{ Register::BANK_0::INT_SOURCE0, INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 },
|
||||
// Register | Set bits, Clear bits
|
||||
{ Register::BANK_0::INT_CONFIG, INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY },
|
||||
{ Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 },
|
||||
{ Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS_SET | GYRO_CONFIG0_BIT::GYRO_ODR_1600HZ_SET, GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS_CLEAR | GYRO_CONFIG0_BIT::GYRO_ODR_1600HZ_CLEAR },
|
||||
{ Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_UI_FS_SEL_16G_SET | ACCEL_CONFIG0_BIT::ACCEL_ODR_1600HZ_SET, ACCEL_CONFIG0_BIT::ACCEL_UI_FS_SEL_16G_SET | ACCEL_CONFIG0_BIT::ACCEL_ODR_1600HZ_CLEAR },
|
||||
{ Register::BANK_0::GYRO_CONFIG1, 0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_BW_BYPASSED_CLEAR },
|
||||
{ Register::BANK_0::ACCEL_CONFIG1, 0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_BW_BYPASSED_CLEAR },
|
||||
{ Register::BANK_0::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_MODE_STOP_ON_FULL, FIFO_CONFIG1_BIT::FIFO_BYPASS },
|
||||
{ Register::BANK_0::FIFO_CONFIG2, 0, 0 }, // FIFO_WM[7:0] set at runtime
|
||||
{ Register::BANK_0::FIFO_CONFIG3, 0, 0 }, // FIFO_WM[11:8] set at runtime
|
||||
{ Register::BANK_0::INT_SOURCE0, INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 },
|
||||
};
|
||||
|
||||
uint8_t _checked_register_mreg1{0};
|
||||
static constexpr uint8_t size_register_mreg1_cfg{2};
|
||||
register_mreg1_config_t _register_mreg1_cfg[size_register_mreg1_cfg] {
|
||||
// Register | Set bits, Clear bits
|
||||
{ Register::MREG1::FIFO_CONFIG5, FIFO_CONFIG5_BIT::FIFO_GYRO_EN | FIFO_CONFIG5_BIT::FIFO_ACCEL_EN, 0 },
|
||||
{ Register::MREG1::INT_CONFIG0, INT_CONFIG0_BIT::FIFO_THS_INT_CLEAR, 0 },
|
||||
};
|
||||
};
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@ -66,21 +66,13 @@ namespace Register
|
||||
{
|
||||
|
||||
enum class BANK_0 : uint8_t {
|
||||
DEVICE_CONFIG = 0x01,
|
||||
SIGNAL_PATH_RESET = 0x02,
|
||||
|
||||
INT_CONFIG = 0x06,
|
||||
|
||||
|
||||
TEMP_DATA1 = 0x09,
|
||||
TEMP_DATA0 = 0x0A,
|
||||
|
||||
INT_STATUS = 0x3A,
|
||||
FIFO_COUNTH = 0x3D,
|
||||
FIFO_COUNTL = 0x3E,
|
||||
FIFO_DATA = 0x3F,
|
||||
|
||||
SIGNAL_PATH_RESET = 0x02,
|
||||
|
||||
PWR_MGMT0 = 0x1F,
|
||||
GYRO_CONFIG0 = 0x20,
|
||||
ACCEL_CONFIG0 = 0x21,
|
||||
@ -90,87 +82,93 @@ enum class BANK_0 : uint8_t {
|
||||
FIFO_CONFIG1 = 0x28,
|
||||
FIFO_CONFIG2 = 0x29,
|
||||
FIFO_CONFIG3 = 0x2A,
|
||||
|
||||
INT_SOURCE0 = 0x2B,
|
||||
|
||||
INT_STATUS = 0x3A,
|
||||
FIFO_COUNTH = 0x3D,
|
||||
FIFO_COUNTL = 0x3E,
|
||||
FIFO_DATA = 0x3F,
|
||||
|
||||
WHO_AM_I = 0x75,
|
||||
// REG_BANK_SEL = 0x76,
|
||||
|
||||
BLK_SEL_W = 0x79,
|
||||
MADDR_W = 0x7A,
|
||||
M_W = 0x7B,
|
||||
|
||||
BLK_SEL_R = 0x7C,
|
||||
MADDR_R = 0x7D,
|
||||
M_R = 0x7E,
|
||||
|
||||
};
|
||||
|
||||
enum class MREG_1 : uint8_t {
|
||||
FIFO_CONFIG5_MREG1 = 0x01,
|
||||
INT_CONFIG0_MREG1 = 0x04,
|
||||
enum class MREG1 : uint8_t {
|
||||
FIFO_CONFIG5 = 0x01,
|
||||
|
||||
INT_CONFIG0 = 0x04,
|
||||
};
|
||||
|
||||
enum class MREG_2 : uint8_t {
|
||||
OTP_CTRL7_MREG2 = 0x06,
|
||||
};
|
||||
|
||||
enum class MREG_3 : uint8_t {
|
||||
XA_ST_DATA_MREG3 = 0x00,
|
||||
YA_ST_DATA_MREG3 = 0x01,
|
||||
ZA_ST_DATA_MREG3 = 0x02,
|
||||
XG_ST_DATA_MREG3 = 0x03,
|
||||
YG_ST_DATA_MREG3 = 0x04,
|
||||
ZG_ST_DATA_MREG3 = 0x05,
|
||||
};
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
//---------------- BANK0 Register bits
|
||||
|
||||
// SIGNAL_PATH_RESET
|
||||
enum SIGNAL_PATH_RESET_BIT : uint8_t {
|
||||
SOFT_RESET_DEVICE_CONFIG = Bit4, //
|
||||
FIFO_FLUSH = Bit2,
|
||||
SOFT_RESET_DEVICE_CONFIG = Bit4, // 1: Software reset enabled
|
||||
FIFO_FLUSH = Bit2, // When set to 1, FIFO will get flushed
|
||||
};
|
||||
|
||||
// INT_CONFIG
|
||||
enum INT_CONFIG_BIT : uint8_t {
|
||||
INT1_MODE = Bit2,
|
||||
INT1_DRIVE_CIRCUIT = Bit1,
|
||||
INT1_POLARITY = Bit0,
|
||||
INT1_MODE = Bit2, // INT1 interrupt mode 1: Latched mode
|
||||
INT1_DRIVE_CIRCUIT = Bit1, // INT1 drive circuit 1: Push pull
|
||||
INT1_POLARITY = Bit0, // INT1 interrupt polarity 0: Active low
|
||||
};
|
||||
|
||||
// PWR_MGMT0
|
||||
enum PWR_MGMT0_BIT : uint8_t {
|
||||
// 3:2 GYRO_MODE
|
||||
GYRO_MODE_LOW_NOISE = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode
|
||||
// 1:0 ACCEL_MODE
|
||||
ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode
|
||||
};
|
||||
|
||||
// GYRO_CONFIG0
|
||||
enum GYRO_CONFIG0_BIT : uint8_t {
|
||||
// 6:5 GYRO_FS_SEL
|
||||
GYRO_FS_SEL_2000_DPS_SET = 0, // 0b000 = ±2000dps
|
||||
GYRO_FS_SEL_2000_DPS_CLEAR = Bit6 | Bit5,
|
||||
|
||||
// 3:0 GYRO_ODR
|
||||
GYRO_ODR_1600HZ_SET = Bit2 | Bit0, // 0101: 1600Hz
|
||||
GYRO_ODR_1600HZ_CLEAR = Bit3 | Bit1,
|
||||
};
|
||||
|
||||
// ACCEL_CONFIG0
|
||||
enum ACCEL_CONFIG0_BIT : uint8_t {
|
||||
// 6:5 ACCEL_UI_FS_SEL
|
||||
ACCEL_UI_FS_SEL_16G_SET = 0, // 000: ±16g
|
||||
ACCEL_UI_FS_SEL_16G_CLEAR = Bit6 | Bit5,
|
||||
|
||||
// 3:0 ACCEL_ODR
|
||||
ACCEL_ODR_1600HZ_SET = Bit2 | Bit0, // 0101: 1600Hz
|
||||
ACCEL_ODR_1600HZ_CLEAR = Bit3 | Bit1,
|
||||
};
|
||||
|
||||
// GYRO_CONFIG1
|
||||
enum GYRO_CONFIG1_BIT : uint8_t {
|
||||
|
||||
// 2:0 GYRO_ODR
|
||||
GYRO_UI_FILT_BW_16Hz = Bit2 | Bit1 | Bit0, // 111: 16Hz
|
||||
GYRO_UI_FILT_BW_25Hz = Bit2 | Bit1, // 110: 25Hz
|
||||
GYRO_UI_FILT_BW_34Hz = Bit2 | Bit0, // 101: 34Hz
|
||||
GYRO_UI_FILT_BW_53Hz = Bit2, // 100: 53Hz
|
||||
GYRO_UI_FILT_BW_73Hz = Bit1 | Bit0, // 011: 73Hz
|
||||
GYRO_UI_FILT_BW_121Hz = Bit1, // 010: 121Hz
|
||||
GYRO_UI_FILT_BW_180Hz = Bit0, // 001: 180Hz
|
||||
// 2:0 GYRO_UI_FILT_BW
|
||||
GYRO_UI_FILT_BW_BYPASSED_CLEAR = Bit2 | Bit1 | Bit0, // 000: Low pass filter bypassed
|
||||
};
|
||||
|
||||
// ACCEL_CONFIG1
|
||||
enum ACCEL_CONFIG1_BIT : uint8_t {
|
||||
|
||||
// 2:0 ACCEL_ODR
|
||||
ACCEL_UI_FILT_BW_16Hz = Bit2 | Bit1 | Bit0, // 111: 16Hz
|
||||
ACCEL_UI_FILT_BW_25Hz = Bit2 | Bit1, // 110: 25Hz
|
||||
ACCEL_UI_FILT_BW_34Hz = Bit2 | Bit0, // 101: 34Hz
|
||||
ACCEL_UI_FILT_BW_53Hz = Bit2, // 100: 53Hz
|
||||
ACCEL_UI_FILT_BW_73Hz = Bit1 | Bit0, // 011: 73Hz
|
||||
ACCEL_UI_FILT_BW_121Hz = Bit1, // 010: 121Hz
|
||||
ACCEL_UI_FILT_BW_180Hz = Bit0, // 001: 180Hz
|
||||
// 2:0 ACCEL_UI_FILT_BW
|
||||
ACCEL_UI_FILT_BW_BYPASSED_CLEAR = Bit2 | Bit1 | Bit0, // 000: Low pass filter bypassed
|
||||
};
|
||||
|
||||
// FIFO_CONFIG1
|
||||
enum FIFO_CONFIG1_BIT : uint8_t {
|
||||
// 1 FIFO_MODE
|
||||
FIFO_MODE_STOP_ON_FULL = Bit1, // 11: STOP-on-FULL Mode
|
||||
// FIFO_MODE
|
||||
FIFO_MODE_STOP_ON_FULL = Bit1, // 1: STOP-on-FULL Mode
|
||||
FIFO_BYPASS = Bit0, // 0: FIFO is not bypassed
|
||||
};
|
||||
|
||||
// INT_STATUS
|
||||
@ -180,113 +178,63 @@ enum INT_STATUS_BIT : uint8_t {
|
||||
FIFO_FULL_INT = Bit1,
|
||||
};
|
||||
|
||||
// PWR_MGMT0
|
||||
enum PWR_MGMT0_BIT : uint8_t {
|
||||
GYRO_MODE_LOW_NOISE = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode
|
||||
ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode
|
||||
// INT_SOURCE0
|
||||
enum INT_SOURCE0_BIT : uint8_t {
|
||||
RESET_DONE_INT1_EN = Bit4, // 1: Reset done interrupt routed to INT1
|
||||
FIFO_THS_INT1_EN = Bit2, // FIFO threshold interrupt routed to INT1
|
||||
};
|
||||
|
||||
// GYRO_CONFIG0
|
||||
enum GYRO_CONFIG0_BIT : uint8_t {
|
||||
// 6:5 GYRO_FS_SEL
|
||||
GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000dps
|
||||
GYRO_FS_SEL_1000_DPS = Bit5, // 0b001 = ±1000 dps
|
||||
GYRO_FS_SEL_500_DPS = Bit6, // 0b010 = ±500 dps
|
||||
GYRO_FS_SEL_250_DPS = Bit6 | Bit5, // 0b011 = ±250 dps
|
||||
|
||||
// 3:0 GYRO_ODR
|
||||
GYRO_ODR_1600Hz = Bit2 | Bit0, // 0101: 1600Hz
|
||||
GYRO_ODR_800Hz = Bit2 | Bit1, // 0110: 800Hz
|
||||
GYRO_ODR_400Hz = Bit2 | Bit1 | Bit0, // 0111: 400Hz
|
||||
GYRO_ODR_200Hz = Bit3, // 1000: 200Hz
|
||||
};
|
||||
|
||||
// ACCEL_CONFIG0
|
||||
enum ACCEL_CONFIG0_BIT : uint8_t {
|
||||
// 6:5 ACCEL_FS_SEL
|
||||
ACCEL_FS_SEL_16G = 0, // 000: ±16g
|
||||
ACCEL_FS_SEL_8G = Bit5, // 001: ±8g
|
||||
ACCEL_FS_SEL_4G = Bit6, // 010: ±4g
|
||||
ACCEL_FS_SEL_2G = Bit6 | Bit5, // 011: ±2g
|
||||
|
||||
// 3:0 ACCEL_ODR
|
||||
ACCEL_ODR_1600Hz = Bit2 | Bit0, // 0101: 1600Hz
|
||||
ACCEL_ODR_800Hz = Bit2 | Bit1, // 0110: 800Hz
|
||||
ACCEL_ODR_400Hz = Bit2 | Bit1 | Bit0, // 0111: 400Hz
|
||||
ACCEL_ODR_200Hz = Bit3, // 1000: 200Hz
|
||||
};
|
||||
//---------------- USER BANK MREG1 Register bits
|
||||
|
||||
// FIFO_CONFIG5
|
||||
enum FIFO_CONFIG5_BIT : uint8_t {
|
||||
FIFO_RESUME_PARTIAL_RD = Bit4,
|
||||
FIFO_WM_GT_TH = Bit5,
|
||||
FIFO_HIRES_EN = Bit3,
|
||||
FIFO_TMST_FSYNC_EN = Bit2,
|
||||
FIFO_GYRO_EN = Bit1,
|
||||
FIFO_ACCEL_EN = Bit0,
|
||||
FIFO_WM_GT_TH = Bit5, // 0: Trigger FIFO Watermark interrupt when FIFO_COUNT = FIFO_WM
|
||||
FIFO_GYRO_EN = Bit1, // 1: Enables Gyro packets to go to FIFO
|
||||
FIFO_ACCEL_EN = Bit0, // 1: Enables Accel packets to go to FIFO
|
||||
};
|
||||
|
||||
// INT_CONFIG0
|
||||
enum INT_CONFIG0_BIT : uint8_t {
|
||||
// 3:2 FIFO_THS_INT_CLEAR
|
||||
CLEAR_ON_FIFO_READ = Bit3,
|
||||
};
|
||||
|
||||
// INT_SOURCE0
|
||||
enum INT_SOURCE0_BIT : uint8_t {
|
||||
ST_INT1_END = Bit7,
|
||||
FSYNC_INT1_EN = Bit6,
|
||||
PLL_RDY_INT1_EN = Bit5,
|
||||
RESET_DONE_INT1_EN = Bit4,
|
||||
DRDY_INT1_EN = Bit3,
|
||||
FIFO_THS_INT1_EN = Bit2, // FIFO threshold interrupt routed to INT1
|
||||
FIFO_FULL_INT1_EN = Bit1,
|
||||
AGC_RDY_INT1_EN = Bit0,
|
||||
};
|
||||
|
||||
// REG_BANK_SEL
|
||||
enum REG_BANK_SEL_BIT : uint8_t {
|
||||
USER_BANK_0 = 0, // 0: Select USER BANK 0.
|
||||
USER_BANK_1 = Bit4, // 1: Select USER BANK 1.
|
||||
USER_BANK_2 = Bit5, // 2: Select USER BANK 2.
|
||||
USER_BANK_3 = Bit5 | Bit4, // 3: Select USER BANK 3.
|
||||
FIFO_THS_INT_CLEAR = Bit3, // 10: Clear on FIFO data 1Byte Read
|
||||
};
|
||||
|
||||
namespace FIFO
|
||||
{
|
||||
static constexpr size_t SIZE = 2048;
|
||||
static constexpr size_t SIZE = 1024;
|
||||
|
||||
// FIFO_DATA layout when FIFO_CONFIG1 has FIFO_GYRO_EN and FIFO_ACCEL_EN set
|
||||
|
||||
// Packet 3
|
||||
struct DATA {
|
||||
uint8_t FIFO_Header;
|
||||
uint8_t ACCEL_DATA_X1;
|
||||
uint8_t ACCEL_DATA_X0;
|
||||
uint8_t ACCEL_DATA_Y1;
|
||||
uint8_t ACCEL_DATA_Y0;
|
||||
uint8_t ACCEL_DATA_Z1;
|
||||
uint8_t ACCEL_DATA_Z0;
|
||||
uint8_t GYRO_DATA_X1;
|
||||
uint8_t GYRO_DATA_X0;
|
||||
uint8_t GYRO_DATA_Y1;
|
||||
uint8_t GYRO_DATA_Y0;
|
||||
uint8_t GYRO_DATA_Z1;
|
||||
uint8_t GYRO_DATA_Z0;
|
||||
uint8_t temperature; // Temperature[7:0]
|
||||
uint8_t timestamp_l;
|
||||
uint8_t timestamp_h;
|
||||
uint8_t ACCEL_DATA_X1; // Accel X [19:12]
|
||||
uint8_t ACCEL_DATA_X0; // Accel X [11:4]
|
||||
uint8_t ACCEL_DATA_Y1; // Accel Y [19:12]
|
||||
uint8_t ACCEL_DATA_Y0; // Accel Y [11:4]
|
||||
uint8_t ACCEL_DATA_Z1; // Accel Z [19:12]
|
||||
uint8_t ACCEL_DATA_Z0; // Accel Z [11:4]
|
||||
uint8_t GYRO_DATA_X1; // Gyro X [19:12]
|
||||
uint8_t GYRO_DATA_X0; // Gyro X [11:4]
|
||||
uint8_t GYRO_DATA_Y1; // Gyro Y [19:12]
|
||||
uint8_t GYRO_DATA_Y0; // Gyro Y [11:4]
|
||||
uint8_t GYRO_DATA_Z1; // Gyro Z [19:12]
|
||||
uint8_t GYRO_DATA_Z0; // Gyro Z [11:4]
|
||||
uint8_t temperature; // Temperature[7:0]
|
||||
uint8_t TimeStamp_h; // TimeStamp[15:8]
|
||||
uint8_t TimeStamp_l; // TimeStamp[7:0]
|
||||
};
|
||||
|
||||
// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
|
||||
enum FIFO_HEADER_BIT : uint8_t {
|
||||
HEADER_MSG = Bit7, // 1: FIFO is empty
|
||||
HEADER_ACCEL = Bit6,
|
||||
HEADER_GYRO = Bit5,
|
||||
HEADER_20 = Bit4,
|
||||
HEADER_ACCEL = Bit6, // 1: Packet is sized so that accel data have location in the packet, FIFO_ACCEL_EN must be 1
|
||||
HEADER_GYRO = Bit5, // 1: Packet is sized so that gyro data have location in the packet, FIFO_GYRO_EN must be1
|
||||
HEADER_20 = Bit4, // 1: Packet has a new and valid sample of extended 20-bit data for gyro and/or accel
|
||||
HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2,
|
||||
HEADER_ODR_ACCEL = Bit1,
|
||||
HEADER_ODR_GYRO = Bit0,
|
||||
HEADER_ODR_ACCEL = Bit1, // 1: The ODR for accel is different for this accel data packet compared to the previous accel packet
|
||||
HEADER_ODR_GYRO = Bit0, // 1: The ODR for gyro is different for this gyro data packet compared to the previous gyro packet
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user