AC_Precland: bug fix to update ef angles only with new readings

This commit is contained in:
Daniel Nugent 2015-09-11 19:50:16 +09:00 committed by Randy Mackay
parent db8f28f2aa
commit 21ac12f1c3
1 changed files with 2 additions and 0 deletions

View File

@ -132,11 +132,13 @@ void AC_PrecLand::calc_angles_and_pos(float alt_above_terrain_cm)
// exit immediately if not enabled // exit immediately if not enabled
if (_backend == NULL) { if (_backend == NULL) {
_have_estimate = false; _have_estimate = false;
return;
} }
// get body-frame angles to target from backend // get body-frame angles to target from backend
if (!_backend->get_angle_to_target(_bf_angle_to_target.x, _bf_angle_to_target.y)) { if (!_backend->get_angle_to_target(_bf_angle_to_target.x, _bf_angle_to_target.y)) {
_have_estimate = false; _have_estimate = false;
return;
} }
// subtract vehicle lean angles // subtract vehicle lean angles