[navigation for multirotor] Sqrt Controller for Alt-Hold and improvements

[DzikuVx]

This replaces #7679

[DzikuVx]

@Jetrell tested this today on a 7" build and everything was just fine. I think we can merge it soon to master

[breadoven]

@DzikuVx I noticed slightly odd behaviour for posControl.desiredState.pos.z shown in the attached BB log screenshot which I tracked down to being caused by this change. Should be noted this was from ground testing RTH so things would likely be different if the MR was free to actually climb. Haven't got around to flight testing yet.

Target position Z in the BB screenshot jumps between the set ~30m RTH altitude and an altitude that corresponds to 10 + actual position Z. The 10 seems to come from a max climb rate setting of 500 cm/s and nav_mc_pos_z_p set to 50.

posControl.desiredState.pos.z initially gets set by:

inav/src/main/navigation/navigation.c

Line 1236 in f3f0cf0

setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);

But is then changed by:

inav/src/main/navigation/navigation_multicopter.c

Line 78 in f3f0cf0

posControl.desiredState.pos.z = pos_desired_z;

I'm guessing this is a logging issue (causing messy BB logs) more than a problem with altitude control given motor control should be based on the required velocity output from the sqrt controller corrected values (target Z velocity shown in the BB log screenshot behaves as expected) ? No dount related to the fact applyWaypointNavigationAndAltitudeHold is updated faster than updateWaypointsAndNavigationMode perhaps ?