Position Control Refactoring Part 5

[MaEtUgR]

Describe problem solved by this pull request
This is part 5 of the refactoring from the monster pr #12072 and should be easy to understand and review since the diff is manageable.

Describe your solution

• Switched naming and definition order of the PositionControl library to a clear interface best shown in the unit test for it: https://github.com/PX4/Firmware/blob/6bd543156eab2ccfe639e90d9214e2e0128daa09/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp#L105-L109 with better doxygen descriptions and internal naming.
• Simplified constraints checks above global limit e.g. !(constraints.tilt < _lim_tilt) to (constraints.tilt > _lim_tilt).
• When working on Commander: start pulling arming related parts into separate folder #13224 I was told we should rather write full include paths if they are in a subfolder of the same module such that it's more clear where the libreries are located even though cmake can do it all for you. That's why I also adapted the position control local includes to e.g. #include "PositiopnControl/PositionControl.hpp".
• Added a takeoff unit test for basic functionality of states and ramp. It's harder to cover all corner cases but at least we see if it directly breaks.
• Replaced all = Vector3f(NAN, NAN, NAN); with .setNaN();, initialized all FlightTask members with {}.
• Simplified thrustToAttitude() implementation.
• constraints.speed_xy is not used in the controller for a long time so I remove the processing to avoid confusion. Once tasks don't use it internally for communicating speeds accross inheritance anymore we can get rid of the message field.
• Remove control flags logic for correct logging, adding unit tests to make sure it's still correct.
• Removing the skip_controller flag is not yet possible with the old _interfaceMapping() but this will be replaced in the next pr.

Test data / coverage
SITL hover goto tested and unit tests pass.

Additional context
Part 4 that goes before this: #13347

[bresch]

Just one comment, the reset looks good

[bresch]

Could you maybe extract those next lines

                // Already received a valid thrust set-point.
		// Limit the thrust vector.
		float thr_mag = _thr_sp.length();

		if (thr_mag > _lim_thr_max) {
			_thr_sp = _thr_sp.normalized() * _lim_thr_max;

		} else if (thr_mag < _lim_thr_min && thr_mag > FLT_EPSILON) {
			_thr_sp = _thr_sp.normalized() * _lim_thr_min;
		}

Into a "constrainThrustSetpoint" function?

[MaEtUgR]

I could but this snippet will be gone in the next step so I didn't want to make an effort:
https://github.com/PX4/Firmware/pull/12072/files#diff-dcd121f41884535de662c42a30c6f928R107-R120

[MaEtUgR]

There will be one place at the end of the update sequence where thrust is limited for all cases and that one I can then break out into a function, good input 👍 :
position-control-refactoring5...acceleration-based-input#diff-dcd121f41884535de662c42a30c6f928R151-R158

[MaEtUgR]

I rebased without conflicts on master.

[bresch]

@PX4/testflights can you please test this PR in all flight modes on several multicopters?
Thanks in advance!

[Junkim3DR]

Tested on NXP FMUK66 v3

Modes Tested

• Position Mode: Good.
• Altitude Mode: Good.
• Stabilized Mode: Good.

Procedure
Arm and Take off in position mode, after flying for approximately one minute, switched to altitude then stabilized mode.

Notes
No issues noted, good flight in general.

Logs
https://review.px4.io/plot_app?log=920329d4-7468-435a-b258-17a4cfcb860e

Modes Tested

• Mission Plan Mode (Automated): Good.
• RTL (Return To Land): Good.

Procedure
Arm and Take off in mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint.

Notes
Good flight in general.

Logs
https://review.px4.io/plot_app?log=cc006579-0704-4735-a0fb-d88350890309

Modes Tested

• Position Mode: Good.
• Mission Plan Mode (Automated): Good.
• RTL (Return To Land): Good.

Procedure
Arm and Take off in position mode, after flying for approximately one minute, switched to mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint.

Notes
Good flight in general.

Logs
https://review.px4.io/plot_app?log=bf4ef893-d4c3-4c22-bdd9-f63f0f3654d1

[dannyfpv]

Tested on Pixhawk 4 v5 f450
position mode: no issues
mission mode: no issues
altitude mode: no issues
stabilize mode: no issues

log:
https://review.px4.io/plot_app?log=fef4dbeb-5b69-4b3d-9630-82258cf2f898

[jorge789]

Tested on PixRacer V4:

Maximum wind 10.2 m/s

Modes Tested

Position Mode: Good.
Altitude Mode: Good.
Stabilized Mode: Good.
Mission Plan Mode (Automated): Good.
RTL: Good.

- Procedure
Arm and Take off in position mode, after flying for approximately one minute, switched to altitude then stabilized mode proceed to switch to mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoin activate RTL and see landing behaviour.

Notes:
When the vehicle was landing, it did not descend vertically, drifted away with the wind and landed the vehicle manually

Log: https://review.px4.io/plot_app?log=1221ea62-3709-4c9d-bb9a-943495a3ccc3

Log https://review.px4.io/plot_app?log=d6764bd5-f1e4-48cd-a8aa-79828a80ab2e

[bresch]

I went again through the changes and it seems fine to me. You can merge this if the reported log files looks good to you and that CI passes.

[bresch]

When the vehicle was landing, it did not descend vertically, drifted away with the wind and landed the vehicle manually

@MaEtUgR This is because the tilt is limited to 12 degrees during the whole landing phase. Should be do like for the horizontal and vertical speed: gradually limit the tilt between alt1 and alt2 ?

[MaEtUgR]

This is because the tilt is limited to 12 degrees during the whole landing phase. Should be do like for the horizontal and vertical speed: gradually limit the tilt between alt1 and alt2 ?

I know this issue. I mostly see when I do some crazy user test going full stick and shortly after takeoff it does a twitch because the tilt limit jumps. The problem is we can't get rid of the tilt limit, that is worse I figured out. Let's do something gradual but I'm fairly confident it's unrelated to this pr.