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

drivers/imu: set sample timestamp from interrupt (if available) 

- minor scheduling improvements
   - if expected number of samples larger than expected adjust timestamp sample and save sample for next iteration
   - when scheduling via data ready interrupt (with no FIFO watermark) count continuously without clearing
This commit is contained in:
Daniel Agar 2021-07-08 11:10:07 -04:00
parent 6ac6917430
commit b1eb762753
50 changed files with 585 additions and 393 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp
View File

@ -104,6 +104,9 @@ ADIS16448::ADIS16448(const I2CSPIDriverConfig &config) :
_px4_gyro(get_device_id(), config.rotation),
_px4_mag(get_device_id(), config.rotation)
{
if (_drdy_gpio != 0) {
_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
}
}
ADIS16448::~ADIS16448()
@ -112,6 +115,7 @@ ADIS16448::~ADIS16448()
perf_free(_bad_register_perf);
perf_free(_bad_transfer_perf);
perf_free(_perf_crc_bad);
perf_free(_drdy_missed_perf);
}
int ADIS16448::init()
@ -149,6 +153,7 @@ void ADIS16448::print_status()
perf_print_counter(_bad_register_perf);
perf_print_counter(_bad_transfer_perf);
perf_print_counter(_perf_crc_bad);
perf_print_counter(_drdy_missed_perf);
}
int ADIS16448::probe()
@ -326,7 +331,19 @@ void ADIS16448::RunImpl()
break;
case STATE::READ: {
hrt_abstime timestamp_sample = now;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
if ((now - drdy_timestamp_sample) < SAMPLE_INTERVAL_US) {
timestamp_sample = drdy_timestamp_sample;
} else {
perf_count(_drdy_missed_perf);
}
// push backup schedule back
ScheduleDelayed(SAMPLE_INTERVAL_US * 2);
}
@ -414,8 +431,8 @@ void ADIS16448::RunImpl()
}
if (imu_updated) {
_px4_accel.update(now, accel_x, accel_y, accel_z);
_px4_gyro.update(now, gyro_x, gyro_y, gyro_z);
_px4_accel.update(timestamp_sample, accel_x, accel_y, accel_z);
_px4_gyro.update(timestamp_sample, gyro_x, gyro_y, gyro_z);
}
// DIAG_STAT bit 7: New data, xMAGN_OUT/BARO_OUT
@ -426,13 +443,13 @@ void ADIS16448::RunImpl()
const int16_t mag_x = buffer.XMAGN_OUT;
const int16_t mag_y = (buffer.YMAGN_OUT == INT16_MIN) ? INT16_MAX : -buffer.YMAGN_OUT;
const int16_t mag_z = (buffer.ZMAGN_OUT == INT16_MIN) ? INT16_MAX : -buffer.ZMAGN_OUT;
_px4_mag.update(now, mag_x, mag_y, mag_z);
_px4_mag.update(timestamp_sample, mag_x, mag_y, mag_z);
_px4_baro.set_error_count(error_count);
_px4_baro.set_temperature(temperature);
float pressure_pa = buffer.BARO_OUT * 0.02f; // 20 μbar per LSB
_px4_baro.update(now, pressure_pa);
_px4_baro.update(timestamp_sample, pressure_pa);
}
success = true;

4
src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp
View File

@ -105,14 +105,16 @@ private:
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
perf_counter_t _perf_crc_bad{nullptr};
perf_counter_t _drdy_missed_perf{nullptr};
hrt_abstime _reset_timestamp{0};
hrt_abstime _last_config_check_timestamp{0};
int _failure_count{0};
bool _check_crc{false}; // CRC-16 not supported on earlier models (eg ADIS16448AMLZ)
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
bool _data_ready_interrupt_enabled{false};
bool _check_crc{false}; // CRC-16 not supported on earlier models (eg ADIS16448AMLZ)
bool _self_test_passed{false};
int16_t _accel_prev[3] {};

27
src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp
View File

