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

AC_AttitudeControl: bodyframe roll log target attitude bugfix

This commit is contained in:
Mark Whitehorn 2019-03-29 11:24:19 -06:00 committed by Randy Mackay
parent 9ab8a55c8f
commit b7957e820f
1 changed files with 8 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -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);