From 56c86e77bdbe9811ce2ac0f9ca736b19f51f5fe1 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 31 May 2020 21:25:13 -0400 Subject: [PATCH] replace FIFOSample -> sensor_accel_fifo/sensor_gyro_fifo --- .../imu/bosch/bmi055/BMI055_Accelerometer.cpp | 2 +- .../imu/bosch/bmi055/BMI055_Accelerometer.hpp | 2 +- .../imu/bosch/bmi055/BMI055_Gyroscope.cpp | 2 +- .../imu/bosch/bmi055/BMI055_Gyroscope.hpp | 2 +- .../imu/bosch/bmi088/BMI088_Accelerometer.cpp | 2 +- .../imu/bosch/bmi088/BMI088_Accelerometer.hpp | 2 +- .../imu/bosch/bmi088/BMI088_Gyroscope.cpp | 2 +- .../imu/bosch/bmi088/BMI088_Gyroscope.hpp | 2 +- .../imu/invensense/icm20602/ICM20602.cpp | 4 +-- .../imu/invensense/icm20602/ICM20602.hpp | 2 +- .../imu/invensense/icm20608g/ICM20608G.cpp | 4 +-- .../imu/invensense/icm20608g/ICM20608G.hpp | 2 +- .../imu/invensense/icm20649/ICM20649.cpp | 4 +-- .../imu/invensense/icm20649/ICM20649.hpp | 2 +- .../imu/invensense/icm20689/ICM20689.cpp | 4 +-- .../imu/invensense/icm20689/ICM20689.hpp | 2 +- .../imu/invensense/icm20948/ICM20948.cpp | 4 +-- .../imu/invensense/icm20948/ICM20948.hpp | 2 +- .../imu/invensense/icm40609d/ICM40609D.cpp | 4 +-- .../imu/invensense/icm40609d/ICM40609D.hpp | 2 +- .../imu/invensense/icm42688p/ICM42688P.cpp | 4 +-- .../imu/invensense/icm42688p/ICM42688P.hpp | 2 +- .../imu/invensense/mpu6000/MPU6000.cpp | 4 +-- .../imu/invensense/mpu6000/MPU6000.hpp | 2 +- .../imu/invensense/mpu6500/MPU6500.cpp | 4 +-- .../imu/invensense/mpu6500/MPU6500.hpp | 2 +- .../imu/invensense/mpu9250/MPU9250.cpp | 4 +-- .../imu/invensense/mpu9250/MPU9250.hpp | 2 +- src/drivers/imu/st/ism330dlc/ISM330DLC.cpp | 4 +-- src/drivers/imu/st/ism330dlc/ISM330DLC.hpp | 2 +- .../accelerometer/PX4Accelerometer.cpp | 29 ++++++------------- .../accelerometer/PX4Accelerometer.hpp | 15 +--------- src/lib/drivers/gyroscope/PX4Gyroscope.cpp | 29 ++++++------------- src/lib/drivers/gyroscope/PX4Gyroscope.hpp | 15 +--------- 34 files changed, 61 insertions(+), 109 deletions(-) diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp index 7cf628859e..90fede65f4 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp @@ -402,7 +402,7 @@ bool BMI055_Accelerometer::FIFORead(const hrt_abstime ×tamp_sample, uint8_t perf_end(_transfer_perf); - PX4Accelerometer::FIFOSample accel; + sensor_accel_fifo_s accel{}; accel.timestamp_sample = timestamp_sample; accel.samples = samples; accel.dt = FIFO_SAMPLE_DT; diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp index 3eba20a445..360c8e55bf 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp @@ -59,7 +59,7 @@ private: static constexpr uint32_t ACCEL_RATE{2000}; // 2000 Hz accel static constexpr float FIFO_SAMPLE_DT{1e6f / ACCEL_RATE}; - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(PX4Accelerometer::FIFOSample::x) / sizeof(PX4Accelerometer::FIFOSample::x[0]))}; + 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]))}; // Transfer data struct FIFOTransferBuffer { diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp index b9526f72b9..bb1ebb1b4d 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp @@ -407,7 +407,7 @@ bool BMI055_Gyroscope::FIFORead(const hrt_abstime ×tamp_sample, uint8_t sam perf_end(_transfer_perf); - PX4Gyroscope::FIFOSample gyro; + sensor_gyro_fifo_s gyro{}; gyro.timestamp_sample = timestamp_sample; gyro.samples = samples; gyro.dt = FIFO_SAMPLE_DT; diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp index 9fcf09cdba..a9cb009aa9 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp @@ -59,7 +59,7 @@ private: static constexpr uint32_t GYRO_RATE{2000}; // 2000 Hz gyro static constexpr float FIFO_SAMPLE_DT{1e6f / GYRO_RATE}; - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))}; + 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]))}; // Transfer data struct FIFOTransferBuffer { diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp index 8edc1a4ad2..ce0a8d9b2f 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp +++ b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp @@ -464,7 +464,7 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime ×tamp_sample, uint8_t perf_end(_transfer_perf); - PX4Accelerometer::FIFOSample accel; + sensor_accel_fifo_s accel{}; accel.timestamp_sample = timestamp_sample; accel.samples = 0; accel.dt = FIFO_SAMPLE_DT; diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp index bc9485dd89..82b5788f0f 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp +++ b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp @@ -59,7 +59,7 @@ private: static constexpr uint32_t ACCEL_RATE{1600}; // 1600 Hz accel static constexpr float FIFO_SAMPLE_DT{1e6f / ACCEL_RATE}; - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(PX4Accelerometer::FIFOSample::x) / sizeof(PX4Accelerometer::FIFOSample::x[0]))}; + 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]))}; // Transfer data struct FIFOTransferBuffer { diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp index aaf5fb7ce3..ee3598d64e 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp @@ -408,7 +408,7 @@ bool BMI088_Gyroscope::FIFORead(const hrt_abstime ×tamp_sample, uint8_t sam perf_end(_transfer_perf); - PX4Gyroscope::FIFOSample gyro; + sensor_gyro_fifo_s gyro{}; gyro.timestamp_sample = timestamp_sample; gyro.samples = samples; gyro.dt = FIFO_SAMPLE_DT; diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp index b3455f868a..ad0e6ebf9f 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp +++ b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp @@ -59,7 +59,7 @@ private: static constexpr uint32_t GYRO_RATE{2000}; // 2000 Hz gyro static constexpr float FIFO_SAMPLE_DT{1e6f / GYRO_RATE}; - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))}; + 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]))}; // Transfer data struct FIFOTransferBuffer { diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.cpp b/src/drivers/imu/invensense/icm20602/ICM20602.cpp index a6772806df..430c02f811 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.cpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.cpp @@ -520,7 +520,7 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1) bool ICM20602::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples) { - PX4Accelerometer::FIFOSample accel; + sensor_accel_fifo_s accel{}; accel.timestamp_sample = timestamp_sample; accel.dt = _fifo_empty_interval_us / _fifo_accel_samples; @@ -571,7 +571,7 @@ bool ICM20602::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTrans void ICM20602::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples) { - PX4Gyroscope::FIFOSample gyro; + sensor_gyro_fifo_s gyro{}; gyro.timestamp_sample = timestamp_sample; gyro.samples = samples; gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples; diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.hpp b/src/drivers/imu/invensense/icm20602/ICM20602.hpp index b97c3004d5..250fb0e732 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.hpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.hpp @@ -78,7 +78,7 @@ private: static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 8 kHz gyro static constexpr float ACCEL_RATE{GYRO_RATE / 2.f}; // 4 kHz accel - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))}; + 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]))}; // Transfer data struct FIFOTransferBuffer { diff --git a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp index 9bc4ee0b49..2bc9beec72 100644 --- a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp +++ b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp @@ -513,7 +513,7 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1) bool ICM20608G::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) { - PX4Accelerometer::FIFOSample accel; + sensor_accel_fifo_s accel{}; accel.timestamp_sample = timestamp_sample; accel.samples = 0; accel.dt = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER; @@ -566,7 +566,7 @@ bool ICM20608G::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DA void ICM20608G::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) { - PX4Gyroscope::FIFOSample gyro; + sensor_gyro_fifo_s gyro{}; gyro.timestamp_sample = timestamp_sample; gyro.samples = samples; gyro.dt = FIFO_SAMPLE_DT; diff --git a/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp b/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp index 6593b0d2f2..8ee5196be3 100644 --- a/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp +++ b/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp @@ -78,7 +78,7 @@ private: 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 - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))}; + 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]))}; // Transfer data struct FIFOTransferBuffer { diff --git a/src/drivers/imu/invensense/icm20649/ICM20649.cpp b/src/drivers/imu/invensense/icm20649/ICM20649.cpp index 9dd230c8d6..0c02608fb9 100644 --- a/src/drivers/imu/invensense/icm20649/ICM20649.cpp +++ b/src/drivers/imu/invensense/icm20649/ICM20649.cpp @@ -574,7 +574,7 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1) bool ICM20649::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples) { - PX4Accelerometer::FIFOSample accel; + sensor_accel_fifo_s accel{}; accel.timestamp_sample = timestamp_sample; accel.dt = _fifo_empty_interval_us / _fifo_accel_samples; @@ -625,7 +625,7 @@ bool ICM20649::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTrans void ICM20649::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples) { - PX4Gyroscope::FIFOSample gyro; + sensor_gyro_fifo_s gyro{}; gyro.timestamp_sample = timestamp_sample; gyro.samples = samples; gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples; diff --git a/src/drivers/imu/invensense/icm20649/ICM20649.hpp b/src/drivers/imu/invensense/icm20649/ICM20649.hpp index 7413074821..4daccdb12b 100644 --- a/src/drivers/imu/invensense/icm20649/ICM20649.hpp +++ b/src/drivers/imu/invensense/icm20649/ICM20649.hpp @@ -78,7 +78,7 @@ private: static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 9000 Hz gyro static constexpr float ACCEL_RATE{GYRO_RATE / 2.f}; // 4500 Hz accel - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))}; + 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]))}; // Transfer data struct FIFOTransferBuffer { diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.cpp b/src/drivers/imu/invensense/icm20689/ICM20689.cpp index f1891dd6af..2536218564 100644 --- a/src/drivers/imu/invensense/icm20689/ICM20689.cpp +++ b/src/drivers/imu/invensense/icm20689/ICM20689.cpp @@ -512,7 +512,7 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1) bool ICM20689::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) { - PX4Accelerometer::FIFOSample accel; + sensor_accel_fifo_s accel{}; accel.timestamp_sample = timestamp_sample; accel.samples = 0; accel.dt = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER; @@ -565,7 +565,7 @@ bool ICM20689::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DAT void ICM20689::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) { - PX4Gyroscope::FIFOSample gyro; + sensor_gyro_fifo_s gyro{}; gyro.timestamp_sample = timestamp_sample; gyro.samples = samples; gyro.dt = FIFO_SAMPLE_DT; diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.hpp b/src/drivers/imu/invensense/icm20689/ICM20689.hpp index 2d696f419e..a3e40af8b1 100644 --- a/src/drivers/imu/invensense/icm20689/ICM20689.hpp +++ b/src/drivers/imu/invensense/icm20689/ICM20689.hpp @@ -78,7 +78,7 @@ private: 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 - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))}; + 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]))}; // Transfer data struct FIFOTransferBuffer { diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.cpp b/src/drivers/imu/invensense/icm20948/ICM20948.cpp index 2e7548f54f..31478139f2 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948.cpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948.cpp @@ -608,7 +608,7 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1) bool ICM20948::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples) { - PX4Accelerometer::FIFOSample accel; + sensor_accel_fifo_s accel{}; accel.timestamp_sample = timestamp_sample; accel.dt = _fifo_empty_interval_us / _fifo_accel_samples; @@ -659,7 +659,7 @@ bool ICM20948::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTrans void ICM20948::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples) { - PX4Gyroscope::FIFOSample gyro; + sensor_gyro_fifo_s gyro{}; gyro.timestamp_sample = timestamp_sample; gyro.samples = samples; gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples; diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.hpp b/src/drivers/imu/invensense/icm20948/ICM20948.hpp index 898482cdd1..4aed84c4fd 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948.hpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948.hpp @@ -80,7 +80,7 @@ private: static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 9000 Hz gyro static constexpr float ACCEL_RATE{GYRO_RATE / 2.f}; // 4500 Hz accel - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))}; + 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]))}; // Transfer data struct FIFOTransferBuffer { diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp index 60daf4ad78..2ad37b756f 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp @@ -549,7 +549,7 @@ void ICM40609D::FIFOReset() void ICM40609D::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples) { - PX4Accelerometer::FIFOSample accel; + sensor_accel_fifo_s accel{}; accel.timestamp_sample = timestamp_sample; accel.dt = _fifo_empty_interval_us / _fifo_accel_samples; @@ -577,7 +577,7 @@ void ICM40609D::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTran void ICM40609D::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples) { - PX4Gyroscope::FIFOSample gyro; + sensor_gyro_fifo_s gyro{}; gyro.timestamp_sample = timestamp_sample; gyro.samples = samples; gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples; diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp index 40e4e23b41..aba0b60255 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp @@ -78,7 +78,7 @@ private: static constexpr float GYRO_RATE{1000000 / FIFO_SAMPLE_DT}; // 8 kHz gyro static constexpr float ACCEL_RATE{GYRO_RATE}; // 8 kHz accel - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))}; + 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]))}; // Transfer data struct FIFOTransferBuffer { diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp index 7a9a8bb7f5..946f2edce3 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp @@ -552,7 +552,7 @@ void ICM42688P::FIFOReset() void ICM42688P::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples) { - PX4Accelerometer::FIFOSample accel; + sensor_accel_fifo_s accel{}; accel.timestamp_sample = timestamp_sample; accel.dt = _fifo_empty_interval_us / _fifo_accel_samples; @@ -580,7 +580,7 @@ void ICM42688P::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTran void ICM42688P::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples) { - PX4Gyroscope::FIFOSample gyro; + sensor_gyro_fifo_s gyro{}; gyro.timestamp_sample = timestamp_sample; gyro.samples = samples; gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples; diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp index b520e3e6a6..6cde39f3b6 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp @@ -78,7 +78,7 @@ private: static constexpr float GYRO_RATE{1000000 / FIFO_SAMPLE_DT}; // 8 kHz gyro static constexpr float ACCEL_RATE{GYRO_RATE}; // 8 kHz accel - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))}; + 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]))}; // Transfer data struct FIFOTransferBuffer { diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp index 162ac3ed33..3c868daea1 100644 --- a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp +++ b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp @@ -517,7 +517,7 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1) bool MPU6000::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) { - PX4Accelerometer::FIFOSample accel; + sensor_accel_fifo_s accel{}; accel.timestamp_sample = timestamp_sample; accel.samples = 0; accel.dt = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER; @@ -569,7 +569,7 @@ bool MPU6000::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA void MPU6000::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) { - PX4Gyroscope::FIFOSample gyro; + sensor_gyro_fifo_s gyro{}; gyro.timestamp_sample = timestamp_sample; gyro.samples = samples; gyro.dt = FIFO_SAMPLE_DT; diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp index 94548daa8e..0ad7b8c89a 100644 --- a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp +++ b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp @@ -78,7 +78,7 @@ private: 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 - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))}; + 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]))}; // Transfer data struct FIFOTransferBuffer { diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp index f1870435fc..26d41f9288 100644 --- a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp +++ b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp @@ -504,7 +504,7 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1) bool MPU6500::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples) { - PX4Accelerometer::FIFOSample accel; + sensor_accel_fifo_s accel{}; accel.timestamp_sample = timestamp_sample; accel.dt = _fifo_empty_interval_us / _fifo_accel_samples; @@ -555,7 +555,7 @@ bool MPU6500::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransf void MPU6500::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples) { - PX4Gyroscope::FIFOSample gyro; + sensor_gyro_fifo_s gyro{}; gyro.timestamp_sample = timestamp_sample; gyro.samples = samples; gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples; diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.hpp b/src/drivers/imu/invensense/mpu6500/MPU6500.hpp index 21498ecb56..9af14080a5 100644 --- a/src/drivers/imu/invensense/mpu6500/MPU6500.hpp +++ b/src/drivers/imu/invensense/mpu6500/MPU6500.hpp @@ -78,7 +78,7 @@ private: static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 8 kHz gyro static constexpr float ACCEL_RATE{GYRO_RATE / 2.f}; // 4 kHz accel - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))}; + 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]))}; // Transfer data struct FIFOTransferBuffer { diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp index e02e55178d..d5a7df48ca 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp @@ -537,7 +537,7 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1) bool MPU9250::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples) { - PX4Accelerometer::FIFOSample accel; + sensor_accel_fifo_s accel{}; accel.timestamp_sample = timestamp_sample; accel.dt = _fifo_empty_interval_us / _fifo_accel_samples; @@ -588,7 +588,7 @@ bool MPU9250::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransf void MPU9250::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples) { - PX4Gyroscope::FIFOSample gyro; + sensor_gyro_fifo_s gyro{}; gyro.timestamp_sample = timestamp_sample; gyro.samples = samples; gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples; diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp index 200fecdcaf..6a2c1ebcd5 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp @@ -80,7 +80,7 @@ private: static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 8 kHz gyro static constexpr float ACCEL_RATE{GYRO_RATE / 2.f}; // 4 kHz accel - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))}; + 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]))}; // Transfer data struct FIFOTransferBuffer { diff --git a/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp b/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp index 9f5c36c217..05133fdabc 100644 --- a/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp +++ b/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp @@ -284,12 +284,12 @@ void ISM330DLC::RunImpl() perf_end(_transfer_perf); - PX4Accelerometer::FIFOSample accel; + sensor_accel_fifo_s accel{}; accel.timestamp_sample = timestamp_sample; accel.samples = samples; accel.dt = 1000000 / ST_ISM330DLC::LA_ODR; - PX4Gyroscope::FIFOSample gyro; + sensor_gyro_fifo_s gyro{}; gyro.timestamp_sample = timestamp_sample; gyro.samples = samples; gyro.dt = 1000000 / ST_ISM330DLC::G_ODR; diff --git a/src/drivers/imu/st/ism330dlc/ISM330DLC.hpp b/src/drivers/imu/st/ism330dlc/ISM330DLC.hpp index 87c6b1ad14..d4827a80b7 100644 --- a/src/drivers/imu/st/ism330dlc/ISM330DLC.hpp +++ b/src/drivers/imu/st/ism330dlc/ISM330DLC.hpp @@ -80,7 +80,7 @@ private: // Sensor Configuration static constexpr uint32_t GYRO_RATE{ST_ISM330DLC::G_ODR}; static constexpr uint32_t ACCEL_RATE{ST_ISM330DLC::LA_ODR}; - static constexpr uint32_t FIFO_MAX_SAMPLES{ math::min(FIFO::SIZE / sizeof(FIFO::DATA) + 1, sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))}; + static constexpr uint32_t FIFO_MAX_SAMPLES{ math::min(FIFO::SIZE / sizeof(FIFO::DATA) + 1, sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))}; // Transfer data struct FIFOTransferBuffer { diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index d684b68b15..ef13598e1f 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp @@ -107,13 +107,19 @@ void PX4Accelerometer::update(const hrt_abstime ×tamp_sample, float x, floa Publish(timestamp_sample, x, y, z, clip_count); } -void PX4Accelerometer::updateFIFO(const FIFOSample &sample) +void PX4Accelerometer::updateFIFO(sensor_accel_fifo_s &sample) { - const uint8_t N = sample.samples; - const float dt = sample.dt; + // publish fifo + sample.device_id = _device_id; + sample.scale = _scale; + sample.rotation = _rotation; + + sample.timestamp = hrt_absolute_time(); + _sensor_fifo_pub.publish(sample); { // trapezoidal integration (equally spaced, scaled by dt later) + const uint8_t N = sample.samples; const Vector3f integral{ (0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)), (0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)), @@ -138,23 +144,6 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample) // publish Publish(sample.timestamp_sample, x, y, z, clip_count); } - - // publish fifo - sensor_accel_fifo_s fifo{}; - - fifo.device_id = _device_id; - fifo.timestamp_sample = sample.timestamp_sample; - fifo.dt = dt; - fifo.scale = _scale; - fifo.samples = N; - fifo.rotation = _rotation; - - memcpy(fifo.x, sample.x, sizeof(sample.x[0]) * N); - memcpy(fifo.y, sample.y, sizeof(sample.y[0]) * N); - memcpy(fifo.z, sample.z, sizeof(sample.z[0]) * N); - - fifo.timestamp = hrt_absolute_time(); - _sensor_fifo_pub.publish(fifo); } void PX4Accelerometer::Publish(const hrt_abstime ×tamp_sample, float x, float y, float z, uint8_t clip_count[3]) diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index 93ba55e35b..6c96164272 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -61,20 +61,7 @@ public: void update(const hrt_abstime ×tamp_sample, float x, float y, float z); - struct FIFOSample { - hrt_abstime timestamp_sample; - uint8_t samples; // number of samples - float dt; // in microseconds - - int16_t x[32]; - int16_t y[32]; - int16_t z[32]; - }; - static_assert(sizeof(FIFOSample::x) == sizeof(sensor_accel_fifo_s::x), "FIFOSample.x invalid size"); - static_assert(sizeof(FIFOSample::y) == sizeof(sensor_accel_fifo_s::y), "FIFOSample.y invalid size"); - static_assert(sizeof(FIFOSample::z) == sizeof(sensor_accel_fifo_s::z), "FIFOSample.z invalid size"); - - void updateFIFO(const FIFOSample &sample); + void updateFIFO(sensor_accel_fifo_s &sample); private: void Publish(const hrt_abstime ×tamp_sample, float x, float y, float z, uint8_t clip_count[3]); diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index 56e3df72c5..975db95c9d 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -88,13 +88,19 @@ void PX4Gyroscope::update(const hrt_abstime ×tamp_sample, float x, float y, Publish(timestamp_sample, x, y, z); } -void PX4Gyroscope::updateFIFO(const FIFOSample &sample) +void PX4Gyroscope::updateFIFO(sensor_gyro_fifo_s &sample) { - const uint8_t N = sample.samples; - const float dt = sample.dt; + // publish fifo + sample.device_id = _device_id; + sample.scale = _scale; + sample.rotation = _rotation; + + sample.timestamp = hrt_absolute_time(); + _sensor_fifo_pub.publish(sample); { // trapezoidal integration (equally spaced, scaled by dt later) + const uint8_t N = sample.samples; const Vector3f integral{ (0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)), (0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)), @@ -112,23 +118,6 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample) // publish Publish(sample.timestamp_sample, x, y, z); } - - // publish fifo - sensor_gyro_fifo_s fifo{}; - - fifo.device_id = _device_id; - fifo.timestamp_sample = sample.timestamp_sample; - fifo.dt = dt; - fifo.scale = _scale; - fifo.samples = N; - fifo.rotation = _rotation; - - memcpy(fifo.x, sample.x, sizeof(sample.x[0]) * N); - memcpy(fifo.y, sample.y, sizeof(sample.y[0]) * N); - memcpy(fifo.z, sample.z, sizeof(sample.z[0]) * N); - - fifo.timestamp = hrt_absolute_time(); - _sensor_fifo_pub.publish(fifo); } void PX4Gyroscope::Publish(const hrt_abstime ×tamp_sample, float x, float y, float z) diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index 8ec25fd195..9066392844 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -60,20 +60,7 @@ public: void update(const hrt_abstime ×tamp_sample, float x, float y, float z); - struct FIFOSample { - hrt_abstime timestamp_sample; - uint8_t samples; // number of samples - float dt; // in microseconds - - int16_t x[32]; - int16_t y[32]; - int16_t z[32]; - }; - static_assert(sizeof(FIFOSample::x) == sizeof(sensor_gyro_fifo_s::x), "FIFOSample.x invalid size"); - static_assert(sizeof(FIFOSample::y) == sizeof(sensor_gyro_fifo_s::y), "FIFOSample.y invalid size"); - static_assert(sizeof(FIFOSample::z) == sizeof(sensor_gyro_fifo_s::z), "FIFOSample.z invalid size"); - - void updateFIFO(const FIFOSample &sample); + void updateFIFO(sensor_gyro_fifo_s &sample); private: void Publish(const hrt_abstime ×tamp_sample, float x, float y, float z);