|
|
|
@ -480,8 +480,12 @@ float AC_AttitudeControl::input_shaping_angle(float error_angle, float smoothing
@@ -480,8 +480,12 @@ float AC_AttitudeControl::input_shaping_angle(float error_angle, float smoothing
|
|
|
|
|
float ang_vel = sqrt_controller(error_angle, smoothing_gain, accel_max); |
|
|
|
|
|
|
|
|
|
// Acceleration is limited directly to smooth the beginning of the curve.
|
|
|
|
|
float delta_ang_vel = accel_max * _dt; |
|
|
|
|
return constrain_float(ang_vel, target_ang_vel-delta_ang_vel, target_ang_vel+delta_ang_vel); |
|
|
|
|
if (accel_max > 0) { |
|
|
|
|
float delta_ang_vel = accel_max * _dt; |
|
|
|
|
return constrain_float(ang_vel, target_ang_vel-delta_ang_vel, target_ang_vel+delta_ang_vel); |
|
|
|
|
} else { |
|
|
|
|
return ang_vel; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// limits the acceleration and deceleration of a velocity request
|
|
|
|
|