From 0103ae2eb0da918ebaa460d4252b568663ea9443 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 30 Apr 2014 12:03:09 +0900 Subject: [PATCH] AC_WPNav: add WPNAV_ACCEL_Z Allows configurable z-axis acceleration during missions --- libraries/AC_WPNav/AC_WPNav.cpp | 14 ++++++++++++-- libraries/AC_WPNav/AC_WPNav.h | 8 +++++++- 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 252e13c4d4..d4fb3fa8f0 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -61,6 +61,15 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = { // @User: Standard AP_GROUPINFO("ACCEL", 5, AC_WPNav, _wp_accel_cms, WPNAV_ACCELERATION), + // @Param: ACCEL_Z + // @DisplayName: Waypoint Vertical Acceleration + // @Description: Defines the vertical acceleration in cm/s/s used during missions + // @Units: cm/s/s + // @Range: 50 500 + // @Increment: 10 + // @User: Standard + AP_GROUPINFO("ACCEL_Z", 6, AC_WPNav, _wp_accel_z_cms, WPNAV_WP_ACCEL_Z_DEFAULT), + AP_GROUPEND }; @@ -323,6 +332,7 @@ void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto _pos_control.set_speed_xy(_wp_speed_cms); _pos_control.set_accel_xy(_wp_accel_cms); _pos_control.set_speed_z(-_wp_speed_down_cms, _wp_speed_up_cms); + _pos_control.set_accel_z(_wp_accel_z_cms); _pos_control.calc_leash_length_xy(); _pos_control.calc_leash_length_z(); @@ -552,11 +562,11 @@ void AC_WPNav::calculate_wp_leash_length() _track_speed = _wp_speed_cms/pos_delta_unit_xy; _track_leash_length = _pos_control.get_leash_xy()/pos_delta_unit_xy; }else if(pos_delta_unit_xy == 0){ - _track_accel = _pos_control.get_accel_z()/pos_delta_unit_z; + _track_accel = _wp_accel_z_cms/pos_delta_unit_z; _track_speed = speed_z/pos_delta_unit_z; _track_leash_length = leash_z/pos_delta_unit_z; }else{ - _track_accel = min(_pos_control.get_accel_z()/pos_delta_unit_z, _wp_accel_cms/pos_delta_unit_xy); + _track_accel = min(_wp_accel_z_cms/pos_delta_unit_z, _wp_accel_cms/pos_delta_unit_xy); _track_speed = min(speed_z/pos_delta_unit_z, _wp_speed_cms/pos_delta_unit_xy); _track_leash_length = min(leash_z/pos_delta_unit_z, _pos_control.get_leash_xy()/pos_delta_unit_xy); } diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index caeaed82c1..3063bd309d 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -25,6 +25,8 @@ #define WPNAV_WP_SPEED_UP 250.0f // default maximum climb velocity #define WPNAV_WP_SPEED_DOWN 150.0f // default maximum descent velocity +#define WPNAV_WP_ACCEL_Z_DEFAULT 100.0f // default vertical acceleration betwen waypoints in cm/s/s + #define WPNAV_LEASH_LENGTH_MIN 100.0f // minimum leash lengths in cm #if HAL_CPU_CLASS >= HAL_CPU_CLASS_75 @@ -100,6 +102,9 @@ public: /// 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_speed_z - returns target descent speed in cm/s during missions. Note: always positive + float get_accel_z() const { return _wp_accel_z_cms; } + /// get_wp_radius - access for waypoint radius in cm float get_wp_radius() const { return _wp_radius_cm; } @@ -249,7 +254,8 @@ protected: 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_radius_cm; // distance from a waypoint in cm that, when crossed, indicates the wp has been reached - AP_Float _wp_accel_cms; // acceleration in cm/s/s during missions + AP_Float _wp_accel_cms; // horizontal acceleration in cm/s/s during missions + AP_Float _wp_accel_z_cms; // vertical acceleration in cm/s/s during missions // loiter controller internal variables uint32_t _loiter_last_update; // time of last update_loiter call