AC_AttitudeControl: use AHRS for attitude init

the attitude controller may not have been running at the time we init
the pos controller, so the target attitudes from the attitude
controller may not be valid. Use AHRS attitude instead

this affects quadplanes starting position control on landing, but
could also impact multirotors switching from ACRO mode
This commit is contained in:
Andrew Tridgell 2022-02-01 09:02:41 +11:00
parent dee3f9dce1
commit dec75b96e8
1 changed files with 4 additions and 4 deletions

View File

@ -469,10 +469,10 @@ void AC_PosControl::relax_velocity_controller_xy()
void AC_PosControl::init_xy_controller()
{
// set roll, pitch lean angle targets to current attitude
const Vector3f &att_target_euler_cd = _attitude_control.get_att_target_euler_cd();
_roll_target = att_target_euler_cd.x;
_pitch_target = att_target_euler_cd.y;
_yaw_target = att_target_euler_cd.z; // todo: this should be thrust vector heading, not yaw.
const auto &ahrs = AP::ahrs();
_roll_target = ahrs.roll_sensor;
_pitch_target = ahrs.pitch_sensor;
_yaw_target = ahrs.yaw_sensor; // todo: this should be thrust vector heading, not yaw.
_yaw_rate_target = 0.0f;
_pos_target.xy() = _inav.get_position_xy_cm().topostype();