From 0fff010046a3917319890f76b7ba48eedef623a5 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 31 Aug 2021 13:47:32 +0930 Subject: [PATCH] AC_AttitudeControl: AC_PosControl: Separate landing and terrain following. --- .../AC_AttitudeControl/AC_PosControl.cpp | 29 +++++++++++++++++-- libraries/AC_AttitudeControl/AC_PosControl.h | 13 +++++++-- 2 files changed, 37 insertions(+), 5 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index a881505e070ff..af56b501266e6 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -790,6 +790,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; @@ -855,11 +856,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. +/// 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) +{ + // 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::set_pos_target_z_from_climb_rate_cm(const float vel, bool ignore_descent_limit) +void AC_PosControl::land_at_climb_rate_cm(const float vel, bool ignore_descent_limit) { - float vel2 = vel; - input_vel_accel_z(vel2, 0, 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. diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index f3452fb9555a5..c4cab730ae042 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -196,10 +196,15 @@ class AC_PosControl /// 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 @@ class AC_PosControl /// 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 @@ class AC_PosControl 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