From f0de50c20d2c1b33e06691d19008d59799cb8c31 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 7 Feb 2022 20:34:08 -0500 Subject: [PATCH] WIP: drivers/imu/bosch output data changes --- msg/sensor_accel.msg | 2 + msg/sensor_gyro.msg | 2 + .../imu/bosch/bmi055/BMI055_Accelerometer.cpp | 81 ++++++++++--------- .../imu/bosch/bmi055/BMI055_Accelerometer.hpp | 17 ++-- .../imu/bosch/bmi055/BMI055_Gyroscope.cpp | 79 +++++++++--------- .../imu/bosch/bmi055/BMI055_Gyroscope.hpp | 15 ++-- src/drivers/imu/bosch/bmi055/CMakeLists.txt | 3 +- .../imu/bosch/bmi088/BMI088_Accelerometer.cpp | 78 ++++++++++-------- .../imu/bosch/bmi088/BMI088_Accelerometer.hpp | 17 ++-- .../imu/bosch/bmi088/BMI088_Gyroscope.cpp | 79 +++++++++--------- .../imu/bosch/bmi088/BMI088_Gyroscope.hpp | 15 ++-- src/drivers/imu/bosch/bmi088/CMakeLists.txt | 3 +- .../bosch/bmi088_i2c/BMI088_Accelerometer.cpp | 26 +++--- .../bosch/bmi088_i2c/BMI088_Accelerometer.hpp | 8 +- .../imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp | 6 +- .../imu/bosch/bmi088_i2c/BMI088_Gyroscope.hpp | 5 +- .../imu/bosch/bmi088_i2c/CMakeLists.txt | 2 - 17 files changed, 238 insertions(+), 200 deletions(-) diff --git a/msg/sensor_accel.msg b/msg/sensor_accel.msg index e47d813a7a..f27170e043 100644 --- a/msg/sensor_accel.msg +++ b/msg/sensor_accel.msg @@ -7,6 +7,8 @@ float32 x # acceleration in the FRD board frame X-axis in m/s^2 float32 y # acceleration in the FRD board frame Y-axis in m/s^2 float32 z # acceleration in the FRD board frame Z-axis in m/s^2 +float32 range # TODO + float32 temperature # temperature in degrees Celsius uint32 error_count diff --git a/msg/sensor_gyro.msg b/msg/sensor_gyro.msg index b1025e1d12..3c9c4e952a 100644 --- a/msg/sensor_gyro.msg +++ b/msg/sensor_gyro.msg @@ -7,6 +7,8 @@ float32 x # angular velocity in the FRD board frame X-axis in ra float32 y # angular velocity in the FRD board frame Y-axis in rad/s float32 z # angular velocity in the FRD board frame Z-axis in rad/s +float32 range # TODO + float32 temperature # temperature in degrees Celsius uint32 error_count diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp index 77cf537302..0404c8b49c 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -42,13 +42,13 @@ namespace Bosch::BMI055::Accelerometer BMI055_Accelerometer::BMI055_Accelerometer(const I2CSPIDriverConfig &config) : BMI055(config), - _px4_accel(get_device_id(), config.rotation) + _rotation(config.rotation) { if (config.drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_accel: DRDY missed"); } - ConfigureSampleRate(_px4_accel.get_max_rate_hz()); + ConfigureSampleRate(RATE); } BMI055_Accelerometer::~BMI055_Accelerometer() @@ -202,16 +202,6 @@ void BMI055_Accelerometer::RunImpl() 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 - static_cast(FIFO_SAMPLE_DT); - } - - samples--; - } - if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) { success = true; @@ -263,23 +253,23 @@ void BMI055_Accelerometer::ConfigureAccel() switch (PMU_RANGE) { 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); + _accel_scale = CONSTANTS_ONE_G / 1024.f; // 1024 LSB/g, 0.98mg/LSB + _accel_range = 2.f * CONSTANTS_ONE_G; break; 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); + _accel_scale = CONSTANTS_ONE_G / 512.f; // 512 LSB/g, 1.95mg/LSB + _accel_range = 4.f * CONSTANTS_ONE_G; break; 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); + _accel_scale = CONSTANTS_ONE_G / 256.f; // 256 LSB/g, 3.91mg/LSB + _accel_range = 8.f * CONSTANTS_ONE_G; break; 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); + _accel_scale = CONSTANTS_ONE_G / 128.f; // 128 LSB/g, 7.81mg/LSB + _accel_range = 16.f * CONSTANTS_ONE_G; break; } } @@ -415,32 +405,51 @@ bool BMI055_Accelerometer::FIFORead(const hrt_abstime ×tamp_sample, uint8_t return false; } - sensor_accel_fifo_s accel{}; - accel.timestamp_sample = timestamp_sample; - accel.samples = samples; - accel.dt = FIFO_SAMPLE_DT; + sensor_accel_s sensor_accel{}; + sensor_accel.timestamp_sample = timestamp_sample; + sensor_accel.device_id = get_device_id(); + + float accel_sum[3] {}; for (int i = 0; i < samples; i++) { const FIFO::DATA &fifo_sample = buffer.f[i]; // acc_x_msb<11:4> + acc_x_lsb<3:0> - const int16_t accel_x = combine(fifo_sample.ACCD_X_MSB, fifo_sample.ACCD_X_LSB) >> 4; - const int16_t accel_y = combine(fifo_sample.ACCD_Y_MSB, fifo_sample.ACCD_Y_LSB) >> 4; - const int16_t accel_z = combine(fifo_sample.ACCD_Z_MSB, fifo_sample.ACCD_Z_LSB) >> 4; + int16_t accel_x = combine(fifo_sample.ACCD_X_MSB, fifo_sample.ACCD_X_LSB) >> 4; + int16_t accel_y = combine(fifo_sample.ACCD_Y_MSB, fifo_sample.ACCD_Y_LSB) >> 4; + int16_t accel_z = combine(fifo_sample.ACCD_Z_MSB, fifo_sample.ACCD_Z_LSB) >> 4; // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) - accel.x[i] = accel_x; - accel.y[i] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[i] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + // accel_x = accel_x; + accel_y = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; + accel_z = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + + rotate_3i(_rotation, accel_x, accel_y, accel_z); + + accel_sum[0] += accel_x; + accel_sum[1] += accel_y; + accel_sum[2] += accel_z; } - _px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + - perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); + if (samples > 0) { + sensor_accel.x = (accel_sum[0] / samples) * _accel_scale; + sensor_accel.y = (accel_sum[1] / samples) * _accel_scale; + sensor_accel.z = (accel_sum[2] / samples) * _accel_scale; - _px4_accel.updateFIFO(accel); + sensor_accel.range = _accel_range; + sensor_accel.samples = samples; + sensor_accel.temperature = _temperature; + sensor_accel.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf); - return true; + sensor_accel.timestamp = hrt_absolute_time(); + _sensor_accel_pub.publish(sensor_accel); + + return true; + } + + return false; } void BMI055_Accelerometer::FIFOReset() @@ -472,7 +481,7 @@ void BMI055_Accelerometer::UpdateTemperature() float temperature = static_cast(RegisterRead(Register::ACCD_TEMP)) * 0.5f + 23.f; if (PX4_ISFINITE(temperature)) { - _px4_accel.set_temperature(temperature); + _temperature = temperature; } else { perf_count(_bad_transfer_perf); diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp index 31906c8d96..de50f92dcb 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,10 +35,11 @@ #include "BMI055.hpp" -#include - #include "Bosch_BMI055_Accelerometer_Registers.hpp" +#include +#include + namespace Bosch::BMI055::Accelerometer { @@ -58,7 +59,7 @@ private: static constexpr uint32_t RATE{2000}; // 2000 Hz static constexpr float FIFO_SAMPLE_DT{1e6f / RATE}; - 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]))}; + static constexpr int32_t FIFO_MAX_SAMPLES{FIFO::SIZE / sizeof(FIFO::DATA)}; // Transfer data struct FIFOTransferBuffer { @@ -97,7 +98,13 @@ private: void UpdateTemperature(); - PX4Accelerometer _px4_accel; + uORB::PublicationMulti _sensor_accel_pub{ORB_ID(sensor_accel)}; + + const enum Rotation _rotation; + float _accel_scale{0.f}; + float _accel_range{0.f}; + + float _temperature{NAN}; perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad register")}; perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad transfer")}; diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp index d2ea514530..1594e026ed 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,8 +33,6 @@ #include "BMI055_Gyroscope.hpp" -#include - using namespace time_literals; namespace Bosch::BMI055::Gyroscope @@ -42,13 +40,13 @@ namespace Bosch::BMI055::Gyroscope BMI055_Gyroscope::BMI055_Gyroscope(const I2CSPIDriverConfig &config) : BMI055(config), - _px4_gyro(get_device_id(), config.rotation) + _rotation(config.rotation) { if (config.drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_gyro: DRDY missed"); } - ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); + ConfigureSampleRate(RATE); } BMI055_Gyroscope::~BMI055_Gyroscope() @@ -202,16 +200,6 @@ void BMI055_Gyroscope::RunImpl() 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 - static_cast(FIFO_SAMPLE_DT); - } - - samples--; - } - if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) { success = true; @@ -256,28 +244,28 @@ void BMI055_Gyroscope::ConfigureGyro() switch (RANGE) { case gyro_range_2000_dps: - _px4_gyro.set_scale(math::radians(1.f / 16.384f)); - _px4_gyro.set_range(math::radians(2000.f)); + _gyro_scale = math::radians(1.f / 16.384f); + _gyro_range = math::radians(2000.f); break; case gyro_range_1000_dps: - _px4_gyro.set_scale(math::radians(1.f / 32.768f)); - _px4_gyro.set_range(math::radians(1000.f)); + _gyro_scale = math::radians(1.f / 32.768f); + _gyro_range = math::radians(1000.f); break; case gyro_range_500_dps: - _px4_gyro.set_scale(math::radians(1.f / 65.536f)); - _px4_gyro.set_range(math::radians(500.f)); + _gyro_scale = math::radians(1.f / 65.536f); + _gyro_range = math::radians(500.f); break; case gyro_range_250_dps: - _px4_gyro.set_scale(math::radians(1.f / 131.072f)); - _px4_gyro.set_range(math::radians(250.f)); + _gyro_scale = math::radians(1.f / 131.072f); + _gyro_range = math::radians(250.f); break; case gyro_range_125_dps: - _px4_gyro.set_scale(math::radians(1.f / 262.144f)); - _px4_gyro.set_range(math::radians(125.f)); + _gyro_scale = math::radians(1.f / 262.144f); + _gyro_range = math::radians(125.f); break; } } @@ -413,29 +401,44 @@ bool BMI055_Gyroscope::FIFORead(const hrt_abstime ×tamp_sample, uint8_t sam return false; } - sensor_gyro_fifo_s gyro{}; - gyro.timestamp_sample = timestamp_sample; - gyro.samples = samples; - gyro.dt = FIFO_SAMPLE_DT; + sensor_gyro_s sensor_gyro{}; + sensor_gyro.timestamp_sample = timestamp_sample; + sensor_gyro.device_id = get_device_id(); + + float gyro_sum[3] {}; for (int i = 0; i < samples; i++) { const FIFO::DATA &fifo_sample = buffer.f[i]; - const int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB); - const int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB); - const int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB); + int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB); + int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB); + int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB); // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) - gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + // gyro_x = gyro_x; + gyro_y = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; + gyro_z = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + + rotate_3i(_rotation, gyro_x, gyro_y, gyro_z); + + gyro_sum[0] += gyro_x * _gyro_scale; + gyro_sum[1] += gyro_y * _gyro_scale; + gyro_sum[2] += gyro_z * _gyro_scale; } - _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + - perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); + sensor_gyro.x = gyro_sum[0] / samples; + sensor_gyro.y = gyro_sum[1] / samples; + sensor_gyro.z = gyro_sum[2] / samples; - _px4_gyro.updateFIFO(gyro); + sensor_gyro.range = _gyro_range; + sensor_gyro.samples = samples; + sensor_gyro.temperature = NAN; + sensor_gyro.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf); + + sensor_gyro.timestamp = hrt_absolute_time(); + _sensor_gyro_pub.publish(sensor_gyro); return true; } diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp index 13c0468f0d..0f062e3e63 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,10 +35,11 @@ #include "BMI055.hpp" -#include - #include "Bosch_BMI055_Gyroscope_Registers.hpp" +#include +#include + namespace Bosch::BMI055::Gyroscope { @@ -58,7 +59,7 @@ private: static constexpr uint32_t RATE{2000}; // 2000 Hz static constexpr float FIFO_SAMPLE_DT{1e6f / RATE}; - static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))}; + static constexpr int32_t FIFO_MAX_SAMPLES{FIFO::SIZE / sizeof(FIFO::DATA)}; // Transfer data struct FIFOTransferBuffer { @@ -95,7 +96,11 @@ private: bool FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples); void FIFOReset(); - PX4Gyroscope _px4_gyro; + uORB::PublicationMulti _sensor_gyro_pub{ORB_ID(sensor_gyro)}; + + const enum Rotation _rotation; + float _gyro_scale{0.f}; + float _gyro_range{0.f}; perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad register")}; perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad transfer")}; diff --git a/src/drivers/imu/bosch/bmi055/CMakeLists.txt b/src/drivers/imu/bosch/bmi055/CMakeLists.txt index bf24faffde..a00191e54a 100644 --- a/src/drivers/imu/bosch/bmi055/CMakeLists.txt +++ b/src/drivers/imu/bosch/bmi055/CMakeLists.txt @@ -35,6 +35,7 @@ px4_add_module( MODULE drivers__imu__bosch__bmi055 MAIN bmi055 COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} SRCS Bosch_BMI055_Accelerometer_Registers.hpp Bosch_BMI055_Gyroscope_Registers.hpp @@ -48,7 +49,5 @@ px4_add_module( bmi055_main.cpp DEPENDS - drivers_accelerometer - drivers_gyroscope px4_work_queue ) diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp index 8f0dc0068b..b2976dbcc3 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp +++ b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -42,13 +42,13 @@ namespace Bosch::BMI088::Accelerometer BMI088_Accelerometer::BMI088_Accelerometer(const I2CSPIDriverConfig &config) : BMI088(config), - _px4_accel(get_device_id(), config.rotation) + _rotation(config.rotation) { if (config.drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_accel: DRDY missed"); } - ConfigureSampleRate(_px4_accel.get_max_rate_hz()); + ConfigureSampleRate(RATE); } BMI088_Accelerometer::~BMI088_Accelerometer() @@ -218,12 +218,6 @@ 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 -= static_cast(FIFO_SAMPLE_DT); - samples--; - } - if (samples > FIFO_MAX_SAMPLES) { // not technically an overflow, but more samples than we expected or can publish FIFOReset(); @@ -286,23 +280,23 @@ void BMI088_Accelerometer::ConfigureAccel() switch (ACC_RANGE) { case acc_range_3g: - _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f); - _px4_accel.set_range(3.f * CONSTANTS_ONE_G); + _accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f; + _accel_range = 3.f * CONSTANTS_ONE_G; break; case acc_range_6g: - _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f); - _px4_accel.set_range(6.f * CONSTANTS_ONE_G); + _accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f; + _accel_range = 6.f * CONSTANTS_ONE_G; break; case acc_range_12g: - _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f); - _px4_accel.set_range(12.f * CONSTANTS_ONE_G); + _accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f; + _accel_range = 12.f * CONSTANTS_ONE_G; break; case acc_range_24g: - _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f); - _px4_accel.set_range(24.f * CONSTANTS_ONE_G); + _accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f; + _accel_range = 24.f * CONSTANTS_ONE_G; break; } } @@ -483,15 +477,17 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime ×tamp_sample, uint8_t return false; } - sensor_accel_fifo_s accel{}; - accel.timestamp_sample = timestamp_sample; - accel.samples = 0; - accel.dt = FIFO_SAMPLE_DT; + sensor_accel_s sensor_accel{}; + sensor_accel.timestamp_sample = timestamp_sample; + sensor_accel.device_id = get_device_id(); // first find all sensor data frames in the buffer uint8_t *data_buffer = (uint8_t *)&buffer.f[0]; unsigned fifo_buffer_index = 0; // start of buffer + float accel_sum[3] {}; + int accel_samples = 0; + while (fifo_buffer_index < math::min(fifo_byte_counter, transfer_size - 4)) { // look for header signature (first 6 bits) followed by two bits indicating the status of INT1 and INT2 switch (data_buffer[fifo_buffer_index] & 0xFC) { @@ -500,16 +496,23 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime ×tamp_sample, uint8_t // Frame length: 7 bytes (1 byte header + 6 bytes payload) FIFO::DATA *fifo_sample = (FIFO::DATA *)&data_buffer[fifo_buffer_index]; - const int16_t accel_x = combine(fifo_sample->ACC_X_MSB, fifo_sample->ACC_X_LSB); - const int16_t accel_y = combine(fifo_sample->ACC_Y_MSB, fifo_sample->ACC_Y_LSB); - const int16_t accel_z = combine(fifo_sample->ACC_Z_MSB, fifo_sample->ACC_Z_LSB); + int16_t accel_x = combine(fifo_sample->ACC_X_MSB, fifo_sample->ACC_X_LSB); + int16_t accel_y = combine(fifo_sample->ACC_Y_MSB, fifo_sample->ACC_Y_LSB); + int16_t accel_z = combine(fifo_sample->ACC_Z_MSB, fifo_sample->ACC_Z_LSB); // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) - accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; - accel.samples++; + // accel_x = accel_x; + accel_y = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; + accel_z = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + + rotate_3i(_rotation, accel_x, accel_y, accel_z); + + accel_sum[0] += accel_x; + accel_sum[1] += accel_y; + accel_sum[2] += accel_z; + + accel_samples++; fifo_buffer_index += 7; // move forward to next record } @@ -549,11 +552,20 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime ×tamp_sample, uint8_t } } - _px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + - perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); + if (accel_samples > 0) { + sensor_accel.x = (accel_sum[0] / accel_samples) * _accel_scale; + sensor_accel.y = (accel_sum[1] / accel_samples) * _accel_scale; + sensor_accel.z = (accel_sum[2] / accel_samples) * _accel_scale; + + sensor_accel.range = _accel_range; + sensor_accel.samples = accel_samples; + sensor_accel.temperature = _temperature; + sensor_accel.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf); + + sensor_accel.timestamp = hrt_absolute_time(); + _sensor_accel_pub.publish(sensor_accel); - if (accel.samples > 0) { - _px4_accel.updateFIFO(accel); return true; } @@ -600,7 +612,7 @@ void BMI088_Accelerometer::UpdateTemperature() float temperature = (Temp_int11 * 0.125f) + 23.f; // Temp_int11 * 0.125°C/LSB + 23°C if (PX4_ISFINITE(temperature)) { - _px4_accel.set_temperature(temperature); + _temperature = temperature; } else { perf_count(_bad_transfer_perf); diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp index ccf5f40d3a..63e0ed2709 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp +++ b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,10 +35,11 @@ #include "BMI088.hpp" -#include - #include "Bosch_BMI088_Accelerometer_Registers.hpp" +#include +#include + namespace Bosch::BMI088::Accelerometer { @@ -58,7 +59,7 @@ private: static constexpr uint32_t RATE{1600}; // 1600 Hz static constexpr float FIFO_SAMPLE_DT{1e6f / RATE}; - 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]))}; + static constexpr int32_t FIFO_MAX_SAMPLES{FIFO::SIZE / sizeof(FIFO::DATA)}; // Transfer data struct FIFOTransferBuffer { @@ -101,7 +102,13 @@ private: void UpdateTemperature(); - PX4Accelerometer _px4_accel; + uORB::PublicationMulti _sensor_accel_pub{ORB_ID(sensor_accel)}; + + const enum Rotation _rotation; + float _accel_scale{0.f}; + float _accel_range{0.f}; + + float _temperature{NAN}; perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad register")}; perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad transfer")}; diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp index 7dac23e578..db3efcf7f8 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,8 +33,6 @@ #include "BMI088_Gyroscope.hpp" -#include - using namespace time_literals; namespace Bosch::BMI088::Gyroscope @@ -42,13 +40,13 @@ namespace Bosch::BMI088::Gyroscope BMI088_Gyroscope::BMI088_Gyroscope(const I2CSPIDriverConfig &config) : BMI088(config), - _px4_gyro(get_device_id(), config.rotation) + _rotation(config.rotation) { if (config.drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_gyro: DRDY missed"); } - ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); + ConfigureSampleRate(RATE); } BMI088_Gyroscope::~BMI088_Gyroscope() @@ -203,16 +201,6 @@ void BMI088_Gyroscope::RunImpl() 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 - static_cast(FIFO_SAMPLE_DT); - } - - samples--; - } - if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) { success = true; @@ -257,28 +245,28 @@ void BMI088_Gyroscope::ConfigureGyro() switch (GYRO_RANGE) { case gyro_range_2000_dps: - _px4_gyro.set_scale(math::radians(1.f / 16.384f)); - _px4_gyro.set_range(math::radians(2000.f)); + _gyro_scale = math::radians(1.f / 16.384f); + _gyro_range = math::radians(2000.f); break; case gyro_range_1000_dps: - _px4_gyro.set_scale(math::radians(1.f / 32.768f)); - _px4_gyro.set_range(math::radians(1000.f)); + _gyro_scale = math::radians(1.f / 32.768f); + _gyro_range = math::radians(1000.f); break; case gyro_range_500_dps: - _px4_gyro.set_scale(math::radians(1.f / 65.536f)); - _px4_gyro.set_range(math::radians(500.f)); + _gyro_scale = math::radians(1.f / 65.536f); + _gyro_range = math::radians(500.f); break; case gyro_range_250_dps: - _px4_gyro.set_scale(math::radians(1.f / 131.072f)); - _px4_gyro.set_range(math::radians(250.f)); + _gyro_scale = math::radians(1.f / 131.072f); + _gyro_range = math::radians(250.f); break; case gyro_range_125_dps: - _px4_gyro.set_scale(math::radians(1.f / 262.144f)); - _px4_gyro.set_range(math::radians(125.f)); + _gyro_scale = math::radians(1.f / 262.144f); + _gyro_range = math::radians(125.f); break; } } @@ -414,29 +402,44 @@ bool BMI088_Gyroscope::FIFORead(const hrt_abstime ×tamp_sample, uint8_t sam return false; } - sensor_gyro_fifo_s gyro{}; - gyro.timestamp_sample = timestamp_sample; - gyro.samples = samples; - gyro.dt = FIFO_SAMPLE_DT; + sensor_gyro_s sensor_gyro{}; + sensor_gyro.timestamp_sample = timestamp_sample; + sensor_gyro.device_id = get_device_id(); + + float gyro_sum[3] {}; for (int i = 0; i < samples; i++) { const FIFO::DATA &fifo_sample = buffer.f[i]; - const int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB); - const int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB); - const int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB); + int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB); + int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB); + int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB); // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) - gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + // gyro_x = gyro_x; + gyro_y = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; + gyro_z = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + + rotate_3i(_rotation, gyro_x, gyro_y, gyro_z); + + gyro_sum[0] += gyro_x * _gyro_scale; + gyro_sum[1] += gyro_y * _gyro_scale; + gyro_sum[2] += gyro_z * _gyro_scale; } - _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + - perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); + sensor_gyro.x = gyro_sum[0] / samples; + sensor_gyro.y = gyro_sum[1] / samples; + sensor_gyro.z = gyro_sum[2] / samples; - _px4_gyro.updateFIFO(gyro); + sensor_gyro.range = _gyro_range; + sensor_gyro.samples = samples; + sensor_gyro.temperature = NAN; + sensor_gyro.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf); + + sensor_gyro.timestamp = hrt_absolute_time(); + _sensor_gyro_pub.publish(sensor_gyro); return true; } diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp index 59d847bf02..879caec6df 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp +++ b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,10 +35,11 @@ #include "BMI088.hpp" -#include - #include "Bosch_BMI088_Gyroscope_Registers.hpp" +#include +#include + namespace Bosch::BMI088::Gyroscope { @@ -58,7 +59,7 @@ private: static constexpr uint32_t RATE{2000}; // 2000 Hz static constexpr float FIFO_SAMPLE_DT{1e6f / RATE}; - static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))}; + static constexpr int32_t FIFO_MAX_SAMPLES{FIFO::SIZE / sizeof(FIFO::DATA)}; // Transfer data struct FIFOTransferBuffer { @@ -95,7 +96,11 @@ private: bool FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples); void FIFOReset(); - PX4Gyroscope _px4_gyro; + uORB::PublicationMulti _sensor_gyro_pub{ORB_ID(sensor_gyro)}; + + const enum Rotation _rotation; + float _gyro_scale{0.f}; + float _gyro_range{0.f}; perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad register")}; perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad transfer")}; diff --git a/src/drivers/imu/bosch/bmi088/CMakeLists.txt b/src/drivers/imu/bosch/bmi088/CMakeLists.txt index 8326953f07..c60724d5ab 100644 --- a/src/drivers/imu/bosch/bmi088/CMakeLists.txt +++ b/src/drivers/imu/bosch/bmi088/CMakeLists.txt @@ -35,6 +35,7 @@ px4_add_module( MODULE drivers__imu__bosch__bmi088 MAIN bmi088 COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} SRCS Bosch_BMI088_Accelerometer_Registers.hpp Bosch_BMI088_Gyroscope_Registers.hpp @@ -48,7 +49,5 @@ px4_add_module( bmi088_main.cpp DEPENDS - drivers_accelerometer - drivers_gyroscope px4_work_queue ) diff --git a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp index 6d7e4d6739..7c3198411e 100644 --- a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp +++ b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -41,13 +41,13 @@ namespace Bosch::BMI088::Accelerometer { BMI088_Accelerometer::BMI088_Accelerometer(const I2CSPIDriverConfig &config) : BMI088(config), - _px4_accel(get_device_id(), config.rotation) + _rotation(config.rotation) { if (config.drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_accel: DRDY missed"); } - ConfigureSampleRate(1600); + ConfigureSampleRate(RATE); } BMI088_Accelerometer::~BMI088_Accelerometer() @@ -204,23 +204,23 @@ void BMI088_Accelerometer::ConfigureAccel() switch (ACC_RANGE) { case acc_range_3g: - _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f); - _px4_accel.set_range(3.f * CONSTANTS_ONE_G); + _accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f; + _accel_range = 3.f * CONSTANTS_ONE_G; break; case acc_range_6g: - _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f); - _px4_accel.set_range(6.f * CONSTANTS_ONE_G); + _accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f; + _accel_range = 6.f * CONSTANTS_ONE_G; break; case acc_range_12g: - _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f); - _px4_accel.set_range(12.f * CONSTANTS_ONE_G); + _accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f; + _accel_range = 12.f * CONSTANTS_ONE_G; break; case acc_range_24g: - _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f); - _px4_accel.set_range(24.f * CONSTANTS_ONE_G); + _accel_scale = CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f; + _accel_range = 24.f * CONSTANTS_ONE_G; break; } } @@ -386,10 +386,6 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime ×tamp_sample, uint8_t return false; } - sensor_accel_fifo_s accel{}; - accel.timestamp_sample = timestamp_sample; - accel.samples = 0; - accel.dt = FIFO_SAMPLE_DT; // first find all sensor data frames in the buffer uint8_t *data_buffer = (uint8_t *)&buffer.f[0]; diff --git a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.hpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.hpp index 189ffe9a75..c79f9a2089 100644 --- a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.hpp +++ b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,8 +35,6 @@ #include "BMI088.hpp" -#include - #include "Bosch_BMI088_Accelerometer_Registers.hpp" namespace Bosch::BMI088::Accelerometer @@ -59,7 +57,7 @@ private: static constexpr uint32_t RATE{1600}; // 1600 Hz static constexpr float FIFO_SAMPLE_DT{1e6f / RATE}; - 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]))}; + static constexpr int32_t FIFO_MAX_SAMPLES{FIFO::SIZE / sizeof(FIFO::DATA)}; // Transfer data struct FIFOTransferBuffer { @@ -114,8 +112,6 @@ private: bool SimpleFIFORead(const hrt_abstime ×tamp_sample); bool NormalRead(const hrt_abstime ×tamp_sample); - PX4Accelerometer _px4_accel; - perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad register")}; perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad transfer")}; perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: FIFO empty")}; diff --git a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp index ad177c68e5..fd95f6f5e6 100644 --- a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,8 +33,6 @@ #include "BMI088_Gyroscope.hpp" -#include - using namespace time_literals; namespace Bosch::BMI088::Gyroscope @@ -42,7 +40,7 @@ namespace Bosch::BMI088::Gyroscope BMI088_Gyroscope::BMI088_Gyroscope(const I2CSPIDriverConfig &config) : BMI088(config), - _px4_gyro(get_device_id(), config.rotation) + _rotation(config.rotation) { if (config.drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_gyro: DRDY missed"); diff --git a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.hpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.hpp index 33d95b3b14..b8c874f4bf 100644 --- a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.hpp +++ b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.hpp @@ -35,8 +35,6 @@ #include "BMI088.hpp" -#include - #include "Bosch_BMI088_Gyroscope_Registers.hpp" namespace Bosch::BMI088::Gyroscope @@ -58,7 +56,7 @@ private: static constexpr uint32_t RATE{400}; // 2000 Hz static constexpr float FIFO_SAMPLE_DT{1e6f / RATE}; - static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))}; + static constexpr int32_t FIFO_MAX_SAMPLES{FIFO::SIZE / sizeof(FIFO::DATA)}; // Transfer data struct FIFOTransferBuffer { @@ -98,7 +96,6 @@ private: bool SelfTest(); bool NormalRead(const hrt_abstime ×tamp_sample); bool SimpleFIFORead(const hrt_abstime ×tamp_sample); - PX4Gyroscope _px4_gyro; perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad register")}; perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad transfer")}; diff --git a/src/drivers/imu/bosch/bmi088_i2c/CMakeLists.txt b/src/drivers/imu/bosch/bmi088_i2c/CMakeLists.txt index a86ef7637e..2043ddb0bb 100644 --- a/src/drivers/imu/bosch/bmi088_i2c/CMakeLists.txt +++ b/src/drivers/imu/bosch/bmi088_i2c/CMakeLists.txt @@ -48,7 +48,5 @@ px4_add_module( bmi088_i2c_main.cpp DEPENDS - drivers_accelerometer - drivers_gyroscope px4_work_queue )