Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: AC_PosControl: Simplify and clarify use of vertical controllers

Browse Source
This commit is contained in:
Leonard Hall 2022-12-15 22:13:31 +10:30 committed by Randy Mackay
parent ac3a3d9576
commit 140dc61f2a
2 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -798,13 +798,8 @@ void AC_PosControl::input_accel_z(float accel)
/// 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::input_vel_accel_z(float &vel, float accel, bool ignore_descent_limit, bool limit_output)
void AC_PosControl::input_vel_accel_z(float &vel, float accel, bool limit_output)
{
if (ignore_descent_limit) {
// turn off limits in the negative z direction
_limit_vector.z = MAX(_limit_vector.z, 0.0f);
}
// calculated increased maximum acceleration and jerk if over speed
const float overspeed_gain = calculate_overspeed_gain();
const float accel_max_z_cmss = _accel_max_z_cmss * overspeed_gain;
@ -832,7 +827,7 @@ void AC_PosControl::set_pos_target_z_from_climb_rate_cm(float vel)
_accel_desired.z -= _accel_offset_z;
float vel_temp = vel;
input_vel_accel_z(vel_temp, 0, false);
input_vel_accel_z(vel_temp, 0.0);
// update the vertical position, velocity and acceleration offsets
update_pos_offset_z(_pos_offset_target_z);
@ -848,7 +843,12 @@ void AC_PosControl::set_pos_target_z_from_climb_rate_cm(float vel)
/// ignore_descent_limit turns off output saturation handling to aid in landing detection. ignore_descent_limit should be false unless landing.
void AC_PosControl::land_at_climb_rate_cm(float vel, bool ignore_descent_limit)
{
input_vel_accel_z(vel, 0, ignore_descent_limit);
if (ignore_descent_limit) {
// turn off limits in the negative z direction
_limit_vector.z = MAX(_limit_vector.z, 0.0f);
}
input_vel_accel_z(vel, 0.0);
}
/// input_pos_vel_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration.

2
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -201,7 +201,7 @@ public:
/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_z.
/// The function alters the vel to be the kinematic path based on accel
/// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
virtual void input_vel_accel_z(float &vel, float accel, bool ignore_descent_limit, bool limit_output = true);
virtual void input_vel_accel_z(float &vel, float accel, bool limit_output = true);
/// set_pos_target_z_from_climb_rate_cm - adjusts target up or down using a commanded climb rate in cm/s
/// using the default position control kinematic path.