fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 22:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: _run_altitude if close to max altitude

Browse Source
This commit is contained in:
Dennis Mannhart
2017-02-27 22:54:19 +01:00
committed by Lorenz Meier
parent 3d4e7819a0
commit 6939583650
1 changed files with 17 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+17 -6
View File
@@ -918,21 +918,32 @@ MulticopterPositionControl::reset_alt_sp()
}
}
void
MulticopterPositionControl::limit_altitude()
{
/* in altitude control, limit setpoint */
if (_pos_sp(2) <= -_vehicle_land_detected.alt_max && _run_alt_control) {
if (_run_alt_control && _pos_sp(2) <= -_vehicle_land_detected.alt_max) {
_pos_sp(2) = -_vehicle_land_detected.alt_max;
return;
}
/* in velocity control in z, prevent vehicle from flying upwards if 0.5m close to altitude max*/
if (_pos(2) <= -_vehicle_land_detected.alt_max && !_run_alt_control && _vel_sp(2) <= 0.0f) {
_pos_sp(2) = -_vehicle_land_detected.alt_max;
_run_alt_control = true;
return;
/* in velocity control mode and want to fly upwards */
if (!_run_alt_control && _vel_sp(2) <= 0.0f) {
/* time to travel to reach zero velocity */
float delta_t = -_vel(2) / _params.acc_down_max;
/* predicted position */
float pos_z_next = _pos(2) + _vel(2) * delta_t + 0.5f *
_params.acc_down_max * delta_t *delta_t;
if (pos_z_next <= -_vehicle_land_detected.alt_max) {
_pos_sp(2) = -_vehicle_land_detected.alt_max;
_run_alt_control = true;
return;
}
}
}