Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-26 18:48:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: AC_PosControl: change force_descend to ignore_descent_limit

Browse Source
This commit is contained in:
Leonard Hall 2021-06-22 12:38:34 +09:30 committed by Andrew Tridgell
parent 220662a905
commit b73182db3c
2 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -755,12 +755,13 @@ void AC_PosControl::init_z()
/// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_z and time constant. /// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_z and time constant.
/// The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration. /// The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
/// The time constant also defines the time taken to achieve the maximum acceleration. /// The time constant also defines the time taken to achieve the maximum acceleration.
void AC_PosControl::input_vel_accel_z(Vector3f& vel, const Vector3f& accel, bool force_descend) /// ignore_descent_limit turns off output saturation handling to aid in landing detection. ignore_descent_limit should be true unless landing.
void AC_PosControl::input_vel_accel_z(Vector3f& vel, const Vector3f& accel, bool ignore_descent_limit)
{ {
// check for ekf z position reset // check for ekf z position reset
handle_ekf_z_reset(); handle_ekf_z_reset();
if (force_descend) { if (ignore_descent_limit) {
// turn off limits in the negative z direction // turn off limits in the negative z direction
_limit_vector.z = MAX(_limit_vector.z, 0.0f); _limit_vector.z = MAX(_limit_vector.z, 0.0f);
} }
@ -788,10 +789,11 @@ void AC_PosControl::input_vel_accel_z(Vector3f& vel, const Vector3f& accel, bool
/// set_pos_target_z_from_climb_rate_cm - adjusts target up or down using a commanded climb rate in cm/s /// 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 kinimatic path. /// using the default position control kinimatic path.
void AC_PosControl::set_pos_target_z_from_climb_rate_cm(const float vel, bool force_descend) /// ignore_descent_limit turns off output saturation handling to aid in landing detection. ignore_descent_limit should be true unless landing.
void AC_PosControl::set_pos_target_z_from_climb_rate_cm(const float vel, bool ignore_descent_limit)
{ {
Vector3f vel_3f = Vector3f{0.0f, 0.0f, vel}; Vector3f vel_3f = Vector3f{0.0f, 0.0f, vel};
input_vel_accel_z(vel_3f, Vector3f{0.0f, 0.0f, 0.0f}, force_descend); input_vel_accel_z(vel_3f, Vector3f{0.0f, 0.0f, 0.0f}, ignore_descent_limit);
} }
/// input_pos_vel_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration. /// 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

@ -167,10 +167,12 @@ public:
/// The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration. /// The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
/// The time constant also defines the time taken to achieve the maximum acceleration. /// The time constant also defines the time taken to achieve the maximum acceleration.
/// The function alters the input velocitiy to be the velocity that the system could reach zero acceleration in the minimum time. /// The function alters the input velocitiy to be the velocity that the system could reach zero acceleration in the minimum time.
/// ignore_descent_limit turns off output saturation handling to aid in landing detection. ignore_descent_limit should be true unless landing.
virtual void input_vel_accel_z(Vector3f& vel, const Vector3f& accel, bool force_descend); virtual void input_vel_accel_z(Vector3f& vel, const Vector3f& accel, bool force_descend);
/// set_pos_target_z_from_climb_rate_cm - adjusts target up or down using a climb rate in cm/s /// set_pos_target_z_from_climb_rate_cm - adjusts target up or down using a climb rate in cm/s
/// using the default position control kinimatic path. /// using the default position control kinimatic path.
/// ignore_descent_limit turns off output saturation handling to aid in landing detection. ignore_descent_limit should be true unless landing.
void set_pos_target_z_from_climb_rate_cm(const float vel, bool force_descend); void set_pos_target_z_from_climb_rate_cm(const float vel, bool force_descend);
/// input_pos_vel_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration. /// input_pos_vel_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration.