diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 14f99bc2b647efb7d736972aec3629edebec15e9..234626767e27b283727c7987c569f993281c1568 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 6ba2c112a2bdbd8af52777b75a02da772d340fd4..68e27dbedf7acd58dfcf3ccafca5bb1e63aad71f 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);