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

EKF: Fix yaw reset for fixed wing

Browse Source 
Ensures that a complete reset of velocity and position states will always be performed if yaw has had to be reset using GPS velocity.
Ensures that the yaw_align status cannot be set to false once the filter has aligned.
This commit is contained in:
Paul Riseborough
2017-11-13 06:55:02 +11:00
parent 7852c0ed03
commit 363edf5eb9
3 changed files with 17 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+13 -11
View File
@@ -485,14 +485,12 @@ void Ekf::controlGpsFusion()
if (do_reset) {
// Reset states to the last GPS measurement
if (_control_status.flags.fixed_wing) {
// if flying a fixed wing aircraft, do a complete reset that includes yaw, velocity and position
// if flying a fixed wing aircraft, do a complete reset that includes yaw
realignYawGPS();
resetVelocity();
resetPosition();
} else {
resetVelocity();
resetPosition();
}
resetVelocity();
resetPosition();
_velpos_reset_request = false;
ECL_WARN("EKF GPS fusion timeout - reset to GPS");
// Reset the timeout counters
@@ -1200,12 +1198,16 @@ void Ekf::controlMagFusion()
if (!_flt_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.yaw_align = realignYawGPS();
_flt_mag_align_complete = _control_status.flags.yaw_align;
_flt_mag_align_complete = realignYawGPS();
if (_velpos_reset_request) {
resetVelocity();
resetPosition();
_velpos_reset_request = false;
}
} else {
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
_flt_mag_align_complete = _control_status.flags.yaw_align;
_flt_mag_align_complete = resetMagHeading(_mag_sample_delayed.mag);
}
_control_status.flags.yaw_align = _control_status.flags.yaw_align || _flt_mag_align_complete;
} else {
// reset the mag field covariances
zeroRows(P, 16, 21);
@@ -1261,7 +1263,7 @@ void Ekf::controlMagFusion()
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) {
// if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states
if (!_control_status.flags.mag_3D) {
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag) || _control_status.flags.yaw_align;
}
// always use 3-axis mag fusion