fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 05:40:36 +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/control.cpp
+8 -26
View File
@@ -110,31 +110,6 @@ void Ekf::controlFusionModes()
_gps_data_ready = _gps_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
}
if (_mag_buffer) {
_mag_data_ready = _mag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed);
if (_mag_data_ready) {
_mag_lpf.update(_mag_sample_delayed.mag);
// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
// this is useful if there is a lot of interference on the sensor measurement.
if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_USE_GEO_DECL)
&& (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps))
) {
const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
_mag_sample_delayed.mag(2) = calculate_synthetic_mag_z_measurement(_mag_sample_delayed.mag, mag_earth_pred);
_control_status.flags.synthetic_mag_z = true;
} else {
_control_status.flags.synthetic_mag_z = false;
}
}
}
if (_range_buffer) {
// Get range data from buffer and check validity
bool is_rng_data_ready = _range_buffer->pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
@@ -371,7 +346,14 @@ void Ekf::controlExternalVisionFusion()
resetYawToEv();
}
fuseHeading();
if (shouldUse321RotationSequence(_R_to_earth)) {
float measured_hdg = getEuler321Yaw(_ev_sample_delayed.quat);
fuseYaw321(measured_hdg, _ev_sample_delayed.angVar);
} else {
float measured_hdg = getEuler312Yaw(_ev_sample_delayed.quat);
fuseYaw312(measured_hdg, _ev_sample_delayed.angVar);
}
}
// record observation and estimate for use next time