fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-13 04:27:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: update initial angle alignment check

Browse Source
This commit is contained in:
Paul Riseborough
2016-05-03 20:59:49 +10:00
parent bc34b14779
commit 727a43764f
4 changed files with 3 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+3 -7
View File
@@ -51,13 +51,9 @@ void Ekf::controlFusionModes()
// Get the magnetic declination
calcMagDeclination();
// Check for tilt convergence during initial alignment
// filter the tilt error vector using a 1 sec time constant LPF
float filt_coef = 1.0f * _imu_sample_delayed.delta_ang_dt;
_tilt_err_length_filt = filt_coef * _tilt_err_vec.norm() + (1.0f - filt_coef) * _tilt_err_length_filt;
// Once the tilt error has reduced sufficiently, initialise the yaw and magnetic field states
if (_tilt_err_length_filt < 0.005f && !_control_status.flags.tilt_align) {
// Once the angular uncertainty has reduced sufficiently, initialise the yaw and magnetic field states
float total_angle_variance = P[0][0] + P[1][1] + P[2][2] + P[3][3];
if (total_angle_variance < 0.001f && !_control_status.flags.tilt_align) {
_control_status.flags.tilt_align = true;
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
}