diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 4db1bcbfd6..34805161d1 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -335,12 +335,9 @@ void AC_AttitudeControl::input_quaternion(Quaternion& attitude_desired_quat, Vec Vector3f attitude_error_angle; attitude_error_quat.to_axis_angle(attitude_error_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 _input_tc at the end. - _ang_vel_target_rads.x = input_shaping_angle(wrap_PI(attitude_error_angle.x), _input_tc, get_accel_roll_max_radss(), _ang_vel_target_rads.x, ang_vel_target.x, radians(_ang_vel_roll_max_degs), _dt_s); - _ang_vel_target_rads.y = input_shaping_angle(wrap_PI(attitude_error_angle.y), _input_tc, get_accel_pitch_max_radss(), _ang_vel_target_rads.y, ang_vel_target.y, radians(_ang_vel_pitch_max_degs), _dt_s); - _ang_vel_target_rads.z = input_shaping_angle(wrap_PI(attitude_error_angle.z), _input_tc, get_accel_yaw_max_radss(), _ang_vel_target_rads.z, ang_vel_target.z, radians(_ang_vel_yaw_max_degs), _dt_s); + attitude_command_model(wrap_PI(attitude_error_angle.x), 0.0, _ang_vel_target_rads.x, _ang_accel_target_rads.x, radians(_ang_vel_roll_max_degs), get_accel_roll_max_radss(), _input_tc, _dt_s); + attitude_command_model(wrap_PI(attitude_error_angle.y), 0.0, _ang_vel_target_rads.y, _ang_accel_target_rads.y, radians(_ang_vel_pitch_max_degs), get_accel_pitch_max_radss(), _input_tc, _dt_s); + attitude_command_model(wrap_PI(attitude_error_angle.z), 0.0, _ang_vel_target_rads.z, _ang_accel_target_rads.z, radians(_ang_vel_yaw_max_degs), get_accel_yaw_max_radss(), _rate_y_tc, _dt_s); } else { _attitude_target = attitude_desired_quat; _ang_vel_target_rads = ang_vel_target; @@ -389,25 +386,22 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw_rad(float e euler_roll_angle_rad += get_roll_trim_rad(); if (_rate_bf_ff_enabled) { - // translate the roll pitch and yaw acceleration limits to the euler axis + // translate the roll pitch and yaw rate and acceleration limits to the euler axis const Vector3f euler_accel = euler_accel_limit(_attitude_target, Vector3f{get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()}); + const Vector3f euler_rate_max_rads = euler_accel_limit(_attitude_target, Vector3f{radians(_ang_vel_roll_max_degs), radians(_ang_vel_pitch_max_degs), radians(_ang_vel_yaw_max_degs)}); - // 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. - _euler_rate_target_rads.x = input_shaping_angle(wrap_PI(euler_roll_angle_rad - _euler_angle_target_rad.x), _input_tc, euler_accel.x, _euler_rate_target_rads.x, _dt_s); - _euler_rate_target_rads.y = input_shaping_angle(wrap_PI(euler_pitch_angle_rad - _euler_angle_target_rad.y), _input_tc, euler_accel.y, _euler_rate_target_rads.y, _dt_s); + Vector3f euler_accel_target_rads; + ang_vel_to_euler_rate(_attitude_target, _ang_accel_target_rads, euler_accel_target_rads); + attitude_command_model(wrap_PI(euler_roll_angle_rad - _euler_angle_target_rad.x), 0.0, _euler_rate_target_rads.x, euler_accel_target_rads.x, fabsf(euler_rate_max_rads.x), euler_accel.x, _input_tc, _dt_s); + attitude_command_model(wrap_PI(euler_pitch_angle_rad - _euler_angle_target_rad.y), 0.0, _euler_rate_target_rads.y, euler_accel_target_rads.y, fabsf(euler_rate_max_rads.y), euler_accel.y, _input_tc, _dt_s); // 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. - _euler_rate_target_rads.z = input_shaping_ang_vel(_euler_rate_target_rads.z, euler_yaw_rate_rads, euler_accel.z, _dt_s, _rate_y_tc); + attitude_command_model(0.0, euler_yaw_rate_rads, _euler_rate_target_rads.z, euler_accel_target_rads.z, fabsf(euler_rate_max_rads.z), euler_accel.z, _rate_y_tc, _dt_s); // Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward euler_rate_to_ang_vel(_attitude_target, _euler_rate_target_rads, _ang_vel_target_rads); - // Limit the angular velocity - ang_vel_limit(_ang_vel_target_rads, radians(_ang_vel_roll_max_degs), radians(_ang_vel_pitch_max_degs), radians(_ang_vel_yaw_max_degs)); - // Convert body-frame angular velocity into euler angle derivative of desired attitude - ang_vel_to_euler_rate(_attitude_target, _ang_vel_target_rads, _euler_rate_target_rads); + euler_rate_to_ang_vel(_attitude_target, euler_accel_target_rads, _ang_accel_target_rads); } else { // When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed. _euler_angle_target_rad.x = euler_roll_angle_rad; @@ -419,6 +413,7 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw_rad(float e // Set rate feedforward requests to zero _euler_rate_target_rads.zero(); _ang_vel_target_rads.zero(); + _ang_accel_target_rads.zero(); } // Call quaternion attitude controller @@ -453,34 +448,32 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw_rad(float euler_roll_a const float slew_yaw_max_rads = get_slew_yaw_max_rads(); if (_rate_bf_ff_enabled) { - // translate the roll pitch and yaw acceleration limits to the euler axis + // translate the roll pitch and yaw rate and acceleration limits to the euler axis const Vector3f euler_accel = euler_accel_limit(_attitude_target, Vector3f{get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()}); + const Vector3f euler_rate_max_rads = euler_accel_limit(_attitude_target, Vector3f{radians(_ang_vel_roll_max_degs), radians(_ang_vel_pitch_max_degs), radians(_ang_vel_yaw_max_degs)}); - // 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. - _euler_rate_target_rads.x = input_shaping_angle(wrap_PI(euler_roll_angle_rad - _euler_angle_target_rad.x), _input_tc, euler_accel.x, _euler_rate_target_rads.x, _dt_s); - _euler_rate_target_rads.y = input_shaping_angle(wrap_PI(euler_pitch_angle_rad - _euler_angle_target_rad.y), _input_tc, euler_accel.y, _euler_rate_target_rads.y, _dt_s); - _euler_rate_target_rads.z = input_shaping_angle(wrap_PI(euler_yaw_angle_rad - _euler_angle_target_rad.z), _input_tc, euler_accel.z, _euler_rate_target_rads.z, _dt_s); + Vector3f euler_accel_target_rads; + ang_vel_to_euler_rate(_attitude_target, _ang_accel_target_rads, euler_accel_target_rads); + attitude_command_model(wrap_PI(euler_roll_angle_rad - _euler_angle_target_rad.x), 0.0, _euler_rate_target_rads.x, euler_accel_target_rads.x, fabsf(euler_rate_max_rads.x), euler_accel.x, _input_tc, _dt_s); + attitude_command_model(wrap_PI(euler_pitch_angle_rad - _euler_angle_target_rad.y), 0.0, _euler_rate_target_rads.y, euler_accel_target_rads.y, fabsf(euler_rate_max_rads.y), euler_accel.y, _input_tc, _dt_s); + float yaw_rate_max_rads = euler_rate_max_rads.z; if (slew_yaw) { - _euler_rate_target_rads.z = constrain_float(_euler_rate_target_rads.z, -slew_yaw_max_rads, slew_yaw_max_rads); + yaw_rate_max_rads = MIN(yaw_rate_max_rads, slew_yaw_max_rads); } + attitude_command_model(wrap_PI(euler_yaw_angle_rad - _euler_angle_target_rad.z), 0.0, _euler_rate_target_rads.z, euler_accel_target_rads.z, fabsf(yaw_rate_max_rads), euler_accel.z, _input_tc, _dt_s); // Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward euler_rate_to_ang_vel(_attitude_target, _euler_rate_target_rads, _ang_vel_target_rads); - // Limit the angular velocity - ang_vel_limit(_ang_vel_target_rads, radians(_ang_vel_roll_max_degs), radians(_ang_vel_pitch_max_degs), radians(_ang_vel_yaw_max_degs)); - // Convert body-frame angular velocity into euler angle derivative of desired attitude - ang_vel_to_euler_rate(_attitude_target, _ang_vel_target_rads, _euler_rate_target_rads); + euler_rate_to_ang_vel(_attitude_target, euler_accel_target_rads, _ang_accel_target_rads); } else { // When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed. _euler_angle_target_rad.x = euler_roll_angle_rad; _euler_angle_target_rad.y = euler_pitch_angle_rad; if (slew_yaw) { // Compute constrained angle error - float angle_error = constrain_float(wrap_PI(euler_yaw_angle_rad - _euler_angle_target_rad.z), -slew_yaw_max_rads * _dt_s, slew_yaw_max_rads * _dt_s); - // Update attitude target from constrained angle error - _euler_angle_target_rad.z = wrap_PI(angle_error + _euler_angle_target_rad.z); + const float yaw_error = wrap_PI(euler_yaw_angle_rad - _euler_angle_target_rad.z); + const float yaw_step = constrain_float(yaw_error, -slew_yaw_max_rads * _dt_s, slew_yaw_max_rads * _dt_s); + _euler_angle_target_rad.z = wrap_PI(_euler_angle_target_rad.z + yaw_step); } else { _euler_angle_target_rad.z = euler_yaw_angle_rad; } @@ -490,6 +483,7 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw_rad(float euler_roll_a // Set rate feedforward requests to zero _euler_rate_target_rads.zero(); _ang_vel_target_rads.zero(); + _ang_accel_target_rads.zero(); } // Call quaternion attitude controller @@ -513,14 +507,15 @@ void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw_rads(float euler_roll_r // translate the roll pitch and yaw acceleration limits to the euler axis const Vector3f euler_accel = euler_accel_limit(_attitude_target, Vector3f{get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()}); - // When acceleration limiting is enabled, the input shaper constrains angular acceleration, slewing - // the output rate towards the input rate. - _euler_rate_target_rads.x = input_shaping_ang_vel(_euler_rate_target_rads.x, euler_roll_rate_rads, euler_accel.x, _dt_s, _rate_rp_tc); - _euler_rate_target_rads.y = input_shaping_ang_vel(_euler_rate_target_rads.y, euler_pitch_rate_rads, euler_accel.y, _dt_s, _rate_rp_tc); - _euler_rate_target_rads.z = input_shaping_ang_vel(_euler_rate_target_rads.z, euler_yaw_rate_rads, euler_accel.z, _dt_s, _rate_y_tc); + Vector3f euler_accel_target_rads; + ang_vel_to_euler_rate(_attitude_target, _ang_accel_target_rads, euler_accel_target_rads); + attitude_command_model(0.0, euler_roll_rate_rads, _euler_rate_target_rads.x, euler_accel_target_rads.x, 0.0, euler_accel.x, _rate_rp_tc, _dt_s); + attitude_command_model(0.0, euler_pitch_rate_rads, _euler_rate_target_rads.y, euler_accel_target_rads.y, 0.0, euler_accel.y, _rate_rp_tc, _dt_s); + attitude_command_model(0.0, euler_yaw_rate_rads, _euler_rate_target_rads.z, euler_accel_target_rads.z, 0.0, euler_accel.z, _rate_y_tc, _dt_s); // Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward euler_rate_to_ang_vel(_attitude_target, _euler_rate_target_rads, _ang_vel_target_rads); + euler_rate_to_ang_vel(_attitude_target, euler_accel_target_rads, _ang_accel_target_rads); } else { // When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed. // Pitch angle is restricted to +- 85.0 degrees to avoid gimbal lock discontinuities. @@ -531,6 +526,7 @@ void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw_rads(float euler_roll_r // Set rate feedforward requests to zero _euler_rate_target_rads.zero(); _ang_vel_target_rads.zero(); + _ang_accel_target_rads.zero(); // Compute quaternion target attitude _attitude_target.from_euler(_euler_angle_target_rad.x, _euler_angle_target_rad.y, _euler_angle_target_rad.z); @@ -566,12 +562,9 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_rads(float roll_rate_bf_ra _attitude_target.to_euler(_euler_angle_target_rad); if (_rate_bf_ff_enabled) { - // Compute acceleration-limited body frame rates - // When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing - // the output rate towards the input rate. - _ang_vel_target_rads.x = input_shaping_ang_vel(_ang_vel_target_rads.x, roll_rate_bf_rads, get_accel_roll_max_radss(), _dt_s, _rate_rp_tc); - _ang_vel_target_rads.y = input_shaping_ang_vel(_ang_vel_target_rads.y, pitch_rate_bf_rads, get_accel_pitch_max_radss(), _dt_s, _rate_rp_tc); - _ang_vel_target_rads.z = input_shaping_ang_vel(_ang_vel_target_rads.z, yaw_rate_bf_rads, get_accel_yaw_max_radss(), _dt_s, _rate_y_tc); + attitude_command_model(0.0, roll_rate_bf_rads, _ang_vel_target_rads.x, _ang_accel_target_rads.x, 0.0, get_accel_roll_max_radss(), _rate_rp_tc, _dt_s); + attitude_command_model(0.0, pitch_rate_bf_rads, _ang_vel_target_rads.y, _ang_accel_target_rads.y, 0.0, get_accel_pitch_max_radss(), _rate_rp_tc, _dt_s); + attitude_command_model(0.0, yaw_rate_bf_rads, _ang_vel_target_rads.z, _ang_accel_target_rads.z, 0.0, get_accel_yaw_max_radss(), _rate_y_tc, _dt_s); // Convert body-frame angular velocity into euler angle derivative of desired attitude ang_vel_to_euler_rate(_attitude_target, _ang_vel_target_rads, _euler_rate_target_rads); @@ -585,6 +578,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_rads(float roll_rate_bf_ra // Set rate feedforward requests to zero _euler_rate_target_rads.zero(); _ang_vel_target_rads.zero(); + _ang_accel_target_rads.zero(); } // Call quaternion attitude controller @@ -609,12 +603,9 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2_cds(float roll_rate_bf_c // and no attitude feedback or stabilization. void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2_rads(float roll_rate_bf_rads, float pitch_rate_bf_rads, float yaw_rate_bf_rads) { - // Compute acceleration-limited body frame rates - // When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing - // the output rate towards the input rate. - _ang_vel_target_rads.x = input_shaping_ang_vel(_ang_vel_target_rads.x, roll_rate_bf_rads, get_accel_roll_max_radss(), _dt_s, _rate_rp_tc); - _ang_vel_target_rads.y = input_shaping_ang_vel(_ang_vel_target_rads.y, pitch_rate_bf_rads, get_accel_pitch_max_radss(), _dt_s, _rate_rp_tc); - _ang_vel_target_rads.z = input_shaping_ang_vel(_ang_vel_target_rads.z, yaw_rate_bf_rads, get_accel_yaw_max_radss(), _dt_s, _rate_y_tc); + attitude_command_model(0.0, roll_rate_bf_rads, _ang_vel_target_rads.x, _ang_accel_target_rads.x, 0.0, get_accel_roll_max_radss(), _rate_rp_tc, _dt_s); + attitude_command_model(0.0, pitch_rate_bf_rads, _ang_vel_target_rads.y, _ang_accel_target_rads.y, 0.0, get_accel_pitch_max_radss(), _rate_rp_tc, _dt_s); + attitude_command_model(0.0, yaw_rate_bf_rads, _ang_vel_target_rads.z, _ang_accel_target_rads.z, 0.0, get_accel_yaw_max_radss(), _rate_y_tc, _dt_s); // Update the unused targets attitude based on current attitude to condition mode change _ahrs.get_quat_body_to_ned(_attitude_target); @@ -660,12 +651,9 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3_rads(float roll_rate_bf_ _attitude_ang_error = attitude_ang_error_update_quat * _attitude_ang_error; _attitude_ang_error.normalize(); - // Compute acceleration-limited body frame rates - // When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing - // the output rate towards the input rate. - _ang_vel_target_rads.x = input_shaping_ang_vel(_ang_vel_target_rads.x, roll_rate_bf_rads, get_accel_roll_max_radss(), _dt_s, _rate_rp_tc); - _ang_vel_target_rads.y = input_shaping_ang_vel(_ang_vel_target_rads.y, pitch_rate_bf_rads, get_accel_pitch_max_radss(), _dt_s, _rate_rp_tc); - _ang_vel_target_rads.z = input_shaping_ang_vel(_ang_vel_target_rads.z, yaw_rate_bf_rads, get_accel_yaw_max_radss(), _dt_s, _rate_y_tc); + attitude_command_model(0.0, roll_rate_bf_rads, _ang_vel_target_rads.x, _ang_accel_target_rads.x, 0.0, get_accel_roll_max_radss(), _rate_rp_tc, _dt_s); + attitude_command_model(0.0, pitch_rate_bf_rads, _ang_vel_target_rads.y, _ang_accel_target_rads.y, 0.0, get_accel_pitch_max_radss(), _rate_rp_tc, _dt_s); + attitude_command_model(0.0, yaw_rate_bf_rads, _ang_vel_target_rads.z, _ang_accel_target_rads.z, 0.0, get_accel_yaw_max_radss(), _rate_y_tc, _dt_s); // Retrieve quaternion body attitude Quaternion attitude_body; @@ -745,6 +733,7 @@ void AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw_rad(float roll_angle // Set rate feedforward requests to zero _euler_rate_target_rads.zero(); _ang_vel_target_rads.zero(); + _ang_accel_target_rads.zero(); // Call quaternion attitude controller attitude_controller_run_quat(); @@ -759,7 +748,7 @@ void AC_AttitudeControl::input_rate_step_bf_roll_pitch_yaw_rads(float roll_rate_ _attitude_target.to_euler(_euler_angle_target_rad); // Set the target angular velocity to be zero to minimize target overshoot after the rate step finishes _ang_vel_target_rads.zero(); - // Convert body-frame angular velocity into euler angle derivative of desired attitude + _ang_accel_target_rads.zero(); _euler_rate_target_rads.zero(); // finally update the attitude target @@ -797,23 +786,20 @@ void AC_AttitudeControl::input_thrust_vector_rate_heading_rads(const Vector3f& t if (_rate_bf_ff_enabled) { // 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. - _ang_vel_target_rads.x = input_shaping_angle(attitude_error.x, _input_tc, get_accel_roll_max_radss(), _ang_vel_target_rads.x, _dt_s); - _ang_vel_target_rads.y = input_shaping_angle(attitude_error.y, _input_tc, get_accel_pitch_max_radss(), _ang_vel_target_rads.y, _dt_s); + attitude_command_model(attitude_error.x, 0.0, _ang_vel_target_rads.x, _ang_accel_target_rads.x, radians(_ang_vel_roll_max_degs), get_accel_roll_max_radss(), _input_tc, _dt_s); + attitude_command_model(attitude_error.y, 0.0, _ang_vel_target_rads.y, _ang_accel_target_rads.y, radians(_ang_vel_pitch_max_degs), get_accel_pitch_max_radss(), _input_tc, _dt_s); - // 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. - _ang_vel_target_rads.z = input_shaping_ang_vel(_ang_vel_target_rads.z, heading_rate_rads, get_accel_yaw_max_radss(), _dt_s, _rate_y_tc); - - // Limit the angular velocity - ang_vel_limit(_ang_vel_target_rads, radians(_ang_vel_roll_max_degs), radians(_ang_vel_pitch_max_degs), radians(_ang_vel_yaw_max_degs)); + attitude_command_model(0.0, heading_rate_rads, _ang_vel_target_rads.z, _ang_accel_target_rads.z, radians(_ang_vel_yaw_max_degs), get_accel_yaw_max_radss(), _rate_y_tc, _dt_s); } else { Quaternion yaw_quat; yaw_quat.from_axis_angle(Vector3f{0.0f, 0.0f, heading_rate_rads * _dt_s}); _attitude_target = _attitude_target * thrust_vec_correction_quat * yaw_quat; + _attitude_target.normalize(); // Set rate feedforward requests to zero _euler_rate_target_rads.zero(); _ang_vel_target_rads.zero(); + _ang_accel_target_rads.zero(); } // Convert body-frame angular velocity into euler angle derivative of desired attitude @@ -849,14 +835,9 @@ void AC_AttitudeControl::input_thrust_vector_heading_rad(const Vector3f& thrust_ float returned_thrust_vector_angle; thrust_vector_rotation_angles(desired_attitude_quat, _attitude_target, thrust_vec_correction_quat, attitude_error, returned_thrust_vector_angle, thrust_vector_diff_angle); - // 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. - _ang_vel_target_rads.x = input_shaping_angle(attitude_error.x, _input_tc, get_accel_roll_max_radss(), _ang_vel_target_rads.x, _dt_s); - _ang_vel_target_rads.y = input_shaping_angle(attitude_error.y, _input_tc, get_accel_pitch_max_radss(), _ang_vel_target_rads.y, _dt_s); - _ang_vel_target_rads.z = input_shaping_angle(attitude_error.z, _input_tc, get_accel_yaw_max_radss(), _ang_vel_target_rads.z, heading_rate_rads, slew_yaw_max_rads, _dt_s); - - // Limit the angular velocity - ang_vel_limit(_ang_vel_target_rads, radians(_ang_vel_roll_max_degs), radians(_ang_vel_pitch_max_degs), slew_yaw_max_rads); + attitude_command_model(attitude_error.x, 0.0, _ang_vel_target_rads.x, _ang_accel_target_rads.x, radians(_ang_vel_roll_max_degs), get_accel_roll_max_radss(), _input_tc, _dt_s); + attitude_command_model(attitude_error.y, 0.0, _ang_vel_target_rads.y, _ang_accel_target_rads.y, radians(_ang_vel_pitch_max_degs), get_accel_pitch_max_radss(), _input_tc, _dt_s); + attitude_command_model(attitude_error.z, heading_rate_rads, _ang_vel_target_rads.z, _ang_accel_target_rads.z, radians(_ang_vel_yaw_max_degs), get_accel_yaw_max_radss(), _rate_y_tc, _dt_s); } else { // set persisted quaternion target attitude _attitude_target = desired_attitude_quat; @@ -864,6 +845,7 @@ void AC_AttitudeControl::input_thrust_vector_heading_rad(const Vector3f& thrust_ // Set rate feedforward requests to zero _euler_rate_target_rads.zero(); _ang_vel_target_rads.zero(); + _ang_accel_target_rads.zero(); } // Convert body-frame angular velocity into euler angle derivative of desired attitude @@ -1050,44 +1032,38 @@ void AC_AttitudeControl::thrust_vector_rotation_angles(const Quaternion& attitud // calculates the velocity correction from an angle error. The angular velocity has acceleration and // 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 desired_ang_vel, float max_ang_vel, float dt) +void AC_AttitudeControl::attitude_command_model(float error_angle, float desired_ang_vel, float& target_ang_vel, float& target_ang_accel, float max_ang_vel, float accel_max, float input_tc, float dt) const { - // Calculate the velocity as error approaches zero with acceleration limited by accel_max_radss - desired_ang_vel += sqrt_controller(error_angle, 1.0f / MAX(input_tc, 0.01f), accel_max, dt); - if (is_positive(max_ang_vel)) { - desired_ang_vel = constrain_float(desired_ang_vel, -max_ang_vel, max_ang_vel); + if (!is_positive(dt)) { + return; + } + + // protect against divide by zero + if (!is_positive(accel_max)) { + // no acceleration set so default to 1800 degrees/s² + accel_max = radians(1800); } - // 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, 0.0f); -} + if (!is_positive(input_tc)) { + // no acceleration set so default to achieve maximum acceleration in 10 clock cycles + input_tc = dt * 10.0; + } -// Shapes the velocity request based on a rate time constant. The angular acceleration and deceleration is limited. -float AC_AttitudeControl::input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max, float dt, float input_tc) -{ - if (is_positive(input_tc)) { - // Calculate the acceleration to smoothly achieve rate. Jerk is not limited. - float error_rate = desired_ang_vel - target_ang_vel; - float desired_ang_accel = sqrt_controller(error_rate, 1.0f / MAX(input_tc, 0.01f), 0.0f, dt); - desired_ang_vel = target_ang_vel + desired_ang_accel * dt; - } - // Acceleration is limited directly to smooth the beginning of the curve. - if (is_positive(accel_max)) { - float delta_ang_vel = accel_max * dt; - return constrain_float(desired_ang_vel, target_ang_vel - delta_ang_vel, target_ang_vel + delta_ang_vel); - } else { - return desired_ang_vel; - } + shape_angle_vel_accel( error_angle, desired_ang_vel, 0.0, + 0.0, target_ang_vel, target_ang_accel, + max_ang_vel, accel_max, + accel_max / input_tc, dt, false); + target_ang_vel += target_ang_accel * dt; } // calculates the expected angular velocity correction from an angle error based on the AC_AttitudeControl settings. // This function can be used to predict the delay associated with angle requests. -void AC_AttitudeControl::input_shaping_rate_predictor(const Vector2f &error_angle_rad, Vector2f& target_ang_vel_rads, float dt) const +void AC_AttitudeControl::command_model_rate_predictor(const Vector2f &error_angle_rad, Vector2f& target_ang_vel_rads, Vector2f& target_ang_accel_rads, float dt) const { if (_rate_bf_ff_enabled) { // translate the roll pitch and yaw acceleration limits to the euler axis - target_ang_vel_rads.x = input_shaping_angle(wrap_PI(error_angle_rad.x), _input_tc, get_accel_roll_max_radss(), target_ang_vel_rads.x, dt); - target_ang_vel_rads.y = input_shaping_angle(wrap_PI(error_angle_rad.y), _input_tc, get_accel_pitch_max_radss(), target_ang_vel_rads.y, dt); + attitude_command_model(wrap_PI(error_angle_rad.x), 0.0, target_ang_vel_rads.x, target_ang_accel_rads.x, radians(_ang_vel_roll_max_degs), get_accel_roll_max_radss(), _input_tc, _dt_s); + attitude_command_model(wrap_PI(error_angle_rad.y), 0.0, target_ang_vel_rads.y, target_ang_accel_rads.y, radians(_ang_vel_pitch_max_degs), get_accel_pitch_max_radss(), _input_tc, _dt_s); } else { const float angleP_roll = _p_angle_roll.kP() * _angle_P_scale.x; const float angleP_pitch = _p_angle_pitch.kP() * _angle_P_scale.y; @@ -1194,6 +1170,7 @@ void AC_AttitudeControl::reset_target_and_rate(bool reset_rate) if (reset_rate) { _ang_vel_target_rads.zero(); + _ang_accel_target_rads.zero(); _euler_rate_target_rads.zero(); } } @@ -1211,6 +1188,7 @@ void AC_AttitudeControl::reset_yaw_target_and_rate(bool reset_rate) if (reset_rate) { // set yaw rate to zero _euler_rate_target_rads.z = 0.0f; + _ang_accel_target_rads.z = 0.0; // Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward euler_rate_to_ang_vel(_attitude_target, _euler_rate_target_rads, _ang_vel_target_rads); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 88117d2be4..03914e61cc 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -414,15 +414,11 @@ public: float lean_angle_rad() const { return _thrust_angle_rad; } // Calculates the velocity correction from an angle error, applying acceleration/deceleration limits and a simple jerk-limiting mechanism via the smoothing gain. - static float input_shaping_angle(float error_angle, float input_tc, float accel_max, float target_ang_vel, float desired_ang_vel, float max_ang_vel, float dt); - static float input_shaping_angle(float error_angle, float input_tc, float accel_max, float target_ang_vel, float dt){ return input_shaping_angle(error_angle, input_tc, accel_max, target_ang_vel, 0.0f, 0.0f, dt); } - - // Shapes the velocity request based on a rate time constant. The angular acceleration and deceleration is limited. - static float input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max, float dt, float input_tc); + void attitude_command_model(float error_angle, float desired_ang_vel, float& target_ang_vel, float& target_ang_accel, float max_ang_vel, float accel_max, float input_tc, float dt) const; // calculates the expected angular velocity correction from an angle error based on the AC_AttitudeControl settings. // This function can be used to predict the delay associated with angle requests. - void input_shaping_rate_predictor(const Vector2f &error_angle_rad, Vector2f& target_ang_vel_rads, float dt) const; + void command_model_rate_predictor(const Vector2f &error_angle_rad, Vector2f& target_ang_vel_rads, Vector2f& target_ang_accel_rads, float dt) const; // translates body frame acceleration limits to the euler axis void ang_vel_limit(Vector3f& euler_rad, float ang_vel_roll_max_rads, float ang_vel_pitch_max_rads, float ang_vel_yaw_max_rads) const; @@ -618,6 +614,7 @@ protected: // the attitude controller as an angular velocity vector, in radians per second in // the target attitude frame. Vector3f _ang_vel_target_rads; + Vector3f _ang_accel_target_rads; // This represents the angular velocity in radians per second in the body frame, used in the angular // velocity controller and most importantly the rate controller. diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_TS.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_TS.cpp index 159a1db8b1..00f4ee1e60 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_TS.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_TS.cpp @@ -124,6 +124,7 @@ void AC_AttitudeControl_TS::input_euler_rate_yaw_euler_angle_pitch_bf_roll_rad(b // Set rate feedforward requests to zero _euler_rate_target_rads.zero(); + _ang_accel_target_rads.zero(); _ang_vel_target_rads.zero(); // Compute attitude error