AC_AttitudeControl: AC_PosControl: Separate landing and terrain following.

This commit is contained in:
Leonard Hall 2021-08-31 13:47:32 +09:30 committed by Randy Mackay
parent 11426792ef
commit 00ff3cec46
2 changed files with 38 additions and 6 deletions

View File

@ -805,6 +805,7 @@ void AC_PosControl::init_z()
_pid_accel_z.reset_filter();
// initialise vertical offsets
_pos_offset_target_z = 0.0;
_pos_offset_z = 0.0;
_vel_offset_z = 0.0;
_accel_offset_z = 0.0;
@ -870,11 +871,33 @@ void AC_PosControl::input_vel_accel_z(float &vel, const float accel, bool ignore
/// 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.
/// 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)
/// The zero target altitude is varied to follow pos_offset_z
void AC_PosControl::set_pos_target_z_from_climb_rate_cm(const float vel)
{
float vel2 = vel;
input_vel_accel_z(vel2, 0, ignore_descent_limit);
// remove terrain offsets for flat earth assumption
_pos_target.z -= _pos_offset_z;
_vel_desired.z -= _vel_offset_z;
_accel_desired.z -= _accel_offset_z;
float vel_temp = vel;
input_vel_accel_z(vel_temp, 0, false);
// update the vertical position, velocity and acceleration offsets
update_pos_offset_z(_pos_offset_target_z);
// add terrain offsets
_pos_target.z += _pos_offset_z;
_vel_desired.z += _vel_offset_z;
_accel_desired.z += _accel_offset_z;
}
/// land_at_climb_rate_cm - adjusts target up or down using a commanded climb rate in cm/s
/// using the default position control kinematic path.
/// ignore_descent_limit turns off output saturation handling to aid in landing detection. ignore_descent_limit should be true unless landing.
void AC_PosControl::land_at_climb_rate_cm(const float vel, bool ignore_descent_limit)
{
float vel_temp = vel;
input_vel_accel_z(vel_temp, 0, 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.

View File

@ -196,10 +196,15 @@ public:
/// 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, const float accel, bool ignore_descent_limit, bool limit_output = true);
/// 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 commanded climb rate in cm/s
/// using the default position control kinematic path.
/// The zero target altitude is varied to follow pos_offset_z
void set_pos_target_z_from_climb_rate_cm(const float vel);
/// land_at_climb_rate_cm - adjusts target up or down using a commanded climb rate in cm/s
/// using the default position control kinematic 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 ignore_descent_limit);
void land_at_climb_rate_cm(const float vel, bool 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.
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
@ -296,6 +301,9 @@ public:
/// Offset
/// set_pos_offset_target_z_cm - set altitude offset target in cm above home
void set_pos_offset_target_z_cm(float pos_offset_target_z) { _pos_offset_target_z = pos_offset_target_z; }
/// set_pos_offset_z_cm - set altitude offset in cm above home
void set_pos_offset_z_cm(float pos_offset_z) { _pos_offset_z = pos_offset_z; }
@ -461,6 +469,7 @@ protected:
Vector3f _accel_target; // acceleration target in NEU cm/s/s
Vector3f _limit_vector; // the direction that the position controller is limited, zero when not limited
Vector2f _vehicle_horiz_vel; // velocity to use if _flags.vehicle_horiz_vel_override is set
float _pos_offset_target_z; // vertical position offset target in NEU cm from home
float _pos_offset_z; // vertical position offset in NEU cm from home
float _vel_offset_z; // vertical velocity offset in NEU cm/s calculated by pos_to_rate step
float _accel_offset_z; // vertical acceleration offset in NEU cm/s/s