diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 4330ae5483..8db0ea9fb2 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -405,19 +405,19 @@ void AC_AttitudeControl::rate_controller_run() _motors.set_yaw(rate_bf_to_motor_yaw(_ang_vel_target_rads.z)); } -void AC_AttitudeControl::euler_rate_to_ang_vel(const Vector3f& euler_rad, const Vector3f& euler_dot_rads, Vector3f& ang_vel_rads) +void AC_AttitudeControl::euler_rate_to_ang_vel(const Vector3f& euler_rad, const Vector3f& euler_rate_rads, Vector3f& ang_vel_rads) { float sin_theta = sinf(euler_rad.y); float cos_theta = cosf(euler_rad.y); float sin_phi = sinf(euler_rad.x); float cos_phi = cosf(euler_rad.x); - ang_vel_rads.x = euler_dot_rads.x - sin_theta * euler_dot_rads.z; - ang_vel_rads.y = cos_phi * euler_dot_rads.y + sin_phi * cos_theta * euler_dot_rads.z; - ang_vel_rads.z = -sin_phi * euler_dot_rads.y + cos_theta * cos_phi * euler_dot_rads.z; + ang_vel_rads.x = euler_rate_rads.x - sin_theta * euler_rate_rads.z; + ang_vel_rads.y = cos_phi * euler_rate_rads.y + sin_phi * cos_theta * euler_rate_rads.z; + ang_vel_rads.z = -sin_phi * euler_rate_rads.y + cos_theta * cos_phi * euler_rate_rads.z; } -bool AC_AttitudeControl::ang_vel_to_euler_rate(const Vector3f& euler_rad, const Vector3f& ang_vel_rads, Vector3f& euler_dot_rads) +bool AC_AttitudeControl::ang_vel_to_euler_rate(const Vector3f& euler_rad, const Vector3f& ang_vel_rads, Vector3f& euler_rate_rads) { float sin_theta = sinf(euler_rad.y); float cos_theta = cosf(euler_rad.y); @@ -429,9 +429,9 @@ bool AC_AttitudeControl::ang_vel_to_euler_rate(const Vector3f& euler_rad, const return false; } - euler_dot_rads.x = ang_vel_rads.x + sin_phi * (sin_theta/cos_theta) * ang_vel_rads.y + cos_phi * (sin_theta/cos_theta) * ang_vel_rads.z; - euler_dot_rads.y = cos_phi * ang_vel_rads.y - sin_phi * ang_vel_rads.z; - euler_dot_rads.z = (sin_phi / cos_theta) * ang_vel_rads.y + (cos_phi / cos_theta) * ang_vel_rads.z; + euler_rate_rads.x = ang_vel_rads.x + sin_phi * (sin_theta/cos_theta) * ang_vel_rads.y + cos_phi * (sin_theta/cos_theta) * ang_vel_rads.z; + euler_rate_rads.y = cos_phi * ang_vel_rads.y - sin_phi * ang_vel_rads.z; + euler_rate_rads.z = (sin_phi / cos_theta) * ang_vel_rads.y + (cos_phi / cos_theta) * ang_vel_rads.z; return true; } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 38d3dc5194..df5c15c703 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -135,11 +135,11 @@ public: virtual void rate_controller_run(); // Convert a 321-intrinsic euler angle derivative to an angular velocity vector - void euler_rate_to_ang_vel(const Vector3f& euler_rad, const Vector3f& euler_dot_rads, Vector3f& ang_vel_rads); + void euler_rate_to_ang_vel(const Vector3f& euler_rad, const Vector3f& euler_rate_rads, Vector3f& ang_vel_rads); // Convert an angular velocity vector to a 321-intrinsic euler angle derivative // Returns false if the vehicle is pitched 90 degrees up or down - bool ang_vel_to_euler_rate(const Vector3f& euler_rad, const Vector3f& ang_vel_rads, Vector3f& euler_dot_rads); + bool ang_vel_to_euler_rate(const Vector3f& euler_rad, const Vector3f& ang_vel_rads, Vector3f& euler_rate_rads); // Configures whether the attitude controller should limit the rate demand to constrain angular acceleration void limit_angle_to_rate_request(bool limit_request) { _att_ctrl_use_accel_limit = limit_request; }