AC_AttitudeControl: AC_PosControl: seperate kinimatic shaping from pid limit setting

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -393,7 +393,10 @@ void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_offset_z)
 /// Lateral position controller
 ///
 
-/// set_max_speed_accel_xy - set the maximum horizontal speed in cm/s and acceleration in cm/s/s and position controller correction acceleration limit
+/// set_max_speed_accel_xy - set the maximum horizontal speed in cm/s and acceleration in cm/s/s
+///     This function only needs to be called if using the kinimatic shaping.
+///     This can be done at any time as changes in these parameters are handled smoothly
+///     by the kinimatic shaping.
 void AC_PosControl::set_max_speed_accel_xy(float speed_cms, float accel_cmss)
 {
     // return immediately if no change
@@ -403,8 +406,6 @@ void AC_PosControl::set_max_speed_accel_xy(float speed_cms, float accel_cmss)
     _vel_max_xy_cms = speed_cms;
     _accel_max_xy_cmss = accel_cmss;
 
-    _p_pos_xy.set_limits(_vel_max_xy_cms, _accel_max_xy_cmss, 0.0f);
-
     // ensure the horizontal time constant is not less than the vehicle is capable of
     const float lean_angle = _accel_max_xy_cmss / (GRAVITY_MSS * 100.0 * M_PI / 18000.0);
     const float angle_accel = MIN(_attitude_control.get_accel_pitch_max(), _attitude_control.get_accel_roll_max());
@@ -415,6 +416,13 @@ void AC_PosControl::set_max_speed_accel_xy(float speed_cms, float accel_cmss)
     }
 }
 
+/// set_max_speed_accel_xy - set the position controller correction velocity and acceleration limit
+///     This should be done only during initialisation to avoid discontinuities
+void AC_PosControl::set_correction_speed_accel_xy(float speed_cms, float accel_cmss)
+{
+    _p_pos_xy.set_limits(speed_cms, accel_cmss, 0.0f);
+}
+
 /// init_xy_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
 ///     This function is the default initialisation for any position control that provides position, velocity and acceleration.
 void AC_PosControl::init_xy_controller()
@@ -646,8 +654,11 @@ void AC_PosControl::update_xy_controller()
 /// Vertical position controller
 ///
 
-/// set_max_speed_accel_z - set the maximum vertical speed in cm/s and acceleration in cm/s/s and position controller correction acceleration limit
+/// set_max_speed_accel_z - set the maximum vertical speed in cm/s and acceleration in cm/s/s
-///     speed_down can be positive or negative but will always be interpreted as a descent speed
+///     speed_down can be positive or negative but will always be interpreted as a descent speed.
+///     This function only needs to be called if using the kinimatic shaping.
+///     This can be done at any time as changes in these parameters are handled smoothly
+///     by the kinimatic shaping.
 void AC_PosControl::set_max_speed_accel_z(float speed_down, float speed_up, float accel_cmss)
 {
     // ensure speed_down is always negative
@@ -668,8 +679,6 @@ void AC_PosControl::set_max_speed_accel_z(float speed_down, float speed_up, floa
     if (is_positive(accel_cmss)) {
         _accel_max_z_cmss = accel_cmss;
     }
-    // define maximum position error and maximum first and second differential limits
-    _p_pos_z.set_limits(-fabsf(_vel_max_down_cms), _vel_max_up_cms, _accel_max_z_cmss, 0.0f);
 
     // ensure the vertical time constant is not less than the filters in the _pid_accel_z object
     _tc_z_s = _shaping_tc_z_s;
@@ -681,6 +690,15 @@ void AC_PosControl::set_max_speed_accel_z(float speed_down, float speed_up, floa
     }
 }
 
+/// set_correction_speed_accel_z - set the position controller correction velocity and acceleration limit
+///     speed_down can be positive or negative but will always be interpreted as a descent speed.
+///     This should be done only during initialisation to avoid discontinuities
+void AC_PosControl::set_correction_speed_accel_z(float speed_down, float speed_up, float accel_cmss)
+{
+    // define maximum position error and maximum first and second differential limits
+    _p_pos_z.set_limits(-fabsf(speed_down), _vel_max_up_cms, _accel_max_z_cmss, 0.0f);
+}
+
 /// init_z_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
 ///     This function is the default initialisation for any position control that provides position, velocity and acceleration.
 void AC_PosControl::init_z_controller()
@@ -1165,7 +1183,7 @@ bool AC_PosControl::calculate_yaw_and_rate_yaw()
         }
     }
 
-    // update the target yaw if origin and destination are at least 2m apart horizontally
+    // update the target yaw if velocity is greater than 5% _vel_max_xy_cms
     if (vel_desired_xy_len > _vel_max_xy_cms * 0.05f) {
         _yaw_target = degrees(vel_desired_xy.angle()) * 100.0f;
         _yaw_rate_target = turn_rate*degrees(100.0f);

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -64,8 +64,15 @@ public:
     ///
 
     /// set_max_speed_accel_xy - set the maximum horizontal speed in cm/s and acceleration in cm/s/s
+    ///     This function only needs to be called if using the kinimatic shaping.
+    ///     This can be done at any time as changes in these parameters are handled smoothly
+    ///     by the kinimatic shaping.
     void set_max_speed_accel_xy(float speed_cms, float accel_cmss);
 
+    /// set_max_speed_accel_xy - set the position controller correction velocity and acceleration limit
+    ///     This should be done only during initialisation to avoid discontinuities
+    void set_correction_speed_accel_xy(float speed_cms, float accel_cmss);
+
     /// get_max_speed_xy_cms - get the maximum horizontal speed in cm/s
     float get_max_speed_xy_cms() const { return _vel_max_xy_cms; }
 
@@ -129,8 +136,15 @@ public:
 
     /// set_max_speed_accel_z - set the maximum vertical speed in cm/s and acceleration in cm/s/s
     ///     speed_down can be positive or negative but will always be interpreted as a descent speed
+    ///     This can be done at any time as changes in these parameters are handled smoothly
+    ///     by the kinimatic shaping.
     void set_max_speed_accel_z(float speed_down, float speed_up, float accel_cmss);
 
+    /// set_correction_speed_accel_z - set the position controller correction velocity and acceleration limit
+    ///     speed_down can be positive or negative but will always be interpreted as a descent speed
+    ///     This should be done only during initialisation to avoid discontinuities
+    void set_correction_speed_accel_z(float speed_down, float speed_up, float accel_cmss);
+
     /// get_max_accel_z_cmss - get the maximum vertical acceleration in cm/s/s
     float get_max_accel_z_cmss() const { return _accel_max_z_cmss; }
 
@@ -401,11 +415,11 @@ protected:
     uint64_t    _last_update_z_us;      // system time (in microseconds) since last update_z_controller call
     float       _tc_xy_s;               // time constant of the xy kinimatic path generation in seconds used to determine how quickly the aircraft varies the acceleration target
     float       _tc_z_s;                // time constant of the z kinimatic path generation in seconds used to determine how quickly the aircraft varies the acceleration target
-    float       _vel_max_xy_cms;        // max horizontal speed in cm/s
+    float       _vel_max_xy_cms;        // max horizontal speed in cm/s used for kinematic shaping
-    float       _vel_max_up_cms;        // max climb rate in cm/s
+    float       _vel_max_up_cms;        // max climb rate in cm/s used for kinematic shaping
-    float       _vel_max_down_cms;      // max descent rate in cm/s
+    float       _vel_max_down_cms;      // max descent rate in cm/s used for kinematic shaping
-    float       _accel_max_xy_cmss;     // max horizontal acceleration in cm/s/s
+    float       _accel_max_xy_cmss;     // max horizontal acceleration in cm/s/s used for kinematic shaping
-    float       _accel_max_z_cmss;      // max vertical acceleration in cm/s/s
+    float       _accel_max_z_cmss;      // max vertical acceleration in cm/s/s used for kinematic shaping
     float       _vel_z_control_ratio = 2.0f;    // confidence that we have control in the vertical axis
 
     // output from controller