dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

imu/invensense: adjust scheduling dynamically if not using data ready interrupt

This commit is contained in:
Daniel Agar 2021-09-08 17:45:12 -04:00
parent fa4fc5f347
commit e6552370ab
3 changed files with 38 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -271,24 +271,25 @@ void ICM20602::RunImpl()
// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
samples = fifo_count / sizeof(FIFO::DATA);
// tolerate minor jitter, leave sample to next iteration if behind by only 1
if (samples == _fifo_gyro_samples + 1) {
timestamp_sample -= FIFO_SAMPLE_DT;
samples--;
}
if (samples > _fifo_gyro_samples) {
// grab desired number of samples, but reschedule next cycle sooner
int extra_samples = samples - _fifo_gyro_samples;
timestamp_sample -= extra_samples * FIFO_SAMPLE_DT;
samples = _fifo_gyro_samples;
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
FIFOReset();
perf_count(_fifo_overflow_perf);
samples = 0;
ScheduleOnInterval(_fifo_empty_interval_us,
_fifo_empty_interval_us - (extra_samples * FIFO_SAMPLE_DT));
} 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);
}
}
}
bool success = false;
if (samples >= SAMPLES_PER_TRANSFER) {
if (samples == _fifo_gyro_samples) {
if (FIFORead(timestamp_sample, samples)) {
success = true;

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

@ -226,18 +226,21 @@ void ICM20649::RunImpl()
// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
uint8_t samples = fifo_count / sizeof(FIFO::DATA);
// tolerate minor jitter, leave sample to next iteration if behind by only 1
if (samples == _fifo_gyro_samples + 1) {
timestamp_sample -= FIFO_SAMPLE_DT;
samples--;
if (samples > _fifo_gyro_samples) {
// grab desired number of samples, but reschedule next cycle sooner
int extra_samples = samples - _fifo_gyro_samples;
timestamp_sample -= extra_samples * FIFO_SAMPLE_DT;
samples = _fifo_gyro_samples;
ScheduleOnInterval(_fifo_empty_interval_us,
_fifo_empty_interval_us - (extra_samples * FIFO_SAMPLE_DT));
} 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);
}
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
FIFOReset();
perf_count(_fifo_overflow_perf);
} else if (samples >= SAMPLES_PER_TRANSFER) {
if (samples == _fifo_gyro_samples) {
if (FIFORead(timestamp_sample, samples)) {
success = true;

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

@ -262,18 +262,21 @@ void ICM20948::RunImpl()
// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
uint8_t samples = fifo_count / sizeof(FIFO::DATA);
// tolerate minor jitter, leave sample to next iteration if behind by only 1
if (samples == _fifo_gyro_samples + 1) {
timestamp_sample -= FIFO_SAMPLE_DT;
samples--;
if (samples > _fifo_gyro_samples) {
// grab desired number of samples, but reschedule next cycle sooner
int extra_samples = samples - _fifo_gyro_samples;
timestamp_sample -= extra_samples * FIFO_SAMPLE_DT;
samples = _fifo_gyro_samples;
ScheduleOnInterval(_fifo_empty_interval_us,
_fifo_empty_interval_us - (extra_samples * FIFO_SAMPLE_DT));
} 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);
}
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
FIFOReset();
perf_count(_fifo_overflow_perf);
} else if (samples >= SAMPLES_PER_TRANSFER) {
if (samples == _fifo_gyro_samples) {
if (FIFORead(timestamp_sample, samples)) {
success = true;