dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mc_pos_control: fix limit_altitude function

This commit is contained in:
Dennis Mannhart 2017-02-27 17:03:41 +01:00 committed by Lorenz Meier
parent ca84cc7439
commit b887654d69
1 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -654,6 +654,8 @@ MulticopterPositionControl::parameters_update(bool force)
param_get(_params_handles.xy_vel_man_expo, &v); param_get(_params_handles.xy_vel_man_expo, &v);
_params.xy_vel_man_expo = v; _params.xy_vel_man_expo = v;
/* /*
* increase the maximum horizontal acceleration such that stopping * increase the maximum horizontal acceleration such that stopping
* within 1 s from full speed is feasible * within 1 s from full speed is feasible
@ -919,17 +921,16 @@ MulticopterPositionControl::reset_alt_sp()
void void
MulticopterPositionControl::limit_altitude() MulticopterPositionControl::limit_altitude()
{ {
float alt = _pos(2);
/* in altitude control, limit setpoint */ /* in altitude control, limit setpoint */
if (-alt >= _vehicle_land_detected.alt_max && _run_alt_control && _pos_sp(2) <= alt) { if (_pos_sp(2) <= -_vehicle_land_detected.alt_max && _run_alt_control) {
_pos_sp(2) = -_vehicle_land_detected.alt_max; _pos_sp(2) = -_vehicle_land_detected.alt_max;
return; return;
} }
/* in velocity control in z, prevent vehicle from flying upwards if 0.5m close to altitude max*/ /* in velocity control in z, prevent vehicle from flying upwards if 0.5m close to altitude max*/
if (-alt + 0.5f >= _vehicle_land_detected.alt_max && !_run_alt_control && _vel_sp(2) <= 0.0f) { if (_pos(2) <= -_vehicle_land_detected.alt_max && !_run_alt_control && _vel_sp(2) <= 0.0f) {
_pos_sp(2) = -_vehicle_land_detected.alt_max;; _pos_sp(2) = -_vehicle_land_detected.alt_max;
_run_alt_control = true; _run_alt_control = true;
return; return;
} }