2015-08-11 03:28:41 -03:00
# include <AP_HAL/AP_HAL.h>
# include "AC_PosControl.h"
# include <AP_Math/AP_Math.h>
2019-01-18 00:23:42 -04:00
# include <AP_Logger/AP_Logger.h>
2013-12-28 07:15:29 -04:00
extern const AP_HAL : : HAL & hal ;
2018-01-15 06:50:54 -04:00
# if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
// default gains for Plane
2018-01-15 06:52:55 -04:00
# define POSCONTROL_POS_Z_P 1.0f // vertical position controller P gain default
# define POSCONTROL_VEL_Z_P 5.0f // vertical velocity controller P gain default
2020-01-04 02:16:09 -04:00
# define POSCONTROL_VEL_Z_IMAX 1000.0f // vertical velocity controller IMAX gain default
# define POSCONTROL_VEL_Z_FILT_HZ 5.0f // vertical velocity controller input filter
# define POSCONTROL_VEL_Z_FILT_D_HZ 5.0f // vertical velocity controller input filter for D
2018-01-27 02:39:55 -04:00
# define POSCONTROL_ACC_Z_P 0.3f // vertical acceleration controller P gain default
# define POSCONTROL_ACC_Z_I 1.0f // vertical acceleration controller I gain default
# define POSCONTROL_ACC_Z_D 0.0f // vertical acceleration controller D gain default
# define POSCONTROL_ACC_Z_IMAX 800 // vertical acceleration controller IMAX gain default
# define POSCONTROL_ACC_Z_FILT_HZ 10.0f // vertical acceleration controller input filter default
# define POSCONTROL_ACC_Z_DT 0.02f // vertical acceleration controller dt default
2018-01-15 06:52:55 -04:00
# define POSCONTROL_POS_XY_P 1.0f // horizontal position controller P gain default
2018-01-19 02:07:33 -04:00
# define POSCONTROL_VEL_XY_P 1.4f // horizontal velocity controller P gain default
# define POSCONTROL_VEL_XY_I 0.7f // horizontal velocity controller I gain default
# define POSCONTROL_VEL_XY_D 0.35f // horizontal velocity controller D gain default
2018-01-15 06:50:54 -04:00
# define POSCONTROL_VEL_XY_IMAX 1000.0f // horizontal velocity controller IMAX gain default
# define POSCONTROL_VEL_XY_FILT_HZ 5.0f // horizontal velocity controller input filter
# define POSCONTROL_VEL_XY_FILT_D_HZ 5.0f // horizontal velocity controller input filter for D
2018-01-27 02:43:19 -04:00
# elif APM_BUILD_TYPE(APM_BUILD_ArduSub)
// default gains for Sub
# define POSCONTROL_POS_Z_P 3.0f // vertical position controller P gain default
# define POSCONTROL_VEL_Z_P 8.0f // vertical velocity controller P gain default
2020-01-04 02:16:09 -04:00
# define POSCONTROL_VEL_Z_IMAX 1000.0f // vertical velocity controller IMAX gain default
# define POSCONTROL_VEL_Z_FILT_HZ 5.0f // vertical velocity controller input filter
# define POSCONTROL_VEL_Z_FILT_D_HZ 5.0f // vertical velocity controller input filter for D
2018-01-27 02:43:19 -04:00
# define POSCONTROL_ACC_Z_P 0.5f // vertical acceleration controller P gain default
# define POSCONTROL_ACC_Z_I 0.1f // vertical acceleration controller I gain default
# define POSCONTROL_ACC_Z_D 0.0f // vertical acceleration controller D gain default
# define POSCONTROL_ACC_Z_IMAX 100 // vertical acceleration controller IMAX gain default
# define POSCONTROL_ACC_Z_FILT_HZ 20.0f // vertical acceleration controller input filter default
# define POSCONTROL_ACC_Z_DT 0.0025f // vertical acceleration controller dt default
# define POSCONTROL_POS_XY_P 1.0f // horizontal position controller P gain default
# define POSCONTROL_VEL_XY_P 1.0f // horizontal velocity controller P gain default
# define POSCONTROL_VEL_XY_I 0.5f // horizontal velocity controller I gain default
# define POSCONTROL_VEL_XY_D 0.0f // horizontal velocity controller D gain default
# define POSCONTROL_VEL_XY_IMAX 1000.0f // horizontal velocity controller IMAX gain default
# define POSCONTROL_VEL_XY_FILT_HZ 5.0f // horizontal velocity controller input filter
# define POSCONTROL_VEL_XY_FILT_D_HZ 5.0f // horizontal velocity controller input filter for D
2018-01-15 06:50:54 -04:00
# else
2018-01-27 02:43:19 -04:00
// default gains for Copter / TradHeli
2018-01-15 06:52:55 -04:00
# define POSCONTROL_POS_Z_P 1.0f // vertical position controller P gain default
# define POSCONTROL_VEL_Z_P 5.0f // vertical velocity controller P gain default
2020-01-04 02:16:09 -04:00
# define POSCONTROL_VEL_Z_IMAX 1000.0f // vertical velocity controller IMAX gain default
# define POSCONTROL_VEL_Z_FILT_HZ 5.0f // vertical velocity controller input filter
# define POSCONTROL_VEL_Z_FILT_D_HZ 5.0f // vertical velocity controller input filter for D
2018-01-27 02:39:55 -04:00
# define POSCONTROL_ACC_Z_P 0.5f // vertical acceleration controller P gain default
# define POSCONTROL_ACC_Z_I 1.0f // vertical acceleration controller I gain default
# define POSCONTROL_ACC_Z_D 0.0f // vertical acceleration controller D gain default
# define POSCONTROL_ACC_Z_IMAX 800 // vertical acceleration controller IMAX gain default
# define POSCONTROL_ACC_Z_FILT_HZ 20.0f // vertical acceleration controller input filter default
# define POSCONTROL_ACC_Z_DT 0.0025f // vertical acceleration controller dt default
2018-01-15 06:52:55 -04:00
# define POSCONTROL_POS_XY_P 1.0f // horizontal position controller P gain default
2018-01-19 02:07:33 -04:00
# define POSCONTROL_VEL_XY_P 2.0f // horizontal velocity controller P gain default
# define POSCONTROL_VEL_XY_I 1.0f // horizontal velocity controller I gain default
# define POSCONTROL_VEL_XY_D 0.5f // horizontal velocity controller D gain default
2018-01-15 06:50:54 -04:00
# define POSCONTROL_VEL_XY_IMAX 1000.0f // horizontal velocity controller IMAX gain default
# define POSCONTROL_VEL_XY_FILT_HZ 5.0f // horizontal velocity controller input filter
# define POSCONTROL_VEL_XY_FILT_D_HZ 5.0f // horizontal velocity controller input filter for D
# endif
2019-09-14 01:02:02 -03:00
// vibration compensation gains
# define POSCONTROL_VIBE_COMP_P_GAIN 0.250f
# define POSCONTROL_VIBE_COMP_I_GAIN 0.125f
2015-10-25 14:03:46 -03:00
const AP_Param : : GroupInfo AC_PosControl : : var_info [ ] = {
2015-04-16 07:07:33 -03:00
// 0 was used for HOVER
// @Param: _ACC_XY_FILT
// @DisplayName: XY Acceleration filter cutoff frequency
// @Description: Lower values will slow the response of the navigation controller and reduce twitchiness
// @Units: Hz
// @Range: 0.5 5
// @Increment: 0.1
// @User: Advanced
2013-12-28 07:15:29 -04:00
2018-01-15 06:52:55 -04:00
// @Param: _POSZ_P
// @DisplayName: Position (vertical) controller P gain
// @Description: Position (vertical) controller P gain. Converts the difference between the desired altitude and actual altitude into a climb or descent rate which is passed to the throttle rate controller
// @Range: 1.000 3.000
// @User: Standard
2020-01-04 02:16:09 -04:00
AP_SUBGROUPINFO ( _p_pos_z , " _POSZ_ " , 2 , AC_PosControl , AC_P_1D ) ,
2018-01-15 06:52:55 -04:00
// @Param: _VELZ_P
// @DisplayName: Velocity (vertical) controller P gain
// @Description: Velocity (vertical) controller P gain. Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller
// @Range: 1.000 8.000
// @User: Standard
2020-01-04 02:16:09 -04:00
// @Param: _VELZ_I
// @DisplayName: Velocity (vertical) controller I gain
// @Description: Velocity (vertical) controller I gain. Corrects long-term difference in desired velocity to a target acceleration
// @Range: 0.02 1.00
// @Increment: 0.01
// @User: Advanced
// @Param: _VELZ_IMAX
// @DisplayName: Velocity (vertical) controller I gain maximum
// @Description: Velocity (vertical) controller I gain maximum. Constrains the target acceleration that the I gain will output
// @Range: 1.000 8.000
// @User: Standard
// @Param: _VELZ_D
// @DisplayName: Velocity (vertical) controller D gain
// @Description: Velocity (vertical) controller D gain. Corrects short-term changes in velocity
// @Range: 0.00 1.00
// @Increment: 0.001
// @User: Advanced
// @Param: _VELZ_FF
// @DisplayName: Velocity (vertical) controller Feed Forward gain
// @Description: Velocity (vertical) controller Feed Forward gain. Produces an output that is proportional to the magnitude of the target
// @Range: 0 1
// @Increment: 0.01
// @User: Advanced
// @Param: _VELZ_FLTE
// @DisplayName: Velocity (vertical) error filter
2021-03-11 08:05:36 -04:00
// @Description: Velocity (vertical) error filter. This filter (in Hz) is applied to the input for P and I terms
2020-01-04 02:16:09 -04:00
// @Range: 0 100
// @Units: Hz
// @User: Advanced
// @Param: _VELZ_FLTD
// @DisplayName: Velocity (vertical) input filter for D term
2021-03-11 08:05:36 -04:00
// @Description: Velocity (vertical) input filter for D term. This filter (in Hz) is applied to the input for D terms
2020-01-04 02:16:09 -04:00
// @Range: 0 100
// @Units: Hz
// @User: Advanced
AP_SUBGROUPINFO ( _pid_vel_z , " _VELZ_ " , 3 , AC_PosControl , AC_PID_Basic ) ,
2018-01-15 06:52:55 -04:00
2018-01-27 02:39:55 -04:00
// @Param: _ACCZ_P
2018-01-15 06:52:55 -04:00
// @DisplayName: Acceleration (vertical) controller P gain
// @Description: Acceleration (vertical) controller P gain. Converts the difference between desired vertical acceleration and actual acceleration into a motor output
2021-05-24 22:01:37 -03:00
// @Range: 0.200 1.500
2018-01-15 06:52:55 -04:00
// @Increment: 0.05
// @User: Standard
2018-01-27 02:39:55 -04:00
// @Param: _ACCZ_I
2018-01-15 06:52:55 -04:00
// @DisplayName: Acceleration (vertical) controller I gain
// @Description: Acceleration (vertical) controller I gain. Corrects long-term difference in desired vertical acceleration and actual acceleration
// @Range: 0.000 3.000
// @User: Standard
2018-01-27 02:39:55 -04:00
// @Param: _ACCZ_IMAX
2018-01-15 06:52:55 -04:00
// @DisplayName: Acceleration (vertical) controller I gain maximum
// @Description: Acceleration (vertical) controller I gain maximum. Constrains the maximum pwm that the I term will generate
// @Range: 0 1000
// @Units: d%
// @User: Standard
2018-01-27 02:39:55 -04:00
// @Param: _ACCZ_D
2018-01-15 06:52:55 -04:00
// @DisplayName: Acceleration (vertical) controller D gain
// @Description: Acceleration (vertical) controller D gain. Compensates for short-term change in desired vertical acceleration vs actual acceleration
// @Range: 0.000 0.400
// @User: Standard
2020-02-13 05:09:24 -04:00
// @Param: _ACCZ_FF
// @DisplayName: Acceleration (vertical) controller feed forward
// @Description: Acceleration (vertical) controller feed forward
// @Range: 0 0.5
// @Increment: 0.001
// @User: Standard
// @Param: _ACCZ_FLTT
// @DisplayName: Acceleration (vertical) controller target frequency in Hz
// @Description: Acceleration (vertical) controller target frequency in Hz
// @Range: 1 50
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: _ACCZ_FLTE
// @DisplayName: Acceleration (vertical) controller error frequency in Hz
// @Description: Acceleration (vertical) controller error frequency in Hz
// @Range: 1 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: _ACCZ_FLTD
// @DisplayName: Acceleration (vertical) controller derivative frequency in Hz
// @Description: Acceleration (vertical) controller derivative frequency in Hz
// @Range: 1 100
// @Increment: 1
2018-01-15 06:52:55 -04:00
// @Units: Hz
// @User: Standard
2020-09-24 04:26:05 -03:00
// @Param: _ACCZ_SMAX
// @DisplayName: Accel (vertical) slew rate limit
// @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
// @Range: 0 200
// @Increment: 0.5
// @User: Advanced
2018-01-27 02:39:55 -04:00
AP_SUBGROUPINFO ( _pid_accel_z , " _ACCZ_ " , 4 , AC_PosControl , AC_PID ) ,
2018-01-15 06:52:55 -04:00
// @Param: _POSXY_P
2020-11-04 20:26:13 -04:00
// @DisplayName: Position (horizontal) controller P gain
2018-01-15 06:52:55 -04:00
// @Description: Position controller P gain. Converts the distance (in the latitude direction) to the target location into a desired speed which is then passed to the loiter latitude rate controller
// @Range: 0.500 2.000
// @User: Standard
2020-01-04 02:16:09 -04:00
AP_SUBGROUPINFO ( _p_pos_xy , " _POSXY_ " , 5 , AC_PosControl , AC_P_2D ) ,
2018-01-15 06:52:55 -04:00
2018-01-15 06:50:54 -04:00
// @Param: _VELXY_P
// @DisplayName: Velocity (horizontal) P gain
2020-01-04 02:16:09 -04:00
// @Description: Velocity (horizontal) P gain. Converts the difference between desired and actual velocity to a target acceleration
2018-01-15 06:50:54 -04:00
// @Range: 0.1 6.0
// @Increment: 0.1
// @User: Advanced
// @Param: _VELXY_I
// @DisplayName: Velocity (horizontal) I gain
2020-01-04 02:16:09 -04:00
// @Description: Velocity (horizontal) I gain. Corrects long-term difference between desired and actual velocity to a target acceleration
2018-01-15 06:50:54 -04:00
// @Range: 0.02 1.00
// @Increment: 0.01
// @User: Advanced
// @Param: _VELXY_D
// @DisplayName: Velocity (horizontal) D gain
// @Description: Velocity (horizontal) D gain. Corrects short-term changes in velocity
// @Range: 0.00 1.00
// @Increment: 0.001
// @User: Advanced
// @Param: _VELXY_IMAX
// @DisplayName: Velocity (horizontal) integrator maximum
// @Description: Velocity (horizontal) integrator maximum. Constrains the target acceleration that the I gain will output
// @Range: 0 4500
// @Increment: 10
// @Units: cm/s/s
// @User: Advanced
2021-08-03 01:05:35 -03:00
// @Param: _VELXY_FLTE
2018-01-15 06:50:54 -04:00
// @DisplayName: Velocity (horizontal) input filter
2021-03-11 08:05:36 -04:00
// @Description: Velocity (horizontal) input filter. This filter (in Hz) is applied to the input for P and I terms
2018-01-15 06:50:54 -04:00
// @Range: 0 100
// @Units: Hz
// @User: Advanced
2021-08-03 01:05:35 -03:00
// @Param: _VELXY_FLTD
2018-01-15 06:50:54 -04:00
// @DisplayName: Velocity (horizontal) input filter
2021-03-11 08:05:36 -04:00
// @Description: Velocity (horizontal) input filter. This filter (in Hz) is applied to the input for D term
2018-01-15 06:50:54 -04:00
// @Range: 0 100
// @Units: Hz
// @User: Advanced
2020-01-04 02:16:09 -04:00
// @Param: _VELXY_FF
2021-03-11 08:05:36 -04:00
// @DisplayName: Velocity (horizontal) feed forward gain
2020-01-04 02:16:09 -04:00
// @Description: Velocity (horizontal) feed forward gain. Converts the difference between desired velocity to a target acceleration
// @Range: 0 6
// @Increment: 0.01
// @User: Advanced
2018-01-15 06:52:55 -04:00
AP_SUBGROUPINFO ( _pid_vel_xy , " _VELXY_ " , 6 , AC_PosControl , AC_PID_2D ) ,
2018-01-15 06:50:54 -04:00
2017-12-16 09:06:51 -04:00
// @Param: _ANGLE_MAX
// @DisplayName: Position Control Angle Max
// @Description: Maximum lean angle autopilot can request. Set to zero to use ANGLE_MAX parameter value
// @Units: deg
// @Range: 0 45
// @Increment: 1
// @User: Advanced
2019-04-19 08:36:42 -03:00
AP_GROUPINFO ( " _ANGLE_MAX " , 7 , AC_PosControl , _lean_angle_max , 0.0f ) ,
2017-12-16 09:06:51 -04:00
2021-08-06 06:40:56 -03:00
// IDs 8,9 used for _TC_XY and _TC_Z in beta release candidate
// @Param: _JERK_XY
// @DisplayName: Jerk limit for the horizontal kinematic input shaping
// @Description: Jerk limit of the horizontal kinematic path generation used to determine how quickly the aircraft varies the acceleration target
// @Units: m/s/s/s
// @Range: 1 20
// @Increment: 1
2021-04-16 12:53:14 -03:00
// @User: Advanced
2021-08-06 06:40:56 -03:00
AP_GROUPINFO ( " _JERK_XY " , 10 , AC_PosControl , _shaping_jerk_xy , POSCONTROL_JERK_XY ) ,
2021-04-16 12:53:14 -03:00
2021-08-06 06:40:56 -03:00
// @Param: _JERK_Z
// @DisplayName: Jerk limit for the vertical kinematic input shaping
// @Description: Jerk limit of the vertical kinematic path generation used to determine how quickly the aircraft varies the acceleration target
// @Units: m/s/s/s
// @Range: 5 50
// @Increment: 1
2021-04-16 12:53:14 -03:00
// @User: Advanced
2021-08-06 06:40:56 -03:00
AP_GROUPINFO ( " _JERK_Z " , 11 , AC_PosControl , _shaping_jerk_z , POSCONTROL_JERK_Z ) ,
2021-04-16 12:53:14 -03:00
2013-12-28 07:15:29 -04:00
AP_GROUPEND
} ;
// Default constructor.
// Note that the Vector/Matrix constructors already implicitly zero
// their values.
//
2019-12-10 05:31:08 -04:00
AC_PosControl : : AC_PosControl ( AP_AHRS_View & ahrs , const AP_InertialNav & inav ,
2021-04-16 12:53:14 -03:00
const AP_Motors & motors , AC_AttitudeControl & attitude_control , float dt ) :
2013-12-28 07:15:29 -04:00
_ahrs ( ahrs ) ,
2013-12-28 10:04:45 -04:00
_inav ( inav ) ,
_motors ( motors ) ,
2013-12-28 07:15:29 -04:00
_attitude_control ( attitude_control ) ,
2021-04-16 12:53:14 -03:00
_p_pos_z ( POSCONTROL_POS_Z_P , dt ) ,
_pid_vel_z ( POSCONTROL_VEL_Z_P , 0.0f , 0.0f , 0.0f , POSCONTROL_VEL_Z_IMAX , POSCONTROL_VEL_Z_FILT_HZ , POSCONTROL_VEL_Z_FILT_D_HZ , dt ) ,
_pid_accel_z ( POSCONTROL_ACC_Z_P , POSCONTROL_ACC_Z_I , POSCONTROL_ACC_Z_D , 0.0f , POSCONTROL_ACC_Z_IMAX , 0.0f , POSCONTROL_ACC_Z_FILT_HZ , 0.0f , dt ) ,
_p_pos_xy ( POSCONTROL_POS_XY_P , dt ) ,
_pid_vel_xy ( POSCONTROL_VEL_XY_P , POSCONTROL_VEL_XY_I , POSCONTROL_VEL_XY_D , 0.0f , POSCONTROL_VEL_XY_IMAX , POSCONTROL_VEL_XY_FILT_HZ , POSCONTROL_VEL_XY_FILT_D_HZ , dt ) ,
_dt ( dt ) ,
_vel_max_down_cms ( POSCONTROL_SPEED_DOWN ) ,
_vel_max_up_cms ( POSCONTROL_SPEED_UP ) ,
_vel_max_xy_cms ( POSCONTROL_SPEED ) ,
_accel_max_z_cmss ( POSCONTROL_ACCEL_Z ) ,
_accel_max_xy_cmss ( POSCONTROL_ACCEL_XY ) ,
2021-08-06 06:40:56 -03:00
_jerk_xy_max ( POSCONTROL_JERK_XY ) ,
_jerk_z_max ( POSCONTROL_JERK_Z )
2013-12-28 07:15:29 -04:00
{
AP_Param : : setup_object_defaults ( this , var_info ) ;
2014-01-17 22:53:46 -04:00
// initialise flags
2021-04-16 12:53:14 -03:00
_limit . pos_xy = true ;
2015-06-08 01:27:03 -03:00
_limit . pos_up = true ;
_limit . pos_down = true ;
2013-12-28 07:15:29 -04:00
}
2021-04-16 12:53:14 -03:00
2013-12-28 10:04:45 -04:00
///
2021-04-16 12:53:14 -03:00
/// 3D position shaper
2013-12-28 10:04:45 -04:00
///
2013-12-28 07:15:29 -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 jerk parameter and the velocity and acceleration limits 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-04-16 12:53:14 -03:00
/// The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
2021-07-08 04:09:05 -03:00
void AC_PosControl : : input_pos_xyz ( const Vector3p & pos , float pos_offset_z , float pos_offset_z_buffer )
2014-05-27 10:44:57 -03:00
{
2021-07-14 11:18:45 -03:00
// Terrain following velocity scalar must be calculated before we remove the position offset
const float offset_z_scaler = pos_offset_z_scaler ( pos_offset_z , pos_offset_z_buffer ) ;
2021-06-26 10:49:12 -03:00
// 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 ;
2021-04-16 12:53:14 -03:00
// calculated increased maximum acceleration if over speed
float accel_z_cmss = _accel_max_z_cmss ;
if ( _vel_desired . z < _vel_max_down_cms & & ! is_zero ( _vel_max_down_cms ) ) {
accel_z_cmss * = POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired . z / _vel_max_down_cms ;
}
if ( _vel_desired . z > _vel_max_up_cms & & ! is_zero ( _vel_max_up_cms ) ) {
accel_z_cmss * = POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired . z / _vel_max_up_cms ;
}
2014-05-27 10:44:57 -03:00
2021-06-21 04:26:39 -03:00
update_pos_vel_accel_xy ( _pos_target . xy ( ) , _vel_desired . xy ( ) , _accel_desired . xy ( ) , _dt , _limit_vector . xy ( ) ) ;
2014-09-21 05:52:14 -03:00
2021-06-26 10:49:12 -03:00
// adjust desired altitude if motors have not hit their limits
2021-06-21 04:26:39 -03:00
update_pos_vel_accel ( _pos_target . z , _vel_desired . z , _accel_desired . z , _dt , _limit_vector . z ) ;
2021-04-16 12:53:14 -03:00
2021-06-26 10:49:12 -03:00
// calculate the horizontal and vertical velocity limits to travel directly to the destination defined by pos
float vel_max_xy_cms = 0.0f ;
2021-04-16 12:53:14 -03:00
float vel_max_z_cms = 0.0f ;
2021-06-26 10:49:12 -03:00
Vector3f dest_vector = ( pos - _pos_target ) . tofloat ( ) ;
2021-04-16 12:53:14 -03:00
if ( is_positive ( dest_vector . length_squared ( ) ) ) {
dest_vector . normalize ( ) ;
2021-06-17 22:19:29 -03:00
float dest_vector_xy_length = dest_vector . xy ( ) . length ( ) ;
2021-04-16 12:53:14 -03:00
float vel_max_cms = kinematic_limit ( dest_vector , _vel_max_xy_cms , _vel_max_up_cms , _vel_max_down_cms ) ;
vel_max_xy_cms = vel_max_cms * dest_vector_xy_length ;
2021-06-26 10:49:12 -03:00
vel_max_z_cms = fabsf ( vel_max_cms * dest_vector . z ) ;
2021-04-16 12:53:14 -03:00
}
2021-07-08 04:09:05 -03:00
// reduce speed if we are reaching the edge of our vertical buffer
2021-07-14 11:18:45 -03:00
vel_max_xy_cms * = offset_z_scaler ;
2021-07-08 04:09:05 -03:00
2021-06-17 22:19:29 -03:00
Vector2f vel ;
Vector2f accel ;
shape_pos_vel_accel_xy ( pos . xy ( ) , vel , accel , _pos_target . xy ( ) , _vel_desired . xy ( ) , _accel_desired . xy ( ) ,
2021-08-06 06:40:56 -03:00
vel_max_xy_cms , _accel_max_xy_cmss , _jerk_xy_max , _dt , false ) ;
2021-06-26 10:49:12 -03:00
2021-06-21 04:26:39 -03:00
float posz = pos . z ;
2021-06-17 22:19:29 -03:00
shape_pos_vel_accel ( posz , 0 , 0 ,
2021-06-21 04:26:39 -03:00
_pos_target . z , _vel_desired . z , _accel_desired . z ,
2021-08-06 06:40:56 -03:00
- vel_max_z_cms , vel_max_z_cms ,
2021-06-21 04:26:39 -03:00
- constrain_float ( accel_z_cmss , 0.0f , 750.0f ) , accel_z_cmss ,
2021-08-06 06:40:56 -03:00
_jerk_z_max , _dt , false ) ;
2021-06-26 10:49:12 -03:00
// update the vertical position, velocity and acceleration offsets
update_pos_offset_z ( pos_offset_z ) ;
// add terrain offsets
_pos_target . z + = _pos_offset_z ;
_vel_desired . z + = _vel_offset_z ;
_accel_desired . z + = _accel_offset_z ;
2014-05-27 10:44:57 -03:00
}
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 AC_PosControl : : pos_offset_z_scaler ( float pos_offset_z , float pos_offset_z_buffer ) const
2021-07-08 04:09:05 -03:00
{
if ( is_zero ( pos_offset_z_buffer ) ) {
return 1.0 ;
}
const Vector3f curr_pos = _inav . get_position ( ) ;
2021-07-14 11:18:45 -03:00
float pos_offset_error_z = curr_pos . z - ( _pos_target . z - _pos_offset_z + pos_offset_z ) ;
2021-07-08 04:09:05 -03:00
return constrain_float ( ( 1.0 - ( fabsf ( pos_offset_error_z ) - 0.5 * pos_offset_z_buffer ) / ( 0.5 * pos_offset_z_buffer ) ) , 0.01 , 1.0 ) ;
}
2021-04-16 12:53:14 -03:00
///
/// Lateral position controller
///
2021-07-08 01:13:05 -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 AC_PosControl : : set_max_speed_accel_xy ( float speed_cms , float accel_cmss )
2021-04-16 12:53:14 -03:00
{
// return immediately if no change
2021-06-20 05:17:25 -03:00
if ( ( is_equal ( _vel_max_xy_cms , speed_cms ) & & is_equal ( _accel_max_xy_cmss , accel_cmss ) ) ) {
2020-02-19 02:08:16 -04:00
return ;
}
2021-04-16 12:53:14 -03:00
_vel_max_xy_cms = speed_cms ;
_accel_max_xy_cmss = accel_cmss ;
2020-02-19 02:08:16 -04:00
2021-08-06 06:40:56 -03:00
// ensure the horizontal jerk is less than the vehicle is capable of
const float jerk_max_cmsss = MIN ( _attitude_control . get_ang_vel_roll_max_rads ( ) , _attitude_control . get_ang_vel_pitch_max_rads ( ) ) * GRAVITY_MSS * 100.0 ;
const float snap_max_cmssss = MIN ( _attitude_control . get_accel_roll_max_radss ( ) , _attitude_control . get_accel_pitch_max_radss ( ) ) * GRAVITY_MSS * 100.0 ;
// get specified jerk limit
_jerk_xy_max = _shaping_jerk_xy * 100.0 ;
// limit maximum jerk based on maximum angular rate
if ( is_positive ( jerk_max_cmsss ) & & _attitude_control . get_bf_feedforward ( ) ) {
_jerk_xy_max = MIN ( _jerk_xy_max , jerk_max_cmsss ) ;
}
// limit maximum jerk to maximum possible average jerk based on angular acceleration
if ( is_positive ( snap_max_cmssss ) & & _attitude_control . get_bf_feedforward ( ) ) {
_jerk_xy_max = MIN ( 0.5 * safe_sqrt ( _accel_max_xy_cmss * snap_max_cmssss ) , _jerk_xy_max ) ;
2014-01-23 23:27:06 -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 AC_PosControl : : set_correction_speed_accel_xy ( float speed_cms , float accel_cmss )
{
_p_pos_xy . set_limits ( speed_cms , accel_cmss , 0.0f ) ;
}
2021-04-16 12:53:14 -03: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.
void AC_PosControl : : init_xy_controller ( )
2014-01-23 23:27:06 -04:00
{
2021-04-16 12:53:14 -03:00
init_xy ( ) ;
2020-02-19 02:08:16 -04:00
2021-04-16 12:53:14 -03:00
// set resultant acceleration to current attitude target
Vector3f accel_target ;
lean_angles_to_accel_xy ( accel_target . x , accel_target . y ) ;
_pid_vel_xy . set_integrator ( accel_target - _accel_desired ) ;
2014-01-23 23:27:06 -04: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 AC_PosControl : : init_xy_controller_stopping_point ( )
2014-01-23 23:27:06 -04:00
{
2021-04-16 12:53:14 -03:00
init_xy ( ) ;
2021-06-22 12:07:05 -03:00
2021-06-21 04:26:39 -03:00
get_stopping_point_xy_cm ( _pos_target . xy ( ) ) ;
2021-08-22 02:14:43 -03:00
_vel_desired . xy ( ) . zero ( ) ;
2021-06-21 04:26:39 -03:00
_accel_desired . xy ( ) . zero ( ) ;
2021-04-16 12:53:14 -03:00
2021-06-22 12:07:05 -03:00
_pid_vel_xy . set_integrator ( _accel_target ) ;
2021-04-16 12:53:14 -03:00
}
2014-01-23 23:27:06 -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 AC_PosControl : : relax_velocity_controller_xy ( )
{
init_xy ( ) ;
// decay resultant acceleration and therefore current attitude target to zero
float decay = 1.0 - _dt / ( _dt + POSCONTROL_RELAX_TC ) ;
2021-06-22 12:07:05 -03:00
2021-04-16 12:53:14 -03:00
_accel_target . x * = decay ;
_accel_target . y * = decay ;
2021-06-22 12:07:05 -03:00
_pid_vel_xy . set_integrator ( _accel_target - _accel_desired ) ;
2021-04-16 12:53:14 -03:00
}
/// init_xy - initialise the position controller to the current position, velocity and acceleration.
2021-05-19 11:09:31 -03:00
/// This function is private and contains all the shared xy axis initialisation functions
2021-04-16 12:53:14 -03:00
void AC_PosControl : : init_xy ( )
{
// set roll, pitch lean angle targets to current attitude
const Vector3f & att_target_euler_cd = _attitude_control . get_att_target_euler_cd ( ) ;
_roll_target = att_target_euler_cd . x ;
_pitch_target = att_target_euler_cd . y ;
_yaw_target = att_target_euler_cd . z ; // todo: this should be thrust vector heading, not yaw.
_yaw_rate_target = 0.0f ;
2021-05-19 11:09:31 -03:00
const Vector3f curr_pos = _inav . get_position ( ) ;
2021-04-16 12:53:14 -03:00
_pos_target . x = curr_pos . x ;
_pos_target . y = curr_pos . y ;
const Vector3f & curr_vel = _inav . get_velocity ( ) ;
_vel_desired . x = curr_vel . x ;
_vel_desired . y = curr_vel . y ;
_vel_target . x = curr_vel . x ;
_vel_target . y = curr_vel . y ;
2014-01-23 23:27:06 -04:00
2021-04-16 12:53:14 -03:00
const Vector3f & curr_accel = _ahrs . get_accel_ef_blended ( ) * 100.0f ;
_accel_desired . x = curr_accel . x ;
_accel_desired . y = curr_accel . y ;
2021-06-22 12:07:05 -03:00
lean_angles_to_accel_xy ( _accel_target . x , _accel_target . y ) ;
// initialise I terms from lean angles
_pid_vel_xy . reset_filter ( ) ;
_pid_vel_xy . set_integrator ( _accel_target - _accel_desired ) ;
2021-04-16 12:53:14 -03:00
// initialise ekf xy reset handler
init_ekf_xy_reset ( ) ;
// initialise z_controller time out
_last_update_xy_us = AP_HAL : : micros64 ( ) ;
2014-01-23 23:27:06 -04:00
}
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 AC_PosControl : : input_accel_xy ( const Vector3f & accel )
{
// check for ekf xy position reset
handle_ekf_xy_reset ( ) ;
update_pos_vel_accel_xy ( _pos_target . xy ( ) , _vel_desired . xy ( ) , _accel_desired . xy ( ) , _dt , _limit_vector . xy ( ) ) ;
2021-08-06 06:40:56 -03:00
shape_accel_xy ( accel , _accel_desired , _jerk_xy_max , _dt ) ;
2021-06-20 03:02:24 -03:00
}
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.
/// The vel is projected forwards in time based on a time step of dt and acceleration accel.
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.
/// 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 AC_PosControl : : input_vel_accel_xy ( Vector2f & vel , const Vector2f & accel , bool limit_output )
2015-10-27 10:04:24 -03:00
{
2021-06-21 04:26:39 -03:00
update_pos_vel_accel_xy ( _pos_target . xy ( ) , _vel_desired . xy ( ) , _accel_desired . xy ( ) , _dt , _limit_vector . xy ( ) ) ;
2021-04-16 12:53:14 -03:00
2021-06-21 04:26:39 -03:00
shape_vel_accel_xy ( vel , accel , _vel_desired . xy ( ) , _accel_desired . xy ( ) ,
2021-08-06 06:40:56 -03:00
_accel_max_xy_cmss , _jerk_xy_max , _dt , limit_output ) ;
2021-04-16 12:53:14 -03:00
2021-06-21 04:26:39 -03:00
update_vel_accel_xy ( vel , accel , _dt , Vector2f ( ) ) ;
2015-10-27 10:04:24 -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.
/// The pos and vel are projected forwards in time based on a time step of dt and acceleration accel.
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 AC_PosControl : : input_pos_vel_accel_xy ( Vector2p & pos , Vector2f & vel , const Vector2f & accel , bool limit_output )
2014-01-23 23:27:06 -04:00
{
2021-06-21 04:26:39 -03:00
update_pos_vel_accel_xy ( _pos_target . xy ( ) , _vel_desired . xy ( ) , _accel_desired . xy ( ) , _dt , _limit_vector . xy ( ) ) ;
2021-04-16 12:53:14 -03:00
2021-06-21 04:26:39 -03:00
shape_pos_vel_accel_xy ( pos , vel , accel , _pos_target . xy ( ) , _vel_desired . xy ( ) , _accel_desired . xy ( ) ,
2021-08-06 06:40:56 -03:00
_vel_max_xy_cms , _accel_max_xy_cmss , _jerk_xy_max , _dt , limit_output ) ;
2021-04-16 12:53:14 -03:00
2021-06-21 04:26:39 -03:00
update_pos_vel_accel_xy ( pos , vel , accel , _dt , Vector2f ( ) ) ;
2021-04-16 12:53:14 -03:00
}
/// stop_pos_xy_stabilisation - sets the target to the current position to remove any position corrections from the system
void AC_PosControl : : stop_pos_xy_stabilisation ( )
{
2021-05-19 11:09:31 -03:00
const Vector3f & curr_pos = _inav . get_position ( ) ;
2021-04-16 12:53:14 -03:00
_pos_target . x = curr_pos . x ;
_pos_target . y = curr_pos . y ;
}
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 AC_PosControl : : stop_vel_xy_stabilisation ( )
{
2021-05-19 11:09:31 -03:00
const Vector3f curr_pos = _inav . get_position ( ) ;
2021-04-16 12:53:14 -03:00
_pos_target . x = curr_pos . x ;
_pos_target . y = curr_pos . y ;
const Vector3f & curr_vel = _inav . get_velocity ( ) ;
_vel_desired . x = curr_vel . x ;
_vel_desired . y = curr_vel . y ;
2021-08-22 02:14:43 -03:00
// with zero position error _vel_target = _vel_desired
2021-04-16 12:53:14 -03:00
_vel_target . x = curr_vel . x ;
_vel_target . y = curr_vel . y ;
// initialise I terms from lean angles
_pid_vel_xy . reset_filter ( ) ;
_pid_vel_xy . reset_I ( ) ;
}
// is_active_xy - returns true if the xy position controller has been run in the previous 5 loop times
bool AC_PosControl : : is_active_xy ( ) const
{
return ( ( AP_HAL : : micros64 ( ) - _last_update_xy_us ) < = _dt * 5000000.0 ) ;
}
/// 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 AC_PosControl : : update_xy_controller ( )
{
2021-06-23 09:08:14 -03:00
// check for ekf xy position reset
handle_ekf_xy_reset ( ) ;
2021-04-16 12:53:14 -03:00
// Check for position control time out
if ( ! is_active_xy ( ) ) {
init_xy_controller ( ) ;
2021-05-17 22:24:41 -03:00
if ( has_good_timing ( ) ) {
// call internal error because initialisation has not been done
INTERNAL_ERROR ( AP_InternalError : : error_t : : flow_of_control ) ;
}
2015-10-27 10:45:16 -03:00
}
2021-04-16 12:53:14 -03:00
_last_update_xy_us = AP_HAL : : micros64 ( ) ;
2015-10-27 10:45:16 -03:00
2021-04-16 12:53:14 -03:00
float ekfGndSpdLimit , ekfNavVelGainScaler ;
AP : : ahrs_navekf ( ) . getEkfControlLimits ( ekfGndSpdLimit , ekfNavVelGainScaler ) ;
// Position Controller
2015-04-13 06:22:52 -03:00
2021-04-16 12:53:14 -03:00
const Vector3f & curr_pos = _inav . get_position ( ) ;
Vector2f vel_target = _p_pos_xy . update_all ( _pos_target . x , _pos_target . y , curr_pos , _limit . pos_xy ) ;
// add velocity feed-forward scaled to compensate for optical flow measurement induced EKF noise
vel_target * = ekfNavVelGainScaler ;
_vel_target . x = vel_target . x ;
_vel_target . y = vel_target . y ;
_vel_target . x + = _vel_desired . x ;
_vel_target . y + = _vel_desired . y ;
2015-04-13 06:22:52 -03:00
2021-04-16 12:53:14 -03:00
// Velocity Controller
2020-01-04 02:16:09 -04:00
2021-04-16 12:53:14 -03:00
// check if vehicle velocity is being overridden
// todo: remove this and use input shaping
if ( _flags . vehicle_horiz_vel_override ) {
_flags . vehicle_horiz_vel_override = false ;
2020-01-04 02:16:09 -04:00
} else {
2021-04-16 12:53:14 -03:00
_vehicle_horiz_vel . x = _inav . get_velocity ( ) . x ;
_vehicle_horiz_vel . y = _inav . get_velocity ( ) . y ;
}
Vector2f accel_target = _pid_vel_xy . update_all ( Vector2f { _vel_target . x , _vel_target . y } , _vehicle_horiz_vel , Vector2f ( _limit_vector . x , _limit_vector . y ) ) ;
// acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise
accel_target * = ekfNavVelGainScaler ;
// pass the correction acceleration to the target acceleration output
_accel_target . x = accel_target . x ;
_accel_target . y = accel_target . y ;
2015-04-13 06:22:52 -03:00
2021-04-16 12:53:14 -03:00
// Add feed forward into the target acceleration output
_accel_target . x + = _accel_desired . x ;
_accel_target . y + = _accel_desired . y ;
// Acceleration Controller
// limit acceleration using maximum lean angles
2021-06-20 05:17:25 -03:00
_limit_vector . x = 0.0f ;
_limit_vector . y = 0.0f ;
2021-04-16 12:53:14 -03:00
float angle_max = MIN ( _attitude_control . get_althold_lean_angle_max ( ) , get_lean_angle_max_cd ( ) ) ;
float accel_max = GRAVITY_MSS * 100.0f * tanf ( ToRad ( angle_max * 0.01f ) ) ;
if ( _accel_target . limit_length_xy ( accel_max ) ) {
_limit_vector . x = _accel_target . x ;
_limit_vector . y = _accel_target . y ;
2014-01-23 23:27:06 -04:00
}
2021-04-16 12:53:14 -03:00
// update angle targets that will be passed to stabilize controller
accel_to_lean_angles ( _accel_target . x , _accel_target . y , _roll_target , _pitch_target ) ;
calculate_yaw_and_rate_yaw ( ) ;
2014-01-23 23:27:06 -04:00
}
2021-04-16 12:53:14 -03:00
///
/// Vertical position controller
///
2021-07-08 01:13:05 -03:00
/// set_max_speed_accel_z - set the maximum vertical 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.
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-04-16 12:53:14 -03:00
void AC_PosControl : : set_max_speed_accel_z ( float speed_down , float speed_up , float accel_cmss )
2015-04-30 10:18:05 -03:00
{
2021-04-16 12:53:14 -03:00
// ensure speed_down is always negative
speed_down = - fabsf ( speed_down ) ;
// exit immediately if no change in speed up or down
if ( is_equal ( _vel_max_down_cms , speed_down ) & & is_equal ( _vel_max_up_cms , speed_up ) & & is_equal ( _accel_max_z_cmss , accel_cmss ) ) {
return ;
}
// sanity check and update
2021-05-19 11:09:31 -03:00
if ( is_negative ( speed_down ) ) {
2021-04-16 12:53:14 -03:00
_vel_max_down_cms = speed_down ;
}
2021-05-19 11:09:31 -03:00
if ( is_positive ( speed_up ) ) {
2021-04-16 12:53:14 -03:00
_vel_max_up_cms = speed_up ;
}
if ( is_positive ( accel_cmss ) ) {
_accel_max_z_cmss = accel_cmss ;
}
2021-08-06 06:40:56 -03:00
// ensure the vertical Jerk is not limited by the filters in the Z accel PID object
_jerk_z_max = _shaping_jerk_z * 100.0 ;
2021-04-16 12:53:14 -03:00
if ( is_positive ( _pid_accel_z . filt_T_hz ( ) ) ) {
2021-08-06 06:40:56 -03:00
_jerk_z_max = MIN ( _jerk_z_max , MIN ( GRAVITY_MSS * 100.0 , _accel_max_z_cmss ) * ( M_2PI * _pid_accel_z . filt_T_hz ( ) ) / 5.0 ) ;
2021-04-16 12:53:14 -03:00
}
if ( is_positive ( _pid_accel_z . filt_E_hz ( ) ) ) {
2021-08-06 06:40:56 -03:00
_jerk_z_max = MIN ( _jerk_z_max , MIN ( GRAVITY_MSS * 100.0 , _accel_max_z_cmss ) * ( M_2PI * _pid_accel_z . filt_E_hz ( ) ) / 5.0 ) ;
2021-04-16 12:53:14 -03:00
}
2015-04-30 10:18:05 -03:00
}
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 AC_PosControl : : set_correction_speed_accel_z ( float speed_down , float speed_up , float accel_cmss )
{
// define maximum position error and maximum first and second differential limits
_p_pos_z . set_limits ( - fabsf ( speed_down ) , _vel_max_up_cms , _accel_max_z_cmss , 0.0f ) ;
}
2021-04-16 12:53:14 -03: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.
void AC_PosControl : : init_z_controller ( )
2016-11-22 01:36:17 -04:00
{
2021-04-16 12:53:14 -03:00
// Initialise the position controller to the current position, velocity and acceleration.
init_z ( ) ;
// Set accel PID I term based on the current throttle
_pid_accel_z . set_integrator ( ( _attitude_control . get_throttle_in ( ) - _motors . get_throttle_hover ( ) ) * 1000.0f ) ;
2016-11-22 01:36:17 -04:00
}
2021-04-16 12:53:14 -03: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 does not allow any negative velocity or acceleration
void AC_PosControl : : init_z_controller_no_descent ( )
2015-04-14 11:26:33 -03:00
{
2021-04-16 12:53:14 -03:00
// Initialise the position controller to the current throttle, position, velocity and acceleration.
init_z_controller ( ) ;
2021-05-19 11:09:31 -03:00
// remove all descent if present
2021-08-15 00:40:09 -03:00
_vel_desired . z = MAX ( 0.0 , _vel_desired . z ) ;
_vel_target . z = MAX ( 0.0 , _vel_target . z ) ;
_accel_desired . z = MAX ( 0.0 , _accel_desired . z ) ;
_accel_target . z = MAX ( 0.0 , _accel_target . z ) ;
2015-04-14 11:26:33 -03:00
}
2021-04-16 12:53:14 -03:00
/// 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 AC_PosControl : : init_z_controller_stopping_point ( )
2014-01-05 23:30:51 -04:00
{
2021-04-16 12:53:14 -03:00
// Initialise the position controller to the current throttle, position, velocity and acceleration.
init_z_controller ( ) ;
2021-06-21 04:26:39 -03:00
get_stopping_point_z_cm ( _pos_target . z ) ;
2021-08-22 02:14:43 -03:00
_vel_desired . z = 0.0f ;
_accel_desired . z = 0.0f ;
2014-01-05 23:30:51 -04:00
}
2021-04-16 12:53:14 -03:00
// 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 AC_PosControl : : relax_z_controller ( float throttle_setting )
2013-12-28 10:04:45 -04:00
{
2021-04-16 12:53:14 -03:00
// Initialise the position controller to the current position, velocity and acceleration.
init_z ( ) ;
2014-04-30 09:22:47 -03:00
2021-04-16 12:53:14 -03:00
// Set accel PID I term based on the requested throttle
float throttle = _attitude_control . get_throttle_in ( ) ;
throttle_setting = throttle + ( throttle_setting - throttle ) * ( _dt / ( _dt + POSCONTROL_RELAX_TC ) ) ;
_pid_accel_z . set_integrator ( ( throttle_setting - _motors . get_throttle_hover ( ) ) * 1000.0f ) ;
2014-01-25 04:23:55 -04:00
}
2021-04-16 12:53:14 -03:00
/// init_z - initialise the position controller to the current position, velocity and acceleration.
2021-05-19 11:09:31 -03:00
/// This function is private and contains all the shared z axis initialisation functions
2021-04-16 12:53:14 -03:00
void AC_PosControl : : init_z ( )
2014-01-25 04:23:55 -04:00
{
2021-05-19 11:09:31 -03:00
const Vector3f curr_pos = _inav . get_position ( ) ;
2021-04-16 12:53:14 -03:00
_pos_target . z = curr_pos . z ;
2014-01-25 04:23:55 -04:00
2021-04-16 12:53:14 -03:00
const Vector3f & curr_vel = _inav . get_velocity ( ) ;
_vel_desired . z = curr_vel . z ;
2021-08-22 02:14:43 -03:00
// with zero position error _vel_target = _vel_desired
2021-04-16 12:53:14 -03:00
_vel_target . z = curr_vel . z ;
2013-12-28 10:04:45 -04:00
2021-04-16 12:53:14 -03:00
const Vector3f & curr_accel = _ahrs . get_accel_ef_blended ( ) ;
2014-05-02 03:26:04 -03:00
2021-04-16 12:53:14 -03:00
// Reset I term of velocity PID
_pid_vel_z . reset_filter ( ) ;
_pid_vel_z . set_integrator ( 0.0f ) ;
2017-03-14 08:50:32 -03:00
2021-08-15 00:40:09 -03:00
_accel_desired . z = constrain_float ( - ( curr_accel . z + GRAVITY_MSS ) * 100.0f , - _accel_max_z_cmss , _accel_max_z_cmss ) ;
2021-04-16 12:53:14 -03:00
_accel_target . z = - ( curr_accel . z + GRAVITY_MSS ) * 100.0f ;
_pid_accel_z . reset_filter ( ) ;
2013-12-28 10:04:45 -04:00
2021-06-26 10:49:12 -03:00
// initialise vertical offsets
_pos_offset_z = 0.0 ;
_vel_offset_z = 0.0 ;
_accel_offset_z = 0.0 ;
2021-04-16 12:53:14 -03:00
// initialise ekf z reset handler
init_ekf_z_reset ( ) ;
// initialise z_controller time out
_last_update_z_us = AP_HAL : : micros64 ( ) ;
2013-12-28 07:15:29 -04: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.
/// The vel is projected forwards in time based on a time step of dt and acceleration accel.
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 vel to be the kinematic path based on accel
2021-07-09 02:28:58 -03:00
void AC_PosControl : : input_accel_z ( const float accel )
{
// calculated increased maximum acceleration if over speed
float accel_z_cmss = _accel_max_z_cmss ;
if ( _vel_desired . z < _vel_max_down_cms & & ! is_zero ( _vel_max_down_cms ) ) {
accel_z_cmss * = POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired . z / _vel_max_down_cms ;
}
if ( _vel_desired . z > _vel_max_up_cms & & ! is_zero ( _vel_max_up_cms ) ) {
accel_z_cmss * = POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired . z / _vel_max_up_cms ;
}
// adjust desired alt if motors have not hit their limits
update_pos_vel_accel ( _pos_target . z , _vel_desired . z , _accel_desired . z , _dt , _limit_vector . z ) ;
2021-08-06 06:40:56 -03:00
shape_accel ( accel , _accel_desired . z , _jerk_z_max , _dt ) ;
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.
/// 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 AC_PosControl : : input_vel_accel_z ( float & vel , const float accel , bool ignore_descent_limit , bool limit_output )
2013-12-30 09:12:59 -04:00
{
2021-06-22 00:08:34 -03:00
if ( ignore_descent_limit ) {
2021-04-16 12:53:14 -03:00
// turn off limits in the negative z direction
_limit_vector . z = MAX ( _limit_vector . z , 0.0f ) ;
}
2013-12-30 09:12:59 -04:00
2021-04-16 12:53:14 -03:00
// calculated increased maximum acceleration if over speed
float accel_z_cmss = _accel_max_z_cmss ;
if ( _vel_desired . z < _vel_max_down_cms & & ! is_zero ( _vel_max_down_cms ) ) {
accel_z_cmss * = POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired . z / _vel_max_down_cms ;
}
if ( _vel_desired . z > _vel_max_up_cms & & ! is_zero ( _vel_max_up_cms ) ) {
accel_z_cmss * = POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired . z / _vel_max_up_cms ;
}
2016-11-22 01:36:17 -04:00
2021-04-16 12:53:14 -03:00
// adjust desired alt if motors have not hit their limits
2021-06-21 04:26:39 -03:00
update_pos_vel_accel ( _pos_target . z , _vel_desired . z , _accel_desired . z , _dt , _limit_vector . z ) ;
2021-04-16 12:53:14 -03:00
2021-06-21 04:26:39 -03:00
shape_vel_accel ( vel , accel ,
_vel_desired . z , _accel_desired . z ,
- constrain_float ( accel_z_cmss , 0.0f , 750.0f ) , accel_z_cmss ,
2021-08-06 06:40:56 -03:00
_jerk_z_max , _dt , limit_output ) ;
2021-04-16 12:53:14 -03:00
2021-06-21 04:26:39 -03:00
update_vel_accel ( vel , accel , _dt , 0 ) ;
2013-12-30 09:12:59 -04:00
}
2021-04-16 12:53:14 -03:00
/// set_pos_target_z_from_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.
void AC_PosControl : : set_pos_target_z_from_climb_rate_cm ( const float vel , bool ignore_descent_limit )
2014-05-02 00:08:18 -03:00
{
2021-06-21 04:26:39 -03:00
float vel2 = vel ;
input_vel_accel_z ( vel2 , 0 , ignore_descent_limit ) ;
2014-05-02 00:08:18 -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.
/// The pos and vel are projected forwards in time based on a time step of dt and acceleration accel.
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 AC_PosControl : : input_pos_vel_accel_z ( float & pos , float & vel , const float accel , bool limit_output )
2013-12-28 10:04:45 -04:00
{
2021-04-16 12:53:14 -03:00
// calculated increased maximum acceleration if over speed
float accel_z_cmss = _accel_max_z_cmss ;
if ( _vel_desired . z < _vel_max_down_cms & & ! is_zero ( _vel_max_down_cms ) ) {
accel_z_cmss * = POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired . z / _vel_max_down_cms ;
}
if ( _vel_desired . z > _vel_max_up_cms & & ! is_zero ( _vel_max_up_cms ) ) {
accel_z_cmss * = POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired . z / _vel_max_up_cms ;
2014-05-02 00:08:18 -03:00
}
2021-06-26 10:49:12 -03:00
// adjust desired altitude if motors have not hit their limits
2021-06-21 04:26:39 -03:00
update_pos_vel_accel ( _pos_target . z , _vel_desired . z , _accel_desired . z , _dt , _limit_vector . z ) ;
2016-11-22 01:36:17 -04:00
2021-06-21 04:26:39 -03:00
shape_pos_vel_accel ( pos , vel , accel ,
_pos_target . z , _vel_desired . z , _accel_desired . z ,
2021-08-06 06:40:56 -03:00
_vel_max_down_cms , _vel_max_up_cms ,
2021-06-21 04:26:39 -03:00
- constrain_float ( accel_z_cmss , 0.0f , 750.0f ) , accel_z_cmss ,
2021-08-06 06:40:56 -03:00
_jerk_z_max , _dt , limit_output ) ;
2014-01-23 23:27:06 -04:00
2021-06-17 22:19:29 -03:00
postype_t posp = pos ;
update_pos_vel_accel ( posp , vel , accel , _dt , 0 ) ;
pos = posp ;
2013-12-28 10:04:45 -04:00
}
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-04-16 12:53:14 -03:00
void AC_PosControl : : set_alt_target_with_slew ( const float & pos )
2013-12-28 10:04:45 -04:00
{
2021-06-21 04:26:39 -03:00
float posf = pos ;
float zero = 0 ;
input_pos_vel_accel_z ( posf , zero , 0 ) ;
2014-01-23 23:27:06 -04:00
}
2021-06-26 10:49:12 -03:00
/// update_pos_offset_z - updates the vertical offsets used by terrain following
void AC_PosControl : : update_pos_offset_z ( float pos_offset_z )
{
2021-07-14 11:18:45 -03:00
postype_t p_offset_z = _pos_offset_z ;
update_pos_vel_accel ( p_offset_z , _vel_offset_z , _accel_offset_z , _dt , MIN ( _limit_vector . z , 0.0f ) ) ;
_pos_offset_z = p_offset_z ;
2021-06-26 10:49:12 -03:00
// input shape the terrain offset
shape_pos_vel_accel ( pos_offset_z , 0.0f , 0.0f ,
_pos_offset_z , _vel_offset_z , _accel_offset_z ,
2021-08-06 06:40:56 -03:00
get_max_speed_down_cms ( ) , get_max_speed_up_cms ( ) ,
- get_max_accel_z_cmss ( ) , get_max_accel_z_cmss ( ) ,
_jerk_z_max , _dt , false ) ;
2021-06-26 10:49:12 -03:00
}
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
bool AC_PosControl : : is_active_z ( ) const
2014-01-23 23:27:06 -04:00
{
2021-04-16 12:53:14 -03:00
return ( ( AP_HAL : : micros64 ( ) - _last_update_z_us ) < = _dt * 5000000.0 ) ;
}
2015-10-27 10:44:41 -03:00
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
void AC_PosControl : : update_z_controller ( )
{
2021-06-23 09:08:14 -03:00
// check for ekf z-axis position reset
handle_ekf_z_reset ( ) ;
2021-04-16 12:53:14 -03:00
// Check for z_controller time out
if ( ! is_active_z ( ) ) {
init_z_controller ( ) ;
2021-05-17 22:24:41 -03:00
if ( has_good_timing ( ) ) {
// call internal error because initialisation has not been done
INTERNAL_ERROR ( AP_InternalError : : error_t : : flow_of_control ) ;
}
2021-04-16 12:53:14 -03:00
}
_last_update_z_us = AP_HAL : : micros64 ( ) ;
2021-05-19 11:09:31 -03:00
const float curr_alt = _inav . get_position ( ) . z ;
2020-01-04 02:16:09 -04:00
// calculate the target velocity correction
2021-06-17 22:19:29 -03:00
float pos_target_zf = _pos_target . z ;
2021-06-26 10:49:12 -03:00
2021-06-17 22:19:29 -03:00
_vel_target . z = _p_pos_z . update_all ( pos_target_zf , curr_alt , _limit . pos_down , _limit . pos_up ) ;
2021-06-26 10:49:12 -03:00
2021-06-17 22:19:29 -03:00
_pos_target . z = pos_target_zf ;
2021-04-16 12:53:14 -03:00
2015-04-13 06:22:52 -03:00
// add feed forward component
2021-04-16 12:53:14 -03:00
_vel_target . z + = _vel_desired . z ;
2015-04-13 06:22:52 -03:00
2020-01-04 02:16:09 -04:00
// Velocity Controller
2013-12-28 07:15:29 -04:00
2013-12-28 10:04:45 -04:00
const Vector3f & curr_vel = _inav . get_velocity ( ) ;
2021-04-16 12:53:14 -03:00
_accel_target . z = _pid_vel_z . update_all ( _vel_target . z , curr_vel . z , _motors . limit . throttle_lower , _motors . limit . throttle_upper ) ;
2021-06-26 10:49:12 -03:00
// add feed forward component
2017-11-12 08:50:19 -04:00
_accel_target . z + = _accel_desired . z ;
2013-12-28 10:04:45 -04:00
2020-01-04 02:16:09 -04:00
// Acceleration Controller
2013-12-28 10:04:45 -04:00
2021-05-19 11:09:31 -03:00
// Calculate vertical acceleration
2021-01-29 00:43:26 -04:00
const float z_accel_meas = get_z_accel_cmss ( ) ;
2016-09-05 01:49:56 -03:00
// ensure imax is always large enough to overpower hover throttle
if ( _motors . get_throttle_hover ( ) * 1000.0f > _pid_accel_z . imax ( ) ) {
_pid_accel_z . imax ( _motors . get_throttle_hover ( ) * 1000.0f ) ;
}
2019-09-14 01:02:02 -03:00
float thr_out ;
if ( _vibe_comp_enabled ) {
2020-01-04 02:16:09 -04:00
thr_out = get_throttle_with_vibration_override ( ) ;
2019-09-14 01:02:02 -03:00
} else {
thr_out = _pid_accel_z . update_all ( _accel_target . z , z_accel_meas , ( _motors . limit . throttle_lower | | _motors . limit . throttle_upper ) ) * 0.001f ;
2020-01-04 02:16:09 -04:00
thr_out + = _pid_accel_z . get_ff ( ) * 0.001f ;
2019-09-14 01:02:02 -03:00
}
thr_out + = _motors . get_throttle_hover ( ) ;
2015-03-16 01:26:42 -03:00
2020-01-04 02:16:09 -04:00
// Actuator commands
2014-10-04 11:31:33 -03:00
// send throttle to attitude controller with angle boost
2021-05-19 11:09:31 -03:00
_attitude_control . set_throttle_out ( thr_out , true , POSCONTROL_THROTTLE_CUTOFF_FREQ_HZ ) ;
2019-06-17 12:36:22 -03:00
2020-01-04 02:16:09 -04:00
// Check for vertical controller health
2019-06-17 12:36:22 -03:00
// _speed_down_cms is checked to be non-zero when set
2021-06-22 11:39:44 -03:00
float error_ratio = _pid_vel_z . get_error ( ) / _vel_max_down_cms ;
2021-05-19 11:09:31 -03:00
_vel_z_control_ratio + = _dt * 0.1f * ( 0.5 - error_ratio ) ;
2019-06-17 12:36:22 -03:00
_vel_z_control_ratio = constrain_float ( _vel_z_control_ratio , 0.0f , 2.0f ) ;
2013-12-28 07:15:29 -04:00
2021-04-16 12:53:14 -03:00
// set vertical component of the limit vector
if ( _motors . limit . throttle_upper ) {
_limit_vector . z = 1.0f ;
} else if ( _motors . limit . throttle_lower ) {
_limit_vector . z = - 1.0f ;
} else {
_limit_vector . z = 0.0f ;
2020-01-04 02:16:09 -04:00
}
}
2014-01-23 23:27:06 -04:00
///
2021-05-19 11:09:31 -03:00
/// Accessors
2014-01-23 23:27:06 -04:00
///
2013-12-28 07:15:29 -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 AC_PosControl : : get_stopping_point_z_cm ( postype_t & stopping_point ) const
2013-12-28 07:15:29 -04:00
{
2021-04-16 12:53:14 -03:00
const float curr_pos_z = _inav . get_position ( ) . z ;
float curr_vel_z = _inav . get_velocity ( ) . z ;
// avoid divide by zero by using current position if kP is very low or acceleration is zero
2021-05-19 11:09:31 -03:00
if ( ! is_positive ( _p_pos_z . kP ( ) ) | | ! is_positive ( _accel_max_z_cmss ) ) {
2021-06-21 04:26:39 -03:00
stopping_point = curr_pos_z ;
2020-02-19 02:08:16 -04:00
return ;
2013-12-28 07:15:29 -04:00
}
2020-02-19 02:08:16 -04:00
2021-06-21 04:26:39 -03:00
stopping_point = curr_pos_z + constrain_float ( stopping_distance ( curr_vel_z , _p_pos_z . kP ( ) , _accel_max_z_cmss ) , - POSCONTROL_STOPPING_DIST_DOWN_MAX , POSCONTROL_STOPPING_DIST_UP_MAX ) ;
2013-12-28 07:15:29 -04:00
}
2014-01-17 22:53:46 -04:00
2021-04-16 12:53:14 -03:00
/// get_lean_angle_max_cd - returns the maximum lean angle the autopilot may request
float AC_PosControl : : get_lean_angle_max_cd ( ) const
2014-01-17 22:53:46 -04:00
{
2021-04-16 12:53:14 -03:00
if ( is_zero ( _lean_angle_max ) ) {
return _attitude_control . lean_angle_max ( ) ;
}
return _lean_angle_max * 100.0f ;
2014-01-17 22:53:46 -04:00
}
2020-01-04 02:16:09 -04:00
/// set position, velocity and acceleration targets
2021-06-17 22:19:29 -03:00
void AC_PosControl : : set_pos_vel_accel ( const Vector3p & pos , const Vector3f & vel , const Vector3f & accel )
2020-01-04 02:16:09 -04:00
{
_pos_target = pos ;
_vel_desired = vel ;
_accel_desired = accel ;
}
2021-04-16 12:53:14 -03:00
/// set position, velocity and acceleration targets
2021-06-17 22:19:29 -03:00
void AC_PosControl : : set_pos_vel_accel_xy ( const Vector2p & pos , const Vector2f & vel , const Vector2f & accel )
2014-05-06 05:32:30 -03:00
{
2021-04-16 12:53:14 -03:00
_pos_target . x = pos . x ;
_pos_target . y = pos . y ;
_vel_desired . x = vel . x ;
_vel_desired . y = vel . y ;
_accel_desired . x = accel . x ;
_accel_desired . y = accel . y ;
2014-05-06 05:32:30 -03:00
}
2021-04-16 12:53:14 -03:00
// get_lean_angles_to_accel - convert roll, pitch lean target angles to lat/lon frame accelerations in cm/s/s
Vector3f AC_PosControl : : lean_angles_to_accel ( const Vector3f & att_target_euler ) const
2015-10-29 04:30:14 -03:00
{
2021-04-16 12:53:14 -03:00
// rotate our roll, pitch angles into lat/lon frame
const float sin_roll = sinf ( att_target_euler . x ) ;
const float cos_roll = cosf ( att_target_euler . x ) ;
const float sin_pitch = sinf ( att_target_euler . y ) ;
const float cos_pitch = cosf ( att_target_euler . y ) ;
const float sin_yaw = sinf ( att_target_euler . z ) ;
const float cos_yaw = cosf ( att_target_euler . z ) ;
2021-05-19 11:09:31 -03:00
return Vector3f {
( GRAVITY_MSS * 100 ) * ( - cos_yaw * sin_pitch * cos_roll - sin_yaw * sin_roll ) / MAX ( cos_roll * cos_pitch , 0.1f ) ,
( GRAVITY_MSS * 100 ) * ( - sin_yaw * sin_pitch * cos_roll + cos_yaw * sin_roll ) / MAX ( cos_roll * cos_pitch , 0.1f ) ,
( GRAVITY_MSS * 100 )
} ;
2015-10-29 04:30:14 -03:00
}
2021-04-16 12:53:14 -03:00
// returns the NED target acceleration vector for attitude control
Vector3f AC_PosControl : : get_thrust_vector ( ) const
2014-04-30 09:22:47 -03:00
{
2021-04-16 12:53:14 -03:00
Vector3f accel_target = get_accel_target_cmss ( ) ;
accel_target . z = - GRAVITY_MSS * 100.0f ;
return accel_target ;
2014-04-30 09:22:47 -03:00
}
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-05-19 11:09:31 -03:00
/// function does not change the z axis
2021-06-17 22:19:29 -03:00
void AC_PosControl : : get_stopping_point_xy_cm ( Vector2p & stopping_point ) const
2013-12-28 07:15:29 -04:00
{
2016-07-20 15:38:37 -03:00
const Vector3f curr_pos = _inav . get_position ( ) ;
2021-06-17 22:19:29 -03:00
stopping_point = curr_pos . xy ( ) . topostype ( ) ;
2021-05-19 11:09:31 -03:00
float kP = _p_pos_xy . kP ( ) ;
2021-04-16 12:53:14 -03:00
2021-08-22 02:14:43 -03:00
Vector2f curr_vel = _inav . get_velocity ( ) . xy ( ) ;
2014-05-06 05:32:30 -03:00
2013-12-28 07:15:29 -04:00
// calculate current velocity
2021-08-22 02:14:43 -03:00
float vel_total = curr_vel . length ( ) ;
2013-12-28 07:15:29 -04:00
2021-05-19 11:09:31 -03:00
if ( ! is_positive ( vel_total ) ) {
return ;
}
const float stopping_dist = stopping_distance ( constrain_float ( vel_total , 0.0 , _vel_max_xy_cms ) , kP , _accel_max_xy_cmss ) ;
if ( ! is_positive ( stopping_dist ) ) {
return ;
}
2013-12-28 07:15:29 -04:00
2014-01-17 22:53:46 -04:00
// convert the stopping distance into a stopping point using velocity vector
2021-05-19 11:09:31 -03:00
const float t = stopping_dist / vel_total ;
stopping_point . x + = t * curr_vel . x ;
stopping_point . y + = t * curr_vel . y ;
2013-12-28 07:15:29 -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 AC_PosControl : : get_bearing_to_target_cd ( ) const
2017-11-27 08:21:13 -04:00
{
2021-06-17 22:19:29 -03:00
return get_bearing_cd ( _inav . get_position ( ) , _pos_target . tofloat ( ) ) ;
2017-11-27 08:21:13 -04:00
}
2020-02-28 01:01:59 -04:00
2021-04-16 12:53:14 -03:00
///
/// System methods
///
2014-05-06 05:32:30 -03:00
2021-04-16 12:53:14 -03:00
// get throttle using vibration-resistant calculation (uses feed forward with manually calculated gain)
float AC_PosControl : : get_throttle_with_vibration_override ( )
2017-12-16 09:06:51 -04:00
{
2021-04-16 12:53:14 -03:00
const float thr_per_accelz_cmss = _motors . get_throttle_hover ( ) / ( GRAVITY_MSS * 100.0f ) ;
// during vibration compensation use feed forward with manually calculated gain
// ToDo: clear pid_info P, I and D terms for logging
2021-06-22 11:39:44 -03:00
if ( ! ( _motors . limit . throttle_lower | | _motors . limit . throttle_upper ) | | ( ( is_positive ( _pid_accel_z . get_i ( ) ) & & is_negative ( _pid_vel_z . get_error ( ) ) ) | | ( is_negative ( _pid_accel_z . get_i ( ) ) & & is_positive ( _pid_vel_z . get_error ( ) ) ) ) ) {
_pid_accel_z . set_integrator ( _pid_accel_z . get_i ( ) + _dt * thr_per_accelz_cmss * 1000.0f * _pid_vel_z . get_error ( ) * _pid_vel_z . kP ( ) * POSCONTROL_VIBE_COMP_I_GAIN ) ;
2017-12-16 09:06:51 -04:00
}
2021-04-16 12:53:14 -03:00
return POSCONTROL_VIBE_COMP_P_GAIN * thr_per_accelz_cmss * _accel_target . z + _pid_accel_z . get_i ( ) * 0.001f ;
2014-05-06 05:32:30 -03:00
}
2019-09-28 10:38:23 -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 AC_PosControl : : standby_xyz_reset ( )
{
// Set _pid_accel_z integrator to zero.
_pid_accel_z . set_integrator ( 0.0f ) ;
// Set the target position to the current pos.
2021-06-17 22:19:29 -03:00
_pos_target = _inav . get_position ( ) . topostype ( ) ;
2019-09-28 10:38:23 -03:00
2021-04-16 12:53:14 -03:00
// Set _pid_vel_xy integrator and derivative to zero.
2019-09-28 10:38:23 -03:00
_pid_vel_xy . reset_filter ( ) ;
// initialise ekf xy reset handler
init_ekf_xy_reset ( ) ;
}
2021-05-25 01:35:23 -03:00
// write PSC and/or PSCZ logs
2018-03-01 01:42:52 -04:00
void AC_PosControl : : write_log ( )
{
2021-05-25 01:35:23 -03:00
if ( is_active_xy ( ) ) {
float accel_x , accel_y ;
lean_angles_to_accel_xy ( accel_x , accel_y ) ;
2021-06-17 22:19:29 -03:00
AP : : logger ( ) . Write_PSC ( get_pos_target_cm ( ) . tofloat ( ) , _inav . get_position ( ) , get_vel_target_cms ( ) , _inav . get_velocity ( ) , get_accel_target_cmss ( ) , accel_x , accel_y ) ;
2021-05-25 01:35:23 -03:00
}
2014-06-02 06:02:44 -03:00
2021-05-25 01:35:23 -03:00
if ( is_active_z ( ) ) {
AP : : logger ( ) . Write_PSCZ ( get_pos_target_cm ( ) . z , _inav . get_position ( ) . z ,
get_vel_desired_cms ( ) . z , get_vel_target_cms ( ) . z , _inav . get_velocity ( ) . z ,
_accel_desired . z , get_accel_target_cmss ( ) . z , get_z_accel_cmss ( ) , _attitude_control . get_throttle_in ( ) ) ;
}
2014-06-02 06:02:44 -03:00
}
2021-07-20 21:37:00 -03:00
/// crosstrack_error - returns horizontal error to the closest point to the current track
float AC_PosControl : : crosstrack_error ( ) const
{
const Vector3f & curr_pos = _inav . get_position ( ) ;
const Vector2f pos_error = curr_pos . xy ( ) - ( _pos_target . xy ( ) ) . tofloat ( ) ;
if ( is_zero ( _vel_desired . xy ( ) . length_squared ( ) ) ) {
// crosstrack is the horizontal distance to target when stationary
return pos_error . length ( ) ;
} else {
// crosstrack is the horizontal distance to the closest point to the current track
const Vector2f vel_unit = _vel_desired . xy ( ) . normalized ( ) ;
const float dot_error = pos_error * vel_unit ;
2021-07-21 01:53:47 -03:00
// todo: remove MAX of zero when safe_sqrt fixed
return safe_sqrt ( MAX ( pos_error . length_squared ( ) - sq ( dot_error ) , 0.0 ) ) ;
2021-07-20 21:37:00 -03:00
}
}
2014-01-17 22:53:46 -04:00
///
/// private methods
///
2013-12-28 07:15:29 -04:00
2021-04-16 12:53:14 -03:00
// get_lean_angles_to_accel - convert roll, pitch lean angles to NE frame accelerations in cm/s/s
2017-12-16 09:06:51 -04:00
void AC_PosControl : : accel_to_lean_angles ( float accel_x_cmss , float accel_y_cmss , float & roll_target , float & pitch_target ) const
{
2014-12-08 20:56:53 -04:00
// rotate accelerations into body forward-right frame
2021-02-09 22:38:47 -04:00
const float accel_forward = accel_x_cmss * _ahrs . cos_yaw ( ) + accel_y_cmss * _ahrs . sin_yaw ( ) ;
const float accel_right = - accel_x_cmss * _ahrs . sin_yaw ( ) + accel_y_cmss * _ahrs . cos_yaw ( ) ;
2014-11-07 23:47:33 -04:00
2014-12-08 20:56:53 -04:00
// update angle targets that will be passed to stabilize controller
2019-04-19 08:36:42 -03:00
pitch_target = atanf ( - accel_forward / ( GRAVITY_MSS * 100.0f ) ) * ( 18000.0f / M_PI ) ;
float cos_pitch_target = cosf ( pitch_target * M_PI / 18000.0f ) ;
roll_target = atanf ( accel_right * cos_pitch_target / ( GRAVITY_MSS * 100.0f ) ) * ( 18000.0f / M_PI ) ;
2013-12-28 07:15:29 -04:00
}
2021-05-19 11:09:31 -03:00
// lean_angles_to_accel_xy - convert roll, pitch lean target angles to NE frame accelerations in cm/s/s
2021-04-16 12:53:14 -03:00
// todo: this should be based on thrust vector attitude control
void AC_PosControl : : lean_angles_to_accel_xy ( float & accel_x_cmss , float & accel_y_cmss ) const
2014-05-06 05:32:30 -03:00
{
// rotate our roll, pitch angles into lat/lon frame
2021-04-16 12:53:14 -03:00
Vector3f att_target_euler = _attitude_control . get_att_target_euler_rad ( ) ;
att_target_euler . z = _ahrs . yaw ;
Vector3f accel_cmss = lean_angles_to_accel ( att_target_euler ) ;
2021-03-15 13:53:51 -03:00
2021-04-16 12:53:14 -03:00
accel_x_cmss = accel_cmss . x ;
accel_y_cmss = accel_cmss . y ;
2014-05-06 05:32:30 -03:00
}
2021-05-19 11:09:31 -03:00
// calculate_yaw_and_rate_yaw - update the calculated the vehicle yaw and rate of yaw.
2021-04-22 22:16:58 -03:00
bool AC_PosControl : : calculate_yaw_and_rate_yaw ( )
{
// Calculate the turn rate
float turn_rate = 0.0f ;
const Vector2f vel_desired_xy ( _vel_desired . x , _vel_desired . y ) ;
const Vector2f accel_desired_xy ( _accel_desired . x , _accel_desired . y ) ;
const float vel_desired_xy_len = vel_desired_xy . length ( ) ;
if ( is_positive ( vel_desired_xy_len ) ) {
const float accel_forward = ( accel_desired_xy . x * vel_desired_xy . x + accel_desired_xy . y * vel_desired_xy . y ) / vel_desired_xy_len ;
const Vector2f accel_turn = accel_desired_xy - vel_desired_xy * accel_forward / vel_desired_xy_len ;
const float accel_turn_xy_len = accel_turn . length ( ) ;
turn_rate = accel_turn_xy_len / vel_desired_xy_len ;
if ( ( accel_turn . y * vel_desired_xy . x - accel_turn . x * vel_desired_xy . y ) < 0.0 ) {
turn_rate = - turn_rate ;
}
}
2021-07-08 01:13:05 -03:00
// update the target yaw if velocity is greater than 5% _vel_max_xy_cms
2021-04-16 12:53:14 -03:00
if ( vel_desired_xy_len > _vel_max_xy_cms * 0.05f ) {
2021-04-22 22:16:58 -03:00
_yaw_target = degrees ( vel_desired_xy . angle ( ) ) * 100.0f ;
_yaw_rate_target = turn_rate * degrees ( 100.0f ) ;
return true ;
}
return false ;
}
2016-11-22 01:36:17 -04:00
/// initialise ekf xy position reset check
void AC_PosControl : : init_ekf_xy_reset ( )
{
Vector2f pos_shift ;
_ekf_xy_reset_ms = _ahrs . getLastPosNorthEastReset ( pos_shift ) ;
}
2021-05-19 11:09:31 -03:00
/// handle_ekf_xy_reset - check for ekf position reset and adjust loiter or brake target position
void AC_PosControl : : handle_ekf_xy_reset ( )
2016-11-22 01:36:17 -04:00
{
// check for position shift
Vector2f pos_shift ;
uint32_t reset_ms = _ahrs . getLastPosNorthEastReset ( pos_shift ) ;
if ( reset_ms ! = _ekf_xy_reset_ms ) {
2021-04-16 12:53:14 -03:00
const Vector3f & curr_pos = _inav . get_position ( ) ;
_pos_target . x = curr_pos . x + _p_pos_xy . get_error ( ) . x ;
_pos_target . y = curr_pos . y + _p_pos_xy . get_error ( ) . y ;
const Vector3f & curr_vel = _inav . get_velocity ( ) ;
_vel_target . x = curr_vel . x + _pid_vel_xy . get_error ( ) . x ;
_vel_target . y = curr_vel . y + _pid_vel_xy . get_error ( ) . y ;
2016-11-22 01:36:17 -04:00
_ekf_xy_reset_ms = reset_ms ;
}
}
/// initialise ekf z axis reset check
void AC_PosControl : : init_ekf_z_reset ( )
{
float alt_shift ;
_ekf_z_reset_ms = _ahrs . getLastPosDownReset ( alt_shift ) ;
}
2021-05-19 11:09:31 -03:00
/// handle_ekf_z_reset - check for ekf position reset and adjust loiter or brake target position
void AC_PosControl : : handle_ekf_z_reset ( )
2016-11-22 01:36:17 -04:00
{
// check for position shift
float alt_shift ;
uint32_t reset_ms = _ahrs . getLastPosDownReset ( alt_shift ) ;
2018-03-31 01:20:29 -03:00
if ( reset_ms ! = 0 & & reset_ms ! = _ekf_z_reset_ms ) {
2021-04-16 12:53:14 -03:00
const Vector3f & curr_pos = _inav . get_position ( ) ;
_pos_target . z = curr_pos . z + _p_pos_z . get_error ( ) ;
const Vector3f & curr_vel = _inav . get_velocity ( ) ;
_vel_target . z = curr_vel . z + _pid_vel_z . get_error ( ) ;
2016-11-22 01:36:17 -04:00
_ekf_z_reset_ms = reset_ms ;
}
}
2017-11-16 08:29:02 -04:00
2019-03-04 23:27:31 -04:00
bool AC_PosControl : : pre_arm_checks ( const char * param_prefix ,
char * failure_msg ,
const uint8_t failure_msg_len )
{
2020-01-04 02:16:09 -04:00
if ( ! is_positive ( get_pos_xy_p ( ) . kP ( ) ) ) {
hal . util - > snprintf ( failure_msg , failure_msg_len , " %s_POSXY_P must be > 0 " , param_prefix ) ;
return false ;
}
if ( ! is_positive ( get_pos_z_p ( ) . kP ( ) ) ) {
hal . util - > snprintf ( failure_msg , failure_msg_len , " %s_POSZ_P must be > 0 " , param_prefix ) ;
return false ;
}
if ( ! is_positive ( get_vel_z_pid ( ) . kP ( ) ) ) {
hal . util - > snprintf ( failure_msg , failure_msg_len , " %s_VELZ_P must be > 0 " , param_prefix ) ;
return false ;
2019-03-04 23:27:31 -04:00
}
if ( ! is_positive ( get_accel_z_pid ( ) . kP ( ) ) ) {
hal . util - > snprintf ( failure_msg , failure_msg_len , " %s_ACCZ_P must be > 0 " , param_prefix ) ;
return false ;
}
if ( ! is_positive ( get_accel_z_pid ( ) . kI ( ) ) ) {
hal . util - > snprintf ( failure_msg , failure_msg_len , " %s_ACCZ_I must be > 0 " , param_prefix ) ;
return false ;
}
return true ;
}
2021-05-17 22:24:41 -03:00
// return true if on a real vehicle or SITL with lock-step scheduling
bool AC_PosControl : : has_good_timing ( void ) const
{
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
auto * sitl = AP : : sitl ( ) ;
if ( sitl ) {
return sitl - > state . is_lock_step_scheduled ;
}
# endif
// real boards are assumed to have good timing
return true ;
}