Hello everybody,
When trying to understand arducopter firmware, I’ve found something not logical in attitude control.
In AttitudeControl.cpp there are more ways to input targeted attitude, and these input functions, in feedforward case, call input_shaping_angle() or input_shaping_ang_vel(). If input_shaping_angle() is called, then desired_ang_vel is defined
float desired_ang_vel = sqrt_controller(error_angle, 1.0f / MAX(input_tc, 0.01f), accel_max, dt);
Looking at sqrt_controller(), we can find that desired_ang_vel is constrained around zero
return constrain_float(correction_rate, -fabsf(error)/dt, fabsf(error)/dt);
From the name correction_rate and the formula based on angle error, we may conclude that this is only the velocity correction. But then this value is used in calling input_shaping_ang_vel() as absolute velocity
return input_shaping_ang_vel(target_ang_vel, desired_ang_vel, accel_max, dt);
where it is again constrained around target angular velocity which is not zero at all.
return constrain_float(desired_ang_vel, target_ang_vel-delta_ang_vel, target_ang_vel+delta_ang_vel);
We can see that if target_ang_vel is large enough then desired_ang_vel falls outside the constrained range, and the lower border is always returned. I think it is not logical because first it tries to constrain in zero range, then next tries to constrain in other range that is quite far away and the output can be known in advance.
Does anyone understand this issue and can explain me, please. Thank you in advance.