fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

drivers/imu/invensense: icm42xxx minor cleanup and consistency changes

Browse Source
This commit is contained in:
Daniel Agar 2022-04-12 21:31:29 -04:00
parent ea10eacb99
commit fdf118d28c
No known key found for this signature in database
GPG Key ID: FD3CBA98017A69DE
15 changed files with 349 additions and 443 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/drivers/imu/invensense/icm40609d/CMakeLists.txt
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2020 PX4 Development Team. All rights reserved.
# Copyright (c) 2020-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
@ -34,6 +34,7 @@ px4_add_module(
MODULE drivers__imu__invensense__icm40609d
MAIN icm40609d
COMPILE_FLAGS
${MAX_CUSTOM_OPT_LEVEL}
SRCS
icm40609d_main.cpp
ICM40609D.cpp

146
src/drivers/imu/invensense/icm40609d/ICM40609D.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-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
@ -146,7 +146,8 @@ void ICM40609D::RunImpl()
case STATE::WAIT_FOR_RESET:
if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI)
&& (RegisterRead(Register::BANK_0::DEVICE_CONFIG) == 0x00)) {
&& (RegisterRead(Register::BANK_0::DEVICE_CONFIG) == 0x00)
&& (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) {
// Wakeup accel and gyro and schedule remaining configuration
RegisterWrite(Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE);
@ -273,8 +274,8 @@ void ICM40609D::RunImpl()
}
}
// check configuration registers periodically or immediately following any failure
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])
) {
_last_config_check_timestamp = now;
@ -285,13 +286,6 @@ void ICM40609D::RunImpl()
perf_count(_bad_register_perf);
Reset();
}
} else {
// periodically update temperature (~1 Hz)
if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
UpdateTemperature();
_temperature_update_timestamp = now;
}
}
}
@ -299,65 +293,6 @@ void ICM40609D::RunImpl()
}
}
void ICM40609D::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_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;
case ACCEL_FS_SEL_32G:
_px4_accel.set_scale(CONSTANTS_ONE_G / 1024.f);
_px4_accel.set_range(32.f * CONSTANTS_ONE_G);
break;
}
}
void ICM40609D::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_125_DPS:
range_dps = 125.f;
break;
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 ICM40609D::ConfigureSampleRate(int sample_rate)
{
// round down to nearest FIFO sample dt
@ -418,8 +353,11 @@ bool ICM40609D::Configure()
}
}
ConfigureAccel();
ConfigureGyro();
_px4_accel.set_range(32.f * CONSTANTS_ONE_G);
_px4_accel.set_scale(CONSTANTS_ONE_G / 1024.f);
_px4_gyro.set_range(math::radians(2000.f));
_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
return success;
}
@ -584,9 +522,11 @@ bool ICM40609D::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
}
if (valid_samples > 0) {
ProcessGyro(timestamp_sample, buffer.f, valid_samples);
ProcessAccel(timestamp_sample, buffer.f, valid_samples);
return true;
if (ProcessTemperature(buffer.f, valid_samples)) {
ProcessGyro(timestamp_sample, buffer.f, valid_samples);
ProcessAccel(timestamp_sample, buffer.f, valid_samples);
return true;
}
}
return false;
@ -607,7 +547,7 @@ void ICM40609D::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DA
{
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++) {
@ -617,18 +557,15 @@ void ICM40609D::ProcessAccel(const hrt_abstime &timestamp_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 ICM40609D::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
@ -639,15 +576,15 @@ void ICM40609D::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DAT
gyro.dt = FIFO_SAMPLE_DT;
for (int i = 0; i < samples; i++) {
const int16_t gyro_x = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0);
const int16_t gyro_y = combine(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0);
const int16_t gyro_z = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0);
int16_t gyro_x = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0);
int16_t gyro_y = combine(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0);
int16_t gyro_z = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0);
// 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) +
@ -656,25 +593,32 @@ void ICM40609D::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DAT
_px4_gyro.updateFIFO(gyro);
}
void ICM40609D::UpdateTemperature()
bool ICM40609D::ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples)
{
// read current temperature
uint8_t temperature_buf[3] {};
temperature_buf[0] = static_cast<uint8_t>(Register::BANK_0::TEMP_DATA1) | DIR_READ;
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
float temperature_sum{0};
int valid_samples = 0;
if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
perf_count(_bad_transfer_perf);
return;
for (int i = 0; i < samples; i++) {
// Temperature data stored in FIFO is an 8-bit quantity, FIFO_TEMP_DATA
// It can be converted to degrees centigrade by using the following formula:
// Temperature in Degrees Centigrade = (FIFO_TEMP_DATA / 2.07) + 25
temperature_sum += (fifo[i].FIFO_TEMP_DATA / 2.07f) + 25.f;
valid_samples++;
}
const int16_t TEMP_DATA = combine(temperature_buf[1], temperature_buf[2]);
if (valid_samples > 0) {
// use average temperature reading
const float temperature_avg = temperature_sum / valid_samples;
// Temperature in Degrees Centigrade
const float TEMP_degC = (TEMP_DATA / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
if (PX4_ISFINITE(temperature_avg)) {
_px4_accel.set_temperature(temperature_avg);
_px4_gyro.set_temperature(temperature_avg);
return true;
if (PX4_ISFINITE(TEMP_degC)) {
_px4_accel.set_temperature(TEMP_degC);
_px4_gyro.set_temperature(TEMP_degC);
} else {
perf_count(_bad_transfer_perf);
}
}
return false;
}

37
src/drivers/imu/invensense/icm40609d/ICM40609D.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-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
@ -70,12 +70,12 @@ private:
void exit_and_cleanup() override;
// Sensor Configuration
static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f}; // 8000 Hz accel & gyro ODR configured
static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f}; // 8000 Hz accel & gyro ODR configured (must match GYRO_CONFIG0/ACCEL_CONFIG0)
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 int32_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 {
@ -99,8 +99,6 @@ private:
bool Reset();
bool Configure();
void ConfigureAccel();
void ConfigureGyro();
void ConfigureSampleRate(int sample_rate);
void ConfigureFIFOWatermark(uint8_t samples);
@ -125,7 +123,7 @@ private:
void 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();
bool ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples);
const spi_drdy_gpio_t _drdy_gpio;
@ -160,18 +158,21 @@ private:
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};
static constexpr uint8_t size_register_bank0_cfg{13};
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_CONFIG, FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 },
{ 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_8kHz, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 },
{ Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_ODR_8kHz, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 },
{ Register::BANK_0::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, FIFO_CONFIG1_BIT::FIFO_TEMP_EN },
{ 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_CONFIG0, INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ, 0 },
{ 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::FIFO_CONFIG, FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 },
{ 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_8KHZ_SET, GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS_CLEAR | GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_CLEAR },
{ Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_SET, ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_32G_CLEAR | ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_CLEAR },
{ Register::BANK_0::GYRO_CONFIG1, 0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_ORD_1ST_CLEAR },
{ Register::BANK_0::ACCEL_CONFIG1, 0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_ORD_1ST_CLEAR },
{ Register::BANK_0::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_RESUME_PARTIAL_RD | FIFO_CONFIG1_BIT::FIFO_TEMP_EN | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, 0 },
{ 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_CONFIG0, INT_CONFIG0_BIT::FIFO_THS_INT_CLEAR, 0 },
{ Register::BANK_0::INT_CONFIG1, INT_CONFIG1_BIT::INT_TPULSE_DURATION | INT_CONFIG1_BIT::INT_TDEASSERT_DISABLE, INT_CONFIG1_BIT::INT_ASYNC_RESET },
{ Register::BANK_0::INT_SOURCE0, INT_SOURCE0_BIT::FIFO_THS_INT1_EN, INT_SOURCE0_BIT::RESET_DONE_INT1_EN },
};
};

