From 8a2f75d74280a57a14b30f9abf89b98ae8c2dd8e Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 8 Jul 2021 13:43:05 +0930 Subject: [PATCH] AC_AttitudeControl: AC_PosControl: seperate kinimatic shaping from pid limit setting --- .../AC_AttitudeControl/AC_PosControl.cpp | 34 ++++++++++++++----- libraries/AC_AttitudeControl/AC_PosControl.h | 24 ++++++++++--- 2 files changed, 45 insertions(+), 13 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 69f5fbdcaa..d01f84e61d 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/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 -/// speed_down can be positive or negative but will always be interpreted as a descent speed +/// 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 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); diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 592b957f68..d0decc7ca3 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/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_up_cms; // max climb rate in cm/s - float _vel_max_down_cms; // max descent rate in cm/s - float _accel_max_xy_cmss; // max horizontal acceleration in cm/s/s - float _accel_max_z_cmss; // max vertical acceleration in cm/s/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 used for kinematic shaping + 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 used for kinematic shaping + 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