PX4-Autopilot
PX4-Autopilot copied to clipboard
PX4
Add Euler 3-1-2 rotation for gimbal v1 output #19933
Describe problem solved by this pull request
PX4 use ROLL - PITCH - YAW Euler rotation sequence for gimbal v1 output, so there is a singularity when gimbal towards to ground (pitch -90 degrees). With PITCH - ROLL - YAW Euler rotations PX4 will issue correct gimbal angles.
Describe your solution
I've implemented quaternion to Euler angles conversion using 3-1-2 sequence instead of standard 3-2-1.
Test data / coverage
Set gimbal output to V1 and run command: gimbal test pitch -90 yaw -110. Without patch px4 will issue mavlink packets with all zero angles. With patch it will issue right angles: -90 0 -110
Additional context
Fix bug #19933
Does this mean the euler angle sequence are now different for the vehicle and the gimbal?
Does this mean the euler angle sequence are now different for the vehicle and the gimbal?
yes
Does this mean the euler angle sequence are now different for the vehicle and the gimbal?
yes
Wouldn't this be a problem? This just moves the singularity from a downward facing point to a different attitude and we still need to be able to deal with singularities @julianoes
yes, it moves singularity to roll +- 90 degrees. But for gimbal nonzero roll is not used.
Wouldn't this be a problem? This just moves the singularity from a downward facing point to a different attitude and we still need to be able to deal with singularities @julianoes
@Jaeyoung-Lim so without this fix and the one for SITL Gazebo (https://github.com/PX4/PX4-SITL_gazebo/pull/885), you can't change yaw when at pitch -90. I find this very puzzling and I don't really know what to do about it, except merge these two PRs. If you have a better fix, I'm all ears.
yes, it moves singularity to roll +- 90 degrees. But for gimbal nonzero roll is not used.
I agree that it is less common, but if your gimbal is a 2 axis configuration(more common for fixedwing/vtol vehicles) your gimbal will have a nonzero roll and might potentially get close to singularity.
you can't change yaw when at pitch -90.
@julianoes The problem I see is that we are now using two different conventions of euler angles that are different for the vehicle and the gimbal, and there would be no way for the ground station to know which one would be using which (please correct me if this is not true). This IMHO would lead to even more confusing behaviors.
I would prefer if we could have a better strategy of handling the singularities from the command standpoint rather than working around the problem, which would run into the same problem again in the future.
3-1-2 is used only for output in gimbal v1 protocol, because most of gimbals doesn't support v2 (with quaternion inside). The problem is that px4 gimbal service got input in 321, converts to quaternion and converts back to 321. With pitch +-90 (which is normal to gimbal) this leads to singularity. So if we become output angles in 3-1-2 and gimbal expect these euler angle sequence (or event if it doesn't expect but we use zero roll) there will be no any problems for QGC.
The main problems that there are no opensource brushless gimbal firmware now. For example I use Storm32 which is closed source (but cheap) and author don't want to add gimbal v2 protocol support. AlexMos (SimpleBGC) is also closed-source (and very expensive) and doesn't support gimbal v2 too.
With 3-1-2 singularity can be in case of +-90 roll which is unusual for gimbals (most of gimbal usage scenarios are with zero roll because camera horizon should be horizontal or camera looks downside for mapping)
@julianoes The problem I see is that we are now using two different conventions of euler angles that are different for the vehicle and the gimbal, and there would be no way for the ground station to know which one would be using which (please correct me if this is not true). This IMHO would lead to even more confusing behaviors.
I would prefer if we could have a better strategy of handling the singularities from the command standpoint rather than working around the problem, which would run into the same problem again in the future.
In QGC by default there are only pitch and yaw controls for gimbal (gimbal up, down, left, right). In case of two-axis gimbal you can usually only control pitch. Roll used internally by gimbal to stabilize horizon level.
yes, it moves singularity to roll +- 90 degrees. But for gimbal nonzero roll is not used.
I agree that it is less common, but if your gimbal is a 2 axis configuration(more common for fixedwing/vtol vehicles) your gimbal will have a nonzero roll and might potentially get close to singularity.
In case of two-axis gimbal you can usually only control pitch. Roll used internally by gimbal to stabilize horizon level.
I would disagree - a lot of two axis gimbals used in fixedwing/VTOLs only have pitch and yaw stabilization, and roll is not stabilized. A good example is the quantum systems vector, which the gimbal payload has a two axis gimbal mounted on it
anyway for them there are no difference with 3-1-2 and 3-2-1 because of absence of roll axis.
In case of two-axis gimbal you can usually only control pitch. Roll used internally by gimbal to stabilize horizon level.
I would disagree - most two axis gimbals used in fixedwing/VTOLs only have pitch and yaw stabilization, and roll is not stabilized
@Jaeyoung-Lim I don't have an opinion but this PR fixes a problem that exists. So please suggest a fix or otherwise I'd say we merge it and call it a day.
@julianoes thats fair, please go ahead
@svpcom Would there at least be a way to make it clear on the receiving end what euler angle convention the output is on?
Having symptoms of this issue on latest master with a pitch only uavcan servo gimbal that natively faces down; the servo goes full swing after init to one side or the other, but after a manual control input "center" it stabilizes normally.
Just so I said it, the take away is that we should be moving to the gimbal v2 protocol as it works mostly in quaternions (for exactly these reasons) but even there we make the conversions at some point to calculate the actual angles, just a bit later when you know your gimbal geometry and can use the appropriate convention at the time.
I guess that's what I had to fix here: https://github.com/PX4/PX4-SITL_gazebo/pull/885
This is probably more than you want to get into for a quick fix, but long term I'm still hoping to move away from carrying our own Matrix library and directly transition to something like Eigen.
Would it be easier to carry standalone helpers for now? https://github.com/PX4/PX4-Autopilot/blob/f321117568b6a42104b768a79ad682f8e808d770/src/modules/ekf2/EKF/utils.hpp
@svpcom could you instead re-use the helpers from ekf2? I'm moving them to mathlib. https://github.com/PX4/PX4-Autopilot/pull/20011
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
@dagar you wanted to use the new helpers rather than the mathlib. Where are they?
@julianoes What check was failed? Github show that there are some failure but no details :-(
In https://github.com/PX4/PX4-Autopilot/actions/runs/8227451344/job/22495377965:
The following tests FAILED:
11 - unit-MatrixAttitude (Failed)
Or in https://github.com/PX4/PX4-Autopilot/actions/runs/8227451360/job/22495378807:
--- Test case 11 of 17: 'RTL direct home without approaches' failed.
@julianoes I've fixed undefined behavior in tests. Also rebased to latest px4/main branch