From dec75b96e89aefe567f8575ba7d49195d984da1c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 1 Feb 2022 09:02:41 +1100 Subject: [PATCH] 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 --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index d8ab060c4e..ecfdec0434 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -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();