diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index ea5a540e9e..c5625d4380 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -2,6 +2,14 @@ #include #include +#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) + // default gains for Plane + # define AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT 0.2f // Soft +#else + // default gains for Copter and Sub + # define AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT 0.15f // Medium +#endif + // table of user settable parameters const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = { @@ -122,6 +130,15 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = { // @User: Advanced AP_GROUPINFO("RATE_Y_MAX", 19, AC_AttitudeControl, _ang_vel_yaw_max, 0.0f), + // @Param: INPUT_TC + // @DisplayName: Attitude control input time constant (aka smoothing) + // @Description: Attitude control input time constant. Low numbers lead to sharper response, higher numbers to softer response + // @Range: 0 1 + // @Increment: 0.01 + // @Values: 0.5:Very Soft, 0.2:Soft, 0.15:Medium, 0.1:Crisp, 0.05:Very Crisp + // @User: Standard + AP_GROUPINFO("INPUT_TC", 20, AC_AttitudeControl, _input_tc, AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT), + AP_GROUPEND }; @@ -198,10 +215,10 @@ void AC_AttitudeControl::input_quaternion(Quaternion attitude_desired_quat) 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 _input_tc at the end. + _attitude_target_ang_vel.x = input_shaping_angle(wrap_PI(attitude_error_angle.x), _input_tc, 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), _input_tc, 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), _input_tc, get_accel_yaw_max_radss(), _attitude_target_ang_vel.z, _dt); // Limit the angular velocity ang_vel_limit(_attitude_target_ang_vel, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max)); @@ -240,8 +257,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), _input_tc, 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), _input_tc, 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. @@ -290,10 +307,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 _input_tc at the end. + _attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle-_attitude_target_euler_angle.x), _input_tc, 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), _input_tc, 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), _input_tc, 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()); } @@ -537,11 +554,11 @@ 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 -float AC_AttitudeControl::input_shaping_angle(float error_angle, float smoothing_gain, float accel_max, float target_ang_vel, float dt) +// deceleration limits including basic jerk limiting using _input_tc +float AC_AttitudeControl::input_shaping_angle(float error_angle, float input_tc, float accel_max, float target_ang_vel, float dt) { // Calculate the velocity as error approaches zero with acceleration limited by accel_max_radss - float desired_ang_vel = sqrt_controller(error_angle, smoothing_gain, accel_max, dt); + float desired_ang_vel = sqrt_controller(error_angle, 1.0f / MAX(input_tc, 0.01f), accel_max, dt); // Acceleration is limited directly to smooth the beginning of the curve. return input_shaping_ang_vel(target_ang_vel, desired_ang_vel, accel_max, dt); @@ -565,8 +582,8 @@ void AC_AttitudeControl::input_shaping_rate_predictor(Vector2f error_angle, Vect { if (_rate_bf_ff_enabled) { // translate the roll pitch and yaw acceleration limits to the euler axis - target_ang_vel.x = input_shaping_angle(wrap_PI(error_angle.x), _smoothing_gain, get_accel_roll_max_radss(), target_ang_vel.x, dt); - target_ang_vel.y = input_shaping_angle(wrap_PI(error_angle.y), _smoothing_gain, get_accel_pitch_max_radss(), target_ang_vel.y, dt); + target_ang_vel.x = input_shaping_angle(wrap_PI(error_angle.x), _input_tc, get_accel_roll_max_radss(), target_ang_vel.x, dt); + target_ang_vel.y = input_shaping_angle(wrap_PI(error_angle.y), _input_tc, get_accel_pitch_max_radss(), target_ang_vel.y, dt); } else { target_ang_vel.x = _p_angle_roll.get_p(wrap_PI(error_angle.x)); target_ang_vel.y = _p_angle_pitch.get_p(wrap_PI(error_angle.y)); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 9de6493ed8..c52280ca06 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -104,8 +104,8 @@ 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); } + // set the rate control input smoothing time constant + void set_input_tc(float input_tc) { _input_tc = constrain_float(input_tc, 0.0f, 1.0f); } // Ensure attitude controller have zero errors to relax rate controller output void relax_attitude_controllers(); @@ -338,6 +338,9 @@ protected: // Angle limit time constant (to maintain altitude) AP_Float _angle_limit_tc; + // rate controller input smoothing time constant + AP_Float _input_tc; + // Intersampling period in seconds float _dt; @@ -385,9 +388,6 @@ 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;