126
src/drivers/imu/invensense/icm40609d/InvenSense_ICM40609D_registers.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-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,36 +66,38 @@ namespace Register
{
enum class BANK_0 : uint8_t {
DEVICE_CONFIG = 0x11,
DEVICE_CONFIG = 0x11,
INT_CONFIG = 0x14,
INT_CONFIG = 0x14,
FIFO_CONFIG = 0x16,
FIFO_CONFIG = 0x16,
TEMP_DATA1 = 0x1D,
TEMP_DATA0 = 0x1E,
TEMP_DATA1 = 0x1D,
TEMP_DATA0 = 0x1E,
INT_STATUS = 0x2D,
FIFO_COUNTH = 0x2E,
FIFO_COUNTL = 0x2F,
FIFO_DATA = 0x30,
INT_STATUS = 0x2D,
FIFO_COUNTH = 0x2E,
FIFO_COUNTL = 0x2F,
FIFO_DATA = 0x30,
SIGNAL_PATH_RESET = 0x4B,
SIGNAL_PATH_RESET = 0x4B,
PWR_MGMT0 = 0x4E,
GYRO_CONFIG0 = 0x4F,
ACCEL_CONFIG0 = 0x50,
PWR_MGMT0 = 0x4E,
GYRO_CONFIG0 = 0x4F,
ACCEL_CONFIG0 = 0x50,
GYRO_CONFIG1 = 0x51,
ACCEL_CONFIG1 = 0x53,
FIFO_CONFIG1 = 0x5F,
FIFO_CONFIG2 = 0x60,
FIFO_CONFIG3 = 0x61,
FIFO_CONFIG1 = 0x5F,
FIFO_CONFIG2 = 0x60,
FIFO_CONFIG3 = 0x61,
INT_CONFIG0 = 0x63,
INT_CONFIG0 = 0x63,
INT_CONFIG1 = 0x64,
INT_SOURCE0 = 0x65,
INT_SOURCE0 = 0x65,
WHO_AM_I = 0x75,
REG_BANK_SEL = 0x76,
WHO_AM_I = 0x75,
REG_BANK_SEL = 0x76,
};
};
@ -104,7 +106,7 @@ enum class BANK_0 : uint8_t {
// DEVICE_CONFIG
enum DEVICE_CONFIG_BIT : uint8_t {
SOFT_RESET_CONFIG = Bit0, //
SOFT_RESET_CONFIG = Bit0,
};
// INT_CONFIG
@ -122,14 +124,15 @@ enum FIFO_CONFIG_BIT : uint8_t {
// INT_STATUS
enum INT_STATUS_BIT : uint8_t {
DATA_RDY_INT = Bit3,
RESET_DONE_INT = Bit4,
FIFO_THS_INT = Bit2,
FIFO_FULL_INT = Bit1,
};
// SIGNAL_PATH_RESET
enum SIGNAL_PATH_RESET_BIT : uint8_t {
FIFO_FLUSH = Bit1,
FIFO_FLUSH = Bit1,
};
// PWR_MGMT0
@ -141,42 +144,52 @@ enum PWR_MGMT0_BIT : uint8_t {
// GYRO_CONFIG0
enum GYRO_CONFIG0_BIT : uint8_t {
// 7:5 GYRO_FS_SEL
GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000dps (default)
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
GYRO_FS_SEL_125_DPS = Bit7, // 0b100 = ±125 dps
// 0b000: ±2000dps (default)
GYRO_FS_SEL_2000_DPS_CLEAR = Bit7 | Bit6 | Bit5,
// 3:0 GYRO_ODR
GYRO_ODR_32kHz = Bit0, // 0001: 32kHz
GYRO_ODR_16kHz = Bit1, // 0010: 16kHz
GYRO_ODR_8kHz = Bit1 | Bit0, // 0011: 8kHz
GYRO_ODR_4kHz = Bit2, // 0100: 4kHz
GYRO_ODR_2kHz = Bit2 | Bit0, // 0101: 2kHz
GYRO_ODR_1kHz = Bit2 | Bit1, // 0110: 1kHz (default)
// 0b0001: 32kHz (maximum)
GYRO_ODR_32KHZ_SET = Bit0,
GYRO_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
// 0b0011: 8kHz
GYRO_ODR_8KHZ_SET = Bit1 | Bit0,
GYRO_ODR_8KHZ_CLEAR = Bit3 | Bit2,
// 0b0110: 1kHz (default)
GYRO_ODR_1KHZ_SET = Bit2 | Bit1,
GYRO_ODR_1KHZ_CLEAR = Bit3 | Bit0,
};
// ACCEL_CONFIG0
enum ACCEL_CONFIG0_BIT : uint8_t {
// 7:5 ACCEL_FS_SEL
ACCEL_FS_SEL_32G = 0, // 000: ±32g (default)
ACCEL_FS_SEL_16G = Bit5, // 001: ±16g
ACCEL_FS_SEL_8G = Bit6, // 010: ±8g
ACCEL_FS_SEL_4G = Bit6 | Bit5, // 011: ±4g
// 0b000: ±32g (default)
ACCEL_FS_SEL_32G_CLEAR = Bit7 | Bit6 | Bit5,
// 3:0 ACCEL_ODR
ACCEL_ODR_32kHz = Bit0, // 0001: 32kHz
ACCEL_ODR_16kHz = Bit1, // 0010: 16kHz
ACCEL_ODR_8kHz = Bit1 | Bit0, // 0011: 8kHz
ACCEL_ODR_4kHz = Bit2, // 0100: 4kHz
ACCEL_ODR_2kHz = Bit2 | Bit0, // 0101: 2kHz
ACCEL_ODR_1kHz = Bit2 | Bit1, // 0110: 1kHz (default)
// 0b0001: 32kHz (maximum)
ACCEL_ODR_32KHZ_SET = Bit0,
ACCEL_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
// 0b0011: 8kHz
ACCEL_ODR_8KHZ_SET = Bit1 | Bit0,
ACCEL_ODR_8KHZ_CLEAR = Bit3 | Bit2,
// 0b0110: 1kHz (default)
ACCEL_ODR_1KHZ_SET = Bit2 | Bit1,
ACCEL_ODR_1KHZ_CLEAR = Bit3 | Bit0,
};
// GYRO_CONFIG1
enum GYRO_CONFIG1_BIT : uint8_t {
GYRO_UI_FILT_ORD_1ST_CLEAR = Bit3 | Bit2, // 00: 1st Order UI filter
};
// ACCEL_CONFIG1
enum ACCEL_CONFIG1_BIT : uint8_t {
ACCEL_UI_FILT_ORD_1ST_CLEAR = Bit4 | Bit3, // 00: 1st Order UI filter
};
// FIFO_CONFIG1
enum FIFO_CONFIG1_BIT : uint8_t {
FIFO_RESUME_PARTIAL_RD = Bit6,
FIFO_WM_GT_TH = Bit5,
FIFO_TEMP_EN = Bit2,
FIFO_GYRO_EN = Bit1,
@ -186,14 +199,21 @@ enum FIFO_CONFIG1_BIT : uint8_t {
// INT_CONFIG0
enum INT_CONFIG0_BIT : uint8_t {
// 3:2 FIFO_THS_INT_CLEAR
CLEAR_ON_FIFO_READ = Bit3,
FIFO_THS_INT_CLEAR = Bit3, // 10: Clear on FIFO data 1Byte Read
};
// INT_CONFIG1
enum INT_CONFIG1_BIT : uint8_t {
INT_TPULSE_DURATION = Bit6, // 1: Interrupt pulse duration is 8 µs. Required if ODR ≥ 4kHz, optional for ODR < 4kHz.
INT_TDEASSERT_DISABLE = Bit5, // 1: Disables de-assert duration. Required if ODR ≥ 4kHz, optional for ODR < 4kHz.
INT_ASYNC_RESET = Bit4, // User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation
};
// INT_SOURCE0
enum INT_SOURCE0_BIT : uint8_t {
UI_DRDY_INT1_EN = Bit3,
RESET_DONE_INT1_EN = Bit4, // 1: Reset done interrupt routed to INT1 (enabled by default)
FIFO_THS_INT1_EN = Bit2, // FIFO threshold interrupt routed to INT1
FIFO_FULL_INT1_EN = Bit1,
};
// REG_BANK_SEL
@ -225,9 +245,9 @@ struct DATA {
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 FIFO_TEMP_DATA; // 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

8
src/drivers/imu/invensense/icm40609d/icm40609d_main.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020, 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-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
@ -72,13 +72,11 @@ extern "C" int icm40609d_main(int argc, char *argv[])
if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator);
}
if (!strcmp(verb, "stop")) {
} else if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
if (!strcmp(verb, "status")) {
} else if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}

3
src/drivers/imu/invensense/icm42605/CMakeLists.txt
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2020 PX4 Development Team. All rights reserved.
# Copyright (c) 2020-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
@ -34,6 +34,7 @@ px4_add_module(
MODULE drivers__imu__invensense__icm42605
MAIN icm42605
COMPILE_FLAGS
${MAX_CUSTOM_OPT_LEVEL}
SRCS
icm42605_main.cpp
ICM42605.cpp

143
src/drivers/imu/invensense/icm42605/ICM42605.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-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
@ -274,8 +274,8 @@ void ICM42605::RunImpl()
}
}
// check configuration registers periodically or immediately following any failure
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])
) {
_last_config_check_timestamp = now;
@ -286,13 +286,6 @@ void ICM42605::RunImpl()
perf_count(_bad_register_perf);
Reset();
}
} else {
// periodically update temperature (~1 Hz)
if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
UpdateTemperature();
_temperature_update_timestamp = now;
}
}
}
@ -300,65 +293,6 @@ void ICM42605::RunImpl()
}
}
void ICM42605::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 ICM42605::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_125_DPS:
range_dps = 125.f;
break;
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 ICM42605::ConfigureSampleRate(int sample_rate)
{
// round down to nearest FIFO sample dt
@ -419,8 +353,11 @@ bool ICM42605::Configure()
}
}
ConfigureAccel();
ConfigureGyro();
_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
_px4_gyro.set_range(math::radians(2000.f));
_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
return success;
}
@ -585,9 +522,11 @@ bool ICM42605::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
}
if (valid_samples > 0) {
ProcessGyro(timestamp_sample, buffer.f, valid_samples);
ProcessAccel(timestamp_sample, buffer.f, valid_samples);
return true;
if (ProcessTemperature(buffer.f, valid_samples)) {
ProcessGyro(timestamp_sample, buffer.f, valid_samples);
ProcessAccel(timestamp_sample, buffer.f, valid_samples);
return true;
}
}
return false;
@ -608,7 +547,7 @@ void ICM42605::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DAT
{
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++) {
@ -618,18 +557,15 @@ void ICM42605::ProcessAccel(const hrt_abstime &timestamp_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)
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 ICM42605::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
@ -640,15 +576,15 @@ void ICM42605::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA
gyro.dt = FIFO_SAMPLE_DT;
for (int i = 0; i < samples; i++) {
const int16_t gyro_x = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0);
const int16_t gyro_y = combine(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0);
const int16_t gyro_z = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0);
int16_t gyro_x = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0);
int16_t gyro_y = combine(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0);
int16_t gyro_z = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0);
// 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) +
@ -657,25 +593,32 @@ void ICM42605::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA
_px4_gyro.updateFIFO(gyro);
}
void ICM42605::UpdateTemperature()
bool ICM42605::ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples)
{
// read current temperature
uint8_t temperature_buf[3] {};
temperature_buf[0] = static_cast<uint8_t>(Register::BANK_0::TEMP_DATA1) | DIR_READ;
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
float temperature_sum{0};
int valid_samples = 0;
if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
perf_count(_bad_transfer_perf);
return;
for (int i = 0; i < samples; i++) {
// Temperature data stored in FIFO is an 8-bit quantity, FIFO_TEMP_DATA
// It can be converted to degrees centigrade by using the following formula:
// Temperature in Degrees Centigrade = (FIFO_TEMP_DATA / 2.07) + 25
temperature_sum += (fifo[i].FIFO_TEMP_DATA / 2.07f) + 25.f;
valid_samples++;
}
const int16_t TEMP_DATA = combine(temperature_buf[1], temperature_buf[2]);
if (valid_samples > 0) {
// use average temperature reading
const float temperature_avg = temperature_sum / valid_samples;
// Temperature in Degrees Centigrade
const float TEMP_degC = (TEMP_DATA / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
if (PX4_ISFINITE(temperature_avg)) {
_px4_accel.set_temperature(temperature_avg);
_px4_gyro.set_temperature(temperature_avg);
return true;
if (PX4_ISFINITE(TEMP_degC)) {
_px4_accel.set_temperature(TEMP_degC);
_px4_gyro.set_temperature(TEMP_degC);
} else {
perf_count(_bad_transfer_perf);
}
}
return false;
}

35
src/drivers/imu/invensense/icm42605/ICM42605.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-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
@ -70,7 +70,7 @@ private:
void exit_and_cleanup() override;
// Sensor Configuration
static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f}; // 8000 Hz accel & gyro ODR configured
static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f}; // 8000 Hz accel & gyro ODR configured (must match GYRO_CONFIG0/ACCEL_CONFIG0)
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
@ -99,8 +99,6 @@ private:
bool Reset();
bool Configure();
void ConfigureAccel();
void ConfigureGyro();
void ConfigureSampleRate(int sample_rate);
void ConfigureFIFOWatermark(uint8_t samples);
@ -125,7 +123,7 @@ private:
void 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();
bool ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples);
const spi_drdy_gpio_t _drdy_gpio;
@ -160,18 +158,21 @@ private:
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};
static constexpr uint8_t size_register_bank0_cfg{13};
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_CONFIG, FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 },
{ 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_8kHz, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 },
{ Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_ODR_8kHz, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 },
{ Register::BANK_0::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, FIFO_CONFIG1_BIT::FIFO_TEMP_EN },
{ 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_CONFIG0, INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ, 0 },
{ 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::FIFO_CONFIG, FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 },
{ 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_8KHZ_SET, GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS_CLEAR | GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_CLEAR },
{ Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_SET, ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_16G_CLEAR | ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_CLEAR },
{ Register::BANK_0::GYRO_CONFIG1, 0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_ORD_1ST_CLEAR },
{ Register::BANK_0::ACCEL_CONFIG1, 0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_ORD_1ST_CLEAR },
{ Register::BANK_0::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_RESUME_PARTIAL_RD | FIFO_CONFIG1_BIT::FIFO_TEMP_EN | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, 0 },
{ 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_CONFIG0, INT_CONFIG0_BIT::FIFO_THS_INT_CLEAR, 0 },
{ Register::BANK_0::INT_CONFIG1, INT_CONFIG1_BIT::INT_TPULSE_DURATION | INT_CONFIG1_BIT::INT_TDEASSERT_DISABLE, INT_CONFIG1_BIT::INT_ASYNC_RESET },
{ Register::BANK_0::INT_SOURCE0, INT_SOURCE0_BIT::FIFO_THS_INT1_EN, INT_SOURCE0_BIT::RESET_DONE_INT1_EN },
};
};

