fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 20:53:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

PX4Accelerometer/PX4Gyroscope update FIFO case to trapezoidal integration

Browse Source
This commit is contained in:
Daniel Agar
2020-01-22 18:35:41 -05:00
parent 697dbfb9f8
commit 009ba638f5
9 changed files with 215 additions and 214 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/invensense/icm20602/ICM20602.cpp
+7 -6
View File
@@ -300,6 +300,11 @@ void ICM20602::Run()
{
perf_count(_interval_perf);
// use timestamp from the data ready interrupt if available,
// otherwise use the time now roughly corresponding with the last sample we'll pull from the FIFO
const hrt_abstime timestamp_sample = (hrt_elapsed_time(&_time_data_ready) < FIFO_INTERVAL) ? _time_data_ready :
hrt_absolute_time();
// read FIFO count
uint8_t fifo_count_buf[3] {};
fifo_count_buf[0] = static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ;
@@ -351,15 +356,11 @@ void ICM20602::Run()
perf_end(_transfer_perf);
static constexpr uint32_t gyro_dt = FIFO_INTERVAL / FIFO_GYRO_SAMPLES;
// estimate timestamp of first sample in the FIFO from number of samples and fill rate
const hrt_abstime timestamp_sample = _time_data_ready - ((samples - 1) * gyro_dt);
PX4Accelerometer::FIFOSample accel{};
PX4Accelerometer::FIFOSample accel;
accel.timestamp_sample = timestamp_sample;
accel.dt = FIFO_INTERVAL / FIFO_ACCEL_SAMPLES;
PX4Gyroscope::FIFOSample gyro{};
PX4Gyroscope::FIFOSample gyro;
gyro.timestamp_sample = timestamp_sample;
gyro.samples = samples;
gyro.dt = FIFO_INTERVAL / FIFO_GYRO_SAMPLES;
src/drivers/imu/invensense/icm20608-g/ICM20608G.cpp
+7 -6
View File
@@ -276,6 +276,11 @@ void ICM20608G::Run()
{
perf_count(_interval_perf);
// use timestamp from the data ready interrupt if available,
// otherwise use the time now roughly corresponding with the last sample we'll pull from the FIFO
const hrt_abstime timestamp_sample = (hrt_elapsed_time(&_time_data_ready) < FIFO_INTERVAL) ? _time_data_ready :
hrt_absolute_time();
// read FIFO count
uint8_t fifo_count_buf[3] {};
fifo_count_buf[0] = static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ;
@@ -327,15 +332,11 @@ void ICM20608G::Run()
perf_end(_transfer_perf);
static constexpr uint32_t gyro_dt = FIFO_INTERVAL / FIFO_GYRO_SAMPLES;
// estimate timestamp of first sample in the FIFO from number of samples and fill rate
const hrt_abstime timestamp_sample = _time_data_ready - ((samples - 1) * gyro_dt);
PX4Accelerometer::FIFOSample accel{};
PX4Accelerometer::FIFOSample accel;
accel.timestamp_sample = timestamp_sample;
accel.dt = FIFO_INTERVAL / FIFO_ACCEL_SAMPLES;
PX4Gyroscope::FIFOSample gyro{};
PX4Gyroscope::FIFOSample gyro;
gyro.timestamp_sample = timestamp_sample;
gyro.samples = samples;
gyro.dt = FIFO_INTERVAL / FIFO_GYRO_SAMPLES;