Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: fixed EKF yaw reset

Browse Source 
we need to do the yaw reset before updating the rest of quadplane, or
it is not effective

thanks to Leonard for noticing the bug!
This commit is contained in:
Andrew Tridgell 2019-01-13 15:24:51 +11:00
parent 6dffb209d2
commit f35cebcca4
2 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduPlane/ArduPlane.cpp
View File

@ -166,6 +166,9 @@ void Plane::ahrs_update()
steer_state.locked_course_err += ahrs.get_yaw_rate_earth() * G_Dt;
steer_state.locked_course_err = wrap_PI(steer_state.locked_course_err);
// check if we have had a yaw reset from the EKF
quadplane.check_yaw_reset();
// update inertial_nav for quadplane
quadplane.inertial_nav.update(G_Dt);
}

5
ArduPlane/quadplane.cpp
View File

@ -829,6 +829,9 @@ void QuadPlane::init_hover(void)
*/
void QuadPlane::check_yaw_reset(void)
{
if (!initialised) {
return;
}
float yaw_angle_change_rad = 0.0f;
uint32_t new_ekfYawReset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad);
if (new_ekfYawReset_ms != ekfYawReset_ms) {
@ -1485,8 +1488,6 @@ void QuadPlane::update(void)
pos_control->relax_alt_hold_controllers(0);
}
check_yaw_reset();
if (!in_vtol_mode()) {
update_transition();
} else {