fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 23:14:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Move mag down sampling to ECL lib

Browse Source
This commit is contained in:
kamilritz 2020-01-14 11:14:33 +01:00 committed by Roman Bapst
parent 950e75e484
commit d7d7b56519
2 changed files with 19 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
EKF/estimator_interface.cpp
View File

@ -155,18 +155,30 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3])
}
}
// downsample to highest possible sensor rate
// by taking the average of incoming sample
_mag_sample_count++;
_mag_data_sum += Vector3f(data);
_mag_timestamp_sum += time_usec / 1000; // Dividing by 1000 to avoid overflow
// limit data rate to prevent data being lost
if ((time_usec - _time_last_mag) > _min_obs_interval_us) {
magSample mag_sample_new;
mag_sample_new.time_us = time_usec - _params.mag_delay_ms * 1000;
// Use the time in the middle of the downsampling interval for the sample
mag_sample_new.time_us = 1000 * (_mag_timestamp_sum / _mag_sample_count);
mag_sample_new.time_us -= _params.mag_delay_ms * 1000;
mag_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
_time_last_mag = time_usec;
mag_sample_new.mag = Vector3f(data);
mag_sample_new.mag = _mag_data_sum / _mag_sample_count;
_mag_buffer.push(mag_sample_new);
_time_last_mag = time_usec;
_mag_sample_count = 0;
_mag_data_sum.setZero();
_mag_timestamp_sum = 0;
}
}

4
EKF/estimator_interface.h
View File

@ -544,6 +544,10 @@ protected:
//last time the baro ground effect compensation was turned on externally (uSec)
uint64_t _time_last_gnd_effect_on{0};
// Used to downsample magnetometer data
Vector3f _mag_data_sum;
uint8_t _mag_sample_count {0};
uint64_t _mag_timestamp_sum {0};
fault_status_u _fault_status{};