diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp index 98e2dfd2d97db..44f283bdb7879 100644 --- a/ArduCopter/autoyaw.cpp +++ b/ArduCopter/autoyaw.cpp @@ -124,9 +124,9 @@ void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_ds, int8_t di // get turn speed if (!is_positive(turn_rate_ds)) { // default to default slew rate - _fixed_yaw_slewrate_cds = copter.attitude_control->get_slew_yaw_cds(); + _fixed_yaw_slewrate_cds = copter.attitude_control->get_slew_yaw_max_degs() * 100.0; } else { - _fixed_yaw_slewrate_cds = MIN(copter.attitude_control->get_slew_yaw_cds(), turn_rate_ds * 100.0); + _fixed_yaw_slewrate_cds = MIN(copter.attitude_control->get_slew_yaw_max_degs(), turn_rate_ds) * 100.0; } // set yaw mode diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index e706ca1350c88..0350e4b1e98f7 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -147,6 +147,15 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = { AP_GROUPEND }; +// get the slew yaw rate limit in deg/s +float AC_AttitudeControl::get_slew_yaw_max_degs() const +{ + if (!is_positive(_ang_vel_yaw_max)) { + return _slew_yaw * 0.01; + } + return MIN(_ang_vel_yaw_max, _slew_yaw * 0.01); +} + // Ensure attitude controller have zero errors to relax rate controller output void AC_AttitudeControl::relax_attitude_controllers() { @@ -313,6 +322,7 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle // Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors) euler_roll_angle += get_roll_trim_rad(); + 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 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()}); @@ -324,7 +334,7 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle _euler_rate_target.y = input_shaping_angle(wrap_PI(euler_pitch_angle - _euler_angle_target.y), _input_tc, euler_accel.y, _euler_rate_target.y, _dt); _euler_rate_target.z = input_shaping_angle(wrap_PI(euler_yaw_angle - _euler_angle_target.z), _input_tc, euler_accel.z, _euler_rate_target.z, _dt); if (slew_yaw) { - _euler_rate_target.z = constrain_float(_euler_rate_target.z, -get_slew_yaw_rads(), get_slew_yaw_rads()); + _euler_rate_target.z = constrain_float(_euler_rate_target.z, -slew_yaw_max_rads, slew_yaw_max_rads); } // Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward @@ -339,7 +349,7 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle _euler_angle_target.y = euler_pitch_angle; if (slew_yaw) { // 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); + float angle_error = constrain_float(wrap_PI(euler_yaw_angle - _euler_angle_target.z), -slew_yaw_max_rads * _dt, slew_yaw_max_rads * _dt); // Update attitude target from constrained angle error _euler_angle_target.z = wrap_PI(angle_error + _euler_angle_target.z); } else { @@ -568,7 +578,7 @@ void AC_AttitudeControl::input_thrust_vector_rate_heading(const Vector3f& thrust _ang_vel_target.z = input_shaping_ang_vel(_ang_vel_target.z, heading_rate, get_accel_yaw_max_radss(), _dt); // Limit the angular velocity - ang_vel_limit(_ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), MIN(radians(_ang_vel_yaw_max), get_slew_yaw_rads())); + ang_vel_limit(_ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), get_slew_yaw_max_rads()); } else { Quaternion yaw_quat; yaw_quat.from_axis_angle(Vector3f{0.0f, 0.0f, heading_rate * _dt}); @@ -590,10 +600,10 @@ void AC_AttitudeControl::input_thrust_vector_rate_heading(const Vector3f& thrust void AC_AttitudeControl::input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds) { // a zero _angle_vel_yaw_max means that setting is disabled - const float ang_vel_yaw_max_rads = is_zero(_ang_vel_yaw_max) ? get_slew_yaw_rads() : MIN(radians(_ang_vel_yaw_max), get_slew_yaw_rads()); + const float slew_yaw_max_rads = get_slew_yaw_max_rads(); // Convert from centidegrees on public interface to radians - float heading_rate = constrain_float(radians(heading_rate_cds * 0.01f), -ang_vel_yaw_max_rads, ang_vel_yaw_max_rads); + float heading_rate = constrain_float(radians(heading_rate_cds * 0.01f), -slew_yaw_max_rads, slew_yaw_max_rads); float heading_angle = radians(heading_angle_cd * 0.01f); // calculate the attitude target euler angles @@ -614,10 +624,10 @@ void AC_AttitudeControl::input_thrust_vector_heading(const Vector3f& thrust_vect // the output rate towards the input rate. _ang_vel_target.x = input_shaping_angle(attitude_error.x, _input_tc, get_accel_roll_max_radss(), _ang_vel_target.x, _dt); _ang_vel_target.y = input_shaping_angle(attitude_error.y, _input_tc, get_accel_pitch_max_radss(), _ang_vel_target.y, _dt); - _ang_vel_target.z = input_shaping_angle(attitude_error.z, _input_tc, get_accel_yaw_max_radss(), _ang_vel_target.z, heading_rate, get_slew_yaw_rads(), _dt); + _ang_vel_target.z = input_shaping_angle(attitude_error.z, _input_tc, get_accel_yaw_max_radss(), _ang_vel_target.z, heading_rate, slew_yaw_max_rads, _dt); // Limit the angular velocity - ang_vel_limit(_ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), ang_vel_yaw_max_rads); + ang_vel_limit(_ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), slew_yaw_max_rads); } else { // set persisted quaternion target attitude _attitude_target = desired_attitude_quat; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 3e49df57d4804..eb33116dfc9d0 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -116,9 +116,9 @@ class AC_AttitudeControl { // get the yaw angular velocity limit in radians/s float get_ang_vel_yaw_max_rads() const { return radians(_ang_vel_yaw_max); } - - // get the yaw slew limit - float get_slew_yaw_cds() const { return _slew_yaw; } + + // get the slew yaw rate limit in deg/s + float get_slew_yaw_max_degs() const; // get the rate control input smoothing time constant float get_input_tc() const { return _input_tc; } @@ -373,7 +373,7 @@ class AC_AttitudeControl { virtual float get_roll_trim_rad() { return 0;} // Return the yaw slew rate limit in radians/s - float get_slew_yaw_rads() { return radians(_slew_yaw * 0.01f); } + float get_slew_yaw_max_rads() const { return radians(get_slew_yaw_max_degs()); } // Maximum rate the yaw target can be updated in Loiter, RTL, Auto flight modes AP_Float _slew_yaw;