2016-02-17 21:24:42 -04:00
# pragma once
2013-12-28 07:15:29 -04:00
2015-08-11 03:28:41 -03:00
# include <AP_Common/AP_Common.h>
# include <AP_Param/AP_Param.h>
# include <AP_Math/AP_Math.h>
2021-05-12 01:17:38 -03:00
# include <AC_PID/AC_P.h> // P library
# include <AC_PID/AC_PID.h> // PID library
# include <AC_PID/AC_P_1D.h> // P library (1-axis)
# include <AC_PID/AC_P_2D.h> // P library (2-axis)
# include <AC_PID/AC_PI_2D.h> // PI library (2-axis)
# include <AC_PID/AC_PID_Basic.h> // PID library (1-axis)
# include <AC_PID/AC_PID_2D.h> // PID library (2-axis)
# include <AP_InertialNav/AP_InertialNav.h> // Inertial Navigation library
2024-08-02 01:45:17 -03:00
# include <AP_Scripting/AP_Scripting_config.h>
2021-05-12 01:17:38 -03:00
# include "AC_AttitudeControl.h" // Attitude control library
2013-12-28 07:15:29 -04:00
2024-02-28 08:23:13 -04:00
# include <AP_Logger/LogStructure.h>
2013-12-28 10:04:45 -04:00
// position controller default definitions
2014-04-21 09:34:33 -03:00
# define POSCONTROL_ACCEL_XY 100.0f // default horizontal acceleration in cm/s/s. This is overwritten by waypoint and loiter controllers
2021-08-06 06:40:56 -03:00
# define POSCONTROL_JERK_XY 5.0f // default horizontal jerk m/s/s/s
2017-04-27 05:30:40 -03:00
# 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
2013-12-28 07:15:29 -04:00
2014-04-21 09:34:33 -03:00
# define POSCONTROL_SPEED 500.0f // default horizontal speed in cm/s
# define POSCONTROL_SPEED_DOWN -150.0f // default descent rate in cm/s
# define POSCONTROL_SPEED_UP 250.0f // default climb rate in cm/s
2013-12-28 07:15:29 -04:00
2014-04-21 09:34:33 -03:00
# define POSCONTROL_ACCEL_Z 250.0f // default vertical acceleration in cm/s/s.
2021-08-06 06:40:56 -03:00
# define POSCONTROL_JERK_Z 5.0f // default vertical jerk m/s/s/s
2013-12-28 07:15:29 -04:00
2021-05-19 11:09:31 -03:00
# define POSCONTROL_THROTTLE_CUTOFF_FREQ_HZ 2.0f // low-pass filter on acceleration error (unit: Hz)
2013-12-28 07:15:29 -04:00
2015-10-27 10:45:16 -03:00
# 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
2022-01-24 19:43:19 -04:00
# define POSCONTROL_RELAX_TC 0.16f // This is used to decay the I term to 5% in half a second.
2021-04-16 12:53:14 -03:00
2013-12-28 07:15:29 -04:00
class AC_PosControl
{
public :
/// Constructor
2019-12-10 05:31:08 -04:00
AC_PosControl ( AP_AHRS_View & ahrs , const AP_InertialNav & inav ,
2022-12-05 08:21:43 -04:00
const class AP_Motors & motors , AC_AttitudeControl & attitude_control ) ;
2013-12-28 07:15:29 -04:00
2024-09-17 00:54:55 -03:00
// do not allow copying
CLASS_NO_COPY ( AC_PosControl ) ;
2022-12-05 08:21:43 -04:00
/// set_dt / get_dt - dt is the time since the last time the position controllers were updated
/// _dt should be set based on the time of the last IMU read used by these controllers
/// the position controller should run updates for active controllers on each loop to ensure normal operation
void set_dt ( float dt ) { _dt = dt ; }
2013-12-30 09:12:59 -04:00
float get_dt ( ) const { return _dt ; }
2013-12-28 07:15:29 -04:00
2021-08-06 06:40:56 -03:00
/// get_shaping_jerk_xy_cmsss - gets the jerk limit of the xy kinematic path generation in cm/s/s/s
2024-08-02 01:45:17 -03:00
float get_shaping_jerk_xy_cmsss ( ) const { return _shaping_jerk_xy * 100.0 ; }
2021-06-21 06:44:50 -03:00
2021-04-16 12:53:14 -03:00
2014-01-17 22:53:46 -04:00
///
2021-04-16 12:53:14 -03:00
/// 3D position shaper
2014-01-17 22:53:46 -04:00
///
2021-05-30 21:57:52 -03:00
/// input_pos_xyz - calculate a jerk limited path from the current position, velocity and acceleration to an input position.
2021-04-16 12:53:14 -03:00
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
2021-08-06 06:40:56 -03:00
/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_xy.
2024-08-02 01:45:17 -03:00
void input_pos_xyz ( const Vector3p & pos , float pos_terrain_target , float terrain_buffer ) ;
2021-07-08 04:09:05 -03:00
/// pos_offset_z_scaler - calculates a multiplier used to reduce the horizontal velocity to allow the z position controller to stay within the provided buffer range
2021-07-14 11:18:45 -03:00
float pos_offset_z_scaler ( float pos_offset_z , float pos_offset_z_buffer ) const ;
2014-01-24 02:46:45 -04:00
2021-04-16 12:53:14 -03:00
///
/// Lateral position controller
///
2014-01-23 23:27:06 -04:00
2021-04-16 12:53:14 -03:00
/// set_max_speed_accel_xy - set the maximum horizontal speed in cm/s and acceleration in cm/s/s
2021-08-06 06:40:56 -03:00
/// This function only needs to be called if using the kinematic shaping.
2021-07-08 01:13:05 -03:00
/// This can be done at any time as changes in these parameters are handled smoothly
2021-08-06 06:40:56 -03:00
/// by the kinematic shaping.
2021-06-20 05:17:25 -03:00
void set_max_speed_accel_xy ( float speed_cms , float accel_cmss ) ;
2014-01-05 23:30:51 -04:00
2021-07-08 01:13:05 -03:00
/// set_max_speed_accel_xy - set the position controller correction velocity and acceleration limit
/// This should be done only during initialisation to avoid discontinuities
void set_correction_speed_accel_xy ( float speed_cms , float accel_cmss ) ;
2021-04-16 12:53:14 -03:00
/// 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 ; }
2014-01-23 23:27:06 -04:00
2021-04-16 12:53:14 -03:00
/// 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 ; }
2015-10-27 10:04:24 -03:00
2021-04-16 12:53:14 -03:00
// 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 ( ) ; }
2015-04-30 10:18:05 -03:00
2021-04-16 12:53:14 -03:00
/// init_xy_controller_stopping_point - initialise the position controller to the stopping point with zero velocity and acceleration.
2021-08-06 06:40:56 -03:00
/// This function should be used when the expected kinematic path assumes a stationary initial condition but does not specify a specific starting position.
2021-04-16 12:53:14 -03:00
/// 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 ( ) ;
2016-11-22 01:36:17 -04:00
2021-04-16 12:53:14 -03:00
// 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 ( ) ;
2015-04-14 11:26:33 -03:00
2021-12-23 10:24:54 -04:00
/// reduce response for landing
void soften_for_landing_xy ( ) ;
2022-01-01 07:18:00 -04:00
// 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.
/// This function is private and contains all the shared xy axis initialisation functions
void init_xy_controller ( ) ;
2021-06-20 03:02:24 -03:00
/// input_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input 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.
2021-08-06 06:40:56 -03:00
/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_xy.
/// The jerk limit defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
/// The jerk limit also defines the time taken to achieve the maximum acceleration.
2021-06-20 03:02:24 -03:00
void input_accel_xy ( const Vector3f & accel ) ;
2021-05-19 11:09:31 -03:00
/// input_vel_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration.
2021-04-16 12:53:14 -03:00
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
2021-08-06 06:40:56 -03:00
/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_xy.
2021-06-22 21:03:09 -03:00
/// The function alters the vel to be the kinematic path based on accel
2021-08-06 06:40:56 -03:00
/// 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 input_vel_accel_xy ( Vector2f & vel , const Vector2f & accel , bool limit_output = true ) ;
2021-04-16 12:53:14 -03:00
2021-05-19 11:09:31 -03:00
/// input_pos_vel_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration.
2021-04-16 12:53:14 -03:00
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
2021-08-06 06:40:56 -03:00
/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_xy.
2021-06-22 21:03:09 -03:00
/// The function alters the pos and vel to be the kinematic path based on accel
2021-08-06 06:40:56 -03:00
/// 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 input_pos_vel_accel_xy ( Vector2p & pos , Vector2f & vel , const Vector2f & accel , bool limit_output = true ) ;
2021-04-16 12:53:14 -03:00
// is_active_xy - returns true if the xy position controller has been run in the previous 5 loop times
bool is_active_xy ( ) const ;
2014-01-05 23:30:51 -04:00
2021-05-19 11:09:31 -03:00
/// stop_pos_xy_stabilisation - sets the target to the current position to remove any position corrections from the system
2021-04-16 12:53:14 -03:00
void stop_pos_xy_stabilisation ( ) ;
2019-04-19 08:36:42 -03:00
2021-05-19 11:09:31 -03:00
/// stop_vel_xy_stabilisation - sets the target to the current position and velocity to the current velocity to remove any position and velocity corrections from the system
2021-04-16 12:53:14 -03:00
void stop_vel_xy_stabilisation ( ) ;
2013-12-30 09:12:59 -04:00
2021-04-16 12:53:14 -03:00
/// 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 ( ) ;
2014-01-25 04:23:55 -04:00
2021-04-16 12:53:14 -03:00
///
/// Vertical position controller
///
2013-12-28 07:15:29 -04:00
2021-05-19 11:09:31 -03:00
/// set_max_speed_accel_z - set the maximum vertical speed in cm/s and acceleration in cm/s/s
2021-04-16 12:53:14 -03:00
/// speed_down can be positive or negative but will always be interpreted as a descent speed
2021-07-08 01:13:05 -03:00
/// This can be done at any time as changes in these parameters are handled smoothly
2021-08-06 06:40:56 -03:00
/// by the kinematic shaping.
2021-04-16 12:53:14 -03:00
void set_max_speed_accel_z ( float speed_down , float speed_up , float accel_cmss ) ;
2021-07-08 01:13:05 -03:00
/// set_correction_speed_accel_z - set the position controller correction velocity and acceleration limit
/// speed_down can be positive or negative but will always be interpreted as a descent speed
/// This should be done only during initialisation to avoid discontinuities
void set_correction_speed_accel_z ( float speed_down , float speed_up , float accel_cmss ) ;
2021-04-16 12:53:14 -03:00
/// 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 ; }
2021-05-19 11:09:31 -03:00
// get_pos_error_z_up_cm - get the maximum vertical position error up that will be allowed
float get_pos_error_z_up_cm ( ) { return _p_pos_z . get_error_max ( ) ; }
2021-04-16 12:53:14 -03:00
2021-05-19 11:09:31 -03:00
// get_pos_error_z_down_cm - get the maximum vertical position error down that will be allowed
float get_pos_error_z_down_cm ( ) { return _p_pos_z . get_error_min ( ) ; }
2021-04-16 12:53:14 -03:00
/// 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 ; }
/// 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 ; }
2021-05-19 11:09:31 -03:00
/// init_z_controller_no_descent - initialise the position controller to the current position, velocity, acceleration and attitude.
2021-04-16 12:53:14 -03:00
/// 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.
2021-08-06 06:40:56 -03:00
/// This function should be used when the expected kinematic path assumes a stationary initial condition but does not specify a specific starting position.
2021-04-16 12:53:14 -03:00
/// 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 ) ;
2022-01-01 07:18:00 -04:00
// 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 is private and contains all the shared z axis initialisation functions
void init_z_controller ( ) ;
2021-07-09 02:28:58 -03:00
/// input_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input 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.
2021-08-06 06:40:56 -03:00
/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_z.
2021-09-03 00:42:32 -03:00
virtual void input_accel_z ( float accel ) ;
2021-07-09 02:28:58 -03:00
2021-05-19 11:09:31 -03:00
/// input_vel_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration.
2021-04-16 12:53:14 -03:00
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
2021-08-06 06:40:56 -03:00
/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_z.
2021-06-22 21:03:09 -03:00
/// The function alters the vel to be the kinematic path based on accel
2021-08-06 06:40:56 -03:00
/// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
2022-12-15 07:43:31 -04:00
virtual void input_vel_accel_z ( float & vel , float accel , bool limit_output = true ) ;
2021-04-16 12:53:14 -03:00
2021-08-31 01:17:32 -03:00
/// 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
2021-09-03 00:42:32 -03:00
void set_pos_target_z_from_climb_rate_cm ( float vel ) ;
2021-08-31 01:17:32 -03:00
/// land_at_climb_rate_cm - adjusts target up or down using a commanded climb rate in cm/s
2021-08-06 06:40:56 -03:00
/// using the default position control kinematic path.
2021-06-22 00:08:34 -03:00
/// ignore_descent_limit turns off output saturation handling to aid in landing detection. ignore_descent_limit should be true unless landing.
2021-09-03 00:42:32 -03:00
void land_at_climb_rate_cm ( float vel , bool ignore_descent_limit ) ;
2021-04-16 12:53:14 -03:00
2021-05-19 11:09:31 -03:00
/// input_pos_vel_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration.
2021-04-16 12:53:14 -03:00
/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
2021-06-22 21:03:09 -03:00
/// The function alters the pos and vel to be the kinematic path based on accel
2021-08-06 06:40:56 -03:00
/// 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 input_pos_vel_accel_z ( float & pos , float & vel , float accel , bool limit_output = true ) ;
2021-04-16 12:53:14 -03:00
/// set_alt_target_with_slew - adjusts target up or down using a commanded altitude in cm
2021-08-06 06:40:56 -03:00
/// using the default position control kinematic path.
2021-09-03 00:42:32 -03:00
void set_alt_target_with_slew ( float pos ) ;
2021-04-16 12:53:14 -03:00
// is_active_z - returns true if the z position controller has been run in the previous 5 loop times
2014-05-02 00:08:18 -03:00
bool is_active_z ( ) const ;
2021-04-16 12:53:14 -03:00
/// 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
2014-01-17 22:53:46 -04:00
void update_z_controller ( ) ;
2013-12-28 07:15:29 -04:00
2021-04-16 12:53:14 -03:00
2013-12-28 07:15:29 -04:00
///
2021-05-19 11:09:31 -03:00
/// Accessors
2013-12-28 07:15:29 -04:00
///
2021-04-16 12:53:14 -03:00
/// set commanded position (cm), velocity (cm/s) and acceleration (cm/s/s) inputs when the path is created externally.
2021-06-17 22:19:29 -03:00
void set_pos_vel_accel ( const Vector3p & pos , const Vector3f & vel , const Vector3f & accel ) ;
void set_pos_vel_accel_xy ( const Vector2p & pos , const Vector2f & vel , const Vector2f & accel ) ;
2017-12-16 09:06:51 -04:00
2014-05-06 05:32:30 -03:00
2021-04-16 12:53:14 -03:00
/// Position
2019-04-19 08:36:42 -03:00
2022-02-02 01:27:51 -04:00
/// get_pos_target_cm - returns the position target, frame NEU in cm relative to the EKF origin
2021-06-17 22:19:29 -03:00
const Vector3p & get_pos_target_cm ( ) const { return _pos_target ; }
2018-01-11 08:28:20 -04:00
2024-08-02 01:45:17 -03:00
/// set_pos_desired_xy_cm - sets the position target, frame NEU in cm relative to the EKF origin
void set_pos_desired_xy_cm ( const Vector2f & pos ) { _pos_desired . xy ( ) = pos . topostype ( ) ; }
/// get_pos_desired_cm - returns the position desired, frame NEU in cm relative to the EKF origin
const Vector3p & get_pos_desired_cm ( ) const { return _pos_desired ; }
2013-12-28 07:15:29 -04:00
2022-02-02 01:27:51 -04:00
/// get_pos_target_z_cm - get target altitude (in cm above the EKF origin)
2021-04-16 12:53:14 -03:00
float get_pos_target_z_cm ( ) const { return _pos_target . z ; }
2013-12-28 07:15:29 -04:00
2024-08-02 01:45:17 -03:00
/// set_pos_desired_z_cm - set altitude target in cm above the EKF origin
void set_pos_desired_z_cm ( float pos_z ) { _pos_desired . z = pos_z ; }
/// get_pos_desired_z_cm - get target altitude (in cm above the EKF origin)
float get_pos_desired_z_cm ( ) const { return _pos_desired . z ; }
/// Stopping Point
2021-04-16 12:53:14 -03:00
/// get_stopping_point_xy_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration
2021-06-17 22:19:29 -03:00
void get_stopping_point_xy_cm ( Vector2p & stopping_point ) const ;
2020-01-04 02:16:09 -04:00
2021-04-16 12:53:14 -03:00
/// get_stopping_point_z_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration
2021-06-17 22:19:29 -03:00
void get_stopping_point_z_cm ( postype_t & stopping_point ) const ;
2014-05-06 05:32:30 -03:00
2024-08-02 01:45:17 -03:00
/// Position Error
2021-04-16 12:53:14 -03:00
/// get_pos_error_cm - get position error vector between the current and target position
2024-08-02 01:45:17 -03:00
const Vector3f get_pos_error_cm ( ) const { return Vector3f ( _p_pos_xy . get_error ( ) . x , _p_pos_xy . get_error ( ) . y , _p_pos_z . get_error ( ) ) ; }
2015-10-29 04:30:14 -03:00
2021-04-16 12:53:14 -03:00
/// get_pos_error_xy_cm - get the length of the position error vector in the xy plane
2024-08-02 01:45:17 -03:00
float get_pos_error_xy_cm ( ) const { return _p_pos_xy . get_error ( ) . length ( ) ; }
2013-12-28 07:15:29 -04:00
2021-04-16 12:53:14 -03:00
/// get_pos_error_z_cm - returns altitude error in cm
2024-08-02 01:45:17 -03:00
float get_pos_error_z_cm ( ) const { return _p_pos_z . get_error ( ) ; }
2015-10-27 10:04:24 -03:00
2017-11-16 08:29:02 -04:00
2021-04-16 12:53:14 -03:00
/// Velocity
2014-06-02 06:02:44 -03:00
2021-04-16 12:53:14 -03:00
/// set_vel_desired_cms - sets desired velocity in NEU cm/s
void set_vel_desired_cms ( const Vector3f & des_vel ) { _vel_desired = des_vel ; }
2013-12-28 07:15:29 -04:00
2021-04-16 12:53:14 -03:00
/// set_vel_desired_xy_cms - sets horizontal desired velocity in NEU cm/s
2021-06-21 04:26:39 -03:00
void set_vel_desired_xy_cms ( const Vector2f & vel ) { _vel_desired . xy ( ) = vel ; }
2016-07-20 15:38:37 -03:00
2024-08-02 01:45:17 -03:00
/// get_vel_desired_cms - returns desired velocity in cm/s in NEU
2021-04-16 12:53:14 -03:00
const Vector3f & get_vel_desired_cms ( ) { return _vel_desired ; }
2020-02-28 01:01:59 -04:00
2021-04-16 12:53:14 -03:00
// get_vel_target_cms - returns the target velocity in NEU cm/s
const Vector3f & get_vel_target_cms ( ) const { return _vel_target ; }
2014-05-06 05:32:30 -03:00
2021-04-16 12:53:14 -03:00
/// 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 ; }
2014-04-30 09:22:47 -03:00
2023-04-08 16:09:44 -03:00
/// get_vel_target_z_cms - returns target vertical speed in cm/s
2021-04-16 12:53:14 -03:00
float get_vel_target_z_cms ( ) const { return _vel_target . z ; }
2013-12-28 07:15:29 -04:00
2020-12-13 04:47:27 -04:00
2021-04-16 12:53:14 -03:00
/// Acceleration
2013-12-28 07:15:29 -04:00
2024-08-02 01:45:17 -03:00
// set_accel_desired_xy_cmss - set desired acceleration in cm/s in xy axis
2021-06-21 04:26:39 -03:00
void set_accel_desired_xy_cmss ( const Vector2f & accel_cms ) { _accel_desired . xy ( ) = accel_cms ; }
2017-11-27 08:21:13 -04:00
2021-04-16 12:53:14 -03:00
// get_accel_target_cmss - returns the target acceleration in NEU cm/s/s
const Vector3f & get_accel_target_cmss ( ) const { return _accel_target ; }
2014-06-02 06:02:44 -03:00
2024-08-02 01:45:17 -03:00
/// Terrain
// set_pos_terrain_target_cm - set target terrain altitude in cm
void set_pos_terrain_target_cm ( float pos_terrain_target ) { _pos_terrain_target = pos_terrain_target ; }
// init_pos_terrain_cm - initialises the current terrain altitude and target altitude to pos_offset_terrain_cm
void init_pos_terrain_cm ( float pos_offset_terrain_cm ) ;
// get_pos_terrain_cm - returns the current terrain altitude in cm
float get_pos_terrain_cm ( ) { return _pos_terrain ; }
2021-06-26 10:49:12 -03:00
/// Offset
2024-08-02 01:45:17 -03:00
# if AP_SCRIPTING_ENABLED
// position, velocity and acceleration offset target (only used by scripting)
// gets or sets an additional offset to the vehicle's target position, velocity and acceleration
// units are m, m/s and m/s/s in NED frame
bool set_posvelaccel_offset ( const Vector3f & pos_offset_NED , const Vector3f & vel_offset_NED , const Vector3f & accel_offset_NED ) ;
bool get_posvelaccel_offset ( Vector3f & pos_offset_NED , Vector3f & vel_offset_NED , Vector3f & accel_offset_NED ) ;
# endif
/// set the horizontal position, velocity and acceleration offset targets in cm, cms and cm/s/s from EKF origin in NE frame
/// these must be set every 3 seconds (or less) or they will timeout and return to zero
void set_posvelaccel_offset_target_xy_cm ( const Vector2p & pos_offset_target_xy_cm , const Vector2f & vel_offset_target_xy_cms , const Vector2f & accel_offset_target_xy_cmss ) ;
void set_posvelaccel_offset_target_z_cm ( float pos_offset_target_z_cm , float vel_offset_target_z_cms , float accel_offset_target_z_cmss ) ;
/// get the position, velocity or acceleration offets in cm from EKF origin in NEU frame
const Vector3p & get_pos_offset_cm ( ) const { return _pos_offset ; }
const Vector3f & get_vel_offset_cms ( ) const { return _vel_offset ; }
const Vector3f & get_accel_offset_cmss ( ) const { return _accel_offset ; }
2021-08-31 01:17:32 -03:00
2022-02-02 01:27:51 -04:00
/// set_pos_offset_z_cm - set altitude offset in cm above the EKF origin
2024-08-02 01:45:17 -03:00
void set_pos_offset_z_cm ( float pos_offset_z ) { _pos_offset . z = pos_offset_z ; }
2021-06-26 10:49:12 -03:00
2022-02-02 01:27:51 -04:00
/// get_pos_offset_z_cm - returns altitude offset in cm above the EKF origin
2024-08-02 01:45:17 -03:00
float get_pos_offset_z_cm ( ) const { return _pos_offset . z ; }
2021-07-14 11:18:45 -03:00
/// get_vel_offset_z_cm - returns current vertical offset speed in cm/s
2024-08-02 01:45:17 -03:00
float get_vel_offset_z_cms ( ) const { return _vel_offset . z ; }
2021-07-14 11:18:45 -03:00
/// get_accel_offset_z_cm - returns current vertical offset acceleration in cm/s/s
2024-08-02 01:45:17 -03:00
float get_accel_offset_z_cmss ( ) const { return _accel_offset . z ; }
2021-06-26 10:49:12 -03:00
2021-04-16 12:53:14 -03:00
/// Outputs
2014-06-02 06:02:44 -03:00
2021-04-22 22:16:58 -03:00
/// get desired roll and pitch to be passed to the attitude controller
2021-05-12 01:18:20 -03:00
float get_roll_cd ( ) const { return _roll_target ; }
float get_pitch_cd ( ) const { return _pitch_target ; }
2021-04-22 22:16:58 -03:00
/// get desired yaw to be passed to the attitude controller
float get_yaw_cd ( ) const { return _yaw_target ; }
/// 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
2021-04-13 02:12:48 -03:00
Vector3f get_thrust_vector ( ) const ;
2014-01-17 22:53:46 -04:00
2021-04-16 12:53:14 -03:00
/// get_bearing_to_target_cd - get bearing to target position in centi-degrees
int32_t get_bearing_to_target_cd ( ) const ;
/// get_lean_angle_max_cd - returns the maximum lean angle the autopilot may request
float get_lean_angle_max_cd ( ) const ;
2022-03-14 19:26:18 -03:00
/*
set_lean_angle_max_cd - set the maximum lean angle . A value of zero means to use the ANGLE_MAX parameter .
This is reset to zero on init_xy_controller ( )
*/
void set_lean_angle_max_cd ( float angle_max_cd ) { _angle_max_override_cd = angle_max_cd ; }
2021-04-22 22:16:58 -03:00
2021-04-16 12:53:14 -03:00
/// Other
2014-01-23 23:27:06 -04:00
2018-01-15 06:52:55 -04:00
/// get pid controllers
2020-01-04 02:16:09 -04:00
AC_P_2D & get_pos_xy_p ( ) { return _p_pos_xy ; }
AC_P_1D & get_pos_z_p ( ) { return _p_pos_z ; }
2018-01-15 06:50:54 -04:00
AC_PID_2D & get_vel_xy_pid ( ) { return _pid_vel_xy ; }
2020-01-04 02:16:09 -04:00
AC_PID_Basic & get_vel_z_pid ( ) { return _pid_vel_z ; }
AC_PID & get_accel_z_pid ( ) { return _pid_accel_z ; }
2018-01-15 06:50:54 -04:00
2021-04-16 12:53:14 -03:00
/// set_limit_accel_xy - mark that accel has been limited
/// this prevents integrator buildup
2021-05-19 11:09:31 -03:00
void set_externally_limited_xy ( ) { _limit_vector . x = _accel_target . x ; _limit_vector . y = _accel_target . y ; }
2013-12-28 07:15:29 -04:00
2014-05-06 05:32:30 -03:00
// lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s
2021-04-16 12:53:14 -03:00
Vector3f lean_angles_to_accel ( const Vector3f & att_target_euler ) const ;
2014-12-17 17:24:35 -04:00
2021-05-25 01:35:23 -03:00
// write PSC and/or PSCZ logs
2018-03-01 01:42:52 -04:00
void write_log ( ) ;
2019-03-04 23:27:31 -04:00
// provide feedback on whether arming would be a good idea right now:
bool pre_arm_checks ( const char * param_prefix ,
char * failure_msg ,
const uint8_t failure_msg_len ) ;
2019-09-14 01:02:02 -03:00
// enable or disable high vibration compensation
void set_vibe_comp ( bool on_off ) { _vibe_comp_enabled = on_off ; }
2021-04-16 12:53:14 -03:00
/// 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 ) ; }
2021-07-20 21:37:00 -03:00
/// crosstrack_error - returns horizontal error to the closest point to the current track
float crosstrack_error ( ) const ;
2021-04-16 12:53:14 -03:00
/// 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 ( ) ;
2022-07-09 12:15:00 -03:00
// 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 ( ) . z + GRAVITY_MSS ) * 100.0f ; }
2023-09-12 05:36:24 -03:00
/// returns true when the forward pitch demand is limited by the maximum allowed tilt
bool get_fwd_pitch_is_limited ( ) const { return _fwd_pitch_is_limited ; }
2023-08-18 00:57:12 -03:00
// set disturbance north
void set_disturb_pos_cm ( Vector2f disturb_pos ) { _disturb_pos = disturb_pos ; }
// set disturbance north
void set_disturb_vel_cms ( Vector2f disturb_vel ) { _disturb_vel = disturb_vel ; }
2023-09-12 05:36:24 -03:00
2013-12-28 07:15:29 -04:00
static const struct AP_Param : : GroupInfo var_info [ ] ;
2024-08-02 01:45:17 -03:00
static void Write_PSCN ( float pos_desired , float pos_target , float pos , float vel_desired , float vel_target , float vel , float accel_desired , float accel_target , float accel ) ;
static void Write_PSCE ( float pos_desired , float pos_target , float pos , float vel_desired , float vel_target , float vel , float accel_desired , float accel_target , float accel ) ;
static void Write_PSCD ( float pos_desired , float pos_target , float pos , float vel_desired , float vel_target , float vel , float accel_desired , float accel_target , float accel ) ;
static void Write_PSON ( float pos_target_offset_cm , float pos_offset_cm , float vel_target_offset_cms , float vel_offset_cms , float accel_target_offset_cmss , float accel_offset_cmss ) ;
static void Write_PSOE ( float pos_target_offset_cm , float pos_offset_cm , float vel_target_offset_cms , float vel_offset_cms , float accel_target_offset_cmss , float accel_offset_cmss ) ;
static void Write_PSOD ( float pos_target_offset_cm , float pos_offset_cm , float vel_target_offset_cms , float vel_offset_cms , float accel_target_offset_cmss , float accel_offset_cmss ) ;
static void Write_PSOT ( float pos_target_offset_cm , float pos_offset_cm , float vel_target_offset_cms , float vel_offset_cms , float accel_target_offset_cmss , float accel_offset_cmss ) ;
2024-02-28 08:14:23 -04:00
2024-09-17 00:54:55 -03:00
// singleton
static AC_PosControl * get_singleton ( void ) { return _singleton ; }
2017-02-07 20:49:55 -04:00
protected :
2014-01-17 22:53:46 -04:00
2021-03-11 08:05:36 -04:00
// get throttle using vibration-resistant calculation (uses feed forward with manually calculated gain)
2020-01-04 02:16:09 -04:00
float get_throttle_with_vibration_override ( ) ;
2021-04-16 12:53:14 -03:00
// 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 ;
2013-12-28 07:15:29 -04:00
2021-04-16 12:53:14 -03:00
// 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 ;
2013-12-28 07:15:29 -04:00
2021-04-16 12:53:14 -03:00
// calculate_yaw_and_rate_yaw - calculate the vehicle yaw and rate of yaw.
2024-03-22 23:41:51 -03:00
void calculate_yaw_and_rate_yaw ( ) ;
2013-12-28 07:15:29 -04:00
2022-01-24 10:40:38 -04:00
// calculate_overspeed_gain - calculated increased maximum acceleration and jerk if over speed condition is detected
float calculate_overspeed_gain ( ) ;
2024-08-02 01:45:17 -03:00
/// Terrain Following
/// set the position, velocity and acceleration offsets in cm, cms and cm/s/s from EKF origin in NE frame
/// this is used to initiate the offsets when initialise the position controller or do an offset reset
/// note that this sets the actual offsets, not the offset targets
void init_terrain ( ) ;
/// update_terrain - updates the terrain position, velocity and acceleration estimation
/// this moves the estimated terrain position _pos_terrain towards the target _pos_terrain_target
void update_terrain ( ) ;
/// Offsets
/// init_offsets - set the position, velocity and acceleration offsets in cm, cms and cm/s/s from EKF origin in NE frame
/// this is used to initiate the offsets when initialise the position controller or do an offset reset
/// note that this sets the actual offsets, not the offset targets
void init_offsets_xy ( ) ;
void init_offsets_z ( ) ;
/// update_offsets - update the position and velocity offsets
/// this moves the offsets (e.g _pos_offset, _vel_offset, _accel_offset) towards the targets (e.g. _pos_offset_target or _vel_offset_target)
void update_offsets_xy ( ) ;
void update_offsets_z ( ) ;
2016-11-22 01:36:17 -04:00
/// initialise and check for ekf position resets
void init_ekf_xy_reset ( ) ;
2021-05-19 11:09:31 -03:00
void handle_ekf_xy_reset ( ) ;
2016-11-22 01:36:17 -04:00
void init_ekf_z_reset ( ) ;
2021-05-19 11:09:31 -03:00
void handle_ekf_z_reset ( ) ;
2016-11-22 01:36:17 -04:00
2013-12-28 07:15:29 -04:00
// references to inertial nav and ahrs libraries
2021-05-12 01:17:38 -03:00
AP_AHRS_View & _ahrs ;
const AP_InertialNav & _inav ;
2022-12-05 08:21:43 -04:00
const class AP_Motors & _motors ;
2021-05-12 01:17:38 -03:00
AC_AttitudeControl & _attitude_control ;
2013-12-28 07:15:29 -04:00
2015-04-16 07:07:33 -03:00
// parameters
2021-04-16 12:53:14 -03:00
AP_Float _lean_angle_max ; // Maximum autopilot commanded angle (in degrees). Set to zero for Angle Max
2021-08-06 06:40:56 -03:00
AP_Float _shaping_jerk_xy ; // Jerk limit of the xy kinematic path generation in m/s^3 used to determine how quickly the aircraft varies the acceleration target
AP_Float _shaping_jerk_z ; // Jerk limit of the z kinematic path generation in m/s^3 used to determine how quickly the aircraft varies the acceleration target
2021-04-16 12:53:14 -03:00
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
2015-04-16 07:07:33 -03:00
2013-12-28 10:04:45 -04:00
// internal variables
2022-12-05 08:21:43 -04:00
float _dt ; // time difference (in seconds) since the last loop time
2023-01-28 05:43:41 -04:00
uint32_t _last_update_xy_ticks ; // ticks of last last update_xy_controller call
uint32_t _last_update_z_ticks ; // ticks of last update_z_controller call
2021-07-08 01:13:05 -03:00
float _vel_max_xy_cms ; // max horizontal speed in cm/s used for kinematic shaping
float _vel_max_up_cms ; // max climb rate in cm/s used for kinematic shaping
float _vel_max_down_cms ; // max descent rate in cm/s used for kinematic shaping
float _accel_max_xy_cmss ; // max horizontal acceleration in cm/s/s used for kinematic shaping
float _accel_max_z_cmss ; // max vertical acceleration in cm/s/s used for kinematic shaping
2021-10-01 09:02:30 -03:00
float _jerk_max_xy_cmsss ; // Jerk limit of the xy kinematic path generation in cm/s^3 used to determine how quickly the aircraft varies the acceleration target
float _jerk_max_z_cmsss ; // Jerk limit of the z kinematic path generation in cm/s^3 used to determine how quickly the aircraft varies the acceleration target
2021-04-16 12:53:14 -03:00
float _vel_z_control_ratio = 2.0f ; // confidence that we have control in the vertical axis
2024-05-13 20:33:26 -03:00
Vector2f _disturb_pos ; // position disturbance generated by system ID mode
Vector2f _disturb_vel ; // velocity disturbance generated by system ID mode
2013-12-28 07:15:29 -04:00
// output from controller
2014-01-17 22:53:46 -04:00
float _roll_target ; // desired roll angle in centi-degrees calculated by position controller
float _pitch_target ; // desired roll pitch in centi-degrees calculated by position controller
2021-05-12 01:17:38 -03:00
float _yaw_target ; // desired yaw in centi-degrees calculated by position controller
float _yaw_rate_target ; // desired yaw rate in centi-degrees per second calculated by position controller
2013-12-28 07:15:29 -04:00
// position controller internal variables
2024-08-02 01:45:17 -03:00
Vector3p _pos_desired ; // desired location, frame NEU in cm relative to the EKF origin. This is equal to the _pos_target minus offsets
Vector3p _pos_target ; // target location, frame NEU in cm relative to the EKF origin. This is equal to the _pos_desired plus offsets
2021-05-12 01:17:38 -03:00
Vector3f _vel_desired ; // desired velocity in NEU cm/s
Vector3f _vel_target ; // velocity target in NEU cm/s calculated by pos_to_rate step
Vector3f _accel_desired ; // desired acceleration in NEU cm/s/s (feed forward)
Vector3f _accel_target ; // acceleration target in NEU cm/s/s
2021-05-19 11:09:31 -03:00
Vector3f _limit_vector ; // the direction that the position controller is limited, zero when not limited
2022-01-31 10:45:17 -04:00
2023-09-12 05:36:24 -03:00
bool _fwd_pitch_is_limited ; // true when the forward pitch demand is being limited to meet acceleration limits
2024-08-02 01:45:17 -03:00
// terrain handling variables
float _pos_terrain_target ; // position terrain target in cm relative to the EKF origin in NEU frame
float _pos_terrain ; // position terrain in cm from the EKF origin in NEU frame. this terrain moves towards _pos_terrain_target
float _vel_terrain ; // velocity terrain in NEU cm/s calculated by pos_to_rate step. this terrain moves towards _vel_terrain_target
float _accel_terrain ; // acceleration terrain in NEU cm/s/s
// offset handling variables
Vector3p _pos_offset_target ; // position offset target in cm relative to the EKF origin in NEU frame
Vector3p _pos_offset ; // position offset in cm from the EKF origin in NEU frame. this offset moves towards _pos_offset_target
Vector3f _vel_offset_target ; // velocity offset target in cm/s in NEU frame
Vector3f _vel_offset ; // velocity offset in NEU cm/s calculated by pos_to_rate step. this offset moves towards _vel_offset_target
Vector3f _accel_offset_target ; // acceleration offset target in cm/s/s in NEU frame
Vector3f _accel_offset ; // acceleration offset in NEU cm/s/s
uint32_t _posvelaccel_offset_target_xy_ms ; // system time that pos, vel, accel targets were set (used to implement timeouts)
uint32_t _posvelaccel_offset_target_z_ms ; // system time that pos, vel, accel targets were set (used to implement timeouts)
2016-11-22 01:36:17 -04:00
// ekf reset handling
2021-06-26 10:49:12 -03:00
uint32_t _ekf_xy_reset_ms ; // system time of last recorded ekf xy position reset
uint32_t _ekf_z_reset_ms ; // system time of last recorded ekf altitude reset
2019-09-14 01:02:02 -03:00
// high vibration handling
bool _vibe_comp_enabled ; // true when high vibration compensation is on
2021-05-17 22:24:41 -03:00
2022-03-14 19:26:18 -03:00
// angle max override, if zero then use ANGLE_MAX parameter
float _angle_max_override_cd ;
2021-05-17 22:24:41 -03:00
// return true if on a real vehicle or SITL with lock-step scheduling
bool has_good_timing ( void ) const ;
2024-02-28 08:23:13 -04:00
private :
// convenience method for writing out the identical PSCE, PSCN, PSCD - and
// to save bytes
2024-08-02 01:45:17 -03:00
static void Write_PSCx ( LogMessages ID , float pos_desired , float pos_target , float pos , float vel_desired , float vel_target , float vel , float accel_desired , float accel_target , float accel ) ;
// a convenience function for writing out the position controller offsets
static void Write_PSOx ( LogMessages id , float pos_target_offset_cm , float pos_offset_cm ,
float vel_target_offset_cms , float vel_offset_cms ,
float accel_target_offset_cmss , float accel_offset_cmss ) ;
2024-02-28 08:23:13 -04:00
2024-09-17 00:54:55 -03:00
// singleton
static AC_PosControl * _singleton ;
2013-12-28 07:15:29 -04:00
} ;