Skip to content
Snippets Groups Projects
Commit 9f63de9b authored by Randy Mackay's avatar Randy Mackay
Browse files

AC_PosControl: set_speed_z accepts positive descent speeds

parent 466e9db1
No related branches found
No related tags found
No related merge requests found
......@@ -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;
......
......@@ -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);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment