Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: PosControl fixes

This commit is contained in:
Leonard Hall 2021-04-17 01:23:14 +09:30 committed by Andrew Tridgell
parent c4bb8baf0c
commit 92e05e8c4f
4 changed files with 1017 additions and 1080 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1388
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

File diff suppressed because it is too large Load Diff

549
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -17,9 +17,7 @@
// position controller default definitions
#define POSCONTROL_ACCELERATION_MIN 50.0f // minimum horizontal acceleration in cm/s/s - used for sanity checking acceleration in leash length calculation
#define POSCONTROL_ACCEL_XY 100.0f // default horizontal acceleration in cm/s/s. This is overwritten by waypoint and loiter controllers
#define POSCONTROL_ACCEL_XY_MAX (GRAVITY_MSS*100) // max horizontal acceleration in cm/s/s that the position velocity controller will ask from the lower accel controller
#define POSCONTROL_STOPPING_DIST_UP_MAX 300.0f // max stopping distance (in cm) vertically while climbing
#define POSCONTROL_STOPPING_DIST_DOWN_MAX 200.0f // max stopping distance (in cm) vertically while descending
@ -29,245 +27,253 @@
#define POSCONTROL_ACCEL_Z 250.0f // default vertical acceleration in cm/s/s.
#define POSCONTROL_LEASH_LENGTH_MIN 100.0f // minimum leash lengths in cm
#define POSCONTROL_VEL_ERROR_CUTOFF_FREQ 4.0f // low-pass filter on velocity error (unit: Hz)
#define POSCONTROL_THROTTLE_CUTOFF_FREQ 2.0f // low-pass filter on acceleration error (unit: Hz)
#define POSCONTROL_DT_50HZ 0.02f // time difference in seconds for 50hz update rate
#define POSCONTROL_DT_400HZ 0.0025f // time difference in seconds for 400hz update rate
#define POSCONTROL_ACTIVE_TIMEOUT_US 200000 // position controller is considered active if it has been called within the past 0.2 seconds
#define POSCONTROL_VEL_ERROR_CUTOFF_FREQ 4.0f // low-pass filter on velocity error (unit: hz)
#define POSCONTROL_THROTTLE_CUTOFF_FREQ 2.0f // low-pass filter on accel error (unit: hz)
#define POSCONTROL_ACCEL_FILTER_HZ 2.0f // low-pass filter on acceleration (unit: hz)
#define POSCONTROL_JERK_RATIO 1.0f // Defines the time it takes to reach the requested acceleration
#define POSCONTROL_DEFAULT_SHAPER_TC 0.25f // default time constant of the kinimatic path generation in seconds
#define POSCONTROL_OVERSPEED_GAIN_Z 2.0f // gain controlling rate at which z-axis speed is brought back within SPEED_UP and SPEED_DOWN range
#define POSCONTROL_RELAX_TC 0.16f // This is used to decay the relevant variable to 5% in half a second.
class AC_PosControl
{
public:
/// Constructor
AC_PosControl(AP_AHRS_View& ahrs, const AP_InertialNav& inav,
const AP_Motors& motors, AC_AttitudeControl& attitude_control);
const AP_Motors& motors, AC_AttitudeControl& attitude_control, float dt);
///
/// initialisation functions
///
/// set_dt - sets time delta in seconds for all controllers (i.e. 100hz = 0.01, 400hz = 0.0025)
/// updates z axis accel controller's D term filter
void set_dt(float delta_sec);
/// get_dt - gets time delta in seconds for all position controllers
float get_dt() const { return _dt; }
///
/// z position controller
///
/// set_max_speed_z - sets maximum climb and descent rates
/// speed_down can be positive or negative but will always be interpreted as a descent speed
/// leash length will be recalculated
void set_max_speed_z(float speed_down, float speed_up);
/// get_max_speed_up - accessor for current maximum up speed in cm/s
float get_max_speed_up() const { return _speed_up_cms; }
/// get_max_speed_down - accessors for current maximum down speed in cm/s. Will be a negative number
float get_max_speed_down() const { return _speed_down_cms; }
/// get_vel_target_z - returns current vertical speed in cm/s
float get_vel_target_z() const { return _vel_target.z; }
/// set_max_accel_z - set the maximum vertical acceleration in cm/s/s
/// leash length will be recalculated
void set_max_accel_z(float accel_cmss);
/// get_max_accel_z - returns current maximum vertical acceleration in cm/s/s
float get_max_accel_z() const { return _accel_z_cms; }
/// calc_leash_length - calculates the vertical leash lengths from maximum speed, acceleration
/// called by update_z_controller if z-axis speed or accelerations are changed
void calc_leash_length_z();
/// set_alt_target - set altitude target in cm above home
void set_alt_target(float alt_cm) { _pos_target.z = alt_cm; }
/// set_alt_target_with_slew - adjusts target towards a final altitude target
/// should be called continuously (with dt set to be the expected time between calls)
/// actual position target will be moved no faster than the speed_down and speed_up
/// target will also be stopped if the motors hit their limits or leash length is exceeded
void set_alt_target_with_slew(float alt_cm, float dt);
/// set_alt_target_from_climb_rate - adjusts target up or down using a climb rate in cm/s
/// should be called continuously (with dt set to be the expected time between calls)
/// actual position target will be moved no faster than the speed_down and speed_up
/// target will also be stopped if the motors hit their limits or leash length is exceeded
/// set force_descend to true during landing to allow target to move low enough to slow the motors
virtual void set_alt_target_from_climb_rate(float climb_rate_cms, float dt, bool force_descend);
/// set_alt_target_from_climb_rate_ff - adjusts target up or down using a climb rate in cm/s using feed-forward
/// should be called continuously (with dt set to be the expected time between calls)
/// actual position target will be moved no faster than the speed_down and speed_up
/// target will also be stopped if the motors hit their limits or leash length is exceeded
/// set force_descend to true during landing to allow target to move low enough to slow the motors
virtual void set_alt_target_from_climb_rate_ff(float climb_rate_cms, float dt, bool force_descend);
/// add_takeoff_climb_rate - adjusts alt target up or down using a climb rate in cm/s
/// should be called continuously (with dt set to be the expected time between calls)
/// almost no checks are performed on the input
void add_takeoff_climb_rate(float climb_rate_cms, float dt);
/// set_alt_target_to_current_alt - set altitude target to current altitude
void set_alt_target_to_current_alt() { _pos_target.z = _inav.get_altitude(); }
/// shift altitude target (positive means move altitude up)
void shift_alt_target(float z_cm);
/// relax_alt_hold_controllers - set all desired and targets to measured
void relax_alt_hold_controllers(float throttle_setting);
/// get_alt_target - get desired altitude (in cm above home) from loiter or wp controller which should be fed into throttle controller
float get_alt_target() const { return _pos_target.z; }
/// get_alt_error - returns altitude error in cm
float get_alt_error() const;
/// get_vel_z_error_ratio - returns the proportion of error relative to the maximum request
float get_vel_z_control_ratio() const { return constrain_float(_vel_z_control_ratio, 0.0f, 1.0f); }
/// set_target_to_stopping_point_z - sets altitude target to reasonable stopping altitude in cm above home
void set_target_to_stopping_point_z();
/// get_stopping_point_z - calculates stopping point based on current position, velocity, vehicle acceleration
void get_stopping_point_z(Vector3f& stopping_point) const;
/// init_takeoff - initialises target altitude if we are taking off
void init_takeoff();
// is_active - returns true if the z-axis position controller has been run very recently
bool is_active_z() const;
/// update_z_controller - fly to altitude in cm above home
void update_z_controller();
// get_leash_down_z, get_leash_up_z - returns vertical leash lengths in cm
float get_leash_down_z() const { return _leash_down_z; }
float get_leash_up_z() const { return _leash_up_z; }
///
/// xy position controller
/// 3D position shaper
///
/// get_lean_angle_max_cd - returns the maximum lean angle the autopilot may request
float get_lean_angle_max_cd() const;
/// input_vel_accel_xyz 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.
/// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_xy and time constant.
/// 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 function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
void input_pos_vel_accel_xyz(Vector3f& pos);
/// init_xy_controller - initialise the xy controller
/// sets target roll angle, pitch angle and I terms based on vehicle current lean angles
/// should be called once whenever significant changes to the position target are made
/// this does not update the xy target
///
/// Lateral position controller
///
/// set_max_speed_accel_xy - set the maximum horizontal speed in cm/s and acceleration in cm/s/s
/// If set accel_limit_cmss limits the maximum correction from the position controller to be less than the maximum lean angle
void set_max_speed_accel_xy(float speed_cms, float accel_cmss, float accel_limit_cmss = 0.0f);
void set_shaper_tc_xy(float tc_xy_s) {_tc_xy_s = MAX(_tc_xy_s, tc_xy_s);}
/// get_max_speed_xy_cms - get the maximum horizontal speed in cm/s
float get_max_speed_xy_cms() const { return _vel_max_xy_cms; }
/// get_max_accel_xy_cmss - get the maximum horizontal acceleration in cm/s/s
float get_max_accel_xy_cmss() const { return _accel_max_xy_cmss; }
// set the maximum horizontal position error that will be allowed in the horizontal plane
void set_pos_error_max_xy_cm(float error_max) { _p_pos_xy.set_error_max(error_max); }
float get_pos_error_max_xy_cm() { return _p_pos_xy.get_error_max(); }
/// init_xy_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
/// This function is the default initialisation for any position control that provides position, velocity and acceleration.
void init_xy_controller();
/// standby_xyz_reset - resets I terms and removes position error
/// This function will let Loiter and Alt Hold continue to operate
/// in the event that the flight controller is in control of the
/// aircraft when in standby.
void standby_xyz_reset();
/// init_xy_controller_stopping_point - initialise the position controller to the stopping point with zero velocity and acceleration.
/// This function should be used when the expected kinimatic path assumes a stationary initial condition but does not specify a specific starting position.
/// The starting position can be retrieved by getting the position target using get_pos_target_cm() after calling this function.
void init_xy_controller_stopping_point();
/// set_max_accel_xy - set the maximum horizontal acceleration in cm/s/s
/// leash length will be recalculated
void set_max_accel_xy(float accel_cmss);
float get_max_accel_xy() const { return _accel_cms; }
/// set_max_speed_xy - set the maximum horizontal speed maximum in cm/s
/// leash length will be recalculated
void set_max_speed_xy(float speed_cms);
float get_max_speed_xy() const { return _speed_cms; }
/// set_limit_accel_xy - mark that accel has been limited
/// this prevents integrator buildup
void set_limit_accel_xy(void) { _limit.accel_xy = true; }
/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration
/// should be called whenever the speed, acceleration or position kP is modified
void calc_leash_length_xy();
/// set the horizontal leash length
void set_leash_length_xy(float leash) { _leash = leash; _flags.recalc_leash_xy = false; }
/// get_pos_target - get target as position vector (from home in cm)
const Vector3f& get_pos_target() const { return _pos_target; }
/// set_pos_target in cm from home
void set_pos_target(const Vector3f& position);
/// set position, velocity and acceleration targets
void set_pos_vel_accel_target(const Vector3f& pos, const Vector3f& vel, const Vector3f& accel);
/// set_xy_target in cm from home
void set_xy_target(float x, float y);
/// shift position target target in x, y axis
void shift_pos_xy_target(float x_cm, float y_cm);
/// get_desired_velocity - returns xy desired velocity (i.e. feed forward) in cm/s in lat and lon direction
const Vector3f& get_desired_velocity() { return _vel_desired; }
/// set_desired_velocity_z - sets desired velocity in cm/s in z axis
void set_desired_velocity_z(float vel_z_cms) {_vel_desired.z = vel_z_cms;}
// set desired acceleration in cm/s in xy axis
void set_desired_accel_xy(float accel_lat_cms, float accel_lon_cms) { _accel_desired.x = accel_lat_cms; _accel_desired.y = accel_lon_cms; }
/// set_desired_velocity_xy - sets desired velocity in cm/s in lat and lon directions
/// when update_xy_controller is next called the position target is moved based on the desired velocity and
/// the desired velocities are fed forward into the rate_to_accel step
void set_desired_velocity_xy(float vel_lat_cms, float vel_lon_cms) {_vel_desired.x = vel_lat_cms; _vel_desired.y = vel_lon_cms; }
/// set_desired_velocity - sets desired velocity in cm/s in all 3 axis
/// when update_vel_controller_xyz is next called the position target is moved based on the desired velocity
void set_desired_velocity(const Vector3f &des_vel) { _vel_desired = des_vel; }
// overrides the velocity process variable for one timestep
void override_vehicle_velocity_xy(const Vector2f& vel_xy) { _vehicle_horiz_vel = vel_xy; _flags.vehicle_horiz_vel_override = true; }
// relax velocity controller by clearing velocity error and setting velocity target to current velocity
// relax_velocity_controller_xy - initialise the position controller to the current position and velocity with decaying acceleration.
/// This function decays the output acceleration by 95% every half second to achieve a smooth transition to zero requested acceleration.
void relax_velocity_controller_xy();
// is_active_xy - returns true if the xy position controller has been run very recently
/// input_vel_accel_xy calculate a jerk limited path from the current position, velocity and acceleration to an input 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.
/// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_xy and time constant.
/// 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 function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
void input_vel_accel_xy(Vector3f& vel, const Vector3f& accel);
/// input_vel_accel_xy 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.
/// The kinematic path is constrained by the maximum acceleration and time constant set using the function set_max_speed_accel_xy and time constant.
/// 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 function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
void input_pos_vel_accel_xy(Vector3f& pos, Vector3f& vel, const Vector3f& accel);
// is_active_xy - returns true if the xy position controller has been run in the previous 5 loop times
bool is_active_xy() const;
/// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher
/// when use_desired_velocity is true the desired velocity (i.e. feed forward) is incorporated at the pos_to_rate step
/// stop_pos_xy_stabilisation
void stop_pos_xy_stabilisation();
/// stop_vel_xy_stabilisation
void stop_vel_xy_stabilisation();
/// update_xy_controller - runs the horizontal position controller correcting position, velocity and acceleration errors.
/// Position and velocity errors are converted to velocity and acceleration targets using PID objects
/// Desired velocity and accelerations are added to these corrections as they are calculated
/// Kinematically consistent target position and desired velocity and accelerations should be provided before calling this function
void update_xy_controller();
/// set_target_to_stopping_point_xy - sets horizontal target to reasonable stopping position in cm from home
void set_target_to_stopping_point_xy();
///
/// Vertical position controller
///
/// get_stopping_point_xy - calculates stopping point based on current position, velocity, vehicle acceleration
/// results placed in stopping_position vector
/// set_accel_xy() should be called before this method to set vehicle acceleration
/// set_leash_length() should have been called before this method
void get_stopping_point_xy(Vector3f &stopping_point) const;
/// set_max_speed_accel_z - set the maximum horizontal speed in cm/s and acceleration in cm/s/s
/// speed_down can be positive or negative but will always be interpreted as a descent speed
void set_max_speed_accel_z(float speed_down, float speed_up, float accel_cmss);
/// get_pos_error - get position error vector between the current and target position
const Vector3f get_pos_error() const { return _pos_target - _inav.get_position(); }
/// get_max_accel_z_cmss - get the maximum vertical acceleration in cm/s/s
float get_max_accel_z_cmss() const { return _accel_max_z_cmss; }
/// get_pos_error_xy - get the length of the position error vector in the xy plane
float get_pos_error_xy() const { return norm(_pos_target.x - _inav.get_position().x, _pos_target.y - _inav.get_position().y); }
// set_pos_error_max_z - set the maximum vertical position error that will be allowed
void set_pos_error_max_z(float error_min, float error_max) { _p_pos_z.set_error_limits(error_min, error_max); }
/// get_bearing_to_target - get bearing to target position in centi-degrees
int32_t get_bearing_to_target() const;
// get_pos_error_max_z_cm - get the maximum vertical position error that will be allowed
float get_pos_error_max_z_cm() { return _p_pos_z.get_error_max(); }
float get_pos_error_min_z_cm() { return _p_pos_z.get_error_min(); }
/// xyz velocity controller
/// get_max_speed_up_cms - accessors for current maximum up speed in cm/s
float get_max_speed_up_cms() const { return _vel_max_up_cms; }
/// init_vel_controller_xyz - initialise the velocity controller - should be called once before the caller attempts to use the controller
void init_vel_controller_xyz();
/// get_max_speed_down_cms - accessors for current maximum down speed in cm/s. Will be a negative number
float get_max_speed_down_cms() const { return _vel_max_down_cms; }
/// update_velocity_controller_xyz - run the velocity controller - should be called at 100hz or higher
/// velocity targets should we set using set_desired_velocity_xyz() method
/// callers should use get_roll() and get_pitch() methods and sent to the attitude controller
/// throttle targets will be sent directly to the motors
void update_vel_controller_xyz();
/// init_z_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
/// This function is the default initialisation for any position control that provides position, velocity and acceleration.
void init_z_controller();
/// init_z_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
/// This function is the default initialisation for any position control that provides position, velocity and acceleration.
/// This function does not allow any negative velocity or acceleration
void init_z_controller_no_descent();
/// init_z_controller_stopping_point - initialise the position controller to the stopping point with zero velocity and acceleration.
/// This function should be used when the expected kinimatic path assumes a stationary initial condition but does not specify a specific starting position.
/// The starting position can be retrieved by getting the position target using get_pos_target_cm() after calling this function.
void init_z_controller_stopping_point();
// relax_z_controller - initialise the position controller to the current position and velocity with decaying acceleration.
/// This function decays the output acceleration by 95% every half second to achieve a smooth transition to zero requested acceleration.
void relax_z_controller(float throttle_setting);
/// input_vel_accel_z calculate a jerk limited path from the current position, velocity and acceleration to an input 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.
/// 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 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.
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
/// using the default position control kinimatic path.
void set_pos_target_z_from_climb_rate_cm(const float vel, bool force_descend);
/// input_vel_accel_xy 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.
/// 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 also defines the time taken to achieve the maximum acceleration.
/// The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
void input_pos_vel_accel_z(Vector3f& pos, Vector3f& vel, const Vector3f& accel);
/// set_alt_target_with_slew - adjusts target up or down using a commanded altitude in cm
/// using the default position control kinimatic path.
void set_alt_target_with_slew(const float& pos);
// is_active_z - returns true if the z position controller has been run in the previous 5 loop times
bool is_active_z() const;
/// update_z_controller - runs the vertical position controller correcting position, velocity and acceleration errors.
/// Position and velocity errors are converted to velocity and acceleration targets using PID objects
/// Desired velocity and accelerations are added to these corrections as they are calculated
/// Kinematically consistent target position and desired velocity and accelerations should be provided before calling this function
void update_z_controller();
///
/// Assessors
///
/// set commanded position (cm), velocity (cm/s) and acceleration (cm/s/s) inputs when the path is created externally.
void set_pos_vel_accel(const Vector3f& pos, const Vector3f& vel, const Vector3f& accel);
void set_pos_vel_accel_xy(const Vector2f& pos, const Vector2f& vel, const Vector2f& accel);
/// Position
/// set_pos_target_xy_cm - sets the position target in NEU cm from home
void set_pos_target_xy_cm(float pos_x, float pos_y) { _pos_target.x = pos_x; _pos_target.y = pos_y; }
/// get_pos_target_cm - returns the position target in NEU cm from home
const Vector3f& get_pos_target_cm() const { return _pos_target; }
/// set_pos_target_z_cm - set altitude target in cm above home
void set_pos_target_z_cm(float pos_target) { _pos_target.z = pos_target; }
/// get_pos_target_z_cm - get desired altitude (in cm above home)
float get_pos_target_z_cm() const { return _pos_target.z; }
/// get_stopping_point_xy_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration
void get_stopping_point_xy_cm(Vector3f &stopping_point) const;
/// get_stopping_point_z_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration
void get_stopping_point_z_cm(Vector3f& stopping_point) const;
/// get_pos_error_cm - get position error vector between the current and target position
const Vector3f get_pos_error_cm() const { return _pos_target - _inav.get_position(); }
/// get_pos_error_xy_cm - get the length of the position error vector in the xy plane
float get_pos_error_xy_cm() const { return norm(_pos_target.x - _inav.get_position().x, _pos_target.y - _inav.get_position().y); }
/// get_pos_error_z_cm - returns altitude error in cm
float get_pos_error_z_cm() const { return (_pos_target.z - _inav.get_position().z); }
/// Velocity
/// set_vel_desired_cms - sets desired velocity in NEU cm/s
void set_vel_desired_cms(const Vector3f &des_vel) { _vel_desired = des_vel; }
/// set_vel_desired_xy_cms - sets horizontal desired velocity in NEU cm/s
void set_vel_desired_xy_cms(float vel_x, float vel_y) {_vel_desired.x = vel_x; _vel_desired.y = vel_y; }
/// get_vel_desired_cms - returns desired velocity (i.e. feed forward) in cm/s in NEU
const Vector3f& get_vel_desired_cms() { return _vel_desired; }
// get_vel_target_cms - returns the target velocity in NEU cm/s
const Vector3f& get_vel_target_cms() const { return _vel_target; }
// get_vel_target_cms - returns the target velocity in NEU cm/s
const Vector3f& get_vel_error_cms() const { return _vel_error; }
/// set_vel_desired_z_cms - sets desired velocity in cm/s in z axis
void set_vel_desired_z_cms(float vel_z_cms) {_vel_desired.z = vel_z_cms;}
/// get_vel_target_z_cms - returns current vertical speed in cm/s
float get_vel_target_z_cms() const { return _vel_target.z; }
/// Acceleration
// set_accel_desired_xy_cmss set desired acceleration in cm/s in xy axis
void set_accel_desired_xy_cmss(float accel_x_cms, float accel_y_cms) { _accel_desired.x = accel_x_cms; _accel_desired.y = accel_y_cms; }
// get_accel_target_cmss - returns the target acceleration in NEU cm/s/s
const Vector3f& get_accel_target_cmss() const { return _accel_target; }
/// Outputs
/// get desired roll and pitch to be passed to the attitude controller
float get_roll_cd() const { return _roll_target; }
@ -279,15 +285,17 @@ public:
/// get desired yaw rate to be passed to the attitude controller
float get_yaw_rate_cds() const { return _yaw_rate_target; }
/// get desired roll and pitch to be passed to the attitude controller
Vector3f get_thrust_vector() const;
// lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s
bool calculate_yaw_and_rate_yaw();
/// get_bearing_to_target_cd - get bearing to target position in centi-degrees
int32_t get_bearing_to_target_cd() const;
// get_leash_xy - returns horizontal leash length in cm
float get_leash_xy() const { return _leash; }
/// get_lean_angle_max_cd - returns the maximum lean angle the autopilot may request
float get_lean_angle_max_cd() const;
/// Other
/// get pid controllers
AC_P_2D& get_pos_xy_p() { return _p_pos_xy; }
@ -296,18 +304,15 @@ public:
AC_PID_Basic& get_vel_z_pid() { return _pid_vel_z; }
AC_PID& get_accel_z_pid() { return _pid_accel_z; }
/// accessors for reporting
const Vector3f& get_vel_target() const { return _vel_target; }
const Vector3f& get_accel_target() const { return _accel_target; }
/// set_limit_accel_xy - mark that accel has been limited
/// this prevents integrator buildup
void set_limit_xy() { _limit_vector.x = _accel_target.x; _limit_vector.y = _accel_target.y; }
// overrides the velocity process variable for one timestep
void override_vehicle_velocity_xy(const Vector2f& vel_xy) { _vehicle_horiz_vel = vel_xy; _flags.vehicle_horiz_vel_override = true; }
// lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s
void accel_to_lean_angles(float accel_x_cmss, float accel_y_cmss, float& roll_target, float& pitch_target) const;
// lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s
void lean_angles_to_accel(float& accel_x_cmss, float& accel_y_cmss) const;
// time_since_last_xy_update - returns time in seconds since the horizontal position controller was last run
float time_since_last_xy_update() const;
Vector3f lean_angles_to_accel(const Vector3f& att_target_euler) const;
void write_log();
@ -319,39 +324,38 @@ public:
// enable or disable high vibration compensation
void set_vibe_comp(bool on_off) { _vibe_comp_enabled = on_off; }
/// get_vel_z_error_ratio - returns the proportion of error relative to the maximum request
float get_vel_z_control_ratio() const { return constrain_float(_vel_z_control_ratio, 0.0f, 1.0f); }
/// standby_xyz_reset - resets I terms and removes position error
/// This function will let Loiter and Alt Hold continue to operate
/// in the event that the flight controller is in control of the
/// aircraft when in standby.
void standby_xyz_reset();
static const struct AP_Param::GroupInfo var_info[];
protected:
// general purpose flags
struct poscontrol_flags {
uint16_t recalc_leash_z : 1; // 1 if we should recalculate the z axis leash length
uint16_t recalc_leash_xy : 1; // 1 if we should recalculate the xy axis leash length
uint16_t reset_desired_vel_to_pos : 1; // 1 if we should reset the rate_to_accel_xy step
uint16_t reset_accel_to_lean_xy : 1; // 1 if we should reset the accel to lean angle step
uint16_t reset_rate_to_accel_z : 1; // 1 if we should reset the rate_to_accel_z step
uint16_t vehicle_horiz_vel_override : 1; // 1 if we should use _vehicle_horiz_vel as our velocity process variable for one timestep
} _flags;
// limit flags structure
struct poscontrol_limit_flags {
bool pos_up; // true if we have hit the vertical position leash limit while going up
bool pos_down; // true if we have hit the vertical position leash limit while going down
bool vel_up; // true if we have hit the vertical velocity limit going up
bool vel_down; // true if we have hit the vertical velocity limit going down
bool accel_xy; // true if we have hit the horizontal accel limit
bool pos_xy; // true if we have hit a horizontal position limit
bool pos_up; // true if we have hit a vertical position limit while going up
bool pos_down; // true if we have hit a vertical position limit while going down
} _limit;
///
/// z controller private methods
///
/// init_xy - initialise the position controller to the current position, velocity and acceleration.
/// This function is private and contains all the shared xy axis intialisaion functions
void init_xy();
// run position control for Z axis
// target altitude should be set with one of these functions
// set_alt_target
// set_target_to_stopping_point_z
// init_takeoff
void run_z_controller();
/// init_z - initialise the position controller to the current position, velocity and acceleration.
/// This function is private and contains all the shared z axis intialisaion functions
void init_z();
// get throttle using vibration-resistant calculation (uses feed forward with manually calculated gain)
float get_throttle_with_vibration_override();
@ -359,25 +363,14 @@ protected:
// get earth-frame Z-axis acceleration with gravity removed in cm/s/s with +ve being up
float get_z_accel_cmss() const { return -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f; }
///
/// xy controller private methods
///
// lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s
void accel_to_lean_angles(float accel_x_cmss, float accel_y_cmss, float& roll_target, float& pitch_target) const;
/// move velocity target using desired acceleration
void desired_accel_to_vel(float nav_dt);
// lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s
void lean_angles_to_accel_xy(float& accel_x_cmss, float& accel_y_cmss) const;
/// desired_vel_to_pos - move position target using desired velocities
void desired_vel_to_pos(float nav_dt);
/// run horizontal position controller correcting position and velocity
/// converts position (_pos_target) to target velocity (_vel_target)
/// desired velocity (_vel_desired) is combined into final target velocity
/// converts desired velocities in lat/lon directions to accelerations in lat/lon frame
/// converts desired accelerations provided in lat/lon frame to roll/pitch angles
void run_xy_controller(float dt);
/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration and position kP gain
float calc_leash_length(float speed_cms, float accel_cms, float kP) const;
// calculate_yaw_and_rate_yaw - calculate the vehicle yaw and rate of yaw.
bool calculate_yaw_and_rate_yaw();
/// initialise and check for ekf position resets
void init_ekf_xy_reset();
@ -392,28 +385,28 @@ protected:
AC_AttitudeControl& _attitude_control;
// parameters
AP_Float _accel_xy_filt_hz; // XY acceleration filter cutoff frequency
AP_Float _lean_angle_max; // Maximum autopilot commanded angle (in degrees). Set to zero for Angle Max
AC_P_2D _p_pos_xy; // XY axis position controller to convert distance error to desired velocity
AC_P_1D _p_pos_z; // Z axis position controller to convert altitude error to desired climb rate
AC_PID_2D _pid_vel_xy; // XY axis velocity controller to convert velocity error to desired acceleration
AC_PID_Basic _pid_vel_z; // Z axis velocity controller to convert climb rate error to desired acceleration
AC_PID _pid_accel_z; // Z axis acceleration controller to convert desired acceleration to throttle output
AP_Float _lean_angle_max; // Maximum autopilot commanded angle (in degrees). Set to zero for Angle Max
AP_Float _shaping_tc_xy_s; // time constant of the xy kinimatic path generation in seconds used to determine how quickly the aircraft varies the acceleration target
AP_Float _shaping_tc_z_s; // time constant of the z kinimatic path generation in seconds used to determine how quickly the aircraft varies the acceleration target
AC_P_2D _p_pos_xy; // XY axis position controller to convert distance error to desired velocity
AC_P_1D _p_pos_z; // Z axis position controller to convert altitude error to desired climb rate
AC_PID_2D _pid_vel_xy; // XY axis velocity controller to convert velocity error to desired acceleration
AC_PID_Basic _pid_vel_z; // Z axis velocity controller to convert climb rate error to desired acceleration
AC_PID _pid_accel_z; // Z axis acceleration controller to convert desired acceleration to throttle output
// internal variables
float _dt; // time difference (in seconds) between calls from the main program
uint64_t _last_update_xy_us; // system time (in microseconds) since last update_xy_controller call
uint64_t _last_update_z_us; // system time (in microseconds) of last update_z_controller call
float _speed_down_cms; // max descent rate in cm/s
float _speed_up_cms; // max climb rate in cm/s
float _speed_cms; // max horizontal speed in cm/s
float _accel_z_cms; // max vertical acceleration in cm/s/s
float _accel_last_z_cms; // max vertical acceleration in cm/s/s
float _accel_cms; // max horizontal acceleration in cm/s/s
float _leash; // horizontal leash length in cm. target will never be further than this distance from the vehicle
float _leash_down_z; // vertical leash down in cm. target will never be further than this distance below the vehicle
float _leash_up_z; // vertical leash up in cm. target will never be further than this distance above the vehicle
float _vel_z_control_ratio = 2.0f; // confidence that we have control in the vertical axis
uint64_t _last_update_z_us; // system time (in microseconds) since last update_z_controller call
float _tc_xy_s; // time constant of the xy kinimatic path generation in seconds used to determine how quickly the aircraft varies the acceleration target
float _tc_z_s; // time constant of the z kinimatic path generation in seconds used to determine how quickly the aircraft varies the acceleration target
float _vel_max_xy_cms; // max horizontal speed in cm/s
float _vel_max_up_cms; // max climb rate in cm/s
float _vel_max_down_cms; // max descent rate in cm/s
float _accel_max_xy_cmss; // max horizontal acceleration in cm/s/s
float _accel_limit_xy_cmss; // max horizontal acceleration in cm/s/s
float _accel_max_z_cmss; // max vertical acceleration in cm/s/s
float _vel_z_control_ratio = 2.0f; // confidence that we have control in the vertical axis
// output from controller
float _roll_target; // desired roll angle in centi-degrees calculated by position controller
@ -429,10 +422,8 @@ protected:
Vector3f _accel_desired; // desired acceleration in NEU cm/s/s (feed forward)
Vector3f _accel_target; // acceleration target in NEU cm/s/s
Vector3f _accel_error; // acceleration error in NEU cm/s/s
Vector3f _limit_vector; //
Vector2f _vehicle_horiz_vel; // velocity to use if _flags.vehicle_horiz_vel_override is set
LowPassFilterFloat _vel_error_filter; // low-pass-filter on z-axis velocity error
LowPassFilterVector2f _accel_target_filter; // acceleration target filter
// ekf reset handling
uint32_t _ekf_xy_reset_ms; // system time of last recorded ekf xy position reset