119
src/drivers/imu/invensense/icm42605/InvenSense_ICM42605_registers.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-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,36 +66,38 @@ namespace Register
{
enum class BANK_0 : uint8_t {
DEVICE_CONFIG = 0x11,
DEVICE_CONFIG = 0x11,
INT_CONFIG = 0x14,
INT_CONFIG = 0x14,
FIFO_CONFIG = 0x16,
FIFO_CONFIG = 0x16,
TEMP_DATA1 = 0x1D,
TEMP_DATA0 = 0x1E,
TEMP_DATA1 = 0x1D,
TEMP_DATA0 = 0x1E,
INT_STATUS = 0x2D,
FIFO_COUNTH = 0x2E,
FIFO_COUNTL = 0x2F,
FIFO_DATA = 0x30,
INT_STATUS = 0x2D,
FIFO_COUNTH = 0x2E,
FIFO_COUNTL = 0x2F,
FIFO_DATA = 0x30,
SIGNAL_PATH_RESET = 0x4B,
SIGNAL_PATH_RESET = 0x4B,
PWR_MGMT0 = 0x4E,
GYRO_CONFIG0 = 0x4F,
ACCEL_CONFIG0 = 0x50,
PWR_MGMT0 = 0x4E,
GYRO_CONFIG0 = 0x4F,
ACCEL_CONFIG0 = 0x50,
GYRO_CONFIG1 = 0x51,
ACCEL_CONFIG1 = 0x53,
FIFO_CONFIG1 = 0x5F,
FIFO_CONFIG2 = 0x60,
FIFO_CONFIG3 = 0x61,
FIFO_CONFIG1 = 0x5F,
FIFO_CONFIG2 = 0x60,
FIFO_CONFIG3 = 0x61,
INT_CONFIG0 = 0x63,
INT_CONFIG1 = 0x64,
INT_SOURCE0 = 0x65,
INT_CONFIG0 = 0x63,
INT_CONFIG1 = 0x64,
INT_SOURCE0 = 0x65,
WHO_AM_I = 0x75,
REG_BANK_SEL = 0x76,
WHO_AM_I = 0x75,
REG_BANK_SEL = 0x76,
};
};
@ -104,7 +106,7 @@ enum class BANK_0 : uint8_t {
// DEVICE_CONFIG
enum DEVICE_CONFIG_BIT : uint8_t {
SOFT_RESET_CONFIG = Bit0, //
SOFT_RESET_CONFIG = Bit0,
};
// INT_CONFIG
@ -123,15 +125,14 @@ enum FIFO_CONFIG_BIT : uint8_t {
// INT_STATUS
enum INT_STATUS_BIT : uint8_t {
RESET_DONE_INT = Bit4,
DATA_RDY_INT = Bit3,
FIFO_THS_INT = Bit2,
FIFO_FULL_INT = Bit1,
};
// SIGNAL_PATH_RESET
enum SIGNAL_PATH_RESET_BIT : uint8_t {
ABORT_AND_RESET = Bit3,
FIFO_FLUSH = Bit1,
FIFO_FLUSH = Bit1,
};
// PWR_MGMT0
@ -143,38 +144,46 @@ enum PWR_MGMT0_BIT : uint8_t {
// GYRO_CONFIG0
enum GYRO_CONFIG0_BIT : uint8_t {
// 7:5 GYRO_FS_SEL
GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000dps (default)
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
GYRO_FS_SEL_125_DPS = Bit7, // 0b100 = ±125 dps
// 0b000: ±2000dps (default)
GYRO_FS_SEL_2000_DPS_CLEAR = Bit7 | Bit6 | Bit5,
// 3:0 GYRO_ODR
GYRO_ODR_8kHz = Bit1 | Bit0, // 0011: 8kHz
GYRO_ODR_4kHz = Bit2, // 0100: 4kHz
GYRO_ODR_2kHz = Bit2 | Bit0, // 0101: 2kHz
GYRO_ODR_1kHz = Bit2 | Bit1, // 0110: 1kHz (default)
// 0b0011: 8kHz (maximum)
GYRO_ODR_8KHZ_SET = Bit1 | Bit0,
GYRO_ODR_8KHZ_CLEAR = Bit3 | Bit2,
// 0b0110: 1kHz (default)
GYRO_ODR_1KHZ_SET = Bit2 | Bit1,
GYRO_ODR_1KHZ_CLEAR = Bit3 | Bit0,
};
// ACCEL_CONFIG0
enum ACCEL_CONFIG0_BIT : uint8_t {
// 7:5 ACCEL_FS_SEL
ACCEL_FS_SEL_16G = 0, // 000: ±16g (default)
ACCEL_FS_SEL_8G = Bit5, // 001: ±8g
ACCEL_FS_SEL_4G = Bit6, // 010: ±4g
ACCEL_FS_SEL_2G = Bit6 | Bit5, // 011: ±2g
// 0b000: ±16g (default)
ACCEL_FS_SEL_16G_CLEAR = Bit7 | Bit6 | Bit5,
// 3:0 ACCEL_ODR
ACCEL_ODR_8kHz = Bit1 | Bit0, // 0011: 8kHz
ACCEL_ODR_4kHz = Bit2, // 0100: 4kHz
ACCEL_ODR_2kHz = Bit2 | Bit0, // 0101: 2kHz
ACCEL_ODR_1kHz = Bit2 | Bit1, // 0110: 1kHz (default)
// 0b0011: 8kHz (maximum)
ACCEL_ODR_8KHZ_SET = Bit1 | Bit0,
ACCEL_ODR_8KHZ_CLEAR = Bit3 | Bit2,
// 0b0110: 1kHz (default)
ACCEL_ODR_1KHZ_SET = Bit2 | Bit1,
ACCEL_ODR_1KHZ_CLEAR = Bit3 | Bit0,
};
// GYRO_CONFIG1
enum GYRO_CONFIG1_BIT : uint8_t {
GYRO_UI_FILT_ORD_1ST_CLEAR = Bit3 | Bit2, // 00: 1st Order UI filter
};
// ACCEL_CONFIG1
enum ACCEL_CONFIG1_BIT : uint8_t {
ACCEL_UI_FILT_ORD_1ST_CLEAR = Bit4 | Bit3, // 00: 1st Order UI filter
};
// FIFO_CONFIG1
enum FIFO_CONFIG1_BIT : uint8_t {
FIFO_RESUME_PARTIAL_RD = Bit6,
FIFO_WM_GT_TH = Bit5,
FIFO_TEMP_EN = Bit2,
FIFO_GYRO_EN = Bit1,
@ -184,23 +193,21 @@ enum FIFO_CONFIG1_BIT : uint8_t {
// INT_CONFIG0
enum INT_CONFIG0_BIT : uint8_t {
// 3:2 FIFO_THS_INT_CLEAR
CLEAR_ON_FIFO_READ = Bit3,
FIFO_THS_INT_CLEAR = Bit3, // 10: Clear on FIFO data 1Byte Read
};
// INT_CONFIG1
enum INT_CONFIG1_BIT : uint8_t {
INT_TPULSE_DURATION = Bit6, // 0: Interrupt pulse duration is 100μs
INT_TPULSE_DURATION = Bit6, // 1: Interrupt pulse duration is 8 µs. Required if ODR ≥ 4kHz, optional for ODR < 4kHz.
INT_TDEASSERT_DISABLE = Bit5, // 1: Disables de-assert duration. Required if ODR ≥ 4kHz, optional for ODR < 4kHz.
INT_ASYNC_RESET = Bit4, // User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation
};
// INT_SOURCE0
enum INT_SOURCE0_BIT : uint8_t {
UI_FSYNC_INT1_EN = Bit6,
PLL_RDY_INT1_EN = Bit5,
RESET_DONE_INT1_EN = Bit4,
UI_DRDY_INT1_EN = Bit3,
RESET_DONE_INT1_EN = Bit4, // 1: Reset done interrupt routed to INT1 (enabled by default)
FIFO_THS_INT1_EN = Bit2, // FIFO threshold interrupt routed to INT1
FIFO_FULL_INT1_EN = Bit1,
UI_AGC_RDY_INT1_EN = Bit0,
};
// REG_BANK_SEL
@ -232,9 +239,9 @@ struct DATA {
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 FIFO_TEMP_DATA; // 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
@ -242,7 +249,7 @@ enum FIFO_HEADER_BIT : uint8_t {
HEADER_MSG = Bit7, // 1: FIFO is empty
HEADER_ACCEL = Bit6,
HEADER_GYRO = Bit5,
HEADER_20 = Bit4,
HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2,
HEADER_ODR_ACCEL = Bit1,
HEADER_ODR_GYRO = Bit0,

8
src/drivers/imu/invensense/icm42605/icm42605_main.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020, 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-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
@ -72,13 +72,11 @@ extern "C" int icm42605_main(int argc, char *argv[])
if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator);
}
if (!strcmp(verb, "stop")) {
} else if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
if (!strcmp(verb, "status")) {
} else if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}

2
src/drivers/imu/invensense/icm42688p/CMakeLists.txt
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2020 PX4 Development Team. All rights reserved.
# Copyright (c) 2020-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

39
src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-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
@ -275,19 +275,21 @@ void ICM42688P::RunImpl()
}
// check configuration registers periodically or immediately following any failure
if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
&& RegisterCheck(_register_bank1_cfg[_checked_register_bank1])
&& RegisterCheck(_register_bank2_cfg[_checked_register_bank2])
) {
_last_config_check_timestamp = now;
_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
_checked_register_bank1 = (_checked_register_bank1 + 1) % size_register_bank1_cfg;
_checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg;
if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
&& RegisterCheck(_register_bank1_cfg[_checked_register_bank1])
&& RegisterCheck(_register_bank2_cfg[_checked_register_bank2])
) {
_last_config_check_timestamp = now;
_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
_checked_register_bank1 = (_checked_register_bank1 + 1) % size_register_bank1_cfg;
_checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg;
} else {
// register check failed, force reset
perf_count(_bad_register_perf);
Reset();
} else {
// register check failed, force reset
perf_count(_bad_register_perf);
Reset();
}
}
}
@ -378,7 +380,10 @@ bool ICM42688P::Configure()
// 20-bits data format used
// the only FSR settings that are operational are ±2000dps for gyroscope and ±16g for accelerometer
_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
_px4_gyro.set_range(math::radians(2000.f));
_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
return success;
}
@ -668,8 +673,8 @@ void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_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[i] = accel.x[i];
accel.y[i] = (accel.y[i] == INT16_MIN) ? INT16_MAX : -accel.y[i];
accel.z[i] = (accel.z[i] == INT16_MIN) ? INT16_MAX : -accel.z[i];
accel.y[i] = math::negate(accel.y[i]);
accel.z[i] = math::negate(accel.z[i]);
}
_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
@ -740,8 +745,8 @@ void ICM42688P::ProcessGyro(const hrt_abstime &timestamp_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[i];
gyro.y[i] = (gyro.y[i] == INT16_MIN) ? INT16_MAX : -gyro.y[i];
gyro.z[i] = (gyro.z[i] == INT16_MIN) ? INT16_MAX : -gyro.z[i];
gyro.y[i] = math::negate(gyro.y[i]);
gyro.z[i] = math::negate(gyro.z[i]);
}
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +

21
src/drivers/imu/invensense/icm42688p/ICM42688P.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-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
@ -70,7 +70,7 @@ private:
void exit_and_cleanup() override;
// Sensor Configuration
static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f}; // 8000 Hz accel & gyro ODR configured
static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f}; // 8000 Hz accel & gyro ODR configured (must match GYRO_CONFIG0/ACCEL_CONFIG0)
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
@ -172,22 +172,23 @@ private:
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{13};
static constexpr uint8_t size_register_bank0_cfg{14};
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_CONFIG, FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 },
{ 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 | GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_SET, GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_CLEAR },
{ Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_16G | ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_SET, ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_CLEAR },
{ Register::BANK_0::GYRO_CONFIG1, 0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_ORD },
{ Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_SET, GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS_CLEAR | GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_CLEAR },
{ Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_SET, ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_16G_CLEAR | ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_CLEAR },
{ Register::BANK_0::GYRO_CONFIG1, 0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_ORD_1ST_CLEAR },
{ Register::BANK_0::GYRO_ACCEL_CONFIG0, 0, GYRO_ACCEL_CONFIG0_BIT::ACCEL_UI_FILT_BW | GYRO_ACCEL_CONFIG0_BIT::GYRO_UI_FILT_BW },
{ Register::BANK_0::ACCEL_CONFIG1, 0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_ORD },
{ Register::BANK_0::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_HIRES_EN | FIFO_CONFIG1_BIT::FIFO_TEMP_EN | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, 0 },
{ Register::BANK_0::ACCEL_CONFIG1, 0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_ORD_1ST_CLEAR },
{ Register::BANK_0::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_RESUME_PARTIAL_RD | FIFO_CONFIG1_BIT::FIFO_HIRES_EN | FIFO_CONFIG1_BIT::FIFO_TEMP_EN | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, 0 },
{ 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_CONFIG0, INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ, 0 },
{ Register::BANK_0::INT_SOURCE0, INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 },
{ Register::BANK_0::INT_CONFIG0, INT_CONFIG0_BIT::FIFO_THS_INT_CLEAR, 0 },
{ Register::BANK_0::INT_CONFIG1, INT_CONFIG1_BIT::INT_TPULSE_DURATION | INT_CONFIG1_BIT::INT_TDEASSERT_DISABLE, INT_CONFIG1_BIT::INT_ASYNC_RESET },
{ Register::BANK_0::INT_SOURCE0, INT_SOURCE0_BIT::FIFO_THS_INT1_EN, INT_SOURCE0_BIT::RESET_DONE_INT1_EN },
};
uint8_t _checked_register_bank1{0};

