From 0baf86c48593efbceba14e16760ae7c9dd359892 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Wed, 2 Dec 2015 10:45:34 -0800 Subject: [PATCH] AC_AttitudeControl: fixup more names --- libraries/AC_AttitudeControl/AC_AttitudeControl.cpp | 10 +++++----- libraries/AC_AttitudeControl/AC_AttitudeControl.h | 10 +++++----- .../AC_AttitudeControl/AC_AttitudeControl_Heli.cpp | 10 +++++----- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 3543149505..b3c0e54dbb 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -240,7 +240,7 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle // Convert from centidegrees on public interface to radians float euler_roll_angle_rad = radians(euler_roll_angle_cd*0.01f); float euler_pitch_angle_rad = radians(euler_pitch_angle_cd*0.01f); - float yaw_angle_ef_rad = radians(euler_yaw_angle_cd*0.01f); + float euler_yaw_angle_rad = radians(euler_yaw_angle_cd*0.01f); Vector3f att_error_euler_rad; @@ -250,7 +250,7 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle // Set attitude targets from input. _att_target_euler_rad.x = constrain_float(euler_roll_angle_rad, -get_tilt_limit_rad(), get_tilt_limit_rad()); _att_target_euler_rad.y = constrain_float(euler_pitch_angle_rad, -get_tilt_limit_rad(), get_tilt_limit_rad()); - _att_target_euler_rad.z = yaw_angle_ef_rad; + _att_target_euler_rad.z = euler_yaw_angle_rad; // Update attitude error. att_error_euler_rad.x = wrap_PI(_att_target_euler_rad.x - _ahrs.roll); @@ -374,12 +374,12 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, fl input_att_quat_bf_ang_vel(att_target_quat, _att_target_ang_vel_rads); } -void AC_AttitudeControl::input_att_quat_bf_ang_vel(const Quaternion& att_target_quat, const Vector3f& ang_vel_target_rads) +void AC_AttitudeControl::input_att_quat_bf_ang_vel(const Quaternion& att_target_quat, const Vector3f& att_target_ang_vel_rads) { // Update euler attitude target and angular velocity targets att_target_quat.to_euler(_att_target_euler_rad.x,_att_target_euler_rad.y,_att_target_euler_rad.z); - _att_target_ang_vel_rads = ang_vel_target_rads; - ang_vel_to_euler_derivative(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), ang_vel_target_rads, _att_target_euler_deriv_rads); + _att_target_ang_vel_rads = att_target_ang_vel_rads; + ang_vel_to_euler_derivative(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), att_target_ang_vel_rads, _att_target_euler_deriv_rads); // Retrieve quaternion vehicle attitude // TODO add _ahrs.get_quaternion() diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index f9b0240586..420cae53db 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -120,13 +120,13 @@ public: void input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds); // Command an euler roll, pitch and yaw angle - void input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_ef_cd, float euler_yaw_angle_cd, bool slew_yaw); + void input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw); // Command an euler roll, pitch, and yaw rate void input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds); // Command a quaternion attitude and a body-frame angular velocity - void input_att_quat_bf_ang_vel(const Quaternion& att_target_quat, const Vector3f& ang_vel_target_rads); + void input_att_quat_bf_ang_vel(const Quaternion& att_target_quat, const Vector3f& att_target_ang_vel_rads); // Command an angular velocity virtual void input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds); @@ -217,13 +217,13 @@ public: protected: // Update _att_target_euler_rad.x by integrating a 321-intrinsic euler roll angle derivative - void update_att_target_and_error_roll(float roll_rate_ef_rads, Vector3f &angle_ef_error_rad, float overshoot_max_rad); + void update_att_target_and_error_roll(float euler_roll_rate_rads, Vector3f &att_error_euler_rad, float overshoot_max_rad); // Update _att_target_euler_rad.y by integrating a 321-intrinsic euler pitch angle derivative - void update_att_target_and_error_pitch(float pitch_rate_ef_rads, Vector3f &angle_ef_error_rad, float overshoot_max_rad); + void update_att_target_and_error_pitch(float euler_pitch_rate_rads, Vector3f &att_error_euler_rad, float overshoot_max_rad); // Update _att_target_euler_rad.z by integrating a 321-intrinsic euler yaw angle derivative - void update_att_target_and_error_yaw(float yaw_rate_ef_rads, Vector3f &angle_ef_error_rad, float overshoot_max_rad); + void update_att_target_and_error_yaw(float euler_yaw_rate_rads, Vector3f &att_error_euler_rad, float overshoot_max_rad); // Integrate vehicle rate into _att_error_rot_vec_rad void integrate_bf_rate_error_to_angle_errors(); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index c211981039..b1ce5c0448 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -60,14 +60,14 @@ void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_pass _att_error_rot_vec_rad.y = 0; // update our earth-frame angle targets - Vector3f angle_ef_error_rad; + Vector3f att_error_euler_rad; // convert angle error rotation vector into 321-intrinsic euler angle difference // NOTE: this results an an approximation linearized about the vehicle's attitude - if (ang_vel_to_euler_derivative(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _att_error_rot_vec_rad, angle_ef_error_rad)) { - _att_target_euler_rad.x = wrap_PI(angle_ef_error_rad.x + _ahrs.roll); - _att_target_euler_rad.y = wrap_PI(angle_ef_error_rad.y + _ahrs.pitch); - _att_target_euler_rad.z = wrap_2PI(angle_ef_error_rad.z + _ahrs.yaw); + if (ang_vel_to_euler_derivative(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _att_error_rot_vec_rad, att_error_euler_rad)) { + _att_target_euler_rad.x = wrap_PI(att_error_euler_rad.x + _ahrs.roll); + _att_target_euler_rad.y = wrap_PI(att_error_euler_rad.y + _ahrs.pitch); + _att_target_euler_rad.z = wrap_2PI(att_error_euler_rad.z + _ahrs.yaw); } // handle flipping over pitch axis