Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 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
15 changed files with 349 additions and 443 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/invensense/icm40609d/CMakeLists.txt
+2 -1
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 # Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions # modification, are permitted provided that the following conditions
@@ -34,6 +34,7 @@ px4_add_module(
MODULE drivers__imu__invensense__icm40609d MODULE drivers__imu__invensense__icm40609d
MAIN icm40609d MAIN icm40609d
COMPILE_FLAGS COMPILE_FLAGS
${MAX_CUSTOM_OPT_LEVEL}
SRCS SRCS
icm40609d_main.cpp icm40609d_main.cpp
ICM40609D.cpp ICM40609D.cpp
src/drivers/imu/invensense/icm40609d/ICM40609D.cpp
+44 -100
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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -146,7 +146,8 @@ void ICM40609D::RunImpl()
case STATE::WAIT_FOR_RESET: case STATE::WAIT_FOR_RESET:
if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI) 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 // 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); 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()
} }
} }
if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
// check configuration registers periodically or immediately following any failure // check configuration registers periodically or immediately following any failure
if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0]) if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
) { ) {
_last_config_check_timestamp = now; _last_config_check_timestamp = now;
@@ -285,13 +286,6 @@ void ICM40609D::RunImpl()
perf_count(_bad_register_perf); perf_count(_bad_register_perf);
Reset(); 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) void ICM40609D::ConfigureSampleRate(int sample_rate)
{ {
// round down to nearest FIFO sample dt // round down to nearest FIFO sample dt
@@ -418,8 +353,11 @@ bool ICM40609D::Configure()
} }
} }
ConfigureAccel(); _px4_accel.set_range(32.f * CONSTANTS_ONE_G);
ConfigureGyro(); _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; return success;
} }
@@ -584,10 +522,12 @@ bool ICM40609D::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
} }
if (valid_samples > 0) { if (valid_samples > 0) {
if (ProcessTemperature(buffer.f, valid_samples)) {
ProcessGyro(timestamp_sample, buffer.f, valid_samples); ProcessGyro(timestamp_sample, buffer.f, valid_samples);
ProcessAccel(timestamp_sample, buffer.f, valid_samples); ProcessAccel(timestamp_sample, buffer.f, valid_samples);
return true; return true;
} }
}
return false; return false;
} }
@@ -607,7 +547,7 @@ void ICM40609D::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DA
{ {
sensor_accel_fifo_s accel{}; sensor_accel_fifo_s accel{};
accel.timestamp_sample = timestamp_sample; accel.timestamp_sample = timestamp_sample;
accel.samples = 0; accel.samples = samples;
accel.dt = FIFO_SAMPLE_DT; accel.dt = FIFO_SAMPLE_DT;
for (int i = 0; i < samples; i++) { for (int i = 0; i < samples; i++) {
@@ -617,19 +557,16 @@ void ICM40609D::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DA
// sensor's frame is +x forward, +y left, +z up // 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) // flip y & z to publish right handed with z down (x forward, y right, z down)
accel.x[accel.samples] = accel_x; accel.x[i] = accel_x;
accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; accel.y[i] = math::negate(accel_y);
accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; accel.z[i] = math::negate(accel_z);
accel.samples++;
} }
_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + _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)); 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) 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; gyro.dt = FIFO_SAMPLE_DT;
for (int i = 0; i < samples; i++) { for (int i = 0; i < samples; i++) {
const int16_t gyro_x = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0); 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); 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_z = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0);
// sensor's frame is +x forward, +y left, +z up // 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) // flip y & z to publish right handed with z down (x forward, y right, z down)
gyro.x[i] = gyro_x; gyro.x[i] = gyro_x;
gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; gyro.y[i] = math::negate(gyro_y);
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; gyro.z[i] = math::negate(gyro_z);
} }
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + _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); _px4_gyro.updateFIFO(gyro);
} }
void ICM40609D::UpdateTemperature() bool ICM40609D::ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples)
{ {
// read current temperature float temperature_sum{0};
uint8_t temperature_buf[3] {}; int valid_samples = 0;
temperature_buf[0] = static_cast<uint8_t>(Register::BANK_0::TEMP_DATA1) | DIR_READ;
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) { 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++;
}
if (valid_samples > 0) {
// use average temperature reading
const float temperature_avg = temperature_sum / valid_samples;
if (PX4_ISFINITE(temperature_avg)) {
_px4_accel.set_temperature(temperature_avg);
_px4_gyro.set_temperature(temperature_avg);
return true;
} else {
perf_count(_bad_transfer_perf); perf_count(_bad_transfer_perf);
return;
}
const int16_t TEMP_DATA = combine(temperature_buf[1], temperature_buf[2]);
// Temperature in Degrees Centigrade
const float TEMP_degC = (TEMP_DATA / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
if (PX4_ISFINITE(TEMP_degC)) {
_px4_accel.set_temperature(TEMP_degC);
_px4_gyro.set_temperature(TEMP_degC);
} }
} }
return false;
}
src/drivers/imu/invensense/icm40609d/ICM40609D.hpp
+13 -12
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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -70,12 +70,12 @@ private:
void exit_and_cleanup() override; void exit_and_cleanup() override;
// Sensor Configuration // 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 GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
static constexpr float ACCEL_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 // 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 // Transfer data
struct FIFOTransferBuffer { struct FIFOTransferBuffer {
@@ -99,8 +99,6 @@ private:
bool Reset(); bool Reset();
bool Configure(); bool Configure();
void ConfigureAccel();
void ConfigureGyro();
void ConfigureSampleRate(int sample_rate); void ConfigureSampleRate(int sample_rate);
void ConfigureFIFOWatermark(uint8_t samples); 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 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 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; 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))}; int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _checked_register_bank0{0}; 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_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] {
// Register | Set bits, Clear bits // 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::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::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::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::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, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 }, { 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::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::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_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::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_CONFIG0, INT_CONFIG0_BIT::FIFO_THS_INT_CLEAR, 0 },
{ Register::BANK_0::INT_SOURCE0, INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 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 },
}; };
}; };
src/drivers/imu/invensense/icm40609d/InvenSense_ICM40609D_registers.hpp
+52 -32
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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -85,13 +85,15 @@ enum class BANK_0 : uint8_t {
PWR_MGMT0 = 0x4E, PWR_MGMT0 = 0x4E,
GYRO_CONFIG0 = 0x4F, GYRO_CONFIG0 = 0x4F,
ACCEL_CONFIG0 = 0x50, ACCEL_CONFIG0 = 0x50,
GYRO_CONFIG1 = 0x51,
ACCEL_CONFIG1 = 0x53,
FIFO_CONFIG1 = 0x5F, FIFO_CONFIG1 = 0x5F,
FIFO_CONFIG2 = 0x60, FIFO_CONFIG2 = 0x60,
FIFO_CONFIG3 = 0x61, FIFO_CONFIG3 = 0x61,
INT_CONFIG0 = 0x63, INT_CONFIG0 = 0x63,
INT_CONFIG1 = 0x64,
INT_SOURCE0 = 0x65, INT_SOURCE0 = 0x65,
WHO_AM_I = 0x75, WHO_AM_I = 0x75,
@@ -104,7 +106,7 @@ enum class BANK_0 : uint8_t {
// DEVICE_CONFIG // DEVICE_CONFIG
enum DEVICE_CONFIG_BIT : uint8_t { enum DEVICE_CONFIG_BIT : uint8_t {
SOFT_RESET_CONFIG = Bit0, // SOFT_RESET_CONFIG = Bit0,
}; };
// INT_CONFIG // INT_CONFIG
@@ -122,7 +124,8 @@ enum FIFO_CONFIG_BIT : uint8_t {
// INT_STATUS // INT_STATUS
enum INT_STATUS_BIT : uint8_t { enum INT_STATUS_BIT : uint8_t {
DATA_RDY_INT = Bit3, RESET_DONE_INT = Bit4,
FIFO_THS_INT = Bit2, FIFO_THS_INT = Bit2,
FIFO_FULL_INT = Bit1, FIFO_FULL_INT = Bit1,
}; };
@@ -141,42 +144,52 @@ enum PWR_MGMT0_BIT : uint8_t {
// GYRO_CONFIG0 // GYRO_CONFIG0
enum GYRO_CONFIG0_BIT : uint8_t { enum GYRO_CONFIG0_BIT : uint8_t {
// 7:5 GYRO_FS_SEL // 7:5 GYRO_FS_SEL
GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000dps (default) // 0b000: ±2000dps (default)
GYRO_FS_SEL_1000_DPS = Bit5, // 0b001 = ±1000 dps GYRO_FS_SEL_2000_DPS_CLEAR = Bit7 | Bit6 | Bit5,
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
// 3:0 GYRO_ODR // 3:0 GYRO_ODR
GYRO_ODR_32kHz = Bit0, // 0001: 32kHz // 0b0001: 32kHz (maximum)
GYRO_ODR_16kHz = Bit1, // 0010: 16kHz GYRO_ODR_32KHZ_SET = Bit0,
GYRO_ODR_8kHz = Bit1 | Bit0, // 0011: 8kHz GYRO_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
GYRO_ODR_4kHz = Bit2, // 0100: 4kHz // 0b0011: 8kHz
GYRO_ODR_2kHz = Bit2 | Bit0, // 0101: 2kHz GYRO_ODR_8KHZ_SET = Bit1 | Bit0,
GYRO_ODR_1kHz = Bit2 | Bit1, // 0110: 1kHz (default) GYRO_ODR_8KHZ_CLEAR = Bit3 | Bit2,
// 0b0110: 1kHz (default)
GYRO_ODR_1KHZ_SET = Bit2 | Bit1,
GYRO_ODR_1KHZ_CLEAR = Bit3 | Bit0,
}; };
// ACCEL_CONFIG0 // ACCEL_CONFIG0
enum ACCEL_CONFIG0_BIT : uint8_t { enum ACCEL_CONFIG0_BIT : uint8_t {
// 7:5 ACCEL_FS_SEL // 7:5 ACCEL_FS_SEL
ACCEL_FS_SEL_32G = 0, // 000: ±32g (default) // 0b000: ±32g (default)
ACCEL_FS_SEL_16G = Bit5, // 001: ±16g ACCEL_FS_SEL_32G_CLEAR = Bit7 | Bit6 | Bit5,
ACCEL_FS_SEL_8G = Bit6, // 010: ±8g
ACCEL_FS_SEL_4G = Bit6 | Bit5, // 011: ±4g
// 3:0 ACCEL_ODR // 3:0 ACCEL_ODR
ACCEL_ODR_32kHz = Bit0, // 0001: 32kHz // 0b0001: 32kHz (maximum)
ACCEL_ODR_16kHz = Bit1, // 0010: 16kHz ACCEL_ODR_32KHZ_SET = Bit0,
ACCEL_ODR_8kHz = Bit1 | Bit0, // 0011: 8kHz ACCEL_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
ACCEL_ODR_4kHz = Bit2, // 0100: 4kHz // 0b0011: 8kHz
ACCEL_ODR_2kHz = Bit2 | Bit0, // 0101: 2kHz ACCEL_ODR_8KHZ_SET = Bit1 | Bit0,
ACCEL_ODR_1kHz = Bit2 | Bit1, // 0110: 1kHz (default) 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 // FIFO_CONFIG1
enum FIFO_CONFIG1_BIT : uint8_t { enum FIFO_CONFIG1_BIT : uint8_t {
FIFO_RESUME_PARTIAL_RD = Bit6, FIFO_RESUME_PARTIAL_RD = Bit6,
FIFO_WM_GT_TH = Bit5,
FIFO_TEMP_EN = Bit2, FIFO_TEMP_EN = Bit2,
FIFO_GYRO_EN = Bit1, FIFO_GYRO_EN = Bit1,
@@ -186,14 +199,21 @@ enum FIFO_CONFIG1_BIT : uint8_t {
// INT_CONFIG0 // INT_CONFIG0
enum INT_CONFIG0_BIT : uint8_t { enum INT_CONFIG0_BIT : uint8_t {
// 3:2 FIFO_THS_INT_CLEAR // 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 // INT_SOURCE0
enum INT_SOURCE0_BIT : uint8_t { 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_THS_INT1_EN = Bit2, // FIFO threshold interrupt routed to INT1
FIFO_FULL_INT1_EN = Bit1,
}; };
// REG_BANK_SEL // REG_BANK_SEL
@@ -225,9 +245,9 @@ struct DATA {
uint8_t GYRO_DATA_Y0; uint8_t GYRO_DATA_Y0;
uint8_t GYRO_DATA_Z1; uint8_t GYRO_DATA_Z1;
uint8_t GYRO_DATA_Z0; uint8_t GYRO_DATA_Z0;
uint8_t temperature; // Temperature[7:0] uint8_t FIFO_TEMP_DATA; // Temperature[7:0]
uint8_t timestamp_l; uint8_t TimeStamp_h; // TimeStamp[15:8]
uint8_t timestamp_h; uint8_t TimeStamp_l; // TimeStamp[7:0]
}; };
// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx // With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
src/drivers/imu/invensense/icm40609d/icm40609d_main.cpp
+3 -5
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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * 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")) { if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator); return ThisDriver::module_start(cli, iterator);
}
if (!strcmp(verb, "stop")) { } else if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator); return ThisDriver::module_stop(iterator);
}
if (!strcmp(verb, "status")) { } else if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator); return ThisDriver::module_status(iterator);
} }
src/drivers/imu/invensense/icm42605/CMakeLists.txt
+2 -1
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 # Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions # modification, are permitted provided that the following conditions
@@ -34,6 +34,7 @@ px4_add_module(
MODULE drivers__imu__invensense__icm42605 MODULE drivers__imu__invensense__icm42605
MAIN icm42605 MAIN icm42605
COMPILE_FLAGS COMPILE_FLAGS
${MAX_CUSTOM_OPT_LEVEL}
SRCS SRCS
icm42605_main.cpp icm42605_main.cpp
ICM42605.cpp ICM42605.cpp
src/drivers/imu/invensense/icm42605/ICM42605.cpp
+42 -99
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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -274,8 +274,8 @@ void ICM42605::RunImpl()
} }
} }
if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
// check configuration registers periodically or immediately following any failure // check configuration registers periodically or immediately following any failure
if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0]) if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
) { ) {
_last_config_check_timestamp = now; _last_config_check_timestamp = now;
@@ -286,13 +286,6 @@ void ICM42605::RunImpl()
perf_count(_bad_register_perf); perf_count(_bad_register_perf);
Reset(); 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) void ICM42605::ConfigureSampleRate(int sample_rate)
{ {
// round down to nearest FIFO sample dt // round down to nearest FIFO sample dt
@@ -419,8 +353,11 @@ bool ICM42605::Configure()
} }
} }
ConfigureAccel(); _px4_accel.set_range(16.f * CONSTANTS_ONE_G);
ConfigureGyro(); _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; return success;
} }
@@ -585,10 +522,12 @@ bool ICM42605::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
} }
if (valid_samples > 0) { if (valid_samples > 0) {
if (ProcessTemperature(buffer.f, valid_samples)) {
ProcessGyro(timestamp_sample, buffer.f, valid_samples); ProcessGyro(timestamp_sample, buffer.f, valid_samples);
ProcessAccel(timestamp_sample, buffer.f, valid_samples); ProcessAccel(timestamp_sample, buffer.f, valid_samples);
return true; return true;
} }
}
return false; return false;
} }
@@ -608,7 +547,7 @@ void ICM42605::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DAT
{ {
sensor_accel_fifo_s accel{}; sensor_accel_fifo_s accel{};
accel.timestamp_sample = timestamp_sample; accel.timestamp_sample = timestamp_sample;
accel.samples = 0; accel.samples = samples;
accel.dt = FIFO_SAMPLE_DT; accel.dt = FIFO_SAMPLE_DT;
for (int i = 0; i < samples; i++) { for (int i = 0; i < samples; i++) {
@@ -618,19 +557,16 @@ void ICM42605::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DAT
// sensor's frame is +x forward, +y left, +z up // 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) // flip y & z to publish right handed with z down (x forward, y right, z down)
accel.x[accel.samples] = accel_x; accel.x[i] = accel_x;
accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; accel.y[i] = math::negate(accel_y);
accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; accel.z[i] = math::negate(accel_z);
accel.samples++;
} }
_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + _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)); 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) 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; gyro.dt = FIFO_SAMPLE_DT;
for (int i = 0; i < samples; i++) { for (int i = 0; i < samples; i++) {
const int16_t gyro_x = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0); 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); 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_z = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0);
// sensor's frame is +x forward, +y left, +z up // 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) // flip y & z to publish right handed with z down (x forward, y right, z down)
gyro.x[i] = gyro_x; gyro.x[i] = gyro_x;
gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; gyro.y[i] = math::negate(gyro_y);
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; gyro.z[i] = math::negate(gyro_z);
} }
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + _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); _px4_gyro.updateFIFO(gyro);
} }
void ICM42605::UpdateTemperature() bool ICM42605::ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples)
{ {
// read current temperature float temperature_sum{0};
uint8_t temperature_buf[3] {}; int valid_samples = 0;
temperature_buf[0] = static_cast<uint8_t>(Register::BANK_0::TEMP_DATA1) | DIR_READ;
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) { 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++;
}
if (valid_samples > 0) {
// use average temperature reading
const float temperature_avg = temperature_sum / valid_samples;
if (PX4_ISFINITE(temperature_avg)) {
_px4_accel.set_temperature(temperature_avg);
_px4_gyro.set_temperature(temperature_avg);
return true;
} else {
perf_count(_bad_transfer_perf); perf_count(_bad_transfer_perf);
return;
}
const int16_t TEMP_DATA = combine(temperature_buf[1], temperature_buf[2]);
// Temperature in Degrees Centigrade
const float TEMP_degC = (TEMP_DATA / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
if (PX4_ISFINITE(TEMP_degC)) {
_px4_accel.set_temperature(TEMP_degC);
_px4_gyro.set_temperature(TEMP_degC);
} }
} }
return false;
}
src/drivers/imu/invensense/icm42605/ICM42605.hpp
+12 -11
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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -70,7 +70,7 @@ private:
void exit_and_cleanup() override; void exit_and_cleanup() override;
// Sensor Configuration // 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 GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT}; static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
@@ -99,8 +99,6 @@ private:
bool Reset(); bool Reset();
bool Configure(); bool Configure();
void ConfigureAccel();
void ConfigureGyro();
void ConfigureSampleRate(int sample_rate); void ConfigureSampleRate(int sample_rate);
void ConfigureFIFOWatermark(uint8_t samples); 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 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 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; 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))}; int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _checked_register_bank0{0}; 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_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] {
// Register | Set bits, Clear bits // 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::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::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::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::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, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 }, { 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::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::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_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::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_CONFIG0, INT_CONFIG0_BIT::FIFO_THS_INT_CLEAR, 0 },
{ Register::BANK_0::INT_SOURCE0, INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 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 },
}; };
}; };
src/drivers/imu/invensense/icm42605/InvenSense_ICM42605_registers.hpp
+41 -34
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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -85,6 +85,8 @@ enum class BANK_0 : uint8_t {
PWR_MGMT0 = 0x4E, PWR_MGMT0 = 0x4E,
GYRO_CONFIG0 = 0x4F, GYRO_CONFIG0 = 0x4F,
ACCEL_CONFIG0 = 0x50, ACCEL_CONFIG0 = 0x50,
GYRO_CONFIG1 = 0x51,
ACCEL_CONFIG1 = 0x53,
FIFO_CONFIG1 = 0x5F, FIFO_CONFIG1 = 0x5F,
FIFO_CONFIG2 = 0x60, FIFO_CONFIG2 = 0x60,
@@ -104,7 +106,7 @@ enum class BANK_0 : uint8_t {
// DEVICE_CONFIG // DEVICE_CONFIG
enum DEVICE_CONFIG_BIT : uint8_t { enum DEVICE_CONFIG_BIT : uint8_t {
SOFT_RESET_CONFIG = Bit0, // SOFT_RESET_CONFIG = Bit0,
}; };
// INT_CONFIG // INT_CONFIG
@@ -123,14 +125,13 @@ enum FIFO_CONFIG_BIT : uint8_t {
// INT_STATUS // INT_STATUS
enum INT_STATUS_BIT : uint8_t { enum INT_STATUS_BIT : uint8_t {
RESET_DONE_INT = Bit4, RESET_DONE_INT = Bit4,
DATA_RDY_INT = Bit3,
FIFO_THS_INT = Bit2, FIFO_THS_INT = Bit2,
FIFO_FULL_INT = Bit1, FIFO_FULL_INT = Bit1,
}; };
// SIGNAL_PATH_RESET // SIGNAL_PATH_RESET
enum SIGNAL_PATH_RESET_BIT : uint8_t { enum SIGNAL_PATH_RESET_BIT : uint8_t {
ABORT_AND_RESET = Bit3,
FIFO_FLUSH = Bit1, FIFO_FLUSH = Bit1,
}; };
@@ -143,38 +144,46 @@ enum PWR_MGMT0_BIT : uint8_t {
// GYRO_CONFIG0 // GYRO_CONFIG0
enum GYRO_CONFIG0_BIT : uint8_t { enum GYRO_CONFIG0_BIT : uint8_t {
// 7:5 GYRO_FS_SEL // 7:5 GYRO_FS_SEL
GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000dps (default) // 0b000: ±2000dps (default)
GYRO_FS_SEL_1000_DPS = Bit5, // 0b001 = ±1000 dps GYRO_FS_SEL_2000_DPS_CLEAR = Bit7 | Bit6 | Bit5,
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
// 3:0 GYRO_ODR // 3:0 GYRO_ODR
GYRO_ODR_8kHz = Bit1 | Bit0, // 0011: 8kHz // 0b0011: 8kHz (maximum)
GYRO_ODR_4kHz = Bit2, // 0100: 4kHz GYRO_ODR_8KHZ_SET = Bit1 | Bit0,
GYRO_ODR_2kHz = Bit2 | Bit0, // 0101: 2kHz GYRO_ODR_8KHZ_CLEAR = Bit3 | Bit2,
GYRO_ODR_1kHz = Bit2 | Bit1, // 0110: 1kHz (default) // 0b0110: 1kHz (default)
GYRO_ODR_1KHZ_SET = Bit2 | Bit1,
GYRO_ODR_1KHZ_CLEAR = Bit3 | Bit0,
}; };
// ACCEL_CONFIG0 // ACCEL_CONFIG0
enum ACCEL_CONFIG0_BIT : uint8_t { enum ACCEL_CONFIG0_BIT : uint8_t {
// 7:5 ACCEL_FS_SEL // 7:5 ACCEL_FS_SEL
ACCEL_FS_SEL_16G = 0, // 000: ±16g (default) // 0b000: ±16g (default)
ACCEL_FS_SEL_8G = Bit5, // 001: ±8g ACCEL_FS_SEL_16G_CLEAR = Bit7 | Bit6 | Bit5,
ACCEL_FS_SEL_4G = Bit6, // 010: ±4g
ACCEL_FS_SEL_2G = Bit6 | Bit5, // 011: ±2g
// 3:0 ACCEL_ODR // 3:0 ACCEL_ODR
ACCEL_ODR_8kHz = Bit1 | Bit0, // 0011: 8kHz // 0b0011: 8kHz (maximum)
ACCEL_ODR_4kHz = Bit2, // 0100: 4kHz ACCEL_ODR_8KHZ_SET = Bit1 | Bit0,
ACCEL_ODR_2kHz = Bit2 | Bit0, // 0101: 2kHz ACCEL_ODR_8KHZ_CLEAR = Bit3 | Bit2,
ACCEL_ODR_1kHz = Bit2 | Bit1, // 0110: 1kHz (default) // 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 // FIFO_CONFIG1
enum FIFO_CONFIG1_BIT : uint8_t { enum FIFO_CONFIG1_BIT : uint8_t {
FIFO_RESUME_PARTIAL_RD = Bit6, FIFO_RESUME_PARTIAL_RD = Bit6,
FIFO_WM_GT_TH = Bit5,
FIFO_TEMP_EN = Bit2, FIFO_TEMP_EN = Bit2,
FIFO_GYRO_EN = Bit1, FIFO_GYRO_EN = Bit1,
@@ -184,23 +193,21 @@ enum FIFO_CONFIG1_BIT : uint8_t {
// INT_CONFIG0 // INT_CONFIG0
enum INT_CONFIG0_BIT : uint8_t { enum INT_CONFIG0_BIT : uint8_t {
// 3:2 FIFO_THS_INT_CLEAR // 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 // INT_CONFIG1
enum INT_CONFIG1_BIT : uint8_t { 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 // INT_SOURCE0
enum INT_SOURCE0_BIT : uint8_t { enum INT_SOURCE0_BIT : uint8_t {
UI_FSYNC_INT1_EN = Bit6, RESET_DONE_INT1_EN = Bit4, // 1: Reset done interrupt routed to INT1 (enabled by default)
PLL_RDY_INT1_EN = Bit5,
RESET_DONE_INT1_EN = Bit4,
UI_DRDY_INT1_EN = Bit3,
FIFO_THS_INT1_EN = Bit2, // FIFO threshold interrupt routed to INT1 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 // REG_BANK_SEL
@@ -232,9 +239,9 @@ struct DATA {
uint8_t GYRO_DATA_Y0; uint8_t GYRO_DATA_Y0;
uint8_t GYRO_DATA_Z1; uint8_t GYRO_DATA_Z1;
uint8_t GYRO_DATA_Z0; uint8_t GYRO_DATA_Z0;
uint8_t temperature; // Temperature[7:0] uint8_t FIFO_TEMP_DATA; // Temperature[7:0]
uint8_t timestamp_l; uint8_t TimeStamp_h; // TimeStamp[15:8]
uint8_t timestamp_h; uint8_t TimeStamp_l; // TimeStamp[7:0]
}; };
// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx // 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_MSG = Bit7, // 1: FIFO is empty
HEADER_ACCEL = Bit6, HEADER_ACCEL = Bit6,
HEADER_GYRO = Bit5, HEADER_GYRO = Bit5,
HEADER_20 = Bit4,
HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2, HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2,
HEADER_ODR_ACCEL = Bit1, HEADER_ODR_ACCEL = Bit1,
HEADER_ODR_GYRO = Bit0, HEADER_ODR_GYRO = Bit0,
src/drivers/imu/invensense/icm42605/icm42605_main.cpp
+3 -5
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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * 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")) { if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator); return ThisDriver::module_start(cli, iterator);
}
if (!strcmp(verb, "stop")) { } else if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator); return ThisDriver::module_stop(iterator);
}
if (!strcmp(verb, "status")) { } else if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator); return ThisDriver::module_status(iterator);
} }
src/drivers/imu/invensense/icm42688p/CMakeLists.txt
+1 -1
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 # Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions # modification, are permitted provided that the following conditions
src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
+10 -5
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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -275,6 +275,7 @@ void ICM42688P::RunImpl()
} }
// check configuration registers periodically or immediately following any failure // check configuration registers periodically or immediately following any failure
if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0]) if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
&& RegisterCheck(_register_bank1_cfg[_checked_register_bank1]) && RegisterCheck(_register_bank1_cfg[_checked_register_bank1])
&& RegisterCheck(_register_bank2_cfg[_checked_register_bank2]) && RegisterCheck(_register_bank2_cfg[_checked_register_bank2])
@@ -290,6 +291,7 @@ void ICM42688P::RunImpl()
Reset(); Reset();
} }
} }
}
break; break;
} }
@@ -378,7 +380,10 @@ bool ICM42688P::Configure()
// 20-bits data format used // 20-bits data format used
// the only FSR settings that are operational are ±2000dps for gyroscope and ±16g for accelerometer // 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_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_range(math::radians(2000.f));
_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
return success; 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 // 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) // flip y & z to publish right handed with z down (x forward, y right, z down)
accel.x[i] = accel.x[i]; accel.x[i] = accel.x[i];
accel.y[i] = (accel.y[i] == INT16_MIN) ? INT16_MAX : -accel.y[i]; accel.y[i] = math::negate(accel.y[i]);
accel.z[i] = (accel.z[i] == INT16_MIN) ? INT16_MAX : -accel.z[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) + _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 // 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) // flip y & z to publish right handed with z down (x forward, y right, z down)
gyro.x[i] = gyro.x[i]; gyro.x[i] = gyro.x[i];
gyro.y[i] = (gyro.y[i] == INT16_MIN) ? INT16_MAX : -gyro.y[i]; gyro.y[i] = math::negate(gyro.y[i]);
gyro.z[i] = (gyro.z[i] == INT16_MIN) ? INT16_MAX : -gyro.z[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) + _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
src/drivers/imu/invensense/icm42688p/ICM42688P.hpp
+11 -10
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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -70,7 +70,7 @@ private:
void exit_and_cleanup() override; void exit_and_cleanup() override;
// Sensor Configuration // 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 GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
static constexpr float ACCEL_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))}; int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _checked_register_bank0{0}; 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_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] {
// Register | Set bits, Clear bits // 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::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::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::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::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_FS_SEL_16G | ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_SET, ACCEL_CONFIG0_BIT::ACCEL_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 }, { 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::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::ACCEL_CONFIG1, 0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_ORD_1ST_CLEAR },
{ 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::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_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::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_CONFIG0, INT_CONFIG0_BIT::FIFO_THS_INT_CLEAR, 0 },
{ Register::BANK_0::INT_SOURCE0, INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 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}; uint8_t _checked_register_bank1{0};
src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp
+28 -40
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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -81,8 +81,7 @@ enum class BANK_0 : uint8_t {
FIFO_DATA = 0x30, FIFO_DATA = 0x30,
SIGNAL_PATH_RESET = 0x4B, SIGNAL_PATH_RESET = 0x4B,
INTF_CONFIG0 = 0x4C,
INTF_CONFIG1 = 0x4D,
PWR_MGMT0 = 0x4E, PWR_MGMT0 = 0x4E,
GYRO_CONFIG0 = 0x4F, GYRO_CONFIG0 = 0x4F,
ACCEL_CONFIG0 = 0x50, ACCEL_CONFIG0 = 0x50,
@@ -95,7 +94,7 @@ enum class BANK_0 : uint8_t {
FIFO_CONFIG3 = 0x61, FIFO_CONFIG3 = 0x61,
INT_CONFIG0 = 0x63, INT_CONFIG0 = 0x63,
INT_CONFIG1 = 0x64,
INT_SOURCE0 = 0x65, INT_SOURCE0 = 0x65,
SELF_TEST_CONFIG = 0x70, SELF_TEST_CONFIG = 0x70,
@@ -120,7 +119,7 @@ enum class BANK_2 : uint8_t {
// DEVICE_CONFIG // DEVICE_CONFIG
enum DEVICE_CONFIG_BIT : uint8_t { enum DEVICE_CONFIG_BIT : uint8_t {
SOFT_RESET_CONFIG = Bit0, // SOFT_RESET_CONFIG = Bit0,
}; };
// INT_CONFIG // INT_CONFIG
@@ -139,14 +138,13 @@ enum FIFO_CONFIG_BIT : uint8_t {
// INT_STATUS // INT_STATUS
enum INT_STATUS_BIT : uint8_t { enum INT_STATUS_BIT : uint8_t {
RESET_DONE_INT = Bit4, RESET_DONE_INT = Bit4,
DATA_RDY_INT = Bit3,
FIFO_THS_INT = Bit2, FIFO_THS_INT = Bit2,
FIFO_FULL_INT = Bit1, FIFO_FULL_INT = Bit1,
}; };
// SIGNAL_PATH_RESET // SIGNAL_PATH_RESET
enum SIGNAL_PATH_RESET_BIT : uint8_t { enum SIGNAL_PATH_RESET_BIT : uint8_t {
ABORT_AND_RESET = Bit3,
FIFO_FLUSH = Bit1, FIFO_FLUSH = Bit1,
}; };
@@ -159,24 +157,17 @@ enum PWR_MGMT0_BIT : uint8_t {
// GYRO_CONFIG0 // GYRO_CONFIG0
enum GYRO_CONFIG0_BIT : uint8_t { enum GYRO_CONFIG0_BIT : uint8_t {
// 7:5 GYRO_FS_SEL // 7:5 GYRO_FS_SEL
GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000dps (default) // 0b000: ±2000dps (default)
GYRO_FS_SEL_1000_DPS = Bit5, GYRO_FS_SEL_2000_DPS_CLEAR = Bit7 | Bit6 | Bit5,
GYRO_FS_SEL_500_DPS = Bit6,
GYRO_FS_SEL_250_DPS = Bit6 | Bit5,
GYRO_FS_SEL_125_DPS = Bit7,
// 3:0 GYRO_ODR // 3:0 GYRO_ODR
// 0001: 32kHz // 0b0001: 32kHz (maximum)
GYRO_ODR_32KHZ_SET = Bit0, GYRO_ODR_32KHZ_SET = Bit0,
GYRO_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0, GYRO_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
// 0010: 16kHz // 0b0011: 8kHz
GYRO_ODR_16KHZ_SET = Bit1,
GYRO_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
// 0011: 8kHz
GYRO_ODR_8KHZ_SET = Bit1 | Bit0, GYRO_ODR_8KHZ_SET = Bit1 | Bit0,
GYRO_ODR_8KHZ_CLEAR = Bit3 | Bit2, GYRO_ODR_8KHZ_CLEAR = Bit3 | Bit2,
// 0110: 1kHz (default) // 0b0110: 1kHz (default)
GYRO_ODR_1KHZ_SET = Bit2 | Bit1, GYRO_ODR_1KHZ_SET = Bit2 | Bit1,
GYRO_ODR_1KHZ_CLEAR = Bit3 | Bit0, GYRO_ODR_1KHZ_CLEAR = Bit3 | Bit0,
}; };
@@ -184,30 +175,24 @@ enum GYRO_CONFIG0_BIT : uint8_t {
// ACCEL_CONFIG0 // ACCEL_CONFIG0
enum ACCEL_CONFIG0_BIT : uint8_t { enum ACCEL_CONFIG0_BIT : uint8_t {
// 7:5 ACCEL_FS_SEL // 7:5 ACCEL_FS_SEL
ACCEL_FS_SEL_16G = 0, // 000: ±16g (default) // 0b000: ±16g (default)
ACCEL_FS_SEL_8G = Bit5, ACCEL_FS_SEL_16G_CLEAR = Bit7 | Bit6 | Bit5,
ACCEL_FS_SEL_4G = Bit6,
ACCEL_FS_SEL_2G = Bit6 | Bit5,
// 3:0 ACCEL_ODR // 3:0 ACCEL_ODR
// 0001: 32kHz // 0b0001: 32kHz (maximum)
ACCEL_ODR_32KHZ_SET = Bit0, ACCEL_ODR_32KHZ_SET = Bit0,
ACCEL_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0, ACCEL_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
// 0010: 16kHz // 0b0011: 8kHz
ACCEL_ODR_16KHZ_SET = Bit1,
ACCEL_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
// 0011: 8kHz
ACCEL_ODR_8KHZ_SET = Bit1 | Bit0, ACCEL_ODR_8KHZ_SET = Bit1 | Bit0,
ACCEL_ODR_8KHZ_CLEAR = Bit3 | Bit2, ACCEL_ODR_8KHZ_CLEAR = Bit3 | Bit2,
// 0110: 1kHz (default) // 0b0110: 1kHz (default)
ACCEL_ODR_1KHZ_SET = Bit2 | Bit1, ACCEL_ODR_1KHZ_SET = Bit2 | Bit1,
ACCEL_ODR_1KHZ_CLEAR = Bit3 | Bit0, ACCEL_ODR_1KHZ_CLEAR = Bit3 | Bit0,
}; };
// GYRO_CONFIG1 // GYRO_CONFIG1
enum GYRO_CONFIG1_BIT : uint8_t { 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 // GYRO_ACCEL_CONFIG0
@@ -221,13 +206,12 @@ enum GYRO_ACCEL_CONFIG0_BIT : uint8_t {
// ACCEL_CONFIG1 // ACCEL_CONFIG1
enum ACCEL_CONFIG1_BIT : uint8_t { 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 // FIFO_CONFIG1
enum FIFO_CONFIG1_BIT : uint8_t { enum FIFO_CONFIG1_BIT : uint8_t {
FIFO_RESUME_PARTIAL_RD = Bit6, FIFO_RESUME_PARTIAL_RD = Bit6,
FIFO_WM_GT_TH = Bit5,
FIFO_HIRES_EN = Bit4, FIFO_HIRES_EN = Bit4,
FIFO_TEMP_EN = Bit2, FIFO_TEMP_EN = Bit2,
FIFO_GYRO_EN = Bit1, FIFO_GYRO_EN = Bit1,
@@ -237,18 +221,21 @@ enum FIFO_CONFIG1_BIT : uint8_t {
// INT_CONFIG0 // INT_CONFIG0
enum INT_CONFIG0_BIT : uint8_t { enum INT_CONFIG0_BIT : uint8_t {
// 3:2 FIFO_THS_INT_CLEAR // 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 // INT_SOURCE0
enum INT_SOURCE0_BIT : uint8_t { enum INT_SOURCE0_BIT : uint8_t {
UI_FSYNC_INT1_EN = Bit6, RESET_DONE_INT1_EN = Bit4, // 1: Reset done interrupt routed to INT1 (enabled by default)
PLL_RDY_INT1_EN = Bit5,
RESET_DONE_INT1_EN = Bit4,
UI_DRDY_INT1_EN = Bit3,
FIFO_THS_INT1_EN = Bit2, // FIFO threshold interrupt routed to INT1 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 // REG_BANK_SEL
@@ -276,6 +263,7 @@ enum ACCEL_CONFIG_STATIC2_BIT : uint8_t {
ACCEL_AAF_DIS = Bit0, ACCEL_AAF_DIS = Bit0,
}; };
namespace FIFO namespace FIFO
{ {
static constexpr size_t SIZE = 2048; static constexpr size_t SIZE = 2048;
src/drivers/imu/invensense/icm42688p/icm42688p_main.cpp
+3 -5
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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * 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")) { if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator); return ThisDriver::module_start(cli, iterator);
}
if (!strcmp(verb, "stop")) { } else if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator); return ThisDriver::module_stop(iterator);
}
if (!strcmp(verb, "status")) { } else if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator); return ThisDriver::module_status(iterator);
} }