diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 601f331753..e27705c25d 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -113,7 +113,7 @@ AC_WPNav::AC_WPNav(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosC _flags.fast_waypoint = false; // initialise old WPNAV_SPEED value - _last_wp_speed_cms = _wp_speed_down_cms; + _last_wp_speed_cms = get_default_speed_down(); } // get expected source of terrain data if alt-above-terrain command is executed (used by Copter's ModeRTL) @@ -167,8 +167,8 @@ void AC_WPNav::wp_and_spline_init(float speed_cms) // initialise position controller speed and acceleration _pos_control.set_max_speed_accel_xy(_wp_desired_speed_xy_cms, _wp_accel_cmss); _pos_control.set_correction_speed_accel_xy(_wp_desired_speed_xy_cms, _wp_accel_cmss); - _pos_control.set_max_speed_accel_z(-_wp_speed_down_cms, _wp_speed_up_cms, _wp_accel_z_cmss); - _pos_control.set_correction_speed_accel_z(-_wp_speed_down_cms, _wp_speed_up_cms, _wp_accel_z_cmss); + _pos_control.set_max_speed_accel_z(-get_default_speed_down(), _wp_speed_up_cms, _wp_accel_z_cmss); + _pos_control.set_correction_speed_accel_z(-get_default_speed_down(), _wp_speed_up_cms, _wp_accel_z_cmss); // calculate scurve jerk and jerk time if (!is_positive(_wp_jerk)) { diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 27542625dd..a4585d3926 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -71,7 +71,7 @@ public: float get_default_speed_up() const { return _wp_speed_up_cms; } /// get default target descent rate in cm/s during missions. Note: always positive - float get_default_speed_down() const { return _wp_speed_down_cms; } + float get_default_speed_down() const { return fabsf(_wp_speed_down_cms); } /// get_speed_z - returns target descent speed in cm/s during missions. Note: always positive float get_accel_z() const { return _wp_accel_z_cmss; }