From 6175a896eef73c78069ccab75bfedaf830213b17 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 26 Jun 2017 00:03:16 +0930 Subject: [PATCH] AC_AttitudeControl: add set smoothing gain Smoothing gain value should be set once when entering a mode --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 37 +++++++------------ .../AC_AttitudeControl/AC_AttitudeControl.h | 12 ++++-- .../AC_AttitudeControl_Heli.cpp | 8 ++-- .../AC_AttitudeControl_Heli.h | 4 +- 4 files changed, 29 insertions(+), 32 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index cb3568d83b..8f2bc864ac 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -156,14 +156,11 @@ void AC_AttitudeControl::reset_rate_controller_I_terms() // Command a Quaternion attitude with feedforward and smoothing -void AC_AttitudeControl::input_quaternion(Quaternion attitude_desired_quat, float smoothing_gain) +void AC_AttitudeControl::input_quaternion(Quaternion attitude_desired_quat) { // calculate the attitude target euler angles _attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z); - // ensure smoothing gain can not cause overshoot - smoothing_gain = constrain_float(smoothing_gain,1.0f,1/_dt); - Quaternion attitude_error_quat = _attitude_target_quat.inverse() * attitude_desired_quat; Vector3f attitude_error_angle; attitude_error_quat.to_axis_angle(attitude_error_angle); @@ -171,10 +168,10 @@ void AC_AttitudeControl::input_quaternion(Quaternion attitude_desired_quat, floa if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) { // 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 smoothing_gain at the end. - _attitude_target_ang_vel.x = input_shaping_angle(wrap_PI(attitude_error_angle.x), smoothing_gain, get_accel_roll_max_radss(), _attitude_target_ang_vel.x, _dt); - _attitude_target_ang_vel.y = input_shaping_angle(wrap_PI(attitude_error_angle.y), smoothing_gain, get_accel_pitch_max_radss(), _attitude_target_ang_vel.y, _dt); - _attitude_target_ang_vel.z = input_shaping_angle(wrap_PI(attitude_error_angle.z), smoothing_gain, get_accel_yaw_max_radss(), _attitude_target_ang_vel.z, _dt); + // and an exponential decay specified by _smoothing_gain at the end. + _attitude_target_ang_vel.x = input_shaping_angle(wrap_PI(attitude_error_angle.x), _smoothing_gain, get_accel_roll_max_radss(), _attitude_target_ang_vel.x, _dt); + _attitude_target_ang_vel.y = input_shaping_angle(wrap_PI(attitude_error_angle.y), _smoothing_gain, get_accel_pitch_max_radss(), _attitude_target_ang_vel.y, _dt); + _attitude_target_ang_vel.z = input_shaping_angle(wrap_PI(attitude_error_angle.z), _smoothing_gain, get_accel_yaw_max_radss(), _attitude_target_ang_vel.z, _dt); // Convert body-frame angular velocity into euler angle derivative of desired attitude ang_vel_to_euler_rate(_attitude_target_euler_angle, _attitude_target_ang_vel, _attitude_target_euler_rate); @@ -191,7 +188,7 @@ void AC_AttitudeControl::input_quaternion(Quaternion attitude_desired_quat, floa } // Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing -void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain) +void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_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); @@ -201,9 +198,6 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler // calculate the attitude target euler angles _attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z); - // ensure smoothing gain can not cause overshoot - smoothing_gain = constrain_float(smoothing_gain,1.0f,1/_dt); - // Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors) euler_roll_angle += get_roll_trim_rad(); @@ -214,8 +208,8 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler // 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 smoothing_gain at the end. - _attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle-_attitude_target_euler_angle.x), smoothing_gain, euler_accel.x, _attitude_target_euler_rate.x, _dt); - _attitude_target_euler_rate.y = input_shaping_angle(wrap_PI(euler_pitch_angle-_attitude_target_euler_angle.y), smoothing_gain, euler_accel.y, _attitude_target_euler_rate.y, _dt); + _attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle-_attitude_target_euler_angle.x), _smoothing_gain, euler_accel.x, _attitude_target_euler_rate.x, _dt); + _attitude_target_euler_rate.y = input_shaping_angle(wrap_PI(euler_pitch_angle-_attitude_target_euler_angle.y), _smoothing_gain, euler_accel.y, _attitude_target_euler_rate.y, _dt); // 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. @@ -241,7 +235,7 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler } // 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, float smoothing_gain) +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) { // Convert from centidegrees on public interface to radians float euler_roll_angle = radians(euler_roll_angle_cd*0.01f); @@ -251,9 +245,6 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle // calculate the attitude target euler angles _attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z); - // ensure smoothing gain can not cause overshoot - smoothing_gain = constrain_float(smoothing_gain,1.0f,1/_dt); - // Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors) euler_roll_angle += get_roll_trim_rad(); @@ -263,10 +254,10 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle // 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 smoothing_gain at the end. - _attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle-_attitude_target_euler_angle.x), smoothing_gain, euler_accel.x, _attitude_target_euler_rate.x, _dt); - _attitude_target_euler_rate.y = input_shaping_angle(wrap_PI(euler_pitch_angle-_attitude_target_euler_angle.y), smoothing_gain, euler_accel.y, _attitude_target_euler_rate.y, _dt); - _attitude_target_euler_rate.z = input_shaping_angle(wrap_PI(euler_yaw_angle-_attitude_target_euler_angle.z), smoothing_gain, euler_accel.z, _attitude_target_euler_rate.z, _dt); + // and an exponential decay specified by _smoothing_gain at the end. + _attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle-_attitude_target_euler_angle.x), _smoothing_gain, euler_accel.x, _attitude_target_euler_rate.x, _dt); + _attitude_target_euler_rate.y = input_shaping_angle(wrap_PI(euler_pitch_angle-_attitude_target_euler_angle.y), _smoothing_gain, euler_accel.y, _attitude_target_euler_rate.y, _dt); + _attitude_target_euler_rate.z = input_shaping_angle(wrap_PI(euler_yaw_angle-_attitude_target_euler_angle.z), _smoothing_gain, euler_accel.z, _attitude_target_euler_rate.z, _dt); if (slew_yaw) { _attitude_target_euler_rate.z = constrain_float(_attitude_target_euler_rate.z, -get_slew_yaw_rads(), get_slew_yaw_rads()); } @@ -498,7 +489,7 @@ void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& att_to_quat, } // calculates the velocity correction from an angle error. The angular velocity has acceleration and -// deceleration limits including basic jerk limiting using smoothing_gain +// deceleration limits including basic jerk limiting using _smoothing_gain float AC_AttitudeControl::input_shaping_angle(float error_angle, float smoothing_gain, float accel_max, float target_ang_vel, float dt) { // Calculate the velocity as error approaches zero with acceleration limited by accel_max_radss diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 232f8f676f..58de2292ea 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -104,6 +104,9 @@ public: // Sets and saves the yaw acceleration limit in centidegrees/s/s void save_accel_yaw_max(float accel_yaw_max) { _accel_yaw_max.set_and_save(accel_yaw_max); } + // Sets the yaw acceleration limit in centidegrees/s/s + void set_smoothing_gain(float smoothing_gain) { _smoothing_gain = constrain_float(smoothing_gain,1.0f,1/_dt); } + // Ensure attitude controller have zero errors to relax rate controller output void relax_attitude_controllers(); @@ -120,13 +123,13 @@ public: void shift_ef_yaw_target(float yaw_shift_cd); // Command a Quaternion attitude with feedforward and smoothing - void input_quaternion(Quaternion attitude_desired_quat, float smoothing_gain); + void input_quaternion(Quaternion attitude_desired_quat); // 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, float smoothing_gain); + 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, 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, float smoothing_gain); + 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); // Command an euler roll, pitch, and yaw rate with angular velocity feedforward and smoothing void input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds); @@ -370,6 +373,9 @@ protected: // mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1 float _throttle_rpy_mix; + // smoothing gain (should be replaced with s-curve generation) + float _smoothing_gain; + // References to external libraries const AP_AHRS_View& _ahrs; const AP_Vehicle::MultiCopter &_aparm; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index 362400f732..074dd10c7b 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -439,19 +439,19 @@ void AC_AttitudeControl_Heli::set_throttle_out(float throttle_in, bool apply_ang } // Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing -void AC_AttitudeControl_Heli::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain) +void AC_AttitudeControl_Heli::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds) { if (_inverted_flight) { euler_roll_angle_cd = wrap_180_cd(euler_roll_angle_cd + 18000); } - AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_rate_cds, smoothing_gain); + AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_rate_cds); } // Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing -void AC_AttitudeControl_Heli::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw, float smoothing_gain) +void AC_AttitudeControl_Heli::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw) { if (_inverted_flight) { euler_roll_angle_cd = wrap_180_cd(euler_roll_angle_cd + 18000); } - AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_angle_cd, slew_yaw, smoothing_gain); + AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_angle_cd, slew_yaw); } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index 5c26743461..d3769458ba 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h @@ -97,10 +97,10 @@ public: void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) override; // Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing - void input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain) override; + void input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds) override; // Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing - 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, float smoothing_gain) override; + 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) override; // enable/disable inverted flight void set_inverted_flight(bool inverted) override {