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
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
# 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
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
AP_GROUPINFO ( " _ACC_XY_FILT " , 1 , AC_PosControl , _accel_xy_filt_hz , POSCONTROL_ACCEL_FILTER_HZ ) ,
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
AP_SUBGROUPINFO ( _p_pos_z , " _POSZ_ " , 2 , AC_PosControl , AC_P ) ,
// @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
AP_SUBGROUPINFO ( _p_vel_z , " _VELZ_ " , 3 , AC_PosControl , AC_P ) ,
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
// @Range: 0.500 1.500
// @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
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
// @DisplayName: Position (horizonal) controller P gain
// @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
AP_SUBGROUPINFO ( _p_pos_xy , " _POSXY_ " , 5 , AC_PosControl , AC_P ) ,
2018-01-15 06:50:54 -04:00
// @Param: _VELXY_P
// @DisplayName: Velocity (horizontal) P gain
// @Description: Velocity (horizontal) P gain. Converts the difference between desired velocity to a target acceleration
// @Range: 0.1 6.0
// @Increment: 0.1
// @User: Advanced
// @Param: _VELXY_I
// @DisplayName: Velocity (horizontal) I gain
// @Description: Velocity (horizontal) I gain. Corrects long-term difference in desired velocity to a target acceleration
// @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
// @Param: _VELXY_FILT
// @DisplayName: Velocity (horizontal) input filter
// @Description: Velocity (horizontal) input filter. This filter (in hz) is applied to the input for P and I terms
// @Range: 0 100
// @Units: Hz
// @User: Advanced
// @Param: _VELXY_D_FILT
// @DisplayName: Velocity (horizontal) input filter
// @Description: Velocity (horizontal) input filter. This filter (in hz) is applied to the input for P and I terms
// @Range: 0 100
// @Units: Hz
// @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
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 ,
2018-01-15 06:52:55 -04:00
const AP_Motors & motors , AC_AttitudeControl & attitude_control ) :
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 ) ,
2018-01-15 06:52:55 -04:00
_p_pos_z ( POSCONTROL_POS_Z_P ) ,
_p_vel_z ( POSCONTROL_VEL_Z_P ) ,
2019-07-16 03:03:55 -03:00
_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 , POSCONTROL_ACC_Z_DT ) ,
2018-01-15 06:52:55 -04:00
_p_pos_xy ( POSCONTROL_POS_XY_P ) ,
2018-01-15 06:50:54 -04:00
_pid_vel_xy ( POSCONTROL_VEL_XY_P , POSCONTROL_VEL_XY_I , POSCONTROL_VEL_XY_D , POSCONTROL_VEL_XY_IMAX , POSCONTROL_VEL_XY_FILT_HZ , POSCONTROL_VEL_XY_FILT_D_HZ , POSCONTROL_DT_50HZ ) ,
2015-11-17 02:48:05 -04:00
_dt ( POSCONTROL_DT_400HZ ) ,
2014-01-17 22:53:46 -04:00
_speed_down_cms ( POSCONTROL_SPEED_DOWN ) ,
_speed_up_cms ( POSCONTROL_SPEED_UP ) ,
2013-12-28 10:04:45 -04:00
_speed_cms ( POSCONTROL_SPEED ) ,
2014-04-21 09:34:33 -03:00
_accel_z_cms ( POSCONTROL_ACCEL_Z ) ,
_accel_cms ( POSCONTROL_ACCEL_XY ) ,
2014-01-17 22:53:46 -04:00
_leash ( POSCONTROL_LEASH_LENGTH_MIN ) ,
2014-07-14 11:30:23 -03:00
_leash_down_z ( POSCONTROL_LEASH_LENGTH_MIN ) ,
_leash_up_z ( POSCONTROL_LEASH_LENGTH_MIN ) ,
2015-04-16 16:57:28 -03:00
_accel_target_filter ( POSCONTROL_ACCEL_FILTER_HZ )
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
2014-01-23 23:27:06 -04:00
_flags . recalc_leash_z = true ;
2015-06-08 01:27:03 -03:00
_flags . recalc_leash_xy = true ;
2014-05-06 05:32:30 -03:00
_flags . reset_desired_vel_to_pos = true ;
2015-03-13 08:46:21 -03:00
_flags . reset_accel_to_lean_xy = true ;
2014-05-02 00:08:18 -03:00
_flags . reset_rate_to_accel_z = true ;
2015-06-08 01:27:03 -03:00
_flags . freeze_ff_z = true ;
2015-10-28 03:42:17 -03:00
_flags . use_desvel_ff_z = true ;
2015-06-08 01:27:03 -03:00
_limit . pos_up = true ;
_limit . pos_down = true ;
_limit . vel_up = true ;
_limit . vel_down = true ;
_limit . accel_xy = true ;
2013-12-28 07:15:29 -04:00
}
2013-12-28 10:04:45 -04:00
///
/// z-axis position controller
///
2013-12-28 07:15:29 -04:00
2014-05-27 10:44:57 -03:00
/// set_dt - sets time delta in seconds for all controllers (i.e. 100hz = 0.01, 400hz = 0.0025)
void AC_PosControl : : set_dt ( float delta_sec )
{
_dt = delta_sec ;
2018-03-01 03:57:29 -04:00
// update PID controller dt
2015-01-31 02:50:32 -04:00
_pid_accel_z . set_dt ( _dt ) ;
2018-03-01 03:57:29 -04:00
_pid_vel_xy . set_dt ( _dt ) ;
2014-09-21 05:52:14 -03:00
// update rate z-axis velocity error and accel error filters
2015-04-16 16:35:28 -03:00
_vel_error_filter . set_cutoff_frequency ( POSCONTROL_VEL_ERROR_CUTOFF_FREQ ) ;
2014-05-27 10:44:57 -03:00
}
2018-09-20 02:43:48 -03:00
/// set_max_speed_z - set the maximum climb and descent rates
2014-01-25 04:23:55 -04:00
/// To-Do: call this in the main code as part of flight mode initialisation
2018-09-20 02:43:48 -03:00
void AC_PosControl : : set_max_speed_z ( float speed_down , float speed_up )
2014-01-23 23:27:06 -04:00
{
2014-04-29 23:14:48 -03:00
// ensure speed_down is always negative
2015-05-02 06:34:54 -03:00
speed_down = - fabsf ( speed_down ) ;
2014-04-29 23:14:48 -03:00
2020-02-19 02:08:16 -04:00
// exit immediately if no change in speed up or down
if ( is_equal ( _speed_down_cms , speed_down ) & & is_equal ( _speed_up_cms , speed_up ) ) {
return ;
}
// sanity check speeds and update
if ( is_positive ( speed_up ) & & is_negative ( speed_down ) ) {
2014-01-23 23:27:06 -04:00
_speed_down_cms = speed_down ;
_speed_up_cms = speed_up ;
_flags . recalc_leash_z = true ;
2015-05-19 06:15:37 -03:00
calc_leash_length_z ( ) ;
2014-01-23 23:27:06 -04:00
}
}
2018-09-20 02:43:48 -03:00
/// set_max_accel_z - set the maximum vertical acceleration in cm/s/s
void AC_PosControl : : set_max_accel_z ( float accel_cmss )
2014-01-23 23:27:06 -04:00
{
2020-02-19 02:08:16 -04:00
// exit immediately if no change in acceleration
if ( is_equal ( _accel_z_cms , accel_cmss ) ) {
return ;
2014-01-23 23:27:06 -04:00
}
2020-02-19 02:08:16 -04:00
_accel_z_cms = accel_cmss ;
_flags . recalc_leash_z = true ;
calc_leash_length_z ( ) ;
2014-01-23 23:27:06 -04:00
}
/// set_alt_target_with_slew - adjusts target towards a final altitude target
/// should be called continuously (with dt set to be the expected time between calls)
/// actual position target will be moved no faster than the speed_down and speed_up
/// target will also be stopped if the motors hit their limits or leash length is exceeded
void AC_PosControl : : set_alt_target_with_slew ( float alt_cm , float dt )
{
2019-04-19 08:36:42 -03:00
float alt_change = alt_cm - _pos_target . z ;
2015-10-28 03:42:17 -03:00
// do not use z-axis desired velocity feed forward
_flags . use_desvel_ff_z = false ;
2014-01-23 23:27:06 -04:00
// adjust desired alt if motors have not hit their limits
2019-04-19 08:36:42 -03:00
if ( ( alt_change < 0 & & ! _motors . limit . throttle_lower ) | | ( alt_change > 0 & & ! _motors . limit . throttle_upper ) ) {
2016-06-04 01:29:38 -03:00
if ( ! is_zero ( dt ) ) {
2019-04-19 08:36:42 -03:00
float climb_rate_cms = constrain_float ( alt_change / dt , _speed_down_cms , _speed_up_cms ) ;
_pos_target . z + = climb_rate_cms * dt ;
_vel_desired . z = climb_rate_cms ; // recorded for reporting purposes
2016-06-04 01:29:38 -03:00
}
2018-11-19 02:19:35 -04:00
} else {
// recorded for reporting purposes
_vel_desired . z = 0.0f ;
2014-01-23 23:27:06 -04:00
}
// do not let target get too far from current altitude
float curr_alt = _inav . get_altitude ( ) ;
2019-04-19 08:36:42 -03:00
_pos_target . z = constrain_float ( _pos_target . z , curr_alt - _leash_down_z , curr_alt + _leash_up_z ) ;
2014-01-23 23:27:06 -04:00
}
/// set_alt_target_from_climb_rate - adjusts target up or down using a climb rate in cm/s
/// should be called continuously (with dt set to be the expected time between calls)
/// actual position target will be moved no faster than the speed_down and speed_up
/// target will also be stopped if the motors hit their limits or leash length is exceeded
2014-11-13 20:56:38 -04:00
void AC_PosControl : : set_alt_target_from_climb_rate ( float climb_rate_cms , float dt , bool force_descend )
2015-10-27 10:04:24 -03:00
{
// adjust desired alt if motors have not hit their limits
// To-Do: add check of _limit.pos_down?
2019-04-19 08:36:42 -03:00
if ( ( climb_rate_cms < 0 & & ( ! _motors . limit . throttle_lower | | force_descend ) ) | | ( climb_rate_cms > 0 & & ! _motors . limit . throttle_upper & & ! _limit . pos_up ) ) {
2015-10-27 10:04:24 -03:00
_pos_target . z + = climb_rate_cms * dt ;
}
2015-10-28 03:42:17 -03:00
// do not use z-axis desired velocity feed forward
// vel_desired set to desired climb rate for reporting and land-detector
_flags . use_desvel_ff_z = false ;
_vel_desired . z = climb_rate_cms ;
2015-10-27 10:04:24 -03:00
}
/// set_alt_target_from_climb_rate_ff - adjusts target up or down using a climb rate in cm/s using feed-forward
/// should be called continuously (with dt set to be the expected time between calls)
/// actual position target will be moved no faster than the speed_down and speed_up
/// target will also be stopped if the motors hit their limits or leash length is exceeded
/// set force_descend to true during landing to allow target to move low enough to slow the motors
void AC_PosControl : : set_alt_target_from_climb_rate_ff ( float climb_rate_cms , float dt , bool force_descend )
2014-01-23 23:27:06 -04:00
{
2015-10-27 10:45:16 -03:00
// calculated increased maximum acceleration if over speed
float accel_z_cms = _accel_z_cms ;
if ( _vel_desired . z < _speed_down_cms & & ! is_zero ( _speed_down_cms ) ) {
accel_z_cms * = POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired . z / _speed_down_cms ;
}
if ( _vel_desired . z > _speed_up_cms & & ! is_zero ( _speed_up_cms ) ) {
accel_z_cms * = POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired . z / _speed_up_cms ;
}
accel_z_cms = constrain_float ( accel_z_cms , 0.0f , 750.0f ) ;
2015-04-16 07:07:33 -03:00
// jerk_z is calculated to reach full acceleration in 1000ms.
2015-10-27 10:45:16 -03:00
float jerk_z = accel_z_cms * POSCONTROL_JERK_RATIO ;
2015-04-13 06:22:52 -03:00
2019-04-19 08:36:42 -03:00
float accel_z_max = MIN ( accel_z_cms , safe_sqrt ( 2.0f * fabsf ( _vel_desired . z - climb_rate_cms ) * jerk_z ) ) ;
2015-04-13 06:22:52 -03:00
2015-04-16 07:07:33 -03:00
_accel_last_z_cms + = jerk_z * dt ;
2015-11-27 13:11:58 -04:00
_accel_last_z_cms = MIN ( accel_z_max , _accel_last_z_cms ) ;
2015-04-13 06:22:52 -03:00
float vel_change_limit = _accel_last_z_cms * dt ;
2019-04-19 08:36:42 -03:00
_vel_desired . z = constrain_float ( climb_rate_cms , _vel_desired . z - vel_change_limit , _vel_desired . z + vel_change_limit ) ;
2015-10-28 03:42:17 -03:00
_flags . use_desvel_ff_z = true ;
2015-04-13 06:22:52 -03:00
2014-01-23 23:27:06 -04:00
// adjust desired alt if motors have not hit their limits
2015-01-28 17:29:18 -04:00
// To-Do: add check of _limit.pos_down?
2019-04-19 08:36:42 -03:00
if ( ( _vel_desired . z < 0 & & ( ! _motors . limit . throttle_lower | | force_descend ) ) | | ( _vel_desired . z > 0 & & ! _motors . limit . throttle_upper & & ! _limit . pos_up ) ) {
2015-04-13 06:22:52 -03:00
_pos_target . z + = _vel_desired . z * dt ;
2014-01-23 23:27:06 -04:00
}
}
2015-04-30 10:18:05 -03:00
/// add_takeoff_climb_rate - adjusts alt target up or down using a climb rate in cm/s
/// should be called continuously (with dt set to be the expected time between calls)
/// almost no checks are performed on the input
void AC_PosControl : : add_takeoff_climb_rate ( float climb_rate_cms , float dt )
{
_pos_target . z + = climb_rate_cms * dt ;
}
2016-11-22 01:36:17 -04:00
/// shift altitude target (positive means move altitude up)
void AC_PosControl : : shift_alt_target ( float z_cm )
{
_pos_target . z + = z_cm ;
// freeze feedforward to avoid jump
if ( ! is_zero ( z_cm ) ) {
freeze_ff_z ( ) ;
}
}
2015-04-14 11:26:33 -03:00
/// relax_alt_hold_controllers - set all desired and targets to measured
void AC_PosControl : : relax_alt_hold_controllers ( float throttle_setting )
{
_pos_target . z = _inav . get_altitude ( ) ;
_vel_desired . z = 0.0f ;
2015-10-28 03:42:17 -03:00
_flags . use_desvel_ff_z = false ;
2016-07-20 15:38:37 -03:00
_vel_target . z = _inav . get_velocity_z ( ) ;
2015-04-14 11:26:33 -03:00
_vel_last . z = _inav . get_velocity_z ( ) ;
2017-11-16 08:29:02 -04:00
_accel_desired . z = 0.0f ;
2015-04-14 11:26:33 -03:00
_accel_last_z_cms = 0.0f ;
2019-07-15 23:13:55 -03:00
_flags . reset_rate_to_accel_z = true ;
2019-04-19 08:36:42 -03:00
_pid_accel_z . set_integrator ( ( throttle_setting - _motors . get_throttle_hover ( ) ) * 1000.0f ) ;
2019-07-15 23:13:55 -03:00
_accel_target . z = - ( _ahrs . get_accel_ef_blended ( ) . z + GRAVITY_MSS ) * 100.0f ;
_pid_accel_z . reset_filter ( ) ;
2015-04-14 11:26:33 -03:00
}
2014-01-05 23:30:51 -04:00
// get_alt_error - returns altitude error in cm
float AC_PosControl : : get_alt_error ( ) const
{
2014-01-23 23:27:06 -04:00
return ( _pos_target . z - _inav . get_altitude ( ) ) ;
2014-01-05 23:30:51 -04:00
}
2013-12-30 09:12:59 -04:00
/// set_target_to_stopping_point_z - returns reasonable stopping altitude in cm above home
void AC_PosControl : : set_target_to_stopping_point_z ( )
2013-12-28 10:04:45 -04:00
{
2014-04-30 09:22:47 -03:00
// check if z leash needs to be recalculated
calc_leash_length_z ( ) ;
2014-01-25 04:23:55 -04:00
get_stopping_point_z ( _pos_target ) ;
}
2015-01-30 16:42:42 -04:00
/// get_stopping_point_z - calculates stopping point based on current position, velocity, vehicle acceleration
2014-01-27 10:42:49 -04:00
void AC_PosControl : : get_stopping_point_z ( Vector3f & stopping_point ) const
2014-01-25 04:23:55 -04:00
{
const float curr_pos_z = _inav . get_altitude ( ) ;
2014-05-02 03:26:04 -03:00
float curr_vel_z = _inav . get_velocity_z ( ) ;
2014-01-25 04:23:55 -04:00
2019-04-19 08:36:42 -03:00
float linear_distance ; // half the distance we swap between linear and sqrt and the distance we offset sqrt
2013-12-28 10:04:45 -04:00
float linear_velocity ; // the velocity we swap between linear and sqrt
2014-05-02 03:26:04 -03:00
// if position controller is active add current velocity error to avoid sudden jump in acceleration
if ( is_active_z ( ) ) {
curr_vel_z + = _vel_error . z ;
2015-10-28 03:42:17 -03:00
if ( _flags . use_desvel_ff_z ) {
curr_vel_z - = _vel_desired . z ;
}
2014-05-02 03:26:04 -03:00
}
2017-03-14 08:50:32 -03:00
// avoid divide by zero by using current position if kP is very low or acceleration is zero
if ( _p_pos_z . kP ( ) < = 0.0f | | _accel_z_cms < = 0.0f ) {
stopping_point . z = curr_pos_z ;
return ;
}
2014-03-16 21:40:11 -03:00
// calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function
2019-04-19 08:36:42 -03:00
linear_velocity = _accel_z_cms / _p_pos_z . kP ( ) ;
2013-12-28 10:04:45 -04:00
2015-05-02 06:34:54 -03:00
if ( fabsf ( curr_vel_z ) < linear_velocity ) {
2013-12-28 10:04:45 -04:00
// if our current velocity is below the cross-over point we use a linear function
2019-04-19 08:36:42 -03:00
stopping_point . z = curr_pos_z + curr_vel_z / _p_pos_z . kP ( ) ;
2013-12-28 07:15:29 -04:00
} else {
2019-04-19 08:36:42 -03:00
linear_distance = _accel_z_cms / ( 2.0f * _p_pos_z . kP ( ) * _p_pos_z . kP ( ) ) ;
if ( curr_vel_z > 0 ) {
stopping_point . z = curr_pos_z + ( linear_distance + curr_vel_z * curr_vel_z / ( 2.0f * _accel_z_cms ) ) ;
2013-12-28 07:15:29 -04:00
} else {
2019-04-19 08:36:42 -03:00
stopping_point . z = curr_pos_z - ( linear_distance + curr_vel_z * curr_vel_z / ( 2.0f * _accel_z_cms ) ) ;
2013-12-28 07:15:29 -04:00
}
}
2017-04-27 05:30:40 -03:00
stopping_point . z = constrain_float ( stopping_point . z , curr_pos_z - POSCONTROL_STOPPING_DIST_DOWN_MAX , curr_pos_z + POSCONTROL_STOPPING_DIST_UP_MAX ) ;
2013-12-28 07:15:29 -04:00
}
2013-12-30 09:12:59 -04:00
/// init_takeoff - initialises target altitude if we are taking off
void AC_PosControl : : init_takeoff ( )
{
const Vector3f & curr_pos = _inav . get_position ( ) ;
2015-07-01 10:47:28 -03:00
_pos_target . z = curr_pos . z ;
2013-12-30 09:12:59 -04:00
2014-08-04 09:03:35 -03:00
// freeze feedforward to avoid jump
freeze_ff_z ( ) ;
// shift difference between last motor out and hover throttle into accelerometer I
2019-07-16 03:03:55 -03:00
_pid_accel_z . set_integrator ( ( _attitude_control . get_throttle_in ( ) - _motors . get_throttle_hover ( ) ) * 1000.0f ) ;
2016-11-22 01:36:17 -04:00
// initialise ekf reset handler
init_ekf_z_reset ( ) ;
2013-12-30 09:12:59 -04:00
}
2014-05-02 00:08:18 -03:00
// is_active_z - returns true if the z-axis position controller has been run very recently
bool AC_PosControl : : is_active_z ( ) const
{
2019-01-24 07:20:12 -04:00
return ( ( AP_HAL : : micros64 ( ) - _last_update_z_us ) < = POSCONTROL_ACTIVE_TIMEOUT_US ) ;
2014-05-02 00:08:18 -03:00
}
2014-01-17 22:53:46 -04:00
/// update_z_controller - fly to altitude in cm above home
void AC_PosControl : : update_z_controller ( )
2013-12-28 10:04:45 -04:00
{
2014-05-02 00:08:18 -03:00
// check time since last cast
2019-01-30 01:51:17 -04:00
const uint64_t now_us = AP_HAL : : micros64 ( ) ;
2019-01-24 07:20:12 -04:00
if ( now_us - _last_update_z_us > POSCONTROL_ACTIVE_TIMEOUT_US ) {
2014-05-02 00:08:18 -03:00
_flags . reset_rate_to_accel_z = true ;
2019-07-16 03:03:55 -03:00
_pid_accel_z . set_integrator ( ( _attitude_control . get_throttle_in ( ) - _motors . get_throttle_hover ( ) ) * 1000.0f ) ;
2019-07-15 23:13:55 -03:00
_accel_target . z = - ( _ahrs . get_accel_ef_blended ( ) . z + GRAVITY_MSS ) * 100.0f ;
_pid_accel_z . reset_filter ( ) ;
2014-05-02 00:08:18 -03:00
}
2019-01-24 07:20:12 -04:00
_last_update_z_us = now_us ;
2014-05-02 00:08:18 -03:00
2016-11-22 01:36:17 -04:00
// check for ekf altitude reset
check_for_ekf_z_reset ( ) ;
2014-01-23 23:27:06 -04:00
// check if leash lengths need to be recalculated
calc_leash_length_z ( ) ;
2017-11-12 08:50:19 -04:00
// call z-axis position controller
run_z_controller ( ) ;
2013-12-28 10:04:45 -04:00
}
2014-01-23 23:27:06 -04:00
/// calc_leash_length - calculates the vertical leash lengths from maximum speed, acceleration
2017-11-12 08:50:19 -04:00
/// called by update_z_controller if z-axis speed or accelerations are changed
2014-01-23 23:27:06 -04:00
void AC_PosControl : : calc_leash_length_z ( )
2013-12-28 10:04:45 -04:00
{
2014-01-23 23:27:06 -04:00
if ( _flags . recalc_leash_z ) {
2015-01-31 02:50:32 -04:00
_leash_up_z = calc_leash_length ( _speed_up_cms , _accel_z_cms , _p_pos_z . kP ( ) ) ;
_leash_down_z = calc_leash_length ( - _speed_down_cms , _accel_z_cms , _p_pos_z . kP ( ) ) ;
2014-01-23 23:27:06 -04:00
_flags . recalc_leash_z = false ;
}
}
2017-11-12 08:50:19 -04:00
// run position control for Z axis
// target altitude should be set with one of these functions: set_alt_target, set_target_to_stopping_point_z, init_takeoff
2014-01-23 23:27:06 -04:00
// calculates desired rate in earth-frame z axis and passes to rate controller
// vel_up_max, vel_down_max should have already been set before calling this method
2017-11-12 08:50:19 -04:00
void AC_PosControl : : run_z_controller ( )
2014-01-23 23:27:06 -04:00
{
float curr_alt = _inav . get_altitude ( ) ;
2013-12-28 10:04:45 -04:00
// clear position limit flags
_limit . pos_up = false ;
_limit . pos_down = false ;
2014-01-23 23:27:06 -04:00
// calculate altitude error
_pos_error . z = _pos_target . z - curr_alt ;
2013-12-28 10:04:45 -04:00
// do not let target altitude get too far from current altitude
2014-01-23 23:27:06 -04:00
if ( _pos_error . z > _leash_up_z ) {
_pos_target . z = curr_alt + _leash_up_z ;
2014-02-17 06:46:05 -04:00
_pos_error . z = _leash_up_z ;
2013-12-28 10:04:45 -04:00
_limit . pos_up = true ;
}
2014-01-23 23:27:06 -04:00
if ( _pos_error . z < - _leash_down_z ) {
_pos_target . z = curr_alt - _leash_down_z ;
2014-02-17 06:46:05 -04:00
_pos_error . z = - _leash_down_z ;
2014-01-23 23:27:06 -04:00
_limit . pos_down = true ;
}
2013-12-28 10:04:45 -04:00
2014-10-07 10:10:16 -03:00
// calculate _vel_target.z using from _pos_error.z using sqrt controller
2017-11-16 08:29:29 -04:00
_vel_target . z = AC_AttitudeControl : : sqrt_controller ( _pos_error . z , _p_pos_z . kP ( ) , _accel_z_cms , _dt ) ;
2013-12-28 07:15:29 -04:00
2015-10-27 10:44:41 -03:00
// check speed limits
// To-Do: check these speed limits here or in the pos->rate controller
_limit . vel_up = false ;
_limit . vel_down = false ;
if ( _vel_target . z < _speed_down_cms ) {
_vel_target . z = _speed_down_cms ;
_limit . vel_down = true ;
}
if ( _vel_target . z > _speed_up_cms ) {
_vel_target . z = _speed_up_cms ;
_limit . vel_up = true ;
}
2015-04-13 06:22:52 -03:00
// add feed forward component
2015-10-28 03:42:17 -03:00
if ( _flags . use_desvel_ff_z ) {
_vel_target . z + = _vel_desired . z ;
}
2015-04-13 06:22:52 -03:00
2017-11-12 08:50:19 -04:00
// the following section calculates acceleration required to achieve the velocity target
2013-12-28 07:15:29 -04:00
2013-12-28 10:04:45 -04:00
const Vector3f & curr_vel = _inav . get_velocity ( ) ;
2017-11-12 08:50:19 -04:00
// TODO: remove velocity derivative calculation
2014-05-31 10:35:14 -03:00
// reset last velocity target to current target
if ( _flags . reset_rate_to_accel_z ) {
_vel_last . z = _vel_target . z ;
}
// feed forward desired acceleration calculation
if ( _dt > 0.0f ) {
2019-04-19 08:36:42 -03:00
if ( ! _flags . freeze_ff_z ) {
_accel_desired . z = ( _vel_target . z - _vel_last . z ) / _dt ;
2014-05-31 10:35:14 -03:00
} else {
2019-04-19 08:36:42 -03:00
// stop the feed forward being calculated during a known discontinuity
_flags . freeze_ff_z = false ;
}
2014-05-31 10:35:14 -03:00
} else {
2017-11-16 08:29:02 -04:00
_accel_desired . z = 0.0f ;
2014-05-31 10:35:14 -03:00
}
// store this iteration's velocities for the next iteration
_vel_last . z = _vel_target . z ;
2013-12-28 10:04:45 -04:00
// reset velocity error and filter if this controller has just been engaged
2014-05-02 00:08:18 -03:00
if ( _flags . reset_rate_to_accel_z ) {
2013-12-28 10:04:45 -04:00
// Reset Filter
_vel_error . z = 0 ;
2014-09-21 05:53:10 -03:00
_vel_error_filter . reset ( 0 ) ;
2014-05-02 00:08:18 -03:00
_flags . reset_rate_to_accel_z = false ;
2013-12-28 10:04:45 -04:00
} else {
2014-09-21 05:53:10 -03:00
// calculate rate error and filter with cut off frequency of 2 Hz
2015-04-16 16:35:28 -03:00
_vel_error . z = _vel_error_filter . apply ( _vel_target . z - curr_vel . z , _dt ) ;
2013-12-28 10:04:45 -04:00
}
2017-11-12 08:50:19 -04:00
_accel_target . z = _p_vel_z . get_p ( _vel_error . z ) ;
2013-12-28 10:04:45 -04:00
2017-11-12 08:50:19 -04:00
_accel_target . z + = _accel_desired . z ;
2013-12-28 10:04:45 -04:00
2019-07-16 03:03:55 -03:00
2017-11-12 08:50:19 -04:00
// the following section calculates a desired throttle needed to achieve the acceleration target
2013-12-28 10:04:45 -04:00
float z_accel_meas ; // actual acceleration
// Calculate Earth Frame Z acceleration
2014-10-31 19:07:33 -03:00
z_accel_meas = - ( _ahrs . get_accel_ef_blended ( ) . z + GRAVITY_MSS ) * 100.0f ;
2013-12-28 10:04:45 -04:00
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 ) {
_flags . freeze_ff_z = true ;
_accel_desired . z = 0.0f ;
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
if ( ! ( _motors . limit . throttle_lower | | _motors . limit . throttle_upper ) | | ( ( is_positive ( _pid_accel_z . get_i ( ) ) & & is_negative ( _vel_error . z ) ) | | ( is_negative ( _pid_accel_z . get_i ( ) ) & & is_positive ( _vel_error . z ) ) ) ) {
_pid_accel_z . set_integrator ( _pid_accel_z . get_i ( ) + _dt * thr_per_accelz_cmss * 1000.0f * _vel_error . z * _p_vel_z . kP ( ) * POSCONTROL_VIBE_COMP_I_GAIN ) ;
}
thr_out = POSCONTROL_VIBE_COMP_P_GAIN * thr_per_accelz_cmss * _accel_target . z + _pid_accel_z . get_i ( ) * 0.001f ;
} else {
thr_out = _pid_accel_z . update_all ( _accel_target . z , z_accel_meas , ( _motors . limit . throttle_lower | | _motors . limit . throttle_upper ) ) * 0.001f ;
}
thr_out + = _motors . get_throttle_hover ( ) ;
2015-03-16 01:26:42 -03:00
2014-10-04 11:31:33 -03:00
// send throttle to attitude controller with angle boost
2015-04-16 01:56:48 -03:00
_attitude_control . set_throttle_out ( thr_out , true , POSCONTROL_THROTTLE_CUTOFF_FREQ ) ;
2019-06-17 12:36:22 -03:00
// _speed_down_cms is checked to be non-zero when set
float error_ratio = _vel_error . z / _speed_down_cms ;
_vel_z_control_ratio + = _dt * 0.1f * ( 0.5 - error_ratio ) ;
_vel_z_control_ratio = constrain_float ( _vel_z_control_ratio , 0.0f , 2.0f ) ;
2013-12-28 10:04:45 -04:00
}
2013-12-28 07:15:29 -04:00
2014-01-23 23:27:06 -04:00
///
2017-11-12 08:50:19 -04:00
/// lateral position controller
2014-01-23 23:27:06 -04:00
///
2013-12-28 07:15:29 -04:00
2018-09-20 02:43:48 -03:00
/// set_max_accel_xy - set the maximum horizontal acceleration in cm/s/s
void AC_PosControl : : set_max_accel_xy ( float accel_cmss )
2013-12-28 07:15:29 -04:00
{
2020-02-19 02:08:16 -04:00
// return immediately if no change
if ( is_equal ( _accel_cms , accel_cmss ) ) {
return ;
2013-12-28 07:15:29 -04:00
}
2020-02-19 02:08:16 -04:00
_accel_cms = accel_cmss ;
_flags . recalc_leash_xy = true ;
calc_leash_length_xy ( ) ;
2014-01-23 23:27:06 -04:00
}
2013-12-28 07:15:29 -04:00
2018-09-20 02:43:48 -03:00
/// set_max_speed_xy - set the maximum horizontal speed maximum in cm/s
void AC_PosControl : : set_max_speed_xy ( float speed_cms )
2014-01-23 23:27:06 -04:00
{
2020-02-19 02:08:16 -04:00
// return immediately if no change in speed
if ( is_equal ( _speed_cms , speed_cms ) ) {
return ;
2013-12-28 07:15:29 -04:00
}
2020-02-19 02:08:16 -04:00
_speed_cms = speed_cms ;
_flags . recalc_leash_xy = true ;
calc_leash_length_xy ( ) ;
2013-12-28 07:15:29 -04:00
}
2014-01-17 22:53:46 -04:00
/// set_pos_target in cm from home
void AC_PosControl : : set_pos_target ( const Vector3f & position )
{
_pos_target = position ;
2015-10-28 03:42:17 -03:00
_flags . use_desvel_ff_z = false ;
2015-04-13 06:22:52 -03:00
_vel_desired . z = 0.0f ;
2014-01-17 22:53:46 -04:00
// initialise roll and pitch to current roll and pitch. This avoids a twitch between when the target is set and the pos controller is first run
2014-01-23 23:27:06 -04:00
// To-Do: this initialisation of roll and pitch targets needs to go somewhere between when pos-control is initialised and when it completes it's first cycle
//_roll_target = constrain_int32(_ahrs.roll_sensor,-_attitude_control.lean_angle_max(),_attitude_control.lean_angle_max());
//_pitch_target = constrain_int32(_ahrs.pitch_sensor,-_attitude_control.lean_angle_max(),_attitude_control.lean_angle_max());
2014-01-17 22:53:46 -04:00
}
2014-05-06 05:32:30 -03:00
/// set_xy_target in cm from home
void AC_PosControl : : set_xy_target ( float x , float y )
{
_pos_target . x = x ;
_pos_target . y = y ;
}
2015-10-29 04:30:14 -03:00
/// shift position target target in x, y axis
void AC_PosControl : : shift_pos_xy_target ( float x_cm , float y_cm )
{
// move pos controller target
_pos_target . x + = x_cm ;
_pos_target . y + = y_cm ;
}
2014-04-30 09:22:47 -03:00
/// set_target_to_stopping_point_xy - sets horizontal target to reasonable stopping position in cm from home
void AC_PosControl : : set_target_to_stopping_point_xy ( )
{
// check if xy leash needs to be recalculated
calc_leash_length_xy ( ) ;
get_stopping_point_xy ( _pos_target ) ;
}
2014-01-17 22:53:46 -04:00
/// get_stopping_point_xy - calculates stopping point based on current position, velocity, vehicle acceleration
/// distance_max allows limiting distance to stopping point
/// results placed in stopping_position vector
2018-09-20 02:43:48 -03:00
/// set_max_accel_xy() should be called before this method to set vehicle acceleration
2014-01-17 22:53:46 -04:00
/// set_leash_length() should have been called before this method
void AC_PosControl : : get_stopping_point_xy ( Vector3f & 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 ( ) ;
Vector3f curr_vel = _inav . get_velocity ( ) ;
2014-01-17 22:53:46 -04:00
float linear_distance ; // the distance at which we swap from a linear to sqrt response
float linear_velocity ; // the velocity above which we swap from a linear to sqrt response
2019-04-19 08:36:42 -03:00
float stopping_dist ; // the distance within the vehicle can stop
2014-02-14 03:05:29 -04:00
float kP = _p_pos_xy . kP ( ) ;
2013-12-28 07:15:29 -04:00
2014-05-06 05:32:30 -03:00
// add velocity error to current velocity
if ( is_active_xy ( ) ) {
curr_vel . x + = _vel_error . x ;
curr_vel . y + = _vel_error . y ;
}
2013-12-28 07:15:29 -04:00
// calculate current velocity
2016-04-16 06:58:46 -03:00
float vel_total = norm ( curr_vel . x , curr_vel . y ) ;
2013-12-28 07:15:29 -04:00
// avoid divide by zero by using current position if the velocity is below 10cm/s, kP is very low or acceleration is zero
2015-05-04 23:34:37 -03:00
if ( kP < = 0.0f | | _accel_cms < = 0.0f | | is_zero ( vel_total ) ) {
2014-04-30 09:22:47 -03:00
stopping_point . x = curr_pos . x ;
stopping_point . y = curr_pos . y ;
2013-12-28 07:15:29 -04:00
return ;
}
// calculate point at which velocity switches from linear to sqrt
2019-04-19 08:36:42 -03:00
linear_velocity = _accel_cms / kP ;
2013-12-28 07:15:29 -04:00
// calculate distance within which we can stop
if ( vel_total < linear_velocity ) {
2019-04-19 08:36:42 -03:00
stopping_dist = vel_total / kP ;
2013-12-28 07:15:29 -04:00
} else {
2019-04-19 08:36:42 -03:00
linear_distance = _accel_cms / ( 2.0f * kP * kP ) ;
stopping_dist = linear_distance + ( vel_total * vel_total ) / ( 2.0f * _accel_cms ) ;
2013-12-28 07:15:29 -04:00
}
2014-01-17 22:53:46 -04:00
// constrain stopping distance
stopping_dist = constrain_float ( stopping_dist , 0 , _leash ) ;
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
stopping_point . x = curr_pos . x + ( stopping_dist * curr_vel . x / vel_total ) ;
stopping_point . y = curr_pos . y + ( stopping_dist * curr_vel . y / vel_total ) ;
2013-12-28 07:15:29 -04:00
}
2017-11-27 08:21:13 -04:00
/// get_distance_to_target - get horizontal distance to target position in cm
2013-12-28 07:15:29 -04:00
float AC_PosControl : : get_distance_to_target ( ) const
{
2018-10-25 18:46:52 -03:00
return norm ( _pos_error . x , _pos_error . y ) ;
2013-12-28 07:15:29 -04:00
}
2017-11-27 08:21:13 -04:00
/// get_bearing_to_target - get bearing to target position in centi-degrees
int32_t AC_PosControl : : get_bearing_to_target ( ) const
{
return get_bearing_cd ( _inav . get_position ( ) , _pos_target ) ;
}
2020-02-28 01:01:59 -04:00
// relax velocity controller by clearing velocity error and setting velocity target to current velocity
void AC_PosControl : : relax_velocity_controller_xy ( )
{
const Vector3f & curr_vel = _inav . get_velocity ( ) ;
_vel_target . x = curr_vel . x ;
_vel_target . y = curr_vel . y ;
_vel_error . x = 0.0f ;
_vel_error . y = 0.0f ;
}
2014-05-06 05:32:30 -03:00
// is_active_xy - returns true if the xy position controller has been run very recently
bool AC_PosControl : : is_active_xy ( ) const
{
2019-01-24 07:20:12 -04:00
return ( ( AP_HAL : : micros64 ( ) - _last_update_xy_us ) < = POSCONTROL_ACTIVE_TIMEOUT_US ) ;
2014-05-06 05:32:30 -03:00
}
2017-12-16 09:06:51 -04:00
/// get_lean_angle_max_cd - returns the maximum lean angle the autopilot may request
float AC_PosControl : : get_lean_angle_max_cd ( ) const
{
if ( is_zero ( _lean_angle_max ) ) {
return _attitude_control . lean_angle_max ( ) ;
}
return _lean_angle_max * 100.0f ;
}
2014-05-06 05:32:30 -03:00
/// init_xy_controller - initialise the xy controller
2018-08-22 05:30:30 -03:00
/// this should be called after setting the position target and the desired velocity and acceleration
2014-05-06 23:46:39 -03:00
/// sets target roll angle, pitch angle and I terms based on vehicle current lean angles
/// should be called once whenever significant changes to the position target are made
/// this does not update the xy target
2018-08-22 05:30:30 -03:00
void AC_PosControl : : init_xy_controller ( )
2014-05-06 05:32:30 -03:00
{
// set roll, pitch lean angle targets to current attitude
2018-08-22 05:30:30 -03:00
// todo: this should probably be based on the desired attitude not the current attitude
2014-05-06 05:32:30 -03:00
_roll_target = _ahrs . roll_sensor ;
_pitch_target = _ahrs . pitch_sensor ;
2014-05-06 23:46:39 -03:00
// initialise I terms from lean angles
2018-08-22 05:30:30 -03:00
_pid_vel_xy . reset_filter ( ) ;
lean_angles_to_accel ( _accel_target . x , _accel_target . y ) ;
2019-04-19 08:36:42 -03:00
_pid_vel_xy . set_integrator ( _accel_target - _accel_desired ) ;
2014-05-06 05:32:30 -03:00
// flag reset required in rate to accel step
_flags . reset_desired_vel_to_pos = true ;
2015-03-13 08:46:21 -03:00
_flags . reset_accel_to_lean_xy = true ;
2016-11-22 01:36:17 -04:00
// initialise ekf xy reset handler
init_ekf_xy_reset ( ) ;
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.
_pos_target = _inav . get_position ( ) ;
// Set _pid_vel_xy integrators and derivative to zero.
_pid_vel_xy . reset_filter ( ) ;
// initialise ekf xy reset handler
init_ekf_xy_reset ( ) ;
}
2014-04-19 04:40:47 -03:00
/// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher
2018-09-05 06:42:32 -03:00
void AC_PosControl : : update_xy_controller ( )
2013-12-28 07:15:29 -04:00
{
2014-12-17 17:24:35 -04:00
// compute dt
2019-01-30 01:51:17 -04:00
const uint64_t now_us = AP_HAL : : micros64 ( ) ;
2019-01-24 07:20:12 -04:00
float dt = ( now_us - _last_update_xy_us ) * 1.0e-6 f ;
2014-12-17 17:24:35 -04:00
2018-03-01 03:57:29 -04:00
// sanity check dt
2019-01-24 07:20:12 -04:00
if ( dt > = POSCONTROL_ACTIVE_TIMEOUT_US * 1.0e-6 f ) {
2014-12-17 17:24:35 -04:00
dt = 0.0f ;
2013-12-28 07:15:29 -04:00
}
2016-11-22 01:36:17 -04:00
// check for ekf xy position reset
check_for_ekf_xy_reset ( ) ;
2014-01-23 23:27:06 -04:00
// check if xy leash needs to be recalculated
calc_leash_length_xy ( ) ;
2014-12-17 17:24:35 -04:00
// translate any adjustments from pilot to loiter target
desired_vel_to_pos ( dt ) ;
2017-06-27 10:07:14 -03:00
// run horizontal position controller
2018-09-05 06:42:32 -03:00
run_xy_controller ( dt ) ;
2018-03-01 03:57:29 -04:00
// update xy update time
2019-01-24 07:20:12 -04:00
_last_update_xy_us = now_us ;
2014-12-17 17:24:35 -04:00
}
float AC_PosControl : : time_since_last_xy_update ( ) const
{
2019-01-30 01:51:17 -04:00
const uint64_t now_us = AP_HAL : : micros64 ( ) ;
2019-01-24 07:20:12 -04:00
return ( now_us - _last_update_xy_us ) * 1.0e-6 f ;
2013-12-28 07:15:29 -04:00
}
2019-07-16 03:03:55 -03:00
// write log to dataflash
2018-03-01 01:42:52 -04:00
void AC_PosControl : : write_log ( )
{
const Vector3f & pos_target = get_pos_target ( ) ;
const Vector3f & vel_target = get_vel_target ( ) ;
const Vector3f & accel_target = get_accel_target ( ) ;
const Vector3f & position = _inav . get_position ( ) ;
const Vector3f & velocity = _inav . get_velocity ( ) ;
float accel_x , accel_y ;
lean_angles_to_accel ( accel_x , accel_y ) ;
2020-04-05 19:58:56 -03:00
// @LoggerMessage: PSC
// @Description: Position Control data
// @Field: TimeUS: Time since system startup
// @Field: TPX: Target position relative to origin, X-axis
// @Field: TPY: Target position relative to origin, Y-axis
// @Field: PX: Position relative to origin, X-axis
// @Field: PY: Position relative to origin, Y-axis
// @Field: TVX: Target velocity, X-axis
// @Field: TVY: Target velocity, Y-axis
// @Field: VX: Velocity, X-axis
// @Field: VY: Velocity, Y-axis
// @Field: TAX: Target acceleration, X-axis
// @Field: TAY: Target acceleration, Y-axis
// @Field: AX: Acceleration, X-axis
// @Field: AY: Acceleration, Y-axis
2019-03-25 02:36:23 -03:00
AP : : logger ( ) . Write ( " PSC " ,
" TimeUS,TPX,TPY,PX,PY,TVX,TVY,VX,VY,TAX,TAY,AX,AY " ,
" smmmmnnnnoooo " ,
" F000000000000 " ,
" Qffffffffffff " ,
AP_HAL : : micros64 ( ) ,
double ( pos_target . x * 0.01f ) ,
double ( pos_target . y * 0.01f ) ,
double ( position . x * 0.01f ) ,
double ( position . y * 0.01f ) ,
double ( vel_target . x * 0.01f ) ,
double ( vel_target . y * 0.01f ) ,
double ( velocity . x * 0.01f ) ,
double ( velocity . y * 0.01f ) ,
double ( accel_target . x * 0.01f ) ,
double ( accel_target . y * 0.01f ) ,
double ( accel_x * 0.01f ) ,
double ( accel_y * 0.01f ) ) ;
2018-03-01 01:42:52 -04:00
}
2014-06-02 06:02:44 -03:00
/// init_vel_controller_xyz - initialise the velocity controller - should be called once before the caller attempts to use the controller
void AC_PosControl : : init_vel_controller_xyz ( )
{
// set roll, pitch lean angle targets to current attitude
_roll_target = _ahrs . roll_sensor ;
_pitch_target = _ahrs . pitch_sensor ;
2018-08-22 05:30:30 -03:00
_pid_vel_xy . reset_filter ( ) ;
2014-06-02 06:02:44 -03:00
lean_angles_to_accel ( _accel_target . x , _accel_target . y ) ;
2018-01-15 06:50:54 -04:00
_pid_vel_xy . set_integrator ( _accel_target ) ;
2014-06-02 06:02:44 -03:00
// flag reset required in rate to accel step
_flags . reset_desired_vel_to_pos = true ;
2015-03-13 08:46:21 -03:00
_flags . reset_accel_to_lean_xy = true ;
2014-06-02 06:02:44 -03:00
2015-11-17 00:25:51 -04:00
// set target position
2014-06-02 06:02:44 -03:00
const Vector3f & curr_pos = _inav . get_position ( ) ;
set_xy_target ( curr_pos . x , curr_pos . y ) ;
2015-11-17 00:25:51 -04:00
set_alt_target ( curr_pos . z ) ;
2014-06-02 06:02:44 -03:00
// move current vehicle velocity into feed forward velocity
const Vector3f & curr_vel = _inav . get_velocity ( ) ;
2015-11-17 00:25:51 -04:00
set_desired_velocity ( curr_vel ) ;
2016-11-22 01:36:17 -04:00
2017-11-12 08:22:53 -04:00
// set vehicle acceleration to zero
2019-04-19 08:36:42 -03:00
set_desired_accel_xy ( 0.0f , 0.0f ) ;
2017-11-12 08:22:53 -04:00
2016-11-22 01:36:17 -04:00
// initialise ekf reset handlers
init_ekf_xy_reset ( ) ;
init_ekf_z_reset ( ) ;
2014-06-02 06:02:44 -03:00
}
2017-09-08 02:58:59 -03:00
/// update_velocity_controller_xyz - run the velocity controller - should be called at 100hz or higher
/// velocity targets should we set using set_desired_velocity_xyz() method
/// callers should use get_roll() and get_pitch() methods and sent to the attitude controller
/// throttle targets will be sent directly to the motors
2018-09-05 06:42:32 -03:00
void AC_PosControl : : update_vel_controller_xyz ( )
2017-09-08 02:58:59 -03:00
{
2020-03-30 10:33:28 -03:00
update_xy_controller ( ) ;
2014-06-02 06:02:44 -03:00
// update altitude target
2015-11-17 02:48:40 -04:00
set_alt_target_from_climb_rate_ff ( _vel_desired . z , _dt , false ) ;
2014-06-02 06:02:44 -03:00
// run z-axis position controller
update_z_controller ( ) ;
}
2016-07-07 21:36:01 -03:00
float AC_PosControl : : get_horizontal_error ( ) const
{
return norm ( _pos_error . x , _pos_error . y ) ;
}
2014-01-17 22:53:46 -04:00
///
/// private methods
///
2013-12-28 07:15:29 -04:00
2014-01-23 23:27:06 -04:00
/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration
/// should be called whenever the speed, acceleration or position kP is modified
void AC_PosControl : : calc_leash_length_xy ( )
{
2018-01-11 08:28:20 -04:00
// todo: remove _flags.recalc_leash_xy or don't call this function after each variable change.
2014-01-23 23:27:06 -04:00
if ( _flags . recalc_leash_xy ) {
2014-02-14 03:05:29 -04:00
_leash = calc_leash_length ( _speed_cms , _accel_cms , _p_pos_xy . kP ( ) ) ;
2014-01-23 23:27:06 -04:00
_flags . recalc_leash_xy = false ;
}
}
2017-11-16 08:29:02 -04:00
/// move velocity target using desired acceleration
void AC_PosControl : : desired_accel_to_vel ( float nav_dt )
{
// range check nav_dt
if ( nav_dt < 0 ) {
return ;
}
// update target velocity
if ( _flags . reset_desired_vel_to_pos ) {
_flags . reset_desired_vel_to_pos = false ;
} else {
_vel_desired . x + = _accel_desired . x * nav_dt ;
_vel_desired . y + = _accel_desired . y * nav_dt ;
}
}
2014-01-17 22:53:46 -04:00
/// desired_vel_to_pos - move position target using desired velocities
void AC_PosControl : : desired_vel_to_pos ( float nav_dt )
{
// range check nav_dt
2019-04-19 08:36:42 -03:00
if ( nav_dt < 0 ) {
2013-12-28 07:15:29 -04:00
return ;
}
2014-01-17 22:53:46 -04:00
// update target position
2014-05-06 05:32:30 -03:00
if ( _flags . reset_desired_vel_to_pos ) {
_flags . reset_desired_vel_to_pos = false ;
} else {
_pos_target . x + = _vel_desired . x * nav_dt ;
_pos_target . y + = _vel_desired . y * nav_dt ;
}
2013-12-28 07:15:29 -04:00
}
2017-06-27 10:07:14 -03:00
/// run horizontal position controller correcting position and velocity
2014-01-17 22:53:46 -04:00
/// converts position (_pos_target) to target velocity (_vel_target)
2017-06-27 10:07:14 -03:00
/// desired velocity (_vel_desired) is combined into final target velocity
/// converts desired velocities in lat/lon directions to accelerations in lat/lon frame
/// converts desired accelerations provided in lat/lon frame to roll/pitch angles
2018-09-05 06:42:32 -03:00
void AC_PosControl : : run_xy_controller ( float dt )
2013-12-28 07:15:29 -04:00
{
2018-09-05 06:42:32 -03:00
float ekfGndSpdLimit , ekfNavVelGainScaler ;
AP : : ahrs_navekf ( ) . getEkfControlLimits ( ekfGndSpdLimit , ekfNavVelGainScaler ) ;
2014-01-17 22:53:46 -04:00
Vector3f curr_pos = _inav . get_position ( ) ;
2014-11-15 22:03:21 -04:00
float kP = ekfNavVelGainScaler * _p_pos_xy . kP ( ) ; // scale gains to compensate for noisy optical flow measurement in the EKF
2013-12-28 07:15:29 -04:00
// avoid divide by zero
if ( kP < = 0.0f ) {
2014-07-08 06:02:11 -03:00
_vel_target . x = 0.0f ;
_vel_target . y = 0.0f ;
2019-04-19 08:36:42 -03:00
} else {
2013-12-28 07:15:29 -04:00
// calculate distance error
2014-01-17 22:53:46 -04:00
_pos_error . x = _pos_target . x - curr_pos . x ;
_pos_error . y = _pos_target . y - curr_pos . y ;
2017-11-16 08:29:02 -04:00
// Constrain _pos_error and target position
// Constrain the maximum length of _vel_target to the maximum position correction velocity
// TODO: replace the leash length with a user definable maximum position correction
2019-04-19 08:36:42 -03:00
if ( limit_vector_length ( _pos_error . x , _pos_error . y , _leash ) ) {
2017-11-16 08:29:02 -04:00
_pos_target . x = curr_pos . x + _pos_error . x ;
_pos_target . y = curr_pos . y + _pos_error . y ;
2014-01-17 22:53:46 -04:00
}
2015-02-06 03:59:00 -04:00
2017-11-16 08:29:02 -04:00
_vel_target = sqrt_controller ( _pos_error , kP , _accel_cms ) ;
2013-12-28 07:15:29 -04:00
}
2017-11-16 08:29:02 -04:00
// add velocity feed-forward
_vel_target . x + = _vel_desired . x ;
_vel_target . y + = _vel_desired . y ;
2013-12-28 07:15:29 -04:00
2017-11-12 08:50:19 -04:00
// the following section converts desired velocities in lat/lon directions to accelerations in lat/lon frame
2017-06-27 10:07:14 -03:00
2017-11-16 08:29:02 -04:00
Vector2f accel_target , vel_xy_p , vel_xy_i , vel_xy_d ;
2014-05-06 05:32:30 -03:00
2016-07-20 15:38:37 -03:00
// check if vehicle velocity is being overridden
if ( _flags . vehicle_horiz_vel_override ) {
_flags . vehicle_horiz_vel_override = false ;
} else {
_vehicle_horiz_vel . x = _inav . get_velocity ( ) . x ;
_vehicle_horiz_vel . y = _inav . get_velocity ( ) . y ;
}
2013-12-28 07:15:29 -04:00
// calculate velocity error
2016-07-20 15:38:37 -03:00
_vel_error . x = _vel_target . x - _vehicle_horiz_vel . x ;
_vel_error . y = _vel_target . y - _vehicle_horiz_vel . y ;
2017-11-16 08:29:02 -04:00
// TODO: constrain velocity error and velocity target
2013-12-28 07:15:29 -04:00
2015-01-29 02:50:14 -04:00
// call pi controller
2018-01-15 06:50:54 -04:00
_pid_vel_xy . set_input ( _vel_error ) ;
2015-01-23 06:06:17 -04:00
2015-01-29 02:50:14 -04:00
// get p
2018-01-15 06:50:54 -04:00
vel_xy_p = _pid_vel_xy . get_p ( ) ;
2014-04-22 10:33:48 -03:00
// update i term if we have not hit the accel or throttle limits OR the i term will reduce
2017-11-16 08:29:02 -04:00
// TODO: move limit handling into the PI and PID controller
2016-12-01 11:53:25 -04:00
if ( ! _limit . accel_xy & & ! _motors . limit . throttle_upper ) {
2018-01-15 06:50:54 -04:00
vel_xy_i = _pid_vel_xy . get_i ( ) ;
2015-01-29 02:50:14 -04:00
} else {
2018-01-15 06:50:54 -04:00
vel_xy_i = _pid_vel_xy . get_i_shrink ( ) ;
2014-04-22 10:33:48 -03:00
}
2018-01-15 06:50:54 -04:00
// get d
vel_xy_d = _pid_vel_xy . get_d ( ) ;
2017-11-16 08:29:02 -04:00
// acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise
accel_target . x = ( vel_xy_p . x + vel_xy_i . x + vel_xy_d . x ) * ekfNavVelGainScaler ;
accel_target . y = ( vel_xy_p . y + vel_xy_i . y + vel_xy_d . y ) * ekfNavVelGainScaler ;
// reset accel to current desired acceleration
2019-04-19 08:36:42 -03:00
if ( _flags . reset_accel_to_lean_xy ) {
_accel_target_filter . reset ( Vector2f ( accel_target . x , accel_target . y ) ) ;
_flags . reset_accel_to_lean_xy = false ;
}
2017-11-16 08:29:02 -04:00
// filter correction acceleration
2019-04-19 08:36:42 -03:00
_accel_target_filter . set_cutoff_frequency ( MIN ( _accel_xy_filt_hz , 5.0f * ekfNavVelGainScaler ) ) ;
2017-11-16 08:29:02 -04:00
_accel_target_filter . apply ( accel_target , dt ) ;
// pass the correction acceleration to the target acceleration output
_accel_target . x = _accel_target_filter . get ( ) . x ;
_accel_target . y = _accel_target_filter . get ( ) . y ;
// Add feed forward into the target acceleration output
_accel_target . x + = _accel_desired . x ;
_accel_target . y + = _accel_desired . y ;
2015-06-28 03:32:38 -03:00
2017-11-12 08:50:19 -04:00
// the following section converts desired accelerations provided in lat/lon frame to roll/pitch angles
2017-06-27 10:07:14 -03:00
2017-11-16 08:29:02 -04:00
// limit acceleration using maximum lean angles
2017-12-16 09:06:51 -04:00
float angle_max = MIN ( _attitude_control . get_althold_lean_angle_max ( ) , get_lean_angle_max_cd ( ) ) ;
2017-11-17 01:15:35 -04:00
float accel_max = MIN ( GRAVITY_MSS * 100.0f * tanf ( ToRad ( angle_max * 0.01f ) ) , POSCONTROL_ACCEL_XY_MAX ) ;
2017-11-16 08:29:02 -04:00
_limit . accel_xy = limit_vector_length ( _accel_target . x , _accel_target . y , accel_max ) ;
2014-11-07 23:47:33 -04:00
2017-12-16 09:06:51 -04: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 ) ;
}
// get_lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s
void AC_PosControl : : accel_to_lean_angles ( float accel_x_cmss , float accel_y_cmss , float & roll_target , float & pitch_target ) const
{
float accel_right , accel_forward ;
2014-12-08 20:56:53 -04:00
// rotate accelerations into body forward-right frame
2018-08-22 05:30:30 -03:00
// todo: this should probably be based on the desired heading not the current heading
2019-04-19 08:36:42 -03:00
accel_forward = accel_x_cmss * _ahrs . cos_yaw ( ) + accel_y_cmss * _ahrs . sin_yaw ( ) ;
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
}
2014-05-06 05:32:30 -03:00
// get_lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s
2014-05-06 08:40:45 -03:00
void AC_PosControl : : lean_angles_to_accel ( 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
2018-08-22 05:30:30 -03:00
// todo: this should probably be based on the desired attitude not the current attitude
2019-04-19 08:36:42 -03:00
accel_x_cmss = ( GRAVITY_MSS * 100 ) * ( - _ahrs . cos_yaw ( ) * _ahrs . sin_pitch ( ) * _ahrs . cos_roll ( ) - _ahrs . sin_yaw ( ) * _ahrs . sin_roll ( ) ) / MAX ( _ahrs . cos_roll ( ) * _ahrs . cos_pitch ( ) , 0.5f ) ;
accel_y_cmss = ( GRAVITY_MSS * 100 ) * ( - _ahrs . sin_yaw ( ) * _ahrs . sin_pitch ( ) * _ahrs . cos_roll ( ) + _ahrs . cos_yaw ( ) * _ahrs . sin_roll ( ) ) / MAX ( _ahrs . cos_roll ( ) * _ahrs . cos_pitch ( ) , 0.5f ) ;
2014-05-06 05:32:30 -03:00
}
2014-01-17 22:53:46 -04:00
/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration and position kP gain
float AC_PosControl : : calc_leash_length ( float speed_cms , float accel_cms , float kP ) const
2013-12-28 07:15:29 -04:00
{
2014-01-17 22:53:46 -04:00
float leash_length ;
2013-12-28 07:15:29 -04:00
2014-01-17 22:53:46 -04:00
// sanity check acceleration and avoid divide by zero
if ( accel_cms < = 0.0f ) {
accel_cms = POSCONTROL_ACCELERATION_MIN ;
}
// avoid divide by zero
if ( kP < = 0.0f ) {
return POSCONTROL_LEASH_LENGTH_MIN ;
}
// calculate leash length
2019-04-19 08:36:42 -03:00
if ( speed_cms < = accel_cms / kP ) {
2014-01-17 22:53:46 -04:00
// linear leash length based on speed close in
leash_length = speed_cms / kP ;
2019-04-19 08:36:42 -03:00
} else {
2014-01-17 22:53:46 -04:00
// leash length grows at sqrt of speed further out
2019-04-19 08:36:42 -03:00
leash_length = ( accel_cms / ( 2.0f * kP * kP ) ) + ( speed_cms * speed_cms / ( 2.0f * accel_cms ) ) ;
2014-01-17 22:53:46 -04:00
}
// ensure leash is at least 1m long
2019-04-19 08:36:42 -03:00
if ( leash_length < POSCONTROL_LEASH_LENGTH_MIN ) {
2014-01-17 22:53:46 -04:00
leash_length = POSCONTROL_LEASH_LENGTH_MIN ;
}
return leash_length ;
2013-12-28 07:15:29 -04:00
}
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 ) ;
}
/// check for ekf position reset and adjust loiter or brake target position
void AC_PosControl : : check_for_ekf_xy_reset ( )
{
// check for position shift
Vector2f pos_shift ;
uint32_t reset_ms = _ahrs . getLastPosNorthEastReset ( pos_shift ) ;
if ( reset_ms ! = _ekf_xy_reset_ms ) {
shift_pos_xy_target ( pos_shift . x * 100.0f , pos_shift . y * 100.0f ) ;
_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 ) ;
}
/// check for ekf position reset and adjust loiter or brake target position
void AC_PosControl : : check_for_ekf_z_reset ( )
{
// 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 ) {
2016-11-22 01:36:17 -04:00
shift_alt_target ( - alt_shift * 100.0f ) ;
_ekf_z_reset_ms = reset_ms ;
}
}
2017-11-16 08:29:02 -04:00
/// limit vector to a given length, returns true if vector was limited
bool AC_PosControl : : limit_vector_length ( float & vector_x , float & vector_y , float max_length )
{
float vector_length = norm ( vector_x , vector_y ) ;
2018-09-11 10:51:45 -03:00
if ( ( vector_length > max_length ) & & is_positive ( vector_length ) ) {
2017-11-16 08:29:02 -04:00
vector_x * = ( max_length / vector_length ) ;
vector_y * = ( max_length / vector_length ) ;
return true ;
}
return false ;
}
/// Proportional controller with piecewise sqrt sections to constrain second derivative
Vector3f AC_PosControl : : sqrt_controller ( const Vector3f & error , float p , float second_ord_lim )
{
if ( second_ord_lim < 0.0f | | is_zero ( second_ord_lim ) | | is_zero ( p ) ) {
2019-04-19 08:36:42 -03:00
return Vector3f ( error . x * p , error . y * p , error . z ) ;
2017-11-16 08:29:02 -04:00
}
2019-04-19 08:36:42 -03:00
float linear_dist = second_ord_lim / sq ( p ) ;
2017-11-16 08:29:02 -04:00
float error_length = norm ( error . x , error . y ) ;
if ( error_length > linear_dist ) {
2019-04-19 08:36:42 -03:00
float first_order_scale = safe_sqrt ( 2.0f * second_ord_lim * ( error_length - ( linear_dist * 0.5f ) ) ) / error_length ;
return Vector3f ( error . x * first_order_scale , error . y * first_order_scale , error . z ) ;
2017-11-16 08:29:02 -04:00
} else {
2019-04-19 08:36:42 -03:00
return Vector3f ( error . x * p , error . y * p , error . z ) ;
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 )
{
// validate AC_P members:
const struct {
const char * pid_name ;
AC_P & p ;
} ps [ ] = {
{ " POSXY " , get_pos_xy_p ( ) } ,
{ " POSZ " , get_pos_z_p ( ) } ,
{ " VELZ " , get_vel_z_p ( ) } ,
} ;
for ( uint8_t i = 0 ; i < ARRAY_SIZE ( ps ) ; i + + ) {
// all AC_P's must have a positive P value:
if ( ! is_positive ( ps [ i ] . p . kP ( ) ) ) {
hal . util - > snprintf ( failure_msg , failure_msg_len , " %s_%s_P must be > 0 " , param_prefix , ps [ i ] . pid_name ) ;
return false ;
}
}
2019-04-15 07:39:43 -03:00
// z-axis acceleration control PID doesn't use FF, so P and I must be positive
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 ;
}