fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-04 06:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: Improve maximum height limiter

Browse Source 
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.
This commit is contained in:
Paul Riseborough
2018-06-07 14:28:21 +10:00
committed by Lorenz Meier
parent f0a1cd197e
commit d26da5fa3b
1 changed files with 13 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+13 -9
View File
@@ -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;
}