dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #338 from CarlOlsson/fix/fw_pos_reset 

EKF: Fix bug when resetting position and velocities for fw due to som…
This commit is contained in:
Paul Riseborough 2017-10-18 07:02:34 +11:00 committed by GitHub
parent 410697cb0c c81cdfa1ce
commit 61e0c04811
2 changed files with 2 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/control.cpp
View File

@ -454,6 +454,8 @@ void Ekf::controlGpsFusion()
if (_control_status.flags.fixed_wing) {
// if flying a fixed wing aircraft, do a complete reset that includes yaw, velocity and position
realignYawGPS();
resetVelocity();
resetPosition();
} else {
resetVelocity();
resetPosition();

4
EKF/ekf_helper.cpp
View File

@ -414,10 +414,6 @@ bool Ekf::realignYawGPS()
// update transformation matrix from body to world frame using the current state estimate
_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
// reset the velocity and posiiton states as they will be inaccurate due to bad yaw
resetVelocity();
resetPosition();
// Use the last magnetometer measurements to reset the field states
_state.mag_B.zero();
_state.mag_I = _R_to_earth * _mag_sample_delayed.mag;