From 9f63de9b59bc13cbbd2f99beab4cc6d2186f55ce Mon Sep 17 00:00:00 2001 From: Randy Mackay 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 14f99bc2b6..234626767e 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 6ba2c112a2..68e27dbedf 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);