Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: Fill _vel_desired.z for reporting

This commit is contained in:
Jonathan Challinger 2015-01-12 15:24:46 -08:00 committed by Randy Mackay
parent 2a7f78a2cd
commit a580cd83e8
1 changed files with 4 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -117,6 +117,8 @@ void AC_PosControl::set_accel_z(float accel_cmss)
void AC_PosControl::set_alt_target_with_slew(float alt_cm, float dt)
{
float alt_change = alt_cm-_pos_target.z;
_vel_desired.z = constrain_float(alt_change * dt, _speed_down_cms, _speed_up_cms);
// 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)) {
@ -139,6 +141,8 @@ void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float d
if ((climb_rate_cms<0 && (!_motors.limit.throttle_lower || force_descend)) || (climb_rate_cms>0 && !_motors.limit.throttle_upper)) {
_pos_target.z += climb_rate_cms * dt;
}
_vel_desired.z = climb_rate_cms;
}
// get_alt_error - returns altitude error in cm