From d26da5fa3b4ae12e60c433345e5b8b761384f759 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 7 Jun 2018 14:28:21 +1000 Subject: [PATCH] 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. --- .../mc_pos_control/mc_pos_control_main.cpp | 22 +++++++++++-------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index b15780931f..932d9beb62 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/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); + // 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); - 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(); - } + // 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 (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 + // 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; }