ardupilot Refactor attitude control to be thread safe

Broken out from https://github.com/ArduPilot/ardupilot/pull/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.

Aug 14 '24 20:08 andyp1per

Does this just assume that a Vector3f set is atomic?

Aug 15 '24 01:08 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.

Aug 15 '24 07:08 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

Aug 15 '24 11:08 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.

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

Aug 15 '24 18:08 tpwrules

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.

Aug 15 '24 21:08 andyp1per

Thanks for your comments @IamPete1 ! They provoked a good debate / change

Sep 21 '24 14:09 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.

Sep 23 '24 11:09 andyp1per

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

Sep 23 '24 12:09 andyp1per

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

Sep 23 '24 23:09 tridge

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.

Sep 23 '24 23:09 rmackay9

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

Sep 24 '24 02:09 tridge

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 - https://github.com/ArduPilot/ardupilot/pull/28220

Sep 24 '24 12:09 andyp1per

couldn't find any issues running this on Sub