Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 06:14:14 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

drivers/imu/invensense: adjust icm20602/icm20649/icm20948 rescheduling logic

Browse Source 
- this handles the case where the driver might be more than one full
transfer cycle behind
This commit is contained in:
Daniel Agar
2021-10-13 15:02:58 -04:00
parent ef4d4c3093
commit 4559230de6
3 changed files with 30 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/invensense/icm20602/ICM20602.cpp
+10 -3
View File
@@ -277,12 +277,19 @@ void ICM20602::RunImpl()
timestamp_sample -= extra_samples * static_cast<int>(FIFO_SAMPLE_DT);
samples = _fifo_gyro_samples;
ScheduleOnInterval(_fifo_empty_interval_us,
_fifo_empty_interval_us - (extra_samples * FIFO_SAMPLE_DT));
if (_fifo_gyro_samples > extra_samples) {
// reschedule to run when a total of _fifo_gyro_samples should be available in the FIFO
const uint32_t reschedule_delay_us = (_fifo_gyro_samples - extra_samples) * static_cast<int>(FIFO_SAMPLE_DT);
ScheduleOnInterval(_fifo_empty_interval_us, reschedule_delay_us);
} else {
// otherwise reschedule to run immediately
ScheduleOnInterval(_fifo_empty_interval_us);
}
} else if (samples < _fifo_gyro_samples) {
// reschedule next cycle to catch the desired number of samples
ScheduleOnInterval(_fifo_empty_interval_us, (_fifo_gyro_samples - samples) * FIFO_SAMPLE_DT);
ScheduleOnInterval(_fifo_empty_interval_us, (_fifo_gyro_samples - samples) * static_cast<int>(FIFO_SAMPLE_DT));
}
}
}
src/drivers/imu/invensense/icm20649/ICM20649.cpp
+10 -3
View File
@@ -232,12 +232,19 @@ void ICM20649::RunImpl()
timestamp_sample -= extra_samples * static_cast<int>(FIFO_SAMPLE_DT);
samples = _fifo_gyro_samples;
ScheduleOnInterval(_fifo_empty_interval_us,
_fifo_empty_interval_us - (extra_samples * FIFO_SAMPLE_DT));
if (_fifo_gyro_samples > extra_samples) {
// reschedule to run when a total of _fifo_gyro_samples should be available in the FIFO
const uint32_t reschedule_delay_us = (_fifo_gyro_samples - extra_samples) * static_cast<int>(FIFO_SAMPLE_DT);
ScheduleOnInterval(_fifo_empty_interval_us, reschedule_delay_us);
} else {
// otherwise reschedule to run immediately
ScheduleOnInterval(_fifo_empty_interval_us);
}
} else if (samples < _fifo_gyro_samples) {
// reschedule next cycle to catch the desired number of samples
ScheduleOnInterval(_fifo_empty_interval_us, (_fifo_gyro_samples - samples) * FIFO_SAMPLE_DT);
ScheduleOnInterval(_fifo_empty_interval_us, (_fifo_gyro_samples - samples) * static_cast<int>(FIFO_SAMPLE_DT));
}
if (samples == _fifo_gyro_samples) {
src/drivers/imu/invensense/icm20948/ICM20948.cpp
+10 -3
View File
@@ -268,12 +268,19 @@ void ICM20948::RunImpl()
timestamp_sample -= extra_samples * static_cast<int>(FIFO_SAMPLE_DT);
samples = _fifo_gyro_samples;
ScheduleOnInterval(_fifo_empty_interval_us,
_fifo_empty_interval_us - (extra_samples * FIFO_SAMPLE_DT));
if (_fifo_gyro_samples > extra_samples) {
// reschedule to run when a total of _fifo_gyro_samples should be available in the FIFO
const uint32_t reschedule_delay_us = (_fifo_gyro_samples - extra_samples) * static_cast<int>(FIFO_SAMPLE_DT);
ScheduleOnInterval(_fifo_empty_interval_us, reschedule_delay_us);
} else {
// otherwise reschedule to run immediately
ScheduleOnInterval(_fifo_empty_interval_us);
}
} else if (samples < _fifo_gyro_samples) {
// reschedule next cycle to catch the desired number of samples
ScheduleOnInterval(_fifo_empty_interval_us, (_fifo_gyro_samples - samples) * FIFO_SAMPLE_DT);
ScheduleOnInterval(_fifo_empty_interval_us, (_fifo_gyro_samples - samples) * static_cast<int>(FIFO_SAMPLE_DT));
}
if (samples == _fifo_gyro_samples) {