Refactor attitude control to be thread safe

[andyp1per]

Broken out from #27029

The essence of these changes is to make sure that the interaction between the attitude controller and the rate controller are thread safe. This involves avoiding setting object state as intermediate values but instead publishing at the end of a calculation. It also means that changes should be accepted as vectors rather than discrete axes. Experiments were also done using locking but this proved to be moderately expensive at high update rates and did not improve the overall effect of this change.

[tpwrules]

Does this just assume that a Vector3f set is atomic?

[andyp1per]

Does this just assume that a Vector3f set is atomic?

It assumes that it doesn't matter that its not. The individual axes are and because all of the output is sequential, even though you might get some object tearing in practice it doesn't affect things. What does matter is setting things to 0 or other values in intermediate steps - that does make a big difference.

[IamPete1]

Plane does not call these functions in the same order as copter in a loop. I suspect plane would now be using the gyro from one loop behind?

It also makes me slightly nervous that this is all a bit subtle so we could easily break it in the future.

I wonder if a little structure "rate controller inputs" or something like that would make it a bit clearer. You could also have a sempahore on changing those values, it may be a bit of performance cost but it does at least act as a signal that the developer needs to be careful.

[andyp1per]

I wonder if a little structure "rate controller inputs" or something like that would make it a bit clearer. You could also have a sempahore on changing those values, it may be a bit of performance cost but it does at least act as a signal that the developer needs to be careful.

Perhaps simply using a naming prefix would be sufficient here - I have always found it confusing that we have both attitude control and rate control mixed up in the same class

[tpwrules]

Does this just assume that a Vector3f set is atomic?

It assumes that it doesn't matter that its not. The individual axes are and because all of the output is sequential, even though you might get some object tearing in practice it doesn't affect things. What does matter is setting things to 0 or other values in intermediate steps - that does make a big difference.

I'm concerned about this assumption on non-microcontrollers, the tear could be pretty big. In any case, wouldn't it break replay?

[andyp1per]

I'm concerned about this assumption on non-microcontrollers, the tear could be pretty big. In any case, wouldn't it break replay?

Tearing will only happen when the rate loop is in a separate thread. With replay all bets are off with a separate thread - I don't see how this can be made to work well regardless of the locking strategy.

[lthall]

This is looking good. A few PR's to get in first will simplify it further.

[IamPete1]

I think this all looks good for copter and plane. Just needs some some fixups for sub and heli.

Sub needs the new rate_controller_target_reset call after its rate_controller_run.

The sub classes AC_AttitudeControl_Heli, AC_AttitudeControl_Multi_6DoF and AC_AttitudeControl_Sub all override rate_controller_run I think they need to be changed to override the new rate_controller_run_dt.

The heli method passthrough_bf_roll_pitch_rate_yaw is quite tangled, it has not been made safe, I think that is OK, we just need to add a big comment so we can do it at some future point. Likewise for the functions in AC_AttitudeControl_TS

[andyp1per]

I think this all looks good for copter and plane. Just needs some some fixups for sub and heli.

Sub needs the new rate_controller_target_reset call after its rate_controller_run.

The sub classes AC_AttitudeControl_Heli, AC_AttitudeControl_Multi_6DoF and AC_AttitudeControl_Sub all override rate_controller_run I think they need to be changed to override the new rate_controller_run_dt.

The heli method passthrough_bf_roll_pitch_rate_yaw is quite tangled, it has not been made safe, I think that is OK, we just need to add a big comment so we can do it at some future point. Likewise for the functions in AC_AttitudeControl_TS

I believe that all the subclasses that override rate_controller_run() are correct. None of them support sysid or pd_scale from the base class (which is what rate_controller_target_reset does) and so can be left as-is for now.

I agree that Heli could be refactored to use that function, but was planning to leave it alone for now since @bnsgeyer doesn't want fast rate support in Heli for now.

rate_controller_run_dt is currently the equivalent of what was rate_controller_run() which was only used by copter and plane and only exists in AC_AttitudeControl_Multi so cannot be overridden.

So I agree but that some further refactoring could be done but propose that we leave it as-is in this PR to avoid changing the behaviour of these other vehicles.

[andyp1per]

@IamPete1 I have added a config error to the default implementation of rate_controller_run_dt() to avoid it getting called accidentally

[tridge]

needs testing on quadplane, either real aircraft or realflight
especially need to check the scale factor applied to rate control in transition

[rmackay9]

This looks OK to me

I agree that we should be sure to test Heli and Sub to be sure they're unaffected and of course, we should test with the mindset that there is a problem and we just haven't found it yet rather than just to prove that it works as expected.

[tridge]

I've tested quadplane in RealFlight and it does look good to me

[andyp1per]

We have done manual testing of the gain backoff on landing and takeoff successfully and I have also written an autotest to verify which passes - #28220

[Williangalvani]

couldn't find any issues running this on Sub

[peterbarker]

Just waiting on @bnsgeyer to be happy with this and then we can merge it!

[MattKear]

I will have some time tomorrow to test this on heli in Real Flight, if @bnsgeyer doesn't have chance before then.

[andyp1per]

Thanks @MattKear !

[bnsgeyer]

@andyp1per @rmackay9 I tested heli in real flight. It looks fine. No issues.

[andyp1per]

Thanks all!