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

replace FIFOSample -> sensor_accel_fifo/sensor_gyro_fifo

Browse Source
This commit is contained in:
Daniel Agar 2020-05-31 21:25:13 -04:00
parent 6b343c1f5e
commit 56c86e77bd
34 changed files with 61 additions and 109 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -402,7 +402,7 @@ bool BMI055_Accelerometer::FIFORead(const hrt_abstime &timestamp_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;

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

@ -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 {

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

@ -407,7 +407,7 @@ bool BMI055_Gyroscope::FIFORead(const hrt_abstime &timestamp_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;

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

@ -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 {

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

@ -464,7 +464,7 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_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;

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

@ -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 {

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

@ -408,7 +408,7 @@ bool BMI088_Gyroscope::FIFORead(const hrt_abstime &timestamp_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;

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

@ -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 {

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

@ -520,7 +520,7 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
bool ICM20602::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_sample, const FIFOTrans
void ICM20602::ProcessGyro(const hrt_abstime &timestamp_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;

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

@ -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 {

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

@ -513,7 +513,7 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
bool ICM20608G::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_sample, const FIFO::DA
void ICM20608G::ProcessGyro(const hrt_abstime &timestamp_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;

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

@ -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 {

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

@ -574,7 +574,7 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
bool ICM20649::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_sample, const FIFOTrans
void ICM20649::ProcessGyro(const hrt_abstime &timestamp_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;

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

@ -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 {

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

@ -512,7 +512,7 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
bool ICM20689::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_sample, const FIFO::DAT
void ICM20689::ProcessGyro(const hrt_abstime &timestamp_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;

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

@ -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 {

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

@ -608,7 +608,7 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
bool ICM20948::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_sample, const FIFOTrans
void ICM20948::ProcessGyro(const hrt_abstime &timestamp_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;

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

@ -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 {

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

@ -549,7 +549,7 @@ void ICM40609D::FIFOReset()
void ICM40609D::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_sample, const FIFOTran
void ICM40609D::ProcessGyro(const hrt_abstime &timestamp_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;

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

@ -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 {

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

@ -552,7 +552,7 @@ void ICM42688P::FIFOReset()
void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_sample, const FIFOTran
void ICM42688P::ProcessGyro(const hrt_abstime &timestamp_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;

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

@ -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 {

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

@ -517,7 +517,7 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
bool MPU6000::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_sample, const FIFO::DATA
void MPU6000::ProcessGyro(const hrt_abstime &timestamp_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;

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

@ -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 {

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

@ -504,7 +504,7 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
bool MPU6500::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_sample, const FIFOTransf
void MPU6500::ProcessGyro(const hrt_abstime &timestamp_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;

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

@ -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 {

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

@ -537,7 +537,7 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
bool MPU9250::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_sample, const FIFOTransf
void MPU9250::ProcessGyro(const hrt_abstime &timestamp_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;

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

@ -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 {

4
src/drivers/imu/st/ism330dlc/ISM330DLC.cpp
View File

@ -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;

2
src/drivers/imu/st/ism330dlc/ISM330DLC.hpp
View File

@ -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 {

29
src/lib/drivers/accelerometer/PX4Accelerometer.cpp
View File

@ -107,13 +107,19 @@ void PX4Accelerometer::update(const hrt_abstime &timestamp_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 &timestamp_sample, float x, float y, float z, uint8_t clip_count[3])

15
src/lib/drivers/accelerometer/PX4Accelerometer.hpp
View File

@ -61,20 +61,7 @@ public:
void update(const hrt_abstime &timestamp_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 &timestamp_sample, float x, float y, float z, uint8_t clip_count[3]);

29
src/lib/drivers/gyroscope/PX4Gyroscope.cpp
View File

@ -88,13 +88,19 @@ void PX4Gyroscope::update(const hrt_abstime &timestamp_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 &timestamp_sample, float x, float y, float z)

15
src/lib/drivers/gyroscope/PX4Gyroscope.hpp
View File

@ -60,20 +60,7 @@ public:
void update(const hrt_abstime &timestamp_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 &timestamp_sample, float x, float y, float z);