From cde2964f05e6c4f3b7c803011d33a20ce7d4235d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 24 Jan 2019 13:57:11 +0900 Subject: [PATCH] AC_WPNav: split set_speed_z into up and down Also rely on AC_PosControl to store current target up and down speeds so wpnav's defaults are not affected by do-change-speed requests --- libraries/AC_WPNav/AC_WPNav.cpp | 20 +++++++++++++------- libraries/AC_WPNav/AC_WPNav.h | 17 +++++++++-------- 2 files changed, 22 insertions(+), 15 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index d212b2b9f9..3e981e36bc 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -170,12 +170,18 @@ void AC_WPNav::set_speed_xy(float speed_cms) } } -/// set_speed_z - allows main code to pass target vertical velocity for wp navigation -void AC_WPNav::set_speed_z(float speed_down_cms, float speed_up_cms) +/// set current target climb rate during wp navigation +void AC_WPNav::set_speed_up(float speed_up_cms) { - _wp_speed_down_cms = speed_down_cms; - _wp_speed_up_cms = speed_up_cms; - _pos_control.set_max_speed_z(_wp_speed_down_cms, _wp_speed_up_cms); + _pos_control.set_max_speed_z(_pos_control.get_max_speed_down(), speed_up_cms); + // flag that wp leash must be recalculated + _flags.recalc_wp_leash = true; +} + +/// set current target descent rate during wp navigation +void AC_WPNav::set_speed_down(float speed_down_cms) +{ + _pos_control.set_max_speed_z(speed_down_cms, _pos_control.get_max_speed_up()); // flag that wp leash must be recalculated _flags.recalc_wp_leash = true; } @@ -556,10 +562,10 @@ void AC_WPNav::calculate_wp_leash_length() float speed_z; float leash_z; if (_pos_delta_unit.z >= 0.0f) { - speed_z = _wp_speed_up_cms; + speed_z = _pos_control.get_max_speed_up(); leash_z = _pos_control.get_leash_up_z(); }else{ - speed_z = _wp_speed_down_cms; + speed_z = fabsf(_pos_control.get_max_speed_down()); leash_z = _pos_control.get_leash_down_z(); } diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index d9bd6e511f..abec31f10a 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -78,17 +78,18 @@ public: /// set current target horizontal speed during wp navigation void set_speed_xy(float speed_cms); - /// set_speed_z - allows main code to pass target vertical velocity for wp navigation - void set_speed_z(float speed_down_cms, float speed_up_cms); + /// set current target climb or descent rate during wp navigation + void set_speed_up(float speed_up_cms); + void set_speed_down(float speed_down_cms); /// get default target horizontal velocity during wp navigation float get_default_speed_xy() const { return _wp_speed_cms; } - /// get_speed_up - returns target climb speed in cm/s during missions - float get_speed_up() const { return _wp_speed_up_cms; } + /// get default target climb speed in cm/s during missions + float get_default_speed_up() const { return _wp_speed_up_cms; } - /// get_speed_down - returns target descent speed in cm/s during missions. Note: always positive - float get_speed_down() const { return _wp_speed_down_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; } /// 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; } @@ -282,8 +283,8 @@ protected: // parameters AP_Float _wp_speed_cms; // default maximum horizontal speed in cm/s during missions - AP_Float _wp_speed_up_cms; // climb speed target in cm/s - AP_Float _wp_speed_down_cms; // descent speed target in cm/s + AP_Float _wp_speed_up_cms; // default maximum climb rate in cm/s + AP_Float _wp_speed_down_cms; // default maximum descent rate in cm/s AP_Float _wp_radius_cm; // distance from a waypoint in cm that, when crossed, indicates the wp has been reached AP_Float _wp_accel_cmss; // horizontal acceleration in cm/s/s during missions AP_Float _wp_accel_z_cmss; // vertical acceleration in cm/s/s during missions