fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-13 11:20:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mathlib, mc_pos_control: rename functions file and add gradual linear function

Browse Source
This commit is contained in:
Matthias Grob
2017-05-15 10:50:33 +02:00
committed by Lorenz Meier
parent 1c2d54397f
commit 1d7f760a96
3 changed files with 38 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+3 -11
View File
@@ -919,17 +919,9 @@ MulticopterPositionControl::slow_land_gradual_velocity_limit()
* for now we use the home altitude and assume that we want to land on a similar absolute altitude.
*/
float altitude_above_home = -_pos(2) + _home_pos.z;
float vel_limit = _params.vel_max_down;
if (altitude_above_home < _params.slow_land_alt2) {
vel_limit = _params.land_speed;
} else if (altitude_above_home < _params.slow_land_alt1) {
/* linear function between the two altitudes */
float a = (_params.vel_max_down - _params.land_speed) / (_params.slow_land_alt1 - _params.slow_land_alt2);
float b = _params.land_speed - a * _params.slow_land_alt2;
vel_limit = a * altitude_above_home + b;
}
float vel_limit = math::gradual(altitude_above_home,
_params.slow_land_alt2, _params.slow_land_alt1,
_params.land_speed, _params.vel_max_down);
_vel_sp(2) = math::min(_vel_sp(2), vel_limit);
}