Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ArduPlane: Signal EKF to expect takeoff start to improve GSF yaw estimate

This commit is contained in:
Paul Riseborough 2020-06-13 17:47:32 +10:00 committed by Peter Barker
parent d1a0c2eb30
commit e5bd99a66f
2 changed files with 13 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduPlane/servos.cpp
View File

@ -520,6 +520,16 @@ void Plane::set_servos_controlled(void)
}
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, fwd_thr);
}
// let EKF know to start GSF yaw estimator before takeoff movement starts so that yaw angle is better estimated
const float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
if (!is_flying() && arming.is_armed()) {
bool throw_detected = sq(ahrs.get_accel_ef().x) + sq(ahrs.get_accel_ef().y) > sq(g.takeoff_throttle_min_accel);
bool throttle_up_detected = throttle > aparm.throttle_cruise;
if (throw_detected || throttle_up_detected) {
plane.ahrs.setTakeoffExpected(true);
}
}
}
/*

3
ArduPlane/takeoff.cpp
View File

@ -59,6 +59,9 @@ bool Plane::auto_takeoff_check(void)
}
}
// let EKF know to start GSF yaw estimator before takeoff movement starts so that yaw angle is better estimated
plane.ahrs.setTakeoffExpected(true);
// we've reached the acceleration threshold, so start the timer
if (!takeoff_state.launchTimerStarted) {
takeoff_state.launchTimerStarted = true;