mc_pos_control: Improve maximum height limiter

Implements a better method of determining when to switch from velocity to altitude control to keep height limit from being exceeded.
This method removes the overshoot and transients in height caused by the switching of the previous algorithm.

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -867,17 +867,21 @@ MulticopterPositionControl::limit_altitude()
 	// We want to fly upwards. Check that vehicle does not exceed altitude limit
 	if (!_run_alt_control && _vel_sp(2) <= 0.0f) {
 
-		// predict future position based on current position, velocity, max acceleration downwards and time to reach zero velocity
-		float pos_z_next = _pos(2);
-
-		if (_vel(2) < 0.0f) {
-			// allow for stopping distance only when approaching the altitude limit from below
-			// TODO better calculation that allows for delay in braking
-			pos_z_next -= 0.5f * _vel(2) * _vel(2) / _acceleration_z_max_down.get();
-		}
-
-		if (pos_z_next < poz_z_min_limit) {
-			// prevent the vehicle from exceeding maximum altitude by switching back to altitude control with maximum altitude as setpoint
+		// estimate demanded thrust from altitude control loop with setpoint at limit
+		// excluding the integrator and other thrust correction terms
+		float vel_z_sp_altctl = (poz_z_min_limit - _pos(2)) * _pos_p(2);
+		float vel_z_err_altctl = vel_z_sp_altctl - _vel(2);
+		float thrust_altctl = vel_z_err_altctl * _vel_p(2) + _vel_err_d(2) * _vel_d(2);
+
+		// estimate demanded velocity from velocity control loop
+		// excluding the integrator and any other thrust correction terms
+		float vel_z_err_velctl = _vel_sp(2) - _vel(2);
+		float thrust_velctl = vel_z_err_velctl * _vel_p(2) + _vel_err_d(2) * _vel_d(2);
+
+		// if the altitude control about the setpoint is demanding a more positive velocity
+		// then switch to altitude control
+		if (thrust_altctl > thrust_velctl) {
+			// prevent the vehicle from exceeding maximum altitude by limiting the
 			_pos_sp(2) = poz_z_min_limit;
 			_run_alt_control = true;
 		}