@ -47,12 +47,18 @@ ADIS16470::ADIS16470(const I2CSPIDriverConfig &config) :
_px4_accel(get_device_id(), config.rotation),
_px4_gyro(get_device_id(), config.rotation)
{
if (_drdy_gpio != 0) {
_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
}
}
ADIS16470::~ADIS16470()
{
perf_free(_reset_perf);
perf_free(_bad_register_perf);
perf_free(_bad_transfer_perf);
perf_free(_perf_crc_bad);
perf_free(_drdy_missed_perf);
}
int ADIS16470::init()
@ -87,9 +93,10 @@ void ADIS16470::print_status()
I2CSPIDriverBase::print_status();
perf_print_counter(_reset_perf);
perf_print_counter(_perf_crc_bad);
perf_print_counter(_bad_register_perf);
perf_print_counter(_bad_transfer_perf);
perf_print_counter(_perf_crc_bad);
perf_print_counter(_drdy_missed_perf);
}
int ADIS16470::probe()
@ -208,7 +215,19 @@ void ADIS16470::RunImpl()
break;
case STATE::READ: {
hrt_abstime timestamp_sample = now;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
if ((now - drdy_timestamp_sample) < SAMPLE_INTERVAL_US) {
timestamp_sample = drdy_timestamp_sample;
} else {
perf_count(_drdy_missed_perf);
}
// push backup schedule back
ScheduleDelayed(SAMPLE_INTERVAL_US * 2);
}
@ -233,7 +252,6 @@ void ADIS16470::RunImpl()
static_assert(sizeof(BurstRead) == (176 / 8), "ADIS16470 report not 176 bits");
buffer.cmd = static_cast<uint16_t>(Register::GLOB_CMD) << 8;
set_frequency(SPI_SPEED_BURST);
if (transferhword((uint16_t *)&buffer, (uint16_t *)&buffer, sizeof(buffer) / sizeof(uint16_t)) == PX4_OK) {
@ -291,7 +309,7 @@ void ADIS16470::RunImpl()
accel_y = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
accel_z = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
_px4_accel.update(now, accel_x, accel_y, accel_z);
_px4_accel.update(timestamp_sample, accel_x, accel_y, accel_z);
int16_t gyro_x = buffer.X_GYRO_OUT;
@ -301,7 +319,7 @@ void ADIS16470::RunImpl()
// flip y & z to publish right handed with z down (x forward, y right, z down)
gyro_y = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
gyro_z = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
_px4_gyro.update(now, gyro_x, gyro_y, gyro_z);
_px4_gyro.update(timestamp_sample, gyro_x, gyro_y, gyro_z);
success = true;
@ -376,6 +394,7 @@ int ADIS16470::DataReadyInterruptCallback(int irq, void *context, void *arg)
void ADIS16470::DataReady()
{
_drdy_timestamp_sample.store(hrt_absolute_time());
ScheduleNow();
}

67
src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp
View File

@ -1,35 +1,35 @@
/****************************************************************************
*
* Copyright (c) 2021 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
*
* Copyright (c) 2021 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ADIS16470.hpp
@ -98,15 +98,18 @@ private:
PX4Gyroscope _px4_gyro;
perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
perf_counter_t _perf_crc_bad{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": CRC16 bad"))};
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
perf_counter_t _perf_crc_bad{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": CRC16 bad"))};
perf_counter_t _drdy_missed_perf{nullptr};
hrt_abstime _reset_timestamp{0};
hrt_abstime _last_config_check_timestamp{0};
int _failure_count{0};
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
bool _data_ready_interrupt_enabled{false};
bool _self_test_passed{false};
enum class STATE : uint8_t {

8
src/drivers/imu/bosch/bmi055/BMI055.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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,7 +66,7 @@ protected:
hrt_abstime _temperature_update_timestamp{0};
int _failure_count{0};
px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
bool _data_ready_interrupt_enabled{false};
enum class STATE : uint8_t {
@ -74,9 +74,7 @@ protected:
WAIT_FOR_RESET,
CONFIGURE,
FIFO_READ,
};
STATE _state{STATE::RESET};
} _state{STATE::RESET};
uint16_t _fifo_empty_interval_us{2500}; // 2500 us / 400 Hz transfer interval

45
src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -162,9 +162,16 @@ void BMI055_Accelerometer::RunImpl()
break;
case STATE::FIFO_READ: {
hrt_abstime timestamp_sample = 0;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_fifo_read_samples was set
if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_samples) {
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
timestamp_sample = drdy_timestamp_sample;
} else {
perf_count(_drdy_missed_perf);
}
@ -192,7 +199,20 @@ void BMI055_Accelerometer::RunImpl()
perf_count(_fifo_empty_perf);
} else if (fifo_frame_counter >= 1) {
if (FIFORead(now, fifo_frame_counter)) {
uint8_t samples = fifo_frame_counter;
// tolerate minor jitter, leave sample to next iteration if behind by only 1
if (samples == _fifo_samples + 1) {
// sample timestamp set from data ready already corresponds to _fifo_samples
if (timestamp_sample == 0) {
timestamp_sample = now - FIFO_SAMPLE_DT;
}
samples--;
}
if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) {
success = true;
if (_failure_count > 0) {
@ -242,22 +262,22 @@ void BMI055_Accelerometer::ConfigureAccel()
const uint8_t PMU_RANGE = RegisterRead(Register::PMU_RANGE) & (Bit3 | Bit2 | Bit1 | Bit0);
switch (PMU_RANGE) {
case range_2g:
case range_2g_set:
_px4_accel.set_scale(CONSTANTS_ONE_G / 1024.f); // 1024 LSB/g, 0.98mg/LSB
_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
break;
case range_4g:
case range_4g_set:
_px4_accel.set_scale(CONSTANTS_ONE_G / 512.f); // 512 LSB/g, 1.95mg/LSB
_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
break;
case range_8g:
case range_8g_set:
_px4_accel.set_scale(CONSTANTS_ONE_G / 256.f); // 256 LSB/g, 3.91mg/LSB
_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
break;
case range_16g:
case range_16g_set:
_px4_accel.set_scale(CONSTANTS_ONE_G / 128.f); // 128 LSB/g, 7.81mg/LSB
_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
break;
@ -318,11 +338,8 @@ int BMI055_Accelerometer::DataReadyInterruptCallback(int irq, void *context, voi
void BMI055_Accelerometer::DataReady()
{
uint32_t expected = 0;
if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) {
ScheduleNow();
}
_drdy_timestamp_sample.store(hrt_absolute_time());
ScheduleNow();
}
bool BMI055_Accelerometer::DataReadyInterruptConfigure()
@ -437,7 +454,7 @@ void BMI055_Accelerometer::FIFOReset()
RegisterWrite(Register::FIFO_CONFIG_1, 0);
// reset while FIFO is disabled
_drdy_fifo_read_samples.store(0);
_drdy_timestamp_sample.store(0);
// FIFO_CONFIG_0: restore FIFO watermark
// FIFO_CONFIG_1: re-enable FIFO

6
src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -58,7 +58,7 @@ private:
static constexpr uint32_t RATE{2000}; // 2000 Hz
static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
// Transfer data
struct FIFOTransferBuffer {
@ -112,7 +112,7 @@ private:
static constexpr uint8_t size_register_cfg{7};
register_config_t _register_cfg[size_register_cfg] {
// Register | Set bits, Clear bits
{ Register::PMU_RANGE, PMU_RANGE_BIT::range_16g, Bit1 | Bit0},
{ Register::PMU_RANGE, PMU_RANGE_BIT::range_16g_set, PMU_RANGE_BIT::range_16g_clear},
{ Register::ACCD_HBW, ACCD_HBW_BIT::data_high_bw, 0},
{ Register::INT_EN_1, INT_EN_1_BIT::int_fwm_en, 0},
{ Register::INT_MAP_1, INT_MAP_1_BIT::int1_fwm, 0},

37
src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -162,9 +162,16 @@ void BMI055_Gyroscope::RunImpl()
break;
case STATE::FIFO_READ: {
hrt_abstime timestamp_sample = 0;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_fifo_read_samples was set
if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_samples) {
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
timestamp_sample = drdy_timestamp_sample;
} else {
perf_count(_drdy_missed_perf);
}
@ -192,7 +199,20 @@ void BMI055_Gyroscope::RunImpl()
perf_count(_fifo_empty_perf);
} else if (fifo_frame_counter >= 1) {
if (FIFORead(now, fifo_frame_counter)) {
uint8_t samples = fifo_frame_counter;
// tolerate minor jitter, leave sample to next iteration if behind by only 1
if (samples == _fifo_samples + 1) {
// sample timestamp set from data ready already corresponds to _fifo_samples
if (timestamp_sample == 0) {
timestamp_sample = now - FIFO_SAMPLE_DT;
}
samples--;
}
if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) {
success = true;
if (_failure_count > 0) {
@ -316,11 +336,8 @@ int BMI055_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *a
void BMI055_Gyroscope::DataReady()
{
uint32_t expected = 0;
if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) {
ScheduleNow();
}
_drdy_timestamp_sample.store(hrt_absolute_time());
ScheduleNow();
}
bool BMI055_Gyroscope::DataReadyInterruptConfigure()
@ -434,7 +451,7 @@ void BMI055_Gyroscope::FIFOReset()
RegisterWrite(Register::FIFO_CONFIG_1, 0);
// reset while FIFO is disabled
_drdy_fifo_read_samples.store(0);
_drdy_timestamp_sample.store(0);
// FIFO_CONFIG_0: restore FIFO watermark
// FIFO_CONFIG_1: re-enable FIFO

4
src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -58,7 +58,7 @@ private:
static constexpr uint32_t RATE{2000}; // 2000 Hz
static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
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]))};
// Transfer data
struct FIFOTransferBuffer {

17
src/drivers/imu/bosch/bmi055/Bosch_BMI055_Accelerometer_Registers.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -97,10 +97,17 @@ enum ACCD_HBW_BIT : uint8_t {
// PMU_RANGE
enum PMU_RANGE_BIT : uint8_t {
// range<3:0>
range_2g = Bit1 | Bit0, // ́0011b ́ -> ±2g range
range_4g = Bit2 | Bit0, // ́0101b ́ -> ±4g range
range_8g = Bit3, // ́1000b ́ -> ±8g range
range_16g = Bit3 | Bit2, // ́1100b ́ -> ±16g range
range_16g_set = Bit3 | Bit2, // ́1100b ́ -> ±16g range
range_16g_clear = Bit1 | Bit0,
range_8g_set = Bit3, // ́1000b ́ -> ±8g range
range_8g_clear = Bit2 | Bit1 | Bit0,
range_4g_set = Bit2 | Bit0, // ́0101b ́ -> ±4g range
range_4g_clear = Bit3 | Bit1,
range_2g_set = Bit1 | Bit0, // ́0011b ́ -> ±2g range
range_2g_clear = Bit3 | Bit2,
};
// INT_EN_1

2
src/drivers/imu/bosch/bmi088/BMI088.cpp
View File

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

8
src/drivers/imu/bosch/bmi088/BMI088.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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,7 +66,7 @@ protected:
hrt_abstime _temperature_update_timestamp{0};
int _failure_count{0};
px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
bool _data_ready_interrupt_enabled{false};
enum class STATE : uint8_t {
@ -74,9 +74,7 @@ protected:
WAIT_FOR_RESET,
CONFIGURE,
FIFO_READ,
};
STATE _state{STATE::RESET};
} _state{STATE::RESET};
uint16_t _fifo_empty_interval_us{2500}; // 2500 us / 400 Hz transfer interval

33
src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -184,15 +184,19 @@ void BMI088_Accelerometer::RunImpl()
break;
case STATE::FIFO_READ: {
uint32_t samples = 0;
hrt_abstime timestamp_sample = now;
uint8_t samples = 0;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_samples) {
perf_count(_drdy_missed_perf);
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
timestamp_sample = drdy_timestamp_sample;
samples = _fifo_samples;
} else {
samples = _fifo_samples;
perf_count(_drdy_missed_perf);
}
// push backup schedule back
@ -214,6 +218,12 @@ void BMI088_Accelerometer::RunImpl()
} else {
samples = fifo_byte_counter / sizeof(FIFO::DATA);
// tolerate minor jitter, leave sample to next iteration if behind by only 1
if (samples == _fifo_samples + 1) {
timestamp_sample -= FIFO_SAMPLE_DT;
samples--;
}
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
FIFOReset();
@ -226,7 +236,7 @@ void BMI088_Accelerometer::RunImpl()
bool success = false;
if (samples >= 1) {
if (FIFORead(now, samples)) {
if (FIFORead(timestamp_sample, samples)) {
success = true;
if (_failure_count > 0) {
@ -360,11 +370,8 @@ int BMI088_Accelerometer::DataReadyInterruptCallback(int irq, void *context, voi
void BMI088_Accelerometer::DataReady()
{
uint32_t expected = 0;
if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) {
ScheduleNow();
}
_drdy_timestamp_sample.store(hrt_absolute_time());
ScheduleNow();
}
bool BMI088_Accelerometer::DataReadyInterruptConfigure()
@ -561,7 +568,7 @@ void BMI088_Accelerometer::FIFOReset()
RegisterWrite(Register::ACC_SOFTRESET, 0xB0);
// reset while FIFO is disabled
_drdy_fifo_read_samples.store(0);
_drdy_timestamp_sample.store(0);
}
void BMI088_Accelerometer::UpdateTemperature()

4
src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -58,7 +58,7 @@ private:
static constexpr uint32_t RATE{1600}; // 1600 Hz
static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
// Transfer data
struct FIFOTransferBuffer {

37
src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -163,9 +163,16 @@ void BMI088_Gyroscope::RunImpl()
break;
case STATE::FIFO_READ: {
hrt_abstime timestamp_sample = 0;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_fifo_read_samples was set
if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_samples) {
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
timestamp_sample = drdy_timestamp_sample;
} else {
perf_count(_drdy_missed_perf);
}
@ -193,7 +200,20 @@ void BMI088_Gyroscope::RunImpl()
perf_count(_fifo_empty_perf);
} else if (fifo_frame_counter >= 1) {
if (FIFORead(now, fifo_frame_counter)) {
uint8_t samples = fifo_frame_counter;
// tolerate minor jitter, leave sample to next iteration if behind by only 1
if (samples == _fifo_samples + 1) {
// sample timestamp set from data ready already corresponds to _fifo_samples
if (timestamp_sample == 0) {
timestamp_sample = now - FIFO_SAMPLE_DT;
}
samples--;
}
if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) {
success = true;
if (_failure_count > 0) {
@ -317,11 +337,8 @@ int BMI088_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *a
void BMI088_Gyroscope::DataReady()
{
uint32_t expected = 0;
if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) {
ScheduleNow();
}
_drdy_timestamp_sample.store(hrt_absolute_time());
ScheduleNow();
}
bool BMI088_Gyroscope::DataReadyInterruptConfigure()
@ -435,7 +452,7 @@ void BMI088_Gyroscope::FIFOReset()
RegisterWrite(Register::FIFO_CONFIG_1, 0);
// reset while FIFO is disabled
_drdy_fifo_read_samples.store(0);
_drdy_timestamp_sample.store(0);
// FIFO_CONFIG_0: restore FIFO watermark
// FIFO_CONFIG_1: re-enable FIFO

4
src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -58,7 +58,7 @@ private:
static constexpr uint32_t RATE{2000}; // 2000 Hz
static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
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]))};
// Transfer data
struct FIFOTransferBuffer {

2
src/drivers/imu/bosch/bmi088/Bosch_BMI088_Accelerometer_Registers.hpp
View File

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

2
src/drivers/imu/bosch/bmi088/Bosch_BMI088_Gyroscope_Registers.hpp
View File

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

8
src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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,7 +72,7 @@ protected:
int _total_failure_count{0};
px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
bool _data_ready_interrupt_enabled{false};
enum class STATE : uint8_t {
@ -81,9 +81,7 @@ protected:
WAIT_FOR_RESET,
CONFIGURE,
FIFO_READ,
};
STATE _state{STATE::SELFTEST};
} _state{STATE::SELFTEST};
uint16_t _fifo_empty_interval_us{2500}; // 2500 us / 400 Hz transfer interval

10
src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -293,11 +293,7 @@ int BMI088_Accelerometer::DataReadyInterruptCallback(int irq, void *context, voi
void BMI088_Accelerometer::DataReady()
{
uint32_t expected = 0;
if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) {
ScheduleNow();
}
ScheduleNow();
}
bool BMI088_Accelerometer::DataReadyInterruptConfigure()
@ -588,7 +584,7 @@ void BMI088_Accelerometer::FIFOReset()
RegisterWrite(Register::ACC_SOFTRESET, 0xB0);
// reset while FIFO is disabled
_drdy_fifo_read_samples.store(0);
_drdy_timestamp_sample.store(0);
}
void BMI088_Accelerometer::UpdateTemperature()

4
src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -59,7 +59,7 @@ private:
static constexpr uint32_t RATE{1600}; // 1600 Hz
static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
// Transfer data
struct FIFOTransferBuffer {

11
src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -262,11 +262,8 @@ int BMI088_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *a
void BMI088_Gyroscope::DataReady()
{
uint32_t expected = 0;
if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) {
ScheduleNow();
}
_drdy_timestamp_sample.store(hrt_absolute_time());
ScheduleNow();
}
bool BMI088_Gyroscope::DataReadyInterruptConfigure()
@ -382,7 +379,7 @@ void BMI088_Gyroscope::FIFOReset()
RegisterWrite(Register::FIFO_CONFIG_1, 0);
// reset while FIFO is disabled
_drdy_fifo_read_samples.store(0);
_drdy_timestamp_sample.store(0);
// FIFO_CONFIG_0: restore FIFO watermark
// FIFO_CONFIG_1: re-enable FIFO

4
src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -58,7 +58,7 @@ private:
static constexpr uint32_t RATE{400}; // 2000 Hz
static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
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]))};
// Transfer data
struct FIFOTransferBuffer {

2
src/drivers/imu/bosch/bmi088/bmi088_main.cpp
View File

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

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

@ -237,15 +237,19 @@ void ICM20602::RunImpl()
break;
case STATE::FIFO_READ: {
uint32_t samples = 0;
hrt_abstime timestamp_sample = now;
uint8_t samples = 0;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
perf_count(_drdy_missed_perf);
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
timestamp_sample = drdy_timestamp_sample;
samples = _fifo_gyro_samples;
} else {
samples = _fifo_gyro_samples;
perf_count(_drdy_missed_perf);
}
// push backup schedule back
@ -265,7 +269,13 @@ void ICM20602::RunImpl()
} else {
// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) * SAMPLES_PER_TRANSFER; // round down to nearest
samples = fifo_count / sizeof(FIFO::DATA);
// tolerate minor jitter, leave sample to next iteration if behind by only 1
if (samples == _fifo_gyro_samples + 1) {
timestamp_sample -= FIFO_SAMPLE_DT;
samples--;
}
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
@ -279,7 +289,7 @@ void ICM20602::RunImpl()
bool success = false;
if (samples >= SAMPLES_PER_TRANSFER) {
if (FIFORead(now, samples)) {
if (FIFORead(timestamp_sample, samples)) {
success = true;
if (_failure_count > 0) {
@ -435,11 +445,8 @@ int ICM20602::DataReadyInterruptCallback(int irq, void *context, void *arg)
void ICM20602::DataReady()
{
uint32_t expected = 0;
if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
ScheduleNow();
}
_drdy_timestamp_sample.store(hrt_absolute_time());
ScheduleNow();
}
bool ICM20602::DataReadyInterruptConfigure()
@ -530,16 +537,14 @@ bool ICM20602::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
}
const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
if (fifo_count_bytes >= FIFO::SIZE) {
if ((fifo_count_bytes >= FIFO::SIZE) || (fifo_count_samples > FIFO_MAX_SAMPLES)) {
perf_count(_fifo_overflow_perf);
FIFOReset();
return false;
}
const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
if (fifo_count_samples == 0) {
} else if (fifo_count_samples == 0) {
perf_count(_fifo_empty_perf);
return false;
}
@ -571,7 +576,7 @@ void ICM20602::FIFOReset()
RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN);
// reset while FIFO is disabled
_drdy_fifo_read_samples.store(0);
_drdy_timestamp_sample.store(0);
// FIFO_EN: enable both gyro and accel
// USER_CTRL: re-enable FIFO

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

@ -71,12 +71,12 @@ private:
// Sensor Configuration
static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
static constexpr uint32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer
static constexpr int32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 8000 Hz gyro
static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel
// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
static constexpr int32_t FIFO_MAX_SAMPLES{math::min(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 {
@ -140,7 +140,7 @@ private:
hrt_abstime _last_config_check_timestamp{0};
int _failure_count{0};
px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
bool _data_ready_interrupt_enabled{false};
enum class STATE : uint8_t {
@ -151,7 +151,7 @@ private:
} _state{STATE::RESET};
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _checked_register{0};
static constexpr uint8_t size_register_cfg{24};

37
src/drivers/imu/invensense/icm20608g/ICM20608G.cpp
View File

@ -228,9 +228,16 @@ void ICM20608G::RunImpl()
break;
case STATE::FIFO_READ: {
hrt_abstime timestamp_sample = now;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_fifo_read_samples was set
if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
timestamp_sample = drdy_timestamp_sample;
} else {
perf_count(_drdy_missed_perf);
}
@ -251,16 +258,21 @@ void ICM20608G::RunImpl()
} else {
// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) *
SAMPLES_PER_TRANSFER; // round down to nearest
uint8_t samples = fifo_count / sizeof(FIFO::DATA);
// tolerate minor jitter, leave sample to next iteration if behind by only 1
if (samples == _fifo_gyro_samples + 1) {
timestamp_sample -= FIFO_SAMPLE_DT;
samples--;
}
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
FIFOReset();
perf_count(_fifo_overflow_perf);
} else if (samples >= 1) {
if (FIFORead(now, samples)) {
} else if (samples >= SAMPLES_PER_TRANSFER) {
if (FIFORead(timestamp_sample, samples)) {
success = true;
if (_failure_count > 0) {
@ -402,13 +414,10 @@ int ICM20608G::DataReadyInterruptCallback(int irq, void *context, void *arg)
void ICM20608G::DataReady()
{
uint32_t expected = 0;
// at least the required number of samples in the FIFO
if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
&& _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
_drdy_count.store(0);
if (++_drdy_count >= _fifo_gyro_samples) {
_drdy_timestamp_sample.store(hrt_absolute_time());
_drdy_count -= _fifo_gyro_samples;
ScheduleNow();
}
}
@ -516,8 +525,8 @@ void ICM20608G::FIFOReset()
RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN);
// reset while FIFO is disabled
_drdy_count.store(0);
_drdy_fifo_read_samples.store(0);
_drdy_count = 0;
_drdy_timestamp_sample.store(0);
// FIFO_EN: enable both gyro and accel
// USER_CTRL: re-enable FIFO

10
src/drivers/imu/invensense/icm20608g/ICM20608G.hpp
View File

@ -71,12 +71,12 @@ private:
// Sensor Configuration
static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
static constexpr uint32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer
static constexpr int32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 8000 Hz gyro
static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel
// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
static constexpr int32_t FIFO_MAX_SAMPLES{math::min(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 {
@ -138,8 +138,8 @@ private:
hrt_abstime _temperature_update_timestamp{0};
int _failure_count{0};
px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
px4::atomic<uint32_t> _drdy_count{0};
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
int32_t _drdy_count{0};
bool _data_ready_interrupt_enabled{false};
enum class STATE : uint8_t {
@ -150,7 +150,7 @@ private:
} _state{STATE::RESET};
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _checked_register{0};
static constexpr uint8_t size_register_cfg{15};

45
src/drivers/imu/invensense/icm20649/ICM20649.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -194,9 +194,16 @@ void ICM20649::RunImpl()
break;
case STATE::FIFO_READ: {
hrt_abstime timestamp_sample = now;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_fifo_read_samples was set
if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
timestamp_sample = drdy_timestamp_sample;
} else {
perf_count(_drdy_missed_perf);
}
@ -217,16 +224,21 @@ void ICM20649::RunImpl()
} else {
// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) *
SAMPLES_PER_TRANSFER; // round down to nearest
uint8_t samples = fifo_count / sizeof(FIFO::DATA);
// tolerate minor jitter, leave sample to next iteration if behind by only 1
if (samples == _fifo_gyro_samples + 1) {
timestamp_sample -= FIFO_SAMPLE_DT;
samples--;
}
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
FIFOReset();
perf_count(_fifo_overflow_perf);
} else if (samples >= 1) {
if (FIFORead(now, samples)) {
} else if (samples >= SAMPLES_PER_TRANSFER) {
if (FIFORead(timestamp_sample, samples)) {
success = true;
if (_failure_count > 0) {
@ -395,13 +407,10 @@ int ICM20649::DataReadyInterruptCallback(int irq, void *context, void *arg)
void ICM20649::DataReady()
{
uint32_t expected = 0;
// at least the required number of samples in the FIFO
if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
&& _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
_drdy_count.store(0);
if (++_drdy_count >= _fifo_gyro_samples) {
_drdy_timestamp_sample.store(hrt_absolute_time());
_drdy_count -= _fifo_gyro_samples;
ScheduleNow();
}
}
@ -487,10 +496,11 @@ void ICM20649::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits
uint16_t ICM20649::FIFOReadCount()
{
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
// read FIFO count
uint8_t fifo_count_buf[3] {};
fifo_count_buf[0] = static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ;
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
perf_count(_bad_transfer_perf);
@ -502,9 +512,10 @@ uint16_t ICM20649::FIFOReadCount()
bool ICM20649::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
{
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
FIFOTransferBuffer buffer{};
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 3, FIFO::SIZE);
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
perf_count(_bad_transfer_perf);
@ -549,8 +560,8 @@ void ICM20649::FIFOReset()
RegisterClearBits(Register::BANK_0::FIFO_RST, FIFO_RST_BIT::FIFO_RESET);
// reset while FIFO is disabled
_drdy_count.store(0);
_drdy_fifo_read_samples.store(0);
_drdy_count = 0;
_drdy_timestamp_sample.store(0);
}
static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)

16
src/drivers/imu/invensense/icm20649/ICM20649.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -71,12 +71,12 @@ private:
// Sensor Configuration
static constexpr float FIFO_SAMPLE_DT{1e6f / 9000.f};
static constexpr uint32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer
static constexpr int32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 9000 Hz gyro
static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4500 Hz accel
// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
static constexpr int32_t FIFO_MAX_SAMPLES{math::min(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 {
@ -152,8 +152,8 @@ private:
enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
px4::atomic<uint32_t> _drdy_count{0};
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
int32_t _drdy_count{0};
bool _data_ready_interrupt_enabled{false};
enum class STATE : uint8_t {
@ -161,12 +161,10 @@ private:
WAIT_FOR_RESET,
CONFIGURE,
FIFO_READ,
};
STATE _state{STATE::RESET};
} _state{STATE::RESET};
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _checked_register_bank0{0};
static constexpr uint8_t size_register_bank0_cfg{6};

37
src/drivers/imu/invensense/icm20689/ICM20689.cpp
View File

@ -228,9 +228,16 @@ void ICM20689::RunImpl()
break;
case STATE::FIFO_READ: {
hrt_abstime timestamp_sample = now;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_fifo_read_samples was set
if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
timestamp_sample = drdy_timestamp_sample;
} else {
perf_count(_drdy_missed_perf);
}
@ -251,16 +258,21 @@ void ICM20689::RunImpl()
} else {
// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) *
SAMPLES_PER_TRANSFER; // round down to nearest
uint8_t samples = fifo_count / sizeof(FIFO::DATA);
// tolerate minor jitter, leave sample to next iteration if behind by only 1
if (samples == _fifo_gyro_samples + 1) {
timestamp_sample -= FIFO_SAMPLE_DT;
samples--;
}
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
FIFOReset();
perf_count(_fifo_overflow_perf);
} else if (samples >= 1) {
if (FIFORead(now, samples)) {
} else if (samples >= SAMPLES_PER_TRANSFER) {
if (FIFORead(timestamp_sample, samples)) {
success = true;
if (_failure_count > 0) {
@ -402,13 +414,10 @@ int ICM20689::DataReadyInterruptCallback(int irq, void *context, void *arg)
void ICM20689::DataReady()
{
uint32_t expected = 0;
// at least the required number of samples in the FIFO
if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
&& _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
_drdy_count.store(0);
if (++_drdy_count >= _fifo_gyro_samples) {
_drdy_timestamp_sample.store(hrt_absolute_time());
_drdy_count -= _fifo_gyro_samples;
ScheduleNow();
}
}
@ -516,8 +525,8 @@ void ICM20689::FIFOReset()
RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN);
// reset while FIFO is disabled
_drdy_count.store(0);
_drdy_fifo_read_samples.store(0);
_drdy_count = 0;
_drdy_timestamp_sample.store(0);
// FIFO_EN: enable both gyro and accel
// USER_CTRL: re-enable FIFO

10
src/drivers/imu/invensense/icm20689/ICM20689.hpp
View File

@ -71,12 +71,12 @@ private:
// Sensor Configuration
static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
static constexpr uint32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer
static constexpr int32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 8000 Hz gyro
static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel
// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
static constexpr int32_t FIFO_MAX_SAMPLES{math::min(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 {
@ -138,8 +138,8 @@ private:
hrt_abstime _temperature_update_timestamp{0};
int _failure_count{0};
px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
px4::atomic<uint32_t> _drdy_count{0};
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
int32_t _drdy_count{0};
bool _data_ready_interrupt_enabled{false};
enum class STATE : uint8_t {
@ -150,7 +150,7 @@ private:
} _state{STATE::RESET};
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _checked_register{0};
static constexpr uint8_t size_register_cfg{15};

39
src/drivers/imu/invensense/icm20948/ICM20948.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -230,9 +230,16 @@ void ICM20948::RunImpl()
break;
case STATE::FIFO_READ: {
hrt_abstime timestamp_sample = now;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_fifo_read_samples was set
if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
timestamp_sample = drdy_timestamp_sample;
} else {
perf_count(_drdy_missed_perf);
}
@ -253,16 +260,21 @@ void ICM20948::RunImpl()
} else {
// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) *
SAMPLES_PER_TRANSFER; // round down to nearest
uint8_t samples = fifo_count / sizeof(FIFO::DATA);
// tolerate minor jitter, leave sample to next iteration if behind by only 1
if (samples == _fifo_gyro_samples + 1) {
timestamp_sample -= FIFO_SAMPLE_DT;
samples--;
}
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
FIFOReset();
perf_count(_fifo_overflow_perf);
} else if (samples >= 1) {
if (FIFORead(now, samples)) {
} else if (samples >= SAMPLES_PER_TRANSFER) {
if (FIFORead(timestamp_sample, samples)) {
success = true;
if (_failure_count > 0) {
@ -442,13 +454,10 @@ int ICM20948::DataReadyInterruptCallback(int irq, void *context, void *arg)
void ICM20948::DataReady()
{
uint32_t expected = 0;
// at least the required number of samples in the FIFO
if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
&& _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
_drdy_count.store(0);
if (++_drdy_count >= _fifo_gyro_samples) {
_drdy_timestamp_sample.store(hrt_absolute_time());
_drdy_count -= _fifo_gyro_samples;
ScheduleNow();
}
}
@ -598,8 +607,8 @@ void ICM20948::FIFOReset()
RegisterClearBits(Register::BANK_0::FIFO_RST, FIFO_RST_BIT::FIFO_RESET);
// reset while FIFO is disabled
_drdy_count.store(0);
_drdy_fifo_read_samples.store(0);
_drdy_count = 0;
_drdy_timestamp_sample.store(0);
}
static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)

12
src/drivers/imu/invensense/icm20948/ICM20948.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -73,12 +73,12 @@ private:
// Sensor Configuration
static constexpr float FIFO_SAMPLE_DT{1e6f / 9000.f};
static constexpr uint32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer
static constexpr int32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 9000 Hz gyro
static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4500 Hz accel
// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
static constexpr int32_t FIFO_MAX_SAMPLES{math::min(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 {
@ -169,8 +169,8 @@ private:
enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
px4::atomic<uint32_t> _drdy_count{0};
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
int32_t _drdy_count{0};
bool _data_ready_interrupt_enabled{false};
enum class STATE : uint8_t {
@ -181,7 +181,7 @@ private:
} _state{STATE::RESET};
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _checked_register_bank0{0};
static constexpr uint8_t size_register_bank0_cfg{6};

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

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -189,15 +189,19 @@ void ICM40609D::RunImpl()
break;
case STATE::FIFO_READ: {
uint32_t samples = 0;
hrt_abstime timestamp_sample = now;
uint8_t samples = 0;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
perf_count(_drdy_missed_perf);
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
timestamp_sample = drdy_timestamp_sample;
samples = _fifo_gyro_samples;
} else {
samples = _fifo_gyro_samples;
perf_count(_drdy_missed_perf);
}
// push backup schedule back
@ -219,6 +223,12 @@ void ICM40609D::RunImpl()
// FIFO count (size in bytes)
samples = (fifo_count / sizeof(FIFO::DATA));
// tolerate minor jitter, leave sample to next iteration if behind by only 1
if (samples == _fifo_gyro_samples + 1) {
timestamp_sample -= FIFO_SAMPLE_DT;
samples--;
}
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
FIFOReset();
@ -231,7 +241,7 @@ void ICM40609D::RunImpl()
bool success = false;
if (samples >= 1) {
if (FIFORead(now, samples)) {
if (FIFORead(timestamp_sample, samples)) {
success = true;
if (_failure_count > 0) {
@ -409,11 +419,8 @@ int ICM40609D::DataReadyInterruptCallback(int irq, void *context, void *arg)
void ICM40609D::DataReady()
{
uint32_t expected = 0;
if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
ScheduleNow();
}
_drdy_timestamp_sample.store(hrt_absolute_time());
ScheduleNow();
}
bool ICM40609D::DataReadyInterruptConfigure()
@ -580,7 +587,7 @@ void ICM40609D::FIFOReset()
RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
// reset while FIFO is disabled
_drdy_fifo_read_samples.store(0);
_drdy_timestamp_sample.store(0);
}
void ICM40609D::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)

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

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -75,7 +75,7 @@ private:
static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
static constexpr int32_t FIFO_MAX_SAMPLES{math::min(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 {
@ -146,7 +146,7 @@ private:
enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
bool _data_ready_interrupt_enabled{false};
enum class STATE : uint8_t {
@ -154,12 +154,10 @@ private:
WAIT_FOR_RESET,
CONFIGURE,
FIFO_READ,
};
STATE _state{STATE::RESET};
} _state{STATE::RESET};
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _checked_register_bank0{0};
static constexpr uint8_t size_register_bank0_cfg{10};

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

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -190,15 +190,19 @@ void ICM42605::RunImpl()
break;
case STATE::FIFO_READ: {
uint32_t samples = 0;
hrt_abstime timestamp_sample = now;
uint8_t samples = 0;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
perf_count(_drdy_missed_perf);
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
timestamp_sample = drdy_timestamp_sample;
samples = _fifo_gyro_samples;
} else {
samples = _fifo_gyro_samples;
perf_count(_drdy_missed_perf);
}
// push backup schedule back
@ -220,6 +224,12 @@ void ICM42605::RunImpl()
// FIFO count (size in bytes)
samples = (fifo_count / sizeof(FIFO::DATA));
// tolerate minor jitter, leave sample to next iteration if behind by only 1
if (samples == _fifo_gyro_samples + 1) {
timestamp_sample -= FIFO_SAMPLE_DT;
samples--;
}
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
FIFOReset();
@ -232,7 +242,7 @@ void ICM42605::RunImpl()
bool success = false;
if (samples >= 1) {
if (FIFORead(now, samples)) {
if (FIFORead(timestamp_sample, samples)) {
success = true;
if (_failure_count > 0) {
@ -410,11 +420,8 @@ int ICM42605::DataReadyInterruptCallback(int irq, void *context, void *arg)
void ICM42605::DataReady()
{
uint32_t expected = 0;
if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
ScheduleNow();
}
_drdy_timestamp_sample.store(hrt_absolute_time());
ScheduleNow();
}
bool ICM42605::DataReadyInterruptConfigure()
@ -581,7 +588,7 @@ void ICM42605::FIFOReset()
RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
// reset while FIFO is disabled
_drdy_fifo_read_samples.store(0);
_drdy_timestamp_sample.store(0);
}
void ICM42605::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)

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

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -75,7 +75,7 @@ private:
static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
static constexpr int32_t FIFO_MAX_SAMPLES{math::min(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 {
@ -146,7 +146,7 @@ private:
enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
bool _data_ready_interrupt_enabled{false};
enum class STATE : uint8_t {
@ -154,12 +154,10 @@ private:
WAIT_FOR_RESET,
CONFIGURE,
FIFO_READ,
};
STATE _state{STATE::RESET};
} _state{STATE::RESET};
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _checked_register_bank0{0};
static constexpr uint8_t size_register_bank0_cfg{10};

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

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -190,15 +190,19 @@ void ICM42688P::RunImpl()
break;
case STATE::FIFO_READ: {
uint32_t samples = 0;
hrt_abstime timestamp_sample = now;
uint8_t samples = 0;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
perf_count(_drdy_missed_perf);
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
timestamp_sample = drdy_timestamp_sample;
samples = _fifo_gyro_samples;
} else {
samples = _fifo_gyro_samples;
perf_count(_drdy_missed_perf);
}
// push backup schedule back
@ -220,6 +224,12 @@ void ICM42688P::RunImpl()
// FIFO count (size in bytes)
samples = (fifo_count / sizeof(FIFO::DATA));
// tolerate minor jitter, leave sample to next iteration if behind by only 1
if (samples == _fifo_gyro_samples + 1) {
timestamp_sample -= FIFO_SAMPLE_DT;
samples--;
}
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
FIFOReset();
@ -232,7 +242,7 @@ void ICM42688P::RunImpl()
bool success = false;
if (samples >= 1) {
if (FIFORead(now, samples)) {
if (FIFORead(timestamp_sample, samples)) {
success = true;
if (_failure_count > 0) {
@ -368,11 +378,8 @@ int ICM42688P::DataReadyInterruptCallback(int irq, void *context, void *arg)
void ICM42688P::DataReady()
{
uint32_t expected = 0;
if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
ScheduleNow();
}
_drdy_timestamp_sample.store(hrt_absolute_time());
ScheduleNow();
}
bool ICM42688P::DataReadyInterruptConfigure()
@ -553,7 +560,7 @@ void ICM42688P::FIFOReset()
RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
// reset while FIFO is disabled
_drdy_fifo_read_samples.store(0);
_drdy_timestamp_sample.store(0);
}
static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, const uint32_t c)

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

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -75,7 +75,7 @@ private:
static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
static constexpr int32_t FIFO_MAX_SAMPLES{math::min(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 {
@ -158,7 +158,7 @@ private:
enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
bool _data_ready_interrupt_enabled{false};
enum class STATE : uint8_t {
@ -169,7 +169,7 @@ private:
} _state{STATE::RESET};
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _checked_register_bank0{0};
static constexpr uint8_t size_register_bank0_cfg{13};

36
src/drivers/imu/invensense/mpu6000/MPU6000.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -197,9 +197,16 @@ void MPU6000::RunImpl()
break;
case STATE::FIFO_READ: {
hrt_abstime timestamp_sample = now;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_fifo_read_samples was set
if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
timestamp_sample = drdy_timestamp_sample;
} else {
perf_count(_drdy_missed_perf);
}
@ -220,7 +227,13 @@ void MPU6000::RunImpl()
} else {
// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
const uint8_t samples = fifo_count / sizeof(FIFO::DATA);
uint8_t samples = fifo_count / sizeof(FIFO::DATA);
// tolerate minor jitter, leave sample to next iteration if behind by only 1
if (samples == _fifo_gyro_samples + 1) {
timestamp_sample -= FIFO_SAMPLE_DT;
samples--;
}
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
@ -228,7 +241,7 @@ void MPU6000::RunImpl()
perf_count(_fifo_overflow_perf);
} else if (samples >= 1) {
if (FIFORead(now, samples)) {
if (FIFORead(timestamp_sample, samples)) {
success = true;
if (_failure_count > 0) {
@ -370,13 +383,10 @@ int MPU6000::DataReadyInterruptCallback(int irq, void *context, void *arg)
void MPU6000::DataReady()
{
uint32_t expected = 0;
// at least the required number of samples in the FIFO
if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
&& _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
_drdy_count.store(0);
if (++_drdy_count >= _fifo_gyro_samples) {
_drdy_timestamp_sample.store(hrt_absolute_time());
_drdy_count -= _fifo_gyro_samples;
ScheduleNow();
}
}
@ -488,8 +498,8 @@ void MPU6000::FIFOReset()
RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RESET, USER_CTRL_BIT::FIFO_EN);
// reset while FIFO is disabled
_drdy_count.store(0);
_drdy_fifo_read_samples.store(0);
_drdy_count = 0;
_drdy_timestamp_sample.store(0);
// FIFO_EN: enable both gyro and accel
// USER_CTRL: re-enable FIFO

16
src/drivers/imu/invensense/mpu6000/MPU6000.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -71,12 +71,12 @@ private:
// Sensor Configuration
static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
static constexpr uint32_t SAMPLES_PER_TRANSFER{8}; // ensure at least 1 new accel sample per transfer
static constexpr int32_t SAMPLES_PER_TRANSFER{8}; // ensure at least 1 new accel sample per transfer
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 8000 Hz gyro
static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 1000 Hz accel
// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
static constexpr int32_t FIFO_MAX_SAMPLES{math::min(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 {
@ -140,8 +140,8 @@ private:
FIFO::DATA _fifo_sample_last_new_accel{};
uint32_t _fifo_accel_samples_count{0};
px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
px4::atomic<uint32_t> _drdy_count{0};
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
int32_t _drdy_count{0};
bool _data_ready_interrupt_enabled{false};
enum class STATE : uint8_t {
@ -149,12 +149,10 @@ private:
WAIT_FOR_RESET,
CONFIGURE,
FIFO_READ,
};
STATE _state{STATE::RESET};
} _state{STATE::RESET};
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _checked_register{0};
static constexpr uint8_t size_register_cfg{7};

37
src/drivers/imu/invensense/mpu6500/MPU6500.cpp
View File

@ -229,9 +229,16 @@ void MPU6500::RunImpl()
break;
case STATE::FIFO_READ: {
hrt_abstime timestamp_sample = now;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_fifo_read_samples was set
if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
timestamp_sample = drdy_timestamp_sample;
} else {
perf_count(_drdy_missed_perf);
}
@ -252,16 +259,21 @@ void MPU6500::RunImpl()
} else {
// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) *
SAMPLES_PER_TRANSFER; // round down to nearest
uint8_t samples = fifo_count / sizeof(FIFO::DATA);
// tolerate minor jitter, leave sample to next iteration if behind by only 1
if (samples == _fifo_gyro_samples + 1) {
timestamp_sample -= FIFO_SAMPLE_DT;
samples--;
}
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
FIFOReset();
perf_count(_fifo_overflow_perf);
} else if (samples >= 1) {
if (FIFORead(now, samples)) {
} else if (samples >= SAMPLES_PER_TRANSFER) {
if (FIFORead(timestamp_sample, samples)) {
success = true;
if (_failure_count > 0) {
@ -403,13 +415,10 @@ int MPU6500::DataReadyInterruptCallback(int irq, void *context, void *arg)
void MPU6500::DataReady()
{
uint32_t expected = 0;
// at least the required number of samples in the FIFO
if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
&& _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
_drdy_count.store(0);
if (++_drdy_count >= _fifo_gyro_samples) {
_drdy_timestamp_sample.store(hrt_absolute_time());
_drdy_count -= _fifo_gyro_samples;
ScheduleNow();
}
}
@ -521,8 +530,8 @@ void MPU6500::FIFOReset()
RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN);
// reset while FIFO is disabled
_drdy_count.store(0);
_drdy_fifo_read_samples.store(0);
_drdy_count = 0;
_drdy_timestamp_sample.store(0);
// FIFO_EN: enable both gyro and accel
// USER_CTRL: re-enable FIFO

10
src/drivers/imu/invensense/mpu6500/MPU6500.hpp
View File

@ -71,12 +71,12 @@ private:
// Sensor Configuration
static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
static constexpr uint32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer
static constexpr int32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 8000 Hz gyro
static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel
// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
static constexpr int32_t FIFO_MAX_SAMPLES{math::min(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 {
@ -138,8 +138,8 @@ private:
hrt_abstime _temperature_update_timestamp{0};
int _failure_count{0};
px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
px4::atomic<uint32_t> _drdy_count{0};
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
int32_t _drdy_count{0};
bool _data_ready_interrupt_enabled{false};
enum class STATE : uint8_t {
@ -150,7 +150,7 @@ private:
} _state{STATE::RESET};
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _checked_register{0};
static constexpr uint8_t size_register_cfg{15};

37
src/drivers/imu/invensense/mpu9250/MPU9250.cpp
View File

@ -264,9 +264,16 @@ void MPU9250::RunImpl()
break;
case STATE::FIFO_READ: {
hrt_abstime timestamp_sample = now;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_fifo_read_samples was set
if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
timestamp_sample = drdy_timestamp_sample;
} else {
perf_count(_drdy_missed_perf);
}
@ -287,16 +294,21 @@ void MPU9250::RunImpl()
} else {
// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) *
SAMPLES_PER_TRANSFER; // round down to nearest
uint8_t samples = fifo_count / sizeof(FIFO::DATA);
// tolerate minor jitter, leave sample to next iteration if behind by only 1
if (samples == _fifo_gyro_samples + 1) {
timestamp_sample -= FIFO_SAMPLE_DT;
samples--;
}
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
FIFOReset();
perf_count(_fifo_overflow_perf);
} else if (samples >= 1) {
if (FIFORead(now, samples)) {
} else if (samples >= SAMPLES_PER_TRANSFER) {
if (FIFORead(timestamp_sample, samples)) {
success = true;
if (_failure_count > 0) {
@ -438,13 +450,10 @@ int MPU9250::DataReadyInterruptCallback(int irq, void *context, void *arg)
void MPU9250::DataReady()
{
uint32_t expected = 0;
// at least the required number of samples in the FIFO
if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
&& _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
_drdy_count.store(0);
if (++_drdy_count >= _fifo_gyro_samples) {
_drdy_timestamp_sample.store(hrt_absolute_time());
_drdy_count -= _fifo_gyro_samples;
ScheduleNow();
}
}
@ -594,8 +603,8 @@ void MPU9250::FIFOReset()
RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN);
// reset while FIFO is disabled
_drdy_count.store(0);
_drdy_fifo_read_samples.store(0);
_drdy_count = 0;
_drdy_timestamp_sample.store(0);
// FIFO_EN: enable both gyro and accel
// USER_CTRL: re-enable FIFO

10
src/drivers/imu/invensense/mpu9250/MPU9250.hpp
View File

@ -73,12 +73,12 @@ private:
// Sensor Configuration
static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
static constexpr uint32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer
static constexpr int32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 8000 Hz gyro
static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel
// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
static constexpr int32_t FIFO_MAX_SAMPLES{math::min(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 {
@ -150,8 +150,8 @@ private:
hrt_abstime _temperature_update_timestamp{0};
int _failure_count{0};
px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
px4::atomic<uint32_t> _drdy_count{0};
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
int32_t _drdy_count{0};
bool _data_ready_interrupt_enabled{false};
enum class STATE : uint8_t {
@ -162,7 +162,7 @@ private:
} _state{STATE::RESET};
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _checked_register{0};
static constexpr uint8_t size_register_cfg{18};

39
src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -197,9 +197,16 @@ void MPU9250_I2C::RunImpl()
break;
case STATE::FIFO_READ: {
hrt_abstime timestamp_sample = now;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_fifo_read_samples was set
if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
timestamp_sample = drdy_timestamp_sample;
} else {
perf_count(_drdy_missed_perf);
}
@ -220,16 +227,21 @@ void MPU9250_I2C::RunImpl()
} else {
// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) *
SAMPLES_PER_TRANSFER; // round down to nearest
uint8_t samples = fifo_count / sizeof(FIFO::DATA);
// tolerate minor jitter, leave sample to next iteration if behind by only 1
if (samples == _fifo_gyro_samples + 1) {
timestamp_sample -= FIFO_SAMPLE_DT;
samples--;
}
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
FIFOReset();
perf_count(_fifo_overflow_perf);
} else if (samples >= 1) {
if (FIFORead(now, samples)) {
} else if (samples >= SAMPLES_PER_TRANSFER) {
if (FIFORead(timestamp_sample, samples)) {
success = true;
if (_failure_count > 0) {
@ -371,13 +383,10 @@ int MPU9250_I2C::DataReadyInterruptCallback(int irq, void *context, void *arg)
void MPU9250_I2C::DataReady()
{
uint32_t expected = 0;
// at least the required number of samples in the FIFO
if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
&& _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
_drdy_count.store(0);
if (++_drdy_count >= _fifo_gyro_samples) {
_drdy_timestamp_sample.store(hrt_absolute_time());
_drdy_count -= _fifo_gyro_samples;
ScheduleNow();
}
}
@ -495,8 +504,8 @@ void MPU9250_I2C::FIFOReset()
RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN);
// reset while FIFO is disabled
_drdy_count.store(0);
_drdy_fifo_read_samples.store(0);
_drdy_count = 0;
_drdy_timestamp_sample.store(0);
// FIFO_EN: enable both gyro and accel
// USER_CTRL: re-enable FIFO

16
src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -71,12 +71,12 @@ private:
// Sensor Configuration
static constexpr float FIFO_SAMPLE_DT{1e6f / 1000.f};
static constexpr uint32_t SAMPLES_PER_TRANSFER{1}; // ensure at least 1 new accel sample per transfer
static constexpr int32_t SAMPLES_PER_TRANSFER{1}; // ensure at least 1 new accel sample per transfer
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 1000 Hz gyro
static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 1000 Hz accel
// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
static constexpr int32_t FIFO_MAX_SAMPLES{math::min(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 {
@ -136,8 +136,8 @@ private:
hrt_abstime _temperature_update_timestamp{0};
int _failure_count{0};
px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
px4::atomic<uint32_t> _drdy_count{0};
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
int32_t _drdy_count{0};
bool _data_ready_interrupt_enabled{false};
enum class STATE : uint8_t {
@ -145,12 +145,10 @@ private:
WAIT_FOR_RESET,
CONFIGURE,
FIFO_READ,
};
STATE _state{STATE::RESET};
} _state{STATE::RESET};
uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 Hz transfer interval
uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _checked_register{0};
static constexpr uint8_t size_register_cfg{9};

14
src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -170,11 +170,13 @@ void LSM9DS1::RunImpl()
break;
case STATE::FIFO_READ: {
hrt_abstime timestamp_sample = now;
// always check current FIFO count
bool success = false;
// Number of unread words (16-bit axes) stored in FIFO.
const uint8_t FIFO_SRC = RegisterRead(Register::FIFO_SRC);
const uint8_t samples = FIFO_SRC & static_cast<uint8_t>(FIFO_SRC_BIT::FSS);
uint8_t samples = FIFO_SRC & static_cast<uint8_t>(FIFO_SRC_BIT::FSS);
if (FIFO_SRC & FIFO_SRC_BIT::OVRN) {
// overflow
@ -185,13 +187,19 @@ void LSM9DS1::RunImpl()
perf_count(_fifo_empty_perf);
} else {
// tolerate minor jitter, leave sample to next iteration if behind by only 1
if (samples == _fifo_gyro_samples + 1) {
timestamp_sample -= FIFO_SAMPLE_DT;
samples--;
}
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
FIFOReset();
perf_count(_fifo_overflow_perf);
} else if (samples >= 1) {
if (FIFORead(now, samples)) {
if (FIFORead(timestamp_sample, samples)) {
success = true;
if (_failure_count > 0) {

6
src/drivers/imu/st/lsm9ds1/LSM9DS1.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 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
@ -74,7 +74,7 @@ private:
static constexpr float ACCEL_RATE{ST_LSM9DS1::LA_ODR}; // 952 Hz accel
// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / 12, 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(math::min(FIFO::SIZE / 12, 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))};
struct register_config_t {
Register reg;
@ -123,7 +123,7 @@ private:
} _state{STATE::RESET};
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _checked_register{0};
static constexpr uint8_t size_register_cfg{6};