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?