gimbal orientation broken in case of pitch == -90 and yaw != 0

[svpcom]

Describe the bug

QGC --[gimbal v1 proto]--> PX4 --[gimbal v1 proto]--> gimbal It seems there is a bug with euler angles code which leads to crazy gimbal orientation in case of pitch == -90 and yaw != 0 I've tested that QGC send valid angles (via VEHICLE_CMD_DO_MOUNT_CONTROL).

To Reproduce

Steps to reproduce the behavior in simulator:

1. make px4_sitl gazebo_typhoon_h480
2. in QGC vehicle settings set gimbal input as v1 protocol
3. In QGC vehicle settings set gimbal output as v1 protocol
4. gimbal test pitch -90 yaw -110

Expected behavior

Gimbal oriented with pitch -90 and yaw -110 But got: pitch -90, yaw 0 (after adding debug print to mavlink output)

Drone (please complete the following information):

px4 fmu-v4, release 1.13

[svpcom]

The following quaternion 0.405580 -0.579228 -0.405580 -0.579228 translates to nan 0.000000 0.000000 euler angles instead of -90 0 -110 in void OutputBase::_calculate_angle_output(const hrt_abstime &t)

[svpcom]

It seems that problem is that px4 use ROLL - PITCH - YAW for gimbal instead of PITCH - ROLL - YAW euler rotations. Any ideas how to fix it?

[svpcom]

For gimbal unlimited pitch is more practical that unlimited roll because vehicle flight mostly horizontally and roll angle < 90 degrees

[julianoes]

My snarky comment would be to move to the gimbal v2 protocol which was uses quaternions as much as possible to avoid these problems.

[julianoes]

What's odd is that yaw does not seem to be taken into account at pitch -90 for gimbal v2.

[julianoes]

It looks like that's what the mavlink conversion function produces. At pitch -90 deg yaw is left at 0, at pitch -85 deg, everything is correct.

[svpcom]

But because there are no opensource gimbals, transition to gimbal protocol v2 is not possible. For example STorm32 supports only gimbal v1 protocol.

[svpcom]

@julianoes Is it possible to change euler rotation sequence in px4 gimbal module?

[svpcom]

@julianoes

It looks like that's what the mavlink conversion function produces. At pitch -90 deg yaw is left at 0, at pitch -85 deg, everything is correct.

with roll-pitch-yaw rotation sequence with pitch == 90 we got aligned roll and yaw axis and gimbal lock (see page 47 in https://www.basecamelectronics.com/files/v3/SimpleBGC_32bit_manual_eng.pdf). So It seems then only way is to change rotation order, but it seems to be hardcoded in src/lib/matrix/matrix/Euler.hpp.

[svpcom]

@julianoes please review pull request

[igorsgcampos]

I am also experiencing issues when commanding pitch -90, I haven't tested this PR, but intend to to is in near future. When the pitch is -90, the roll actuation seems to be directed to the yaw, please check the gimbal_controls topic of the following topic to see this. https://review.px4.io/plot_app?log=cc00ed13-c0f3-4c3e-8a51-f430f0451b42