From b7957e820ff7e47b0f886cf5b852a417f39a381e Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Fri, 29 Mar 2019 11:24:19 -0600 Subject: [PATCH] AC_AttitudeControl: bodyframe roll log target attitude bugfix --- libraries/AC_AttitudeControl/AC_AttitudeControl.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index a86a3cd5b0..1e4fe859e8 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -383,6 +383,10 @@ void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll_m(float bf_yaw_Q.from_axis_angle(Vector3f(-cosf(euler_pitch), 0, 0), body_roll); _attitude_target_quat = _attitude_target_quat * bf_roll_Q * bf_yaw_Q; + // calculate the attitude target euler angles + _attitude_target_euler_angle.x = _attitude_target_quat.get_euler_roll(); + _attitude_target_euler_angle.y = _attitude_target_quat.get_euler_pitch(); + // Set rate feedforward requests to zero _attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f); _attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f); @@ -429,6 +433,10 @@ void AC_AttitudeControl::input_euler_rate_yaw_euler_angle_pitch_bf_roll_p(float bf_yaw_Q.from_axis_angle(Vector3f(cpitch, 0, 0), euler_yaw_rate); _attitude_target_quat = _attitude_target_quat * bf_roll_Q * bf_yaw_Q; + // calculate the attitude target euler angles + _attitude_target_euler_angle.x = _attitude_target_quat.get_euler_roll(); + _attitude_target_euler_angle.y = _attitude_target_quat.get_euler_pitch(); + // Set rate feedforward requests to zero _attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f); _attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);