From 6e326ee912b85142f31f35538089071d2bee8959 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Tue, 10 Aug 2021 14:28:00 -0300 Subject: [PATCH] AC_PosControl_Sub.h: do not use our own input_vel_accel_z --- .../AC_AttitudeControl/AC_PosControl_Sub.cpp | 35 ------------------- .../AC_AttitudeControl/AC_PosControl_Sub.h | 6 ---- 2 files changed, 41 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp b/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp index 0b88e82afb..d309c137fb 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp @@ -1,37 +1,2 @@ #include "AC_PosControl_Sub.h" -/// input_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration. -/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. -/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_z. -/// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction. -void AC_PosControl_Sub::input_vel_accel_z(float &vel, const float accel, bool force_descend, bool limit_output) -{ - // check for ekf z position reset - handle_ekf_z_reset(); - - // limit desired velocity to prevent breeching altitude limits - if (_alt_min < 0 && _alt_min < _alt_max && _alt_max < 100 && _pos_target.z < _alt_min) { - vel = constrain_float(vel, - sqrt_controller(_alt_min-_pos_target.z, 0.0f, _accel_max_z_cmss, 0.0f), - sqrt_controller(_alt_max-_pos_target.z, 0.0f, _accel_max_z_cmss, 0.0f)); - } - - // calculated increased maximum acceleration and jerk if over speed - float accel_max_z_cmss = _accel_max_z_cmss * calculate_overspeed_gain(); - float jerk_max_xy_cmsss = _jerk_max_xy_cmsss * calculate_overspeed_gain(); - - // adjust desired alt if motors have not hit their limits - update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z, _p_pos_z.get_error(), _pid_vel_z.get_error()); - - // prevent altitude target from breeching altitude limits - if (is_negative(_alt_min) && _alt_min < _alt_max && _alt_max < 100 && _pos_target.z < _alt_min) { - _pos_target.z = constrain_float(_pos_target.z, _alt_min, _alt_max); - } - - shape_vel_accel(vel, accel, - _vel_desired.z, _accel_desired.z, - -accel_max_z_cmss, accel_max_z_cmss, - jerk_max_xy_cmsss, _dt, limit_output); - - update_vel_accel(vel, accel, _dt, 0.0, 0.0); -} diff --git a/libraries/AC_AttitudeControl/AC_PosControl_Sub.h b/libraries/AC_AttitudeControl/AC_PosControl_Sub.h index 3d9c4f5f59..03c73aded4 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl_Sub.h +++ b/libraries/AC_AttitudeControl/AC_PosControl_Sub.h @@ -18,12 +18,6 @@ public: /// set to zero to disable limit void set_alt_min(float alt) { _alt_min = alt; } - /// input_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration. - /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. - /// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_z. - /// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction. - void input_vel_accel_z(float &vel, float accel, bool force_descend, bool limit_output = true) override; - private: float _alt_max; // max altitude - should be updated from the main code with altitude limit from fence float _alt_min; // min altitude - should be updated from the main code with altitude limit from fence