119
libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp
View File

@ -1,101 +1,56 @@
#include "AC_PosControl_Sub.h"
AC_PosControl_Sub::AC_PosControl_Sub(AP_AHRS_View& ahrs, const AP_InertialNav& inav,
const AP_Motors& motors, AC_AttitudeControl& attitude_control) :
AC_PosControl(ahrs, inav, motors, attitude_control),
const AP_Motors& motors, AC_AttitudeControl& attitude_control, float dt) :
AC_PosControl(ahrs, inav, motors, attitude_control, dt),
_alt_max(0.0f),
_alt_min(0.0f)
{}
/// set_alt_target_from_climb_rate - adjusts target up or down using a climb rate in cm/s
/// should be called continuously (with dt set to be the expected time between calls)
/// actual position target will be moved no faster than the speed_down and speed_up
/// target will also be stopped if the motors hit their limits or leash length is exceeded
void AC_PosControl_Sub::set_alt_target_from_climb_rate(float climb_rate_cms, float dt, bool force_descend)
/// input_vel_z calculate a jerk limited path from the current position, velocity and acceleration to an input velocity.
/// 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 :
/// maximum velocity - vel_max,
/// maximum acceleration - accel_max,
/// time constant - tc.
/// 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 must be positive.
/// The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
void AC_PosControl_Sub::input_vel_accel_z(Vector3f& vel, const Vector3f& accel, bool force_descend)
{
// adjust desired alt if motors have not hit their limits
// To-Do: add check of _limit.pos_down?
if ((climb_rate_cms<0 && (!_motors.limit.throttle_lower || force_descend)) || (climb_rate_cms>0 && !_motors.limit.throttle_upper && !_limit.pos_up)) {
_pos_target.z += climb_rate_cms * dt;
// check for ekf z position reset
check_for_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.z = constrain_float(vel.z,
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));
}
// do not let target alt get above limit
if (_alt_max < 100 && _pos_target.z > _alt_max) {
_pos_target.z = _alt_max;
_limit.pos_up = true;
}
// do not let target alt get below limit
if (_alt_min < 0 && _alt_min < _alt_max && _pos_target.z < _alt_min) {
_pos_target.z = _alt_min;
_limit.pos_down = true;
}
// do not use z-axis desired velocity feed forward
// vel_desired set to desired climb rate for reporting and land-detector
_vel_desired.z = 0.0f;
}
/// set_alt_target_from_climb_rate_ff - adjusts target up or down using a climb rate in cm/s using feed-forward
/// should be called continuously (with dt set to be the expected time between calls)
/// actual position target will be moved no faster than the speed_down and speed_up
/// target will also be stopped if the motors hit their limits or leash length is exceeded
/// set force_descend to true during landing to allow target to move low enough to slow the motors
void AC_PosControl_Sub::set_alt_target_from_climb_rate_ff(float climb_rate_cms, float dt, bool force_descend)
{
// calculated increased maximum acceleration if over speed
float accel_z_cms = _accel_z_cms;
if (_vel_desired.z < _speed_down_cms && !is_zero(_speed_down_cms)) {
accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _speed_down_cms;
float accel_z_cms = _accel_max_z_cmss;
if (_vel_desired.z < _vel_max_down_cms && !is_zero(_vel_max_down_cms)) {
accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _vel_max_down_cms;
}
if (_vel_desired.z > _speed_up_cms && !is_zero(_speed_up_cms)) {
accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _speed_up_cms;
if (_vel_desired.z > _vel_max_up_cms && !is_zero(_vel_max_up_cms)) {
accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _vel_max_up_cms;
}
accel_z_cms = constrain_float(accel_z_cms, 0.0f, 750.0f);
// jerk_z is calculated to reach full acceleration in 1000ms.
float jerk_z = accel_z_cms * POSCONTROL_JERK_RATIO;
float accel_z_max = MIN(accel_z_cms, safe_sqrt(2.0f*fabsf(_vel_desired.z - climb_rate_cms)*jerk_z));
_accel_last_z_cms += jerk_z * dt;
_accel_last_z_cms = MIN(accel_z_max, _accel_last_z_cms);
float vel_change_limit = _accel_last_z_cms * dt;
_vel_desired.z = constrain_float(climb_rate_cms, _vel_desired.z-vel_change_limit, _vel_desired.z+vel_change_limit);
// adjust desired alt if motors have not hit their limits
// To-Do: add check of _limit.pos_down?
if ((_vel_desired.z<0 && (!_motors.limit.throttle_lower || force_descend)) || (_vel_desired.z>0 && !_motors.limit.throttle_upper && !_limit.pos_up)) {
_pos_target.z += _vel_desired.z * dt;
update_pos_vel_accel_z(_pos_target, _vel_desired, _accel_desired, _dt, _limit_vector);
// prevent altitude target from breeching altitude limits
if (_alt_min < 0 && _alt_min < _alt_max && _alt_max < 100 && _pos_target.z < _alt_min) {
_pos_target.z = constrain_float(_pos_target.z, _alt_min, _alt_max);
}
// do not let target alt get above limit
if (_alt_max < 100 && _pos_target.z > _alt_max) {
_pos_target.z = _alt_max;
_limit.pos_up = true;
// decelerate feed forward to zero
_vel_desired.z = constrain_float(0.0f, _vel_desired.z-vel_change_limit, _vel_desired.z+vel_change_limit);
}
shape_vel_accel(vel.z, accel.z,
_vel_desired.z, _accel_desired.z,
_vel_max_down_cms, _vel_max_up_cms,
-accel_z_cms, accel_z_cms,
_tc_z_s, _dt);
// do not let target alt get below limit
if (_alt_min < 0 && _alt_min < _alt_max && _pos_target.z < _alt_min) {
_pos_target.z = _alt_min;
_limit.pos_down = true;
// decelerate feed forward to zero
_vel_desired.z = constrain_float(0.0f, _vel_desired.z-vel_change_limit, _vel_desired.z+vel_change_limit);
}
}
/// relax_alt_hold_controllers - set all desired and targets to measured
void AC_PosControl_Sub::relax_alt_hold_controllers()
{
_pos_target.z = _inav.get_altitude();
_vel_desired.z = 0.0f;
_vel_target.z = _inav.get_velocity_z();
_accel_desired.z = 0.0f;
_accel_last_z_cms = 0.0f;
_flags.reset_rate_to_accel_z = true;
_accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;
_pid_accel_z.reset_filter();
update_vel_accel_z(vel, accel, _dt, _limit_vector);
}

41
libraries/AC_AttitudeControl/AC_PosControl_Sub.h
View File

@ -2,43 +2,34 @@
#include "AC_PosControl.h"
#define POSCONTROL_JERK_RATIO 1.0f // Defines the time it takes to reach the requested acceleration
class AC_PosControl_Sub : public AC_PosControl {
public:
AC_PosControl_Sub(AP_AHRS_View & ahrs, const AP_InertialNav& inav,
const AP_Motors& motors, AC_AttitudeControl& attitude_control);
const AP_Motors& motors, AC_AttitudeControl& attitude_control, float dt);
/// set_alt_max - sets maximum altitude above the ekf origin in cm
/// only enforced when set_alt_target_from_climb_rate is used
/// only enforced when set_pos_target_z_from_climb_rate_cm is used
/// set to zero to disable limit
void set_alt_max(float alt) { _alt_max = alt; }
/// set_alt_min - sets the minimum altitude (maximum depth) in cm
/// only enforced when set_alt_target_from_climb_rate is used
/// only enforced when set_pos_target_z_from_climb_rate_cm is used
/// set to zero to disable limit
void set_alt_min(float alt) { _alt_min = alt; }
/// set_alt_target_from_climb_rate - adjusts target up or down using a climb rate in cm/s
/// should be called continuously (with dt set to be the expected time between calls)
/// actual position target will be moved no faster than the speed_down and speed_up
/// target will also be stopped if the motors hit their limits or leash length is exceeded
/// set force_descend to true during landing to allow target to move low enough to slow the motors
void set_alt_target_from_climb_rate(float climb_rate_cms, float dt, bool force_descend) override;
/// set_alt_target_from_climb_rate_ff - adjusts target up or down using a climb rate in cm/s using feed-forward
/// should be called continuously (with dt set to be the expected time between calls)
/// actual position target will be moved no faster than the speed_down and speed_up
/// target will also be stopped if the motors hit their limits or leash length is exceeded
/// set force_descend to true during landing to allow target to move low enough to slow the motors
void set_alt_target_from_climb_rate_ff(float climb_rate_cms, float dt, bool force_descend) override;
/// relax_alt_hold_controllers - set all desired and targets to measured
void relax_alt_hold_controllers(float throttle_setting) {
AC_PosControl::relax_alt_hold_controllers(throttle_setting);
}
/// relax_alt_hold_controllers - set all desired and targets to measured
/// integrator is not reset
void relax_alt_hold_controllers();
/// input_vel_z calculate a jerk limited path from the current position, velocity and acceleration to an input velocity.
/// 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 :
/// maximum velocity - vel_max,
/// maximum acceleration - accel_max,
/// time constant - tc.
/// 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 must be positive.
/// The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
void input_vel_accel_z(Vector3f& vel, const Vector3f& accel, bool force_descend) override;
private:
float _alt_max; // max altitude - should be updated from the main code with altitude limit from fence