diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index c6083a335e..5c87d089c3 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -299,67 +299,6 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler attitude_controller_run_quat(); } -// Command an euler roll pitch and yaw angle and an euler yaw rate for use with input shaped commands -void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, float euler_yaw_rate_cds) -{ - // Convert from centidegrees on public interface to radians - float euler_roll_angle = radians(euler_roll_angle_cd * 0.01f); - float euler_pitch_angle = radians(euler_pitch_angle_cd * 0.01f); - float euler_yaw_angle = radians(euler_yaw_angle_cd * 0.01f); - float euler_yaw_rate = radians(euler_yaw_rate_cds * 0.01f); - - // calculate the attitude target euler angles - _attitude_target.to_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z); - - // Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors) - euler_roll_angle += get_roll_trim_rad(); - - if (_rate_bf_ff_enabled) { - // translate the roll pitch and yaw acceleration limits to the euler axis - const Vector3f euler_accel = euler_accel_limit(_euler_angle_target, Vector3f{get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()}); - - // When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler - // angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration - // and an exponential decay specified by _input_tc at the end. - _attitude_correction_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle - _euler_angle_target.x), 0.0f, euler_accel.x, _attitude_correction_euler_rate.x, _dt); - _attitude_correction_euler_rate.y = input_shaping_angle(wrap_PI(euler_pitch_angle - _euler_angle_target.y), 0.0f, euler_accel.y, _attitude_correction_euler_rate.y, _dt); - _attitude_correction_euler_rate.z = input_shaping_angle(wrap_PI(euler_yaw_angle - _euler_angle_target.z), _input_tc, euler_accel.z, _attitude_correction_euler_rate.z, _dt); - _attitude_correction_euler_rate.z = constrain_float(_attitude_correction_euler_rate.z, -get_slew_yaw_rads(), get_slew_yaw_rads()); - _euler_rate_target = _attitude_correction_euler_rate; - - // When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing - // the output rate towards the input rate. - _attitude_desired_euler_rate.z = input_shaping_ang_vel(_attitude_desired_euler_rate.z, euler_yaw_rate, euler_accel.z, _dt); - _euler_rate_target.z += _attitude_desired_euler_rate.z; - _euler_rate_target.z = constrain_float(_euler_rate_target.z, -get_slew_yaw_rads(), get_slew_yaw_rads()); - - - // Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward - euler_rate_to_ang_vel(_euler_angle_target, _euler_rate_target, _ang_vel_target); - // Limit the angular velocity - ang_vel_limit(_ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max)); - // Convert body-frame angular velocity into euler angle derivative of desired attitude - ang_vel_to_euler_rate(_euler_angle_target, _ang_vel_target, _euler_rate_target); - } else { - // When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed. - _euler_angle_target.x = euler_roll_angle; - _euler_angle_target.y = euler_pitch_angle; - // Compute constrained angle error - float angle_error = constrain_float(wrap_PI(euler_yaw_angle - _euler_angle_target.z), -get_slew_yaw_rads() * _dt, get_slew_yaw_rads() * _dt); - // Update attitude target from constrained angle error - _euler_angle_target.z = wrap_PI(angle_error + _euler_angle_target.z); - // Compute quaternion target attitude - _attitude_target.from_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z); - - // Set rate feedforward requests to zero - _euler_rate_target.zero(); - _ang_vel_target.zero(); - } - - // Call quaternion attitude controller - attitude_controller_run_quat(); -} - // Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw) { diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 38ac359c90..c06f9d0924 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -146,10 +146,6 @@ public: // Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing virtual 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 and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing - virtual void input_euler_angle_roll_pitch_yaw_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, float euler_yaw_rate_cds); - // Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing virtual 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); @@ -416,9 +412,6 @@ protected: // second. Vector3f _euler_rate_target; - Vector3f _attitude_desired_euler_rate; - Vector3f _attitude_correction_euler_rate; - // This represents a quaternion rotation in NED frame to the target (setpoint) // attitude used in the attitude controller. Quaternion _attitude_target;