MC: improve manual stick input handling

[bkueng]

I was quite surprised when I noticed the following in manual mode:

1. The maximum tilt angle (set via MPC_MAN_TILT_MAX) can easily be exceeded just by using a combination of roll & pitch (e.g. by moving the stick to the right upper corner). The formula for the tilt angle is acos(cos(roll) * cos(pitch)). By inserting the maximum angle, the limit is exceeded by roughly 15 degrees (with the default maximum tilt of 35 degrees). The full graph can be seen here: https://www.desmos.com/calculator/5zykbcnn93
2. I would have expected the vehicle to move towards 45 degrees when I move the stick towards a corner (equal roll & pitch). However this is not the case. For a maximum tilt angle of 35 degrees, the direction is off by a bit more than 5 degrees. What's worse is that the direction depends on the tilt angle, meaning if you were to move the stick perfectly straight from the center towards a corner, you would fly a curve. The formula for the direction of maximum tilt is atan(sin(roll) / (sin(pitch) * cos(roll))). For roll == pitch, this is the graph by how much the direction is off from the diagonal: https://www.desmos.com/calculator/3poijoayhx. With a larger maximum tilt angle, this becomes noticeable as well.

I confirmed both points by looking at logs & doing simulations.

At first I only noticed 1. and implemented a solution for that, when I noticed the second point. They cannot be solved independently, so I propose a completely new approach to handle manual input. x and y input are used to control two (different) angles:

• the tilt angle (given by sqrt(x*x + y*y)). This allows a simple limitation of the maximum tilt (it adds a small dead-zone in the corners of the stick range)
• the direction of the tilt in the xy-plane, which defines the (eventual) direction of the motion

Benefits:

• for roll/pitch only inputs (zero pitch/roll) the behavior is exactly the same as before
• maximum tilt is correctly limited in every direction
• the flight direction matches with the direction of the stick input
• changes are linear
• improved flight experience. The vehicle does what you would expect it to do. This is a bit subjective, and I suggest you test it yourself. I made a test branch manual_input_rework_testing. It makes it possible to switch between the old & new behavior in-flight by mapping the aux1 channel.

However there is one point to note: the new behavior changes the yaw as well, when roll & pitch are combined. This can be seen if you pick up an x-quad at 2 diagonal motors, and start rotating it around those 2 motors: the yaw changes a bit.
If a is the tilt angle and b is the angle of the direction of the tilt angle, then the yaw angle is given by yaw = atan((-2 * sin(b) * cos(b) * sin^2(a/2)) / (1 - 2 * cos^2(b) * sin^2(a/2))), and as a graph (x=tilt angle): https://www.desmos.com/calculator/y2mnebvl0j

Testing
I did extensive testing in HIL, including combinations of yawing and maximum tilt angles up to 90 degrees. I noticed that the yaw becomes very uncontrolled (overshoots and slow convergence) at high angles (around 80+). But this is a separate issue, I had the same behavior before this PR.
Then I tested on 2 different quads. So far, with a maximum tilt angle up to 55 degrees:

• 35 degrees max tilt: https://logs.px4.io/plot_app?log=6c4e8474-63dc-4a54-b2ca-9f9ee05566e0
• 55 degrees max tilt (with in-flight switching between old & new behavior): https://logs.px4.io/plot_app?log=8c1af5f2-f90f-41e9-a42e-83d1af481c6f
• 55 degrees max tilt: https://logs.px4.io/plot_app?log=5b613a54-e4f4-481c-ae78-1bed66e9fd24
  In this log the position estimation is off, not sure why. Also the yaw lost tracking when I quickly changed roll & pitch angles. This happens with both the old & new behavior, it's not a setpoint issue.

[bkueng]

More flights at higher angles:

• 65 degrees: https://logs.px4.io/plot_app?log=3938c48a-4889-44b7-ac1d-ad2be4629ba9
• 75 degrees: https://logs.px4.io/plot_app?log=2e647094-96e6-4a36-9b6a-1e93b57ac147

I noticed the same yawing issues as with HIL, especially with 75 degrees max tilt. This makes it pretty much unflyable. I created an issue in #7949.

[Stifael]

The math looks ok, but in the computation you only compute q_tilt and ignoring q_twist(represents yaw).
an attitude qcan be represented as follow:
q = q_tilt * q_twist,
where q_tilt= [cos(0.5*a), sin(0.5*a)*nx, sin(0.5*a)*ny, sin(0.5*a)*nz]with a= tilt angle. In your case, you have q_sp_rpy = q_sp_tilt, where q_sp_tiltis the tilt attitude computed from tilt angle setpoint and rotation axis, but what we ultimately want to have is
q_sp_rpy = q_sp_tilt * q_sp_twist

q_sp_twistis simply just the current attitude twist q_current_twist because we compute the attitude setpoint from roll and pitch and therefore would like to have the same twist attitude without adding yaw to it.

Either we compute the twist attitude setpoint directly from yaw, or we compute the twist attitude setpoint from quaternions:

• b= tilt angle between z_body_axis and [0,0,-1]
• q_current_tilt = [cos(0.5*b), sin(0.5*b)*nx, sin(0.5*b)*ny, sin(0.5*b)*nz] , where [nx,ny,nz] is the rotation axis to get from [0,0,-1] to z_body_axis
• q_current_twist = inverse(q_current_tilt) * q_current
• q_sp_twist = q_current_twist

[bkueng]

@Stifael Thanks for the review. You probably have overlooked _att_sp.yaw_body += euler_sp(2); (https://github.com/PX4/Firmware/pull/7940/files#diff-fc77c3ef569029d45764664c75d4b0c1R2156). It adds q_tilt to the yaw setpoint, doing the same as q_sp_rpy = q_sp_tilt * q_sp_twist just in Euler angles.

[Stifael]

@bkueng, yes I missed that. However, it is also kind of an interesting order of implementation.
First one computes q_sp, then it gets transformed into euler where yaw is added, and then it gets again transformed into quaternion here https://github.com/PX4/Firmware/blob/96cb5760ae3180f65ea514da9a32e4ef7fb420c3/src/modules/mc_pos_control/mc_pos_control_main.cpp#L2197.
I guess it would be nicer to compute the entire quaternion first, and then compute roll, pitch and yaw setpoints, which primarily only should be used for logging. Anyway, looks good.

[bkueng]

I agree and I first had it with quaternions only, but the next block (https://github.com/PX4/Firmware/blob/96cb5760ae3180f65ea514da9a32e4ef7fb420c3/src/modules/mc_pos_control/mc_pos_control_main.cpp#L2159:L2193) then again uses Eulers, so I kept Eulers as well to reduce the number of CPU cycles.

I rebased to master.

[MaEtUgR]

Thanks a lot for thinking this through. I just have some implementation order comments.
The idea behind your implementation is very nice!

What I still don't get exactly is why it should influence yaw at all... I need to think this through again.

I would favor the most quaternion near implementation as possible (without unnecessary conversions) as well.

[MaEtUgR]

typo? "issues"

[MaEtUgR]

Nitpicking: You could calculate v and v_norm before this line and use v_norm instead of sqrtf(x * x + y * y).

[bkueng]

You're right, this is much simpler now: 773df45

[MaEtUgR]

v.normalize() ?

[MaEtUgR]

you're basically multiplying by v_norm here again which means you could only treat the case v_norm is bigger than _params.man_tilt_max to safe steps.

[MaEtUgR]

Nice work!

[MaEtUgR]

I think these comments are very useful. What made me finally understand why the yaw changes here in your spoken explanation is the hint "it's yaw in world frame". Maybe this hint could also help others reading the comments.

[bkueng]

I added that.