AC_AttitudeControl: Fix dt update order

[lthall]

This PR moves the movement of target position to the first step of the attitude controller where it knows the dt.

Currently we assume the next dt will be the same as the last dt. This causes noise in the rate controllers when dt changes and high rates are being requested.

Limitations in ATT logging has made this difficult to observe previously.

Before:

After:

[IamPete1]

Just to make sure I have understood this correctly:

Currently we do the integration of _attitude_target at the end of the loop. This meant we used the angular velocity's from this loop. However the _dt is from the last loop at that point.

By moving the integration to the start of the loop were also using the angular velocity's from the last loop. The result is that the angular velocity's and the _dt are both for the same period of time.

[IamPete1]

There is a change in behavior in the input_angle_step_bf_roll_pitch_yaw case. It does not get the new update_attitude_target call but the old method in attitude_controller_run_quat would have run, Its only used in multi rotor autotune and as far as I can tell multi rotor autotune does not disable _rate_bf_ff_enabled.

[IamPete1]

I suspect this makes the same problem worse on plane. The problem is that copter sets all the dt's in the run_rate_controller call which is the first thing that happens in the loop. On plane we also update the dt's in the rate_controller_run call. However we do that at the end of the loop. So when the attitude controller uses dt it will be the loop before last not the last loop.

I think the fix for plane is to move the setting of the dt's away from the run_rate_controller call and do them at the start of the loop the same as copter. I think we could probably do that without too much testing, however moving the actual run_rate_controller so we completely match copter would be a big job.

Anyway, I don't think we need to worry too much about plane in this PR.

[lthall]

There is a change in behavior in the input_angle_step_bf_roll_pitch_yaw case. It does not get the new update_attitude_target call but the old method in attitude_controller_run_quat would have run, Its only used in multi rotor autotune and as far as I can tell multi rotor autotune does not disable _rate_bf_ff_enabled.

This function sets:

    // Set rate feedforward requests to zero
    _euler_rate_target.zero();
    _ang_vel_target.zero();

So the attitude target does not change.

Anyway, I don't think we need to worry too much about plane in this PR.

I agree. These timing issues become less and less important as aircraft get bigger and the filter frequencies get lower. It would be good for plane to optimise the loop structure but I doubt any of these changes will impact the performance of plane.

[IamPete1]

I think the changes look good. The only question is that this now updates the attitude target in every case not just if _rate_bf_ff_enabled is true?

[lthall]

I think the changes look good. The only question is that this now updates the attitude target in every case not just if _rate_bf_ff_enabled is true?

Yes that is correct. When _rate_bf_ff_enabled is false we are effectively only normalising the attitude target. The main reason for this is I realised I was calculating the euler targets before the update of the attitude target.

[Ryanf55]

Line 501 can be improved by factoring dt.
attitude_target_update.from_axis_angle(Vector3f{roll_rate_rads * _dt, pitch_rate_rads * _dt, yaw_rate_rads * _dt});

[andyp1per]

Tested on my 5" - looks good.