fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-14 18:13:33 +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
EKF/ekf.h
+1
View File
@@ -380,6 +380,7 @@ private:
bool _flt_mag_align_complete{true}; ///< true when the in-flight mag field alignment has been completed
uint64_t _time_last_movement{0}; ///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
float _saved_mag_variance[6] {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
bool _velpos_reset_request{false}; ///< true when a large yaw error has been fixed and a velocity and position state reset is required
gps_check_fail_status_u _gps_check_fail_status{};
EKF/ekf_helper.cpp
+3 -2
View File
@@ -411,6 +411,9 @@ bool Ekf::realignYawGPS()
// calculate new filter quaternion states using corected yaw angle
_state.quat_nominal = Quatf(euler321);
// If heading was bad, then we alos need to reset the velocity and position states
_velpos_reset_request = badMagYaw;
// update transformation matrix from body to world frame using the current state estimate
_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
@@ -453,8 +456,6 @@ bool Ekf::realignYawGPS()
// capture the reset event
_state_reset_status.quat_counter++;
// the alignment using GPS has been successful
_control_status.flags.yaw_align = true;
return true;
} else {