Basically, you need a table of some kind for each camera axis
imu2 roll
imu2 pitch
|
|
V
table
|
|
V
motor 1 PID gains
motor 2 PID gains
motor 3 PID gains
The outputs of the 3 sets of PID equations for each motor are summed to get the motor steps. The hard part is computing the table. All 3 motors have a unique set of gains in the upright position. They have completely different gains in the 2 sideways positions.
There are 2 mane gradients for the PID gains: The roll & yaw motors trade places as imu2 pitch goes from 0 to 90. The yaw motor is replaced by the pitch motor as imu2 roll goes from 0 to 90.
The 2 mane gradients aren't linear. They're a sine wave. When IMU2 is pitched 20 deg over, it adds just 12% of the roll. When it's at 45 deg pitch, it adds 50% of the roll. When it's at 70 deg pitch, it adds 88% of the pitch.
If each motor has 3 sets of gains: the upright yaw, 90 pitched yaw, & 90 rolled yaw, each set of gains has 3 parameters: P1, P2, D2, a total of 27 values need to be manually tuned. The best way to tune it is to tune the fully deflected states.
Only 1 guy outside a corporation ever got the yaw coupling to work:
But it was still unstable when the pitch & yaw motors were parallel. Having 2 motors share the same axis is a buster. You want yaw to be stabilized as much as possible, until the yaw motor is sideways. It requires gradually taking away more & more latitude from 1 of the motors until it's rigid. The easiest solution is to always have the pitch motor control 100% of pitch, with the yaw motor tapering its gains in the yaw direction as it goes horizontal. This doesn't maximize all the available degrees of freedom of the motors.
Without the brute force kinematic search, the table has the motors fighting each other. Doing the brute force search fast enough would be real hard. No-one has tested a Zenmuse to these extremes, but it probably does it right.