control_allocator: Added linearization feature for 4 servo swash plates to prevent binding

[TedObrien]

Solved Problem

When using a 4 servo swash plate, non-linearity due to the rotation of the servo arm can cause servo misalignment and binding, particularly at high collective + cyclic inputs. This results in higher loads on servos.

Solution

Add 2 parameters, CA_LIN_SERVO and CA_MAX_SVO_THROW, to account for the nonlinear output due to servo-arm rotation.

CA_LIN_SERVO: Enables the feature.
CA_MAX_SVO_THROW: Allows you to define the maximum throw of the servos in degrees. Set to 50 degrees by default.

The feature works as follows:

• Calculate the max linear movement of a servo arm of unit length based on the max servo throw. This corresponds to a PWM value of 1000/2000 (max/min control input).
• Calculate the servo arm angle required to achieve desired linear output with arcsin(max_servo_height * input) (input is requested from the controller).
• The arm angle is normalized by dividing it by CA_MAX_SVO_THROW (in radians)

This feature was implemented in Ardupilot a while ago. I have used the same logic and added a parameter to adjust the max servo throw. The following links provide further information.

• https://discuss.ardupilot.org/t/linearizing-servo-mechanical-output/28074/8
• https://discuss.ardupilot.org/t/four-servo-swashplate-support/37936/13
• https://ardupilot.org/copter/docs/traditional-helicopter-swashplate-setup.html#leveling-swashplate-using-linearize-servo-feature

This requires a specific setup to work correctly. It assumes that the servo is centered on its physical throw and the swash plate in its mid position will give hover collective. The above links provide more detail about this.

Test coverage

• Tested on a skynode (v5x) with a 4-servo swash plate

• Disconnected one of the 4 linkages from the swash plate.

• Commanded max collective/cyclic with CA_LIN_SERVO disabled and observed poor alignment of disconnected linkage and swash plate.
  

• Commanded max collective/cyclic with CA_LIN_SERVO enabled and observed improvement in alignment between disconnected linkage and swash plate.
  

[bresch]

Nice! I just added a couple of comments

[TedObrien]

Thanks very much for the comments @bresch. I'll make those changes and let you know once I've built and tested it again.

[TedObrien]

@bresch in the requested changes, there was a semi-colon missing and max_servo_throw was being referenced in the same line it was being declared . I've modified this and it builds fine now. I have also tested it on my test setup (skynode v5x) and it behaves as expected.

[mrpollo]

Hey @TedObrien thanks for the PR! One of the tests caught what appears to be an error here https://github.com/PX4/PX4-Autopilot/actions/runs/12006914552/job/33466430278?pr=23961#step:6:9100

[TedObrien]

Hi @mrpollo, sorry about that. I'm currently stumped as to why it's failing at that line, and only on Ubuntu 24.04, but have recreated the error. I'll try to work out why and request a review once it's sorted. Cheers!

[mrpollo]

Hi @mrpollo, sorry about that. I'm currently stumped as to why it's failing at that line, and only on Ubuntu 24.04, but have recreated the error. I'll try to work out why and request a review once it's sorted. Cheers!

It's due to the newer GCC / CMake combo with updated warnings. We are moving to enforce those very strictly.

[bresch]

@TedObrien Instead of having a parameter to enable the feature and one to specify the throw, could we just default the throw to -1 or 0 and only enable the feature if the throw is >0?

@MaEtUgR Do you have other comments on this?

[TedObrien]

Hi @bresch, sorry for the delay in getting back to you. That makes sense to me. I'll try and be explicit in the parameter description that setting the throw > 0 enables the linearization feature also. I could see someone thinking it needs to be set as part of a standard swash-plate set up otherwise. The two new parameters can currently be changed within the actuators tab. Do you think I should remove it, so the new throw parameter can only be accessed via the parameter list to avoid potential confusion?

[MaEtUgR]

Thank you @TedObrien for the contribution! Sorry that it took so long but I finally went through in detail.
I rebased your commits on the latest main branch solving some conflicts with the rpm control parameter descriptions. I added a commit with some minor wording and ordering suggestions. I didn't have the access rights to push to your remote even though that usually works when you open a public pull request if you don't manually inhibit. So I pushed that branch to the PX4 repository here: https://github.com/PX4/PX4-Autopilot/compare/pr-linearize_swashplate_servos

I suggest you take it over and/or adjust to your liking while doing so such that we keep the same pull request.
I have one last question before we can finally merge:

[MaEtUgR]

Thanks for following up

AFAIK Mathieu was waiting for me to have a look as well.