Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: set desired_vel for reporting purposes even when not used

Browse Source
This commit is contained in:
Leonard Hall 2016-06-04 13:29:38 +09:00 committed by Randy Mackay
parent 558f29b95c
commit 1639e22b74
1 changed files with 5 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -140,11 +140,14 @@ void AC_PosControl::set_alt_target_with_slew(float alt_cm, float dt)
// do not use z-axis desired velocity feed forward
_flags.use_desvel_ff_z = false;
_vel_desired.z = 0.0f;
// adjust desired alt if motors have not hit their limits
if ((alt_change<0 && !_motors.limit.throttle_lower) || (alt_change>0 && !_motors.limit.throttle_upper)) {
_pos_target.z += constrain_float(alt_change, _speed_down_cms*dt, _speed_up_cms*dt);
if (!is_zero(dt)) {
float climb_rate_cms = constrain_float(alt_change/dt, _speed_down_cms, _speed_up_cms);
_pos_target.z += climb_rate_cms*dt;
_vel_desired.z = climb_rate_cms; // recorded for reporting purposes
}
}
// do not let target get too far from current altitude