Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-04 21:23:57 +08:00
Code Issues Actions 4 Packages Projects Releases Wiki Activity

EKF: Ensure FW yaw alignment method is used on first in-air reset

Browse Source
This commit is contained in:
Paul Riseborough
2019-01-30 22:26:00 +11:00
committed by Daniel Agar
parent 3accab1ac5
commit dd58e69549
1 changed files with 28 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+16 -25
View File
@@ -1388,14 +1388,27 @@ void Ekf::controlMagFusion()
if (!_control_status.flags.mag_align_complete) { if (!_control_status.flags.mag_align_complete) {
// Check if height has increased sufficiently to be away from ground magnetic anomalies // Check if height has increased sufficiently to be away from ground magnetic anomalies
// and request a yaw reset if not already requested. // and request a yaw reset if not already requested.
_mag_yaw_reset_req |= (_last_on_ground_posD - _state.pos(2)) > 1.5f; _mag_yaw_reset_req |= ((_last_on_ground_posD - _state.pos(2)) > 1.5f);
} }
// perform a yaw reset if requested by other functions // perform a yaw reset if requested by other functions
if (_mag_yaw_reset_req) { if (_mag_yaw_reset_req && _control_status.flags.tilt_align) {
if (!_mag_use_inhibit ) { if (!_mag_use_inhibit ) {
if (!_control_status.flags.mag_align_complete && _control_status.flags.fixed_wing && _control_status.flags.in_air) {
// A fixed wing vehicle can use GPS to bound yaw errors immediately after launch
_control_status.flags.mag_align_complete = realignYawGPS();
if (_velpos_reset_request) {
resetVelocity();
resetPosition();
_velpos_reset_request = false;
}
} else {
_control_status.flags.mag_align_complete = resetMagHeading(_mag_sample_delayed.mag) && _control_status.flags.in_air; _control_status.flags.mag_align_complete = resetMagHeading(_mag_sample_delayed.mag) && _control_status.flags.in_air;
} }
}
_control_status.flags.yaw_align = _control_status.flags.yaw_align || _control_status.flags.mag_align_complete;
_mag_yaw_reset_req = false; _mag_yaw_reset_req = false;
} }
@@ -1407,9 +1420,6 @@ void Ekf::controlMagFusion()
_control_status.flags.mag_3D = false; _control_status.flags.mag_3D = false;
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO || _params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW) { } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO || _params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW) {
// Check if height has increased sufficiently to be away from ground magnetic anomalies
bool height_achieved = (_last_on_ground_posD - _state.pos(2)) > 1.5f;
// Check if there has been enough change in horizontal velocity to make yaw observable // Check if there has been enough change in horizontal velocity to make yaw observable
// Apply hysteresis to check to avoid rapid toggling // Apply hysteresis to check to avoid rapid toggling
if (_yaw_angle_observable) { if (_yaw_angle_observable) {
@@ -1452,30 +1462,12 @@ void Ekf::controlMagFusion()
// decide whether 3-axis magnetomer fusion can be used // decide whether 3-axis magnetomer fusion can be used
bool use_3D_fusion = _control_status.flags.tilt_align && // Use of 3D fusion requires valid tilt estimates bool use_3D_fusion = _control_status.flags.tilt_align && // Use of 3D fusion requires valid tilt estimates
_control_status.flags.in_air && // don't use when on the ground becasue of magnetic anomalies _control_status.flags.in_air && // don't use when on the ground becasue of magnetic anomalies
(_control_status.flags.mag_align_complete || height_achieved) && // once in-flight field alignment has been performed, ignore relative height _control_status.flags.mag_align_complete &&
((_imu_sample_delayed.time_us - _time_last_movement) < 2 * 1000 * 1000); // Using 3-axis fusion for a minimum period after to allow for false negatives ((_imu_sample_delayed.time_us - _time_last_movement) < 2 * 1000 * 1000); // Using 3-axis fusion for a minimum period after to allow for false negatives
// perform switch-over // perform switch-over
if (use_3D_fusion) { if (use_3D_fusion) {
if (!_control_status.flags.mag_3D) { if (!_control_status.flags.mag_3D) {
if (!_control_status.flags.mag_align_complete) {
// If we are flying a vehicle that flies forward, eg plane, then we can use the GPS course to check and correct the heading
if (_control_status.flags.fixed_wing && _control_status.flags.in_air) {
_control_status.flags.mag_align_complete = realignYawGPS();
if (_velpos_reset_request) {
resetVelocity();
resetPosition();
_velpos_reset_request = false;
}
} else {
_control_status.flags.mag_align_complete = resetMagHeading(_mag_sample_delayed.mag);
}
_control_status.flags.yaw_align = _control_status.flags.yaw_align || _control_status.flags.mag_align_complete;
} else {
// reset the mag field covariances // reset the mag field covariances
zeroRows(P, 16, 21); zeroRows(P, 16, 21);
zeroCols(P, 16, 21); zeroCols(P, 16, 21);
@@ -1491,7 +1483,6 @@ void Ekf::controlMagFusion()
} }
} }
} }
}
// only use one type of mag fusion at the same time // only use one type of mag fusion at the same time
_control_status.flags.mag_3D = _control_status.flags.mag_align_complete; _control_status.flags.mag_3D = _control_status.flags.mag_align_complete;