Pos/Vel PID controller in body-yaw-aligned axes instead of global

[matejkarasek]

Position and Velocity PID controllers are rewritten in body-yaw-aligned coordinates, instead of the previously used global coordinates. This functionality is useful when flying with non-symmetric vehicles whose flight dynamics along x and y directions are not the same, and thus require different gain tuning.
This PR replaces the original #833.

Further updates in this PR:

• low pass filters in the position and control loops are now separated: position and velocity as well as xy and z directions can be set separately
• the above settings can be changed through configuration parameters
• feed-forward terms and gains were added for the velocity loop (useful for vehicles with non-negligible drag)
• the xy position and xy velocity in the "body-yaw-aligned" frame is also logged (posCtl.bodyX, posCtl.bodyVX, ...) for easier PID tuning

[matejkarasek]

Tested on a CF2.1 with the Lighthouse system (position hold in the CF client):

20211119_173519.1.mp4

[knmcguire]

Awesome, thanks @matejkarasek ! I will take a good look at this PR, do some flight test with it this weekend and if all good I'll merge it.

This is btw also helping out this user on the forum: https://forum.bitcraze.io/viewtopic.php?t=5147 As he was confused about the whole x velocity y velocity being controlled by roll and pitch respectively, which does not make sense at the first look of it (pitch raw and roll raw is not the great name for those variables)

[matejkarasek]

Yes, please take a look, good if also someone else tests this as it touches quite an important part of the code :)

Glad to hear this might improve the clarity for some people... though I also expect this will create some confusion for others :)

Btw, not sure if this is clear from the "body-yaw-aligned" name but strictly speaking the controlled is not in body-fixed coordinates, as z keeps pointing upwards. Only x and y get rotated by the yaw angle...

[knmcguire]

yeah true... but let's check it out how to properly name them another time.

Yeah it's a pretty important change, although I'm almost sure it will not affect the rest. Perhaps one of the home crew should also try it in the bigger flight arena as I don't have as much space (currently in the US), to really try it out to the max.

[knmcguire]

I've flown it, I've gone through the code changes and I double check the automatically generated documentation, and it all checks out :)

So for me I say it's a go, but I'll give the those at the office a chance to look at it today before I press merge.

[knmcguire]

Alright! @tobbeanton had a good look at it and it all checks out, two flight test done. I think it's safe to say that we did all we can to make sure that this PR doesn't break anything. :)

So let's go for it!

[matejkarasek]

Perfect, thanks for testing it out!