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

mc_pos_control: fix altitude limitation

Browse Source 
Before if you were above the maximum altitude you could not command to
go down anymore until the position controller had overshoot to under the
maximum altitude again.
This commit is contained in:
Matthias Grob 2019-01-14 11:05:28 +01:00
parent 93ef70c43d
commit db0283ef21
1 changed files with 6 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -486,22 +486,13 @@ MulticopterPositionControl::limit_altitude(vehicle_local_position_setpoint_s &se
return;
}
float altitude_above_home = -(_states.position(2) - _home_pos.z);
// maximum altitude == minimal z-value (NED)
const float min_z = _home_pos.z + (-_vehicle_land_detected.alt_max);
if (altitude_above_home > _vehicle_land_detected.alt_max) {
// we are above maximum altitude
setpoint.z = -_vehicle_land_detected.alt_max + _home_pos.z;
setpoint.vz = 0.0f;
} else if (setpoint.vz <= 0.0f) {
// we want to fly upwards: check if vehicle does not exceed altitude
float delta_p = _vehicle_land_detected.alt_max - altitude_above_home;
if (fabsf(setpoint.vz) * _dt > delta_p) {
setpoint.z = -_vehicle_land_detected.alt_max + _home_pos.z;
setpoint.vz = 0.0f;
}
if (_states.position(2) < min_z) {
// above maximum altitude, only allow downwards flight == positive vz-setpoints (NED)
setpoint.z = min_z;
setpoint.vz = math::max(setpoint.vz, 0.f);
}
}