From 7fa7c8f403402ded66f78d16344694446db6d210 Mon Sep 17 00:00:00 2001 From: chobits Date: Tue, 9 Oct 2018 13:34:30 +0800 Subject: [PATCH] AC_WPNav: add set_speed_z --- libraries/AC_WPNav/AC_WPNav.cpp | 10 ++++++++++ libraries/AC_WPNav/AC_WPNav.h | 3 +++ 2 files changed, 13 insertions(+) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 44378d6c3c..76b4817a17 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -171,6 +171,16 @@ 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) +{ + _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); + // flag that wp leash must be recalculated + _flags.recalc_wp_leash = true; +} + /// set_wp_destination waypoint using location class /// returns false if conversion from location to vector from ekf origin cannot be calculated bool AC_WPNav::set_wp_destination(const Location_Class& destination) diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 1c81d9ecfc..10077a1f93 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -78,6 +78,9 @@ public: /// set_speed_xy - allows main code to pass target horizontal velocity for 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); + /// get_speed_xy - allows main code to retrieve target horizontal velocity for wp navigation float get_speed_xy() const { return _wp_speed_cms; }