[WIP] Different Attitude Setpoint Generation Strategy

[MaEtUgR]

Describe problem solved by this pull request
Currently, the position controller generates a thrust vector setpoint and forwards the desired yaw heading in the world frame. The setpoint generator then calculates an attitude that aligns the body z axis with the desired thrust (basic multicopter dynamics) and incorporates the world frame yaw heading in a way that the body x axis projected onto the world x-y-plane always point to the desired world yaw heading. This seems like a viable solution depending on the use case of the yaw setpoint but I see one big disadvantage of this solution: The body yaw rate is coupled with a thrust vector

Describe your solution
I calculate the desired attitude by aligning the body z axis with the thrust vector like before but this time directly using quaternion math. See PX4/PX4-Matrix#55 for implementation details. Then yaw is applied on top with a different strategy: If the vehicle is level the yaw is the world frame heading but if the vehicle is tilted it just does the shortest roll/pitch tilting without correcting the body yaw angle to allign the body x-axis in to a world frame projection. This results in no body yaw rate corrections resulting from roll and pitch adjustments.

Note that all the 3-axis gimbals for drones I have tested do the right thing when generating the attitude the way proposed in this pr and yaw off the desired world frame heading when aplying the current PX4 conversion.

Test data / coverage
SITL tested and I adjusted the existing unit tests in the following way:

• structure refactoring 4b18af1
• additional body z-axis direction and thrust magnitude checks 4b18af1#diff-ab7081b2f8b080c822f0c8b98f2cef20R47-R48
• adjustment to the new attitude generation strategy 0faf1f8

[MaEtUgR]

@bkueng Just mentioned an important point to me: The navigator yaw reached checks might fail if the setpoint generation strategy is different from the attitude estimate yaw euler angle interpretation because when the vehicle has a roll/pitch offset e.g. high winds the tracked yaw setpoint might be different from the yaw estimate in euler angles.

I have to find a viable setup to verify the fix for it.

[MaEtUgR]

I just rebased on master

[MaEtUgR]

I found the corner case orientation problem that the new position control unit tests in master detected: https://ci.appveyor.com/project/Dronecode/firmware/builds/28990523/job/tg4c25ljktm2b4p6
And fixed it with interactive rebase in https://github.com/PX4/Firmware/compare/d1afaf906e5d47a61fda5bdc2bbb4e41c354ceaf..2f54968b7ee214f403b95ef8856037ae85cc10ae

JFYI @dagar It seems that Appveyor is running CI on a merged version of the branch seen from the console output of the run linked above:

git fetch -q origin +refs/pull/13535/merge:
git checkout -qf FETCH_HEAD

And that's why it was the only make tests instance which failed.

[bresch]

@MaEtUgR Any news on that?

[MaEtUgR]

I rescheduled to pick this up again since it's fairly significant.

[MaEtUgR]

@dagar's good idea was to clearly distinguish between "yaw" and "heading" since the pilot cares about the "heading" when the multicopter is level (zero roll pitch equivalent of current attitude) and not any x-axis projections with the current tilt that constantly requires yaw corrections on roll pitch changes.

I quickly wrote up a test to show the difference in rate control input with the old and proposed attitude setpoint generation strategy given no yaw changes but only 45° diagonal x,y horizontal displacement: d4cbdc1#diff-b07ac130d43e318a5f9bb3f6518b91b4R48

While the attitude generation currently in use results in a commanded body yaw rate and resulting from that twist also an asymmetric roll pitch rate:

0.607
-0.982
0.375

the proposed attitude generation results in the expected symmetric roll pitch rate and no yaw corrections:

0.816
-0.816
0

I will encorporate the test in this pr but what's still missing is to verify compatibility with navigator ("yaw"/)heading reachability checks.

[bresch]

Nice, this will be especially good for quadplanes

[MaEtUgR]

Input from discussion with @priseborough : There is no one yaw value that suits all applications. Either the estimator only publishes the quaternion and the receiver needs to do the conversion he wants or the estimator can do the one or two mainly used conversions but they need to be very clearly described in the interface and not misused. Current descriptions are e.g.
https://github.com/PX4/Firmware/blob/a89b69b0eadab3e24d374aba71deda3a056f446d/msg/vehicle_local_position.msg#L40
https://github.com/PX4/Firmware/blob/a89b69b0eadab3e24d374aba71deda3a056f446d/msg/vehicle_global_position.msg#L19
for the same value which should not be considered a heading in a mission sense by the navigator.

[MaEtUgR]

As an update this is still pending because the yaw/heading needs to be updated for yaw reached checks and for the yaw tracking plot otherwise the result will always be that the euler angle that is computed in a different way is not reached.

[LorenzMeier]

Closing this as stale.

[MaEtUgR]

Revived and rebased such that we finally have this in the control interface 🟢
What's left is EKF publishing the new interpretation of heading and e.g. navigator checking for that in the heading reached check.