94
src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-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
@ -81,8 +81,7 @@ enum class BANK_0 : uint8_t {
FIFO_DATA = 0x30,
SIGNAL_PATH_RESET = 0x4B,
INTF_CONFIG0 = 0x4C,
INTF_CONFIG1 = 0x4D,
PWR_MGMT0 = 0x4E,
GYRO_CONFIG0 = 0x4F,
ACCEL_CONFIG0 = 0x50,
@ -95,7 +94,7 @@ enum class BANK_0 : uint8_t {
FIFO_CONFIG3 = 0x61,
INT_CONFIG0 = 0x63,
INT_CONFIG1 = 0x64,
INT_SOURCE0 = 0x65,
SELF_TEST_CONFIG = 0x70,
@ -120,7 +119,7 @@ enum class BANK_2 : uint8_t {
// DEVICE_CONFIG
enum DEVICE_CONFIG_BIT : uint8_t {
SOFT_RESET_CONFIG = Bit0, //
SOFT_RESET_CONFIG = Bit0,
};
// INT_CONFIG
@ -139,15 +138,14 @@ enum FIFO_CONFIG_BIT : uint8_t {
// INT_STATUS
enum INT_STATUS_BIT : uint8_t {
RESET_DONE_INT = Bit4,
DATA_RDY_INT = Bit3,
FIFO_THS_INT = Bit2,
FIFO_FULL_INT = Bit1,
};
// SIGNAL_PATH_RESET
enum SIGNAL_PATH_RESET_BIT : uint8_t {
ABORT_AND_RESET = Bit3,
FIFO_FLUSH = Bit1,
FIFO_FLUSH = Bit1,
};
// PWR_MGMT0
@ -159,55 +157,42 @@ enum PWR_MGMT0_BIT : uint8_t {
// GYRO_CONFIG0
enum GYRO_CONFIG0_BIT : uint8_t {
// 7:5 GYRO_FS_SEL
GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000dps (default)
GYRO_FS_SEL_1000_DPS = Bit5,
GYRO_FS_SEL_500_DPS = Bit6,
GYRO_FS_SEL_250_DPS = Bit6 | Bit5,
GYRO_FS_SEL_125_DPS = Bit7,
// 0b000: ±2000dps (default)
GYRO_FS_SEL_2000_DPS_CLEAR = Bit7 | Bit6 | Bit5,
// 3:0 GYRO_ODR
// 0001: 32kHz
GYRO_ODR_32KHZ_SET = Bit0,
GYRO_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
// 0010: 16kHz
GYRO_ODR_16KHZ_SET = Bit1,
GYRO_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
// 0011: 8kHz
GYRO_ODR_8KHZ_SET = Bit1 | Bit0,
GYRO_ODR_8KHZ_CLEAR = Bit3 | Bit2,
// 0110: 1kHz (default)
GYRO_ODR_1KHZ_SET = Bit2 | Bit1,
GYRO_ODR_1KHZ_CLEAR = Bit3 | Bit0,
// 0b0001: 32kHz (maximum)
GYRO_ODR_32KHZ_SET = Bit0,
GYRO_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
// 0b0011: 8kHz
GYRO_ODR_8KHZ_SET = Bit1 | Bit0,
GYRO_ODR_8KHZ_CLEAR = Bit3 | Bit2,
// 0b0110: 1kHz (default)
GYRO_ODR_1KHZ_SET = Bit2 | Bit1,
GYRO_ODR_1KHZ_CLEAR = Bit3 | Bit0,
};
// ACCEL_CONFIG0
enum ACCEL_CONFIG0_BIT : uint8_t {
// 7:5 ACCEL_FS_SEL
ACCEL_FS_SEL_16G = 0, // 000: ±16g (default)
ACCEL_FS_SEL_8G = Bit5,
ACCEL_FS_SEL_4G = Bit6,
ACCEL_FS_SEL_2G = Bit6 | Bit5,
// 0b000: ±16g (default)
ACCEL_FS_SEL_16G_CLEAR = Bit7 | Bit6 | Bit5,
// 3:0 ACCEL_ODR
// 0001: 32kHz
ACCEL_ODR_32KHZ_SET = Bit0,
ACCEL_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
// 0010: 16kHz
ACCEL_ODR_16KHZ_SET = Bit1,
ACCEL_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
// 0011: 8kHz
ACCEL_ODR_8KHZ_SET = Bit1 | Bit0,
ACCEL_ODR_8KHZ_CLEAR = Bit3 | Bit2,
// 0110: 1kHz (default)
ACCEL_ODR_1KHZ_SET = Bit2 | Bit1,
ACCEL_ODR_1KHZ_CLEAR = Bit3 | Bit0,
// 0b0001: 32kHz (maximum)
ACCEL_ODR_32KHZ_SET = Bit0,
ACCEL_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
// 0b0011: 8kHz
ACCEL_ODR_8KHZ_SET = Bit1 | Bit0,
ACCEL_ODR_8KHZ_CLEAR = Bit3 | Bit2,
// 0b0110: 1kHz (default)
ACCEL_ODR_1KHZ_SET = Bit2 | Bit1,
ACCEL_ODR_1KHZ_CLEAR = Bit3 | Bit0,
};
// GYRO_CONFIG1
enum GYRO_CONFIG1_BIT : uint8_t {
GYRO_UI_FILT_ORD = Bit3 | Bit2, // 00: 1st Order
GYRO_UI_FILT_ORD_1ST_CLEAR = Bit3 | Bit2, // 00: 1st Order UI filter
};
// GYRO_ACCEL_CONFIG0
@ -221,13 +206,12 @@ enum GYRO_ACCEL_CONFIG0_BIT : uint8_t {
// ACCEL_CONFIG1
enum ACCEL_CONFIG1_BIT : uint8_t {
ACCEL_UI_FILT_ORD = Bit4 | Bit3, // 00: 1st Order
ACCEL_UI_FILT_ORD_1ST_CLEAR = Bit4 | Bit3, // 00: 1st Order UI filter
};
// FIFO_CONFIG1
enum FIFO_CONFIG1_BIT : uint8_t {
FIFO_RESUME_PARTIAL_RD = Bit6,
FIFO_WM_GT_TH = Bit5,
FIFO_HIRES_EN = Bit4,
FIFO_TEMP_EN = Bit2,
FIFO_GYRO_EN = Bit1,
@ -237,18 +221,21 @@ enum FIFO_CONFIG1_BIT : uint8_t {
// INT_CONFIG0
enum INT_CONFIG0_BIT : uint8_t {
// 3:2 FIFO_THS_INT_CLEAR
CLEAR_ON_FIFO_READ = Bit3,
FIFO_THS_INT_CLEAR = Bit3, // 10: Clear on FIFO data 1Byte Read
};
// INT_CONFIG1
enum INT_CONFIG1_BIT : uint8_t {
INT_TPULSE_DURATION = Bit6, // 1: Interrupt pulse duration is 8 µs. Required if ODR ≥ 4kHz, optional for ODR < 4kHz.
INT_TDEASSERT_DISABLE = Bit5, // 1: Disables de-assert duration. Required if ODR ≥ 4kHz, optional for ODR < 4kHz.
INT_ASYNC_RESET = Bit4, // User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation
};
// INT_SOURCE0
enum INT_SOURCE0_BIT : uint8_t {
UI_FSYNC_INT1_EN = Bit6,
PLL_RDY_INT1_EN = Bit5,
RESET_DONE_INT1_EN = Bit4,
UI_DRDY_INT1_EN = Bit3,
RESET_DONE_INT1_EN = Bit4, // 1: Reset done interrupt routed to INT1 (enabled by default)
FIFO_THS_INT1_EN = Bit2, // FIFO threshold interrupt routed to INT1
FIFO_FULL_INT1_EN = Bit1,
UI_AGC_RDY_INT1_EN = Bit0,
};
// REG_BANK_SEL
@ -276,6 +263,7 @@ enum ACCEL_CONFIG_STATIC2_BIT : uint8_t {
ACCEL_AAF_DIS = Bit0,
};
namespace FIFO
{
static constexpr size_t SIZE = 2048;

8
src/drivers/imu/invensense/icm42688p/icm42688p_main.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020, 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-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
@ -72,13 +72,11 @@ extern "C" int icm42688p_main(int argc, char *argv[])
if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator);
}
if (!strcmp(verb, "stop")) {
} else if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
if (!strcmp(verb, "status")) {
} else if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}