Plane: don't set fly-forward when transitioning

when quad motors are providing assistance don't see the fly-forward
flag in the AHRS. This will lower the chance of EKF yaw confusion
This commit is contained in:
Andrew Tridgell 2017-04-30 22:00:21 +10:00
parent 2d4eee0cce
commit 351304ebcb
1 changed files with 5 additions and 2 deletions

View File

@ -595,8 +595,11 @@ void Plane::update_flight_mode(void)
steer_state.hold_course_cd = -1;
}
// ensure we are fly-forward
if (quadplane.in_vtol_mode()) {
// ensure we are fly-forward when we are flying as a pure fixed
// wing aircraft. This helps the EKF produce better state
// estimates as it can make stronger assumptions
if (quadplane.in_vtol_mode() ||
quadplane.in_assisted_flight()) {
ahrs.set_fly_forward(false);
} else {
ahrs.set_fly_forward(true);