From 9f63de9b59bc13cbbd2f99beab4cc6d2186f55ce Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 30 Apr 2014 11:14:48 +0900
Subject: [PATCH] AC_PosControl: set_speed_z accepts positive descent speeds
---
libraries/AC_AttitudeControl/AC_PosControl.cpp | 3 +++
libraries/AC_AttitudeControl/AC_PosControl.h | 3 +--
2 files changed, 4 insertions(+), 2 deletions(-)
diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp
index 14f99bc2b..234626767 100644
--- a/libraries/AC_AttitudeControl/AC_PosControl.cpp
+++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp
@@ -77,6 +77,9 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
/// speed_down should be a negative number
void AC_PosControl::set_speed_z(float speed_down, float speed_up)
{
+ // ensure speed_down is always negative
+ speed_down = -fabs(speed_down);
+
if ((fabs(_speed_down_cms-speed_down) > 1.0f) || (fabs(_speed_up_cms-speed_up) > 1.0f)) {
_speed_down_cms = speed_down;
_speed_up_cms = speed_up;
diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h
index 6ba2c112a..68e27dbed 100644
--- a/libraries/AC_AttitudeControl/AC_PosControl.h
+++ b/libraries/AC_AttitudeControl/AC_PosControl.h
@@ -62,8 +62,7 @@ public:
void set_alt_max(float alt) { _alt_max = alt; }
/// set_speed_z - sets maximum climb and descent rates
- /// To-Do: call this in the main code as part of flight mode initialisation
- /// speed_down should be a negative number
+ /// speed_down can be positive or negative but will always be interpreted as a descent speed
/// leash length will be recalculated the next time update_z_controller() is called
void set_speed_z(float speed_down, float speed_up);
--
GitLab