AC_WPNav: add WPNAV_ACCEL_Z

Allows configurable z-axis acceleration during missions

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);
     }

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