Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 01:04:19 +08:00
Code Issues Actions 12 Packages Projects Releases Wiki Activity

fixed alt hold bug

Browse Source
This commit is contained in:
tumbili
2015-11-20 13:10:46 +01:00
parent 1f33451690
commit ff5db6b1be
1 changed files with 7 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+7 -3
View File
@@ -744,20 +744,24 @@ MulticopterPositionControl::control_manual(float dt)
/* check for pos. hold */
if (fabsf(req_vel_sp(2)) < _params.hold_z_dz)
{
if (!_alt_hold_engaged && (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z))
if (!_alt_hold_engaged)
{
_alt_hold_engaged = true;
if (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z) {
_alt_hold_engaged = true;
} else {
_alt_hold_engaged = false;
}
}
}
else {
_alt_hold_engaged = false;
_pos_sp(2) = _pos(2);
}
/* set requested velocity setpoint */
if (!_alt_hold_engaged) {
_run_alt_control = false; /* request velocity setpoint to be used, instead of altitude setpoint */
_vel_sp(2) = req_vel_sp_scaled(2);
_pos_sp(2) = _pos(2);
}
}
}