AC_AttitudeControl: Remove Unused Function

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)
 {

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;