From a580cd83e86544c4e421f7e62a31724ccee013b2 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Mon, 12 Jan 2015 15:24:46 -0800
Subject: [PATCH] AC_PosControl: Fill _vel_desired.z for reporting
---
libraries/AC_AttitudeControl/AC_PosControl.cpp | 4 ++++
1 file changed, 4 insertions(+)
diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp
index 3475f39f5..12f2b6173 100644
--- a/libraries/AC_AttitudeControl/AC_PosControl.cpp
+++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp
@@ -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
--
GitLab