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

ekf2: refactor mag control

Browse Source 
- remove class _mag_sample_delayed
 - update mag fusion call graph to use mag sample delayed functionally
 - Ekf::resetMagHeading()
   - use low pass mag directly, but check if valid (magnitude)
   - MAG_FUSE_TYPE_INDOOR treat like auto if heading required
This commit is contained in:
Daniel Agar
2022-01-31 11:47:10 -05:00
committed by GitHub
parent ad447ab223
commit 999737ddd5
8 changed files with 151 additions and 213 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.cpp
+13 -9
View File
@@ -143,16 +143,20 @@ bool Ekf::initialiseFilter()
}
// Sum the magnetometer measurements
if (_mag_buffer && _mag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
if (_mag_sample_delayed.time_us != 0) {
if (_mag_counter == 0) {
_mag_lpf.reset(_mag_sample_delayed.mag);
if (_mag_buffer) {
magSample mag_sample;
} else {
_mag_lpf.update(_mag_sample_delayed.mag);
if (_mag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &mag_sample)) {
if (mag_sample.time_us != 0) {
if (_mag_counter == 0) {
_mag_lpf.reset(mag_sample.mag);
} else {
_mag_lpf.update(mag_sample.mag);
}
_mag_counter++;
}
_mag_counter++;
}
}
@@ -192,7 +196,7 @@ bool Ekf::initialiseFilter()
// calculate the initial magnetic field and yaw alignment
// but do not mark the yaw alignement complete as it needs to be
// reset once the leveling phase is done
resetMagHeading(_mag_lpf.getState(), false, false);
resetMagHeading(false, false);
// initialise the state covariance matrix now we have starting values for all the states
initialiseCovariance();