2015-08-11 03:28:41 -03:00
# include <AP_HAL/AP_HAL.h>
# include "AC_PosControl.h"
# include <AP_Math/AP_Math.h>
2018-03-01 01:42:52 -04:00
# include <DataFlash/DataFlash.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
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
2018-01-27 02:39:55 -04:00
// @Param: _ACCZ_FILT
2018-01-15 06:52:55 -04:00
// @DisplayName: Acceleration (vertical) controller filter
// @Description: Filter applied to acceleration to reduce noise. Lower values reduce noise but add delay.
// @Range: 1.000 100.000
// @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
AP_GROUPINFO ( " _ANGLE_MAX " , 7 , AC_PosControl , _lean_angle_max , 0.0f ) ,
2013-12-28 07:15:29 -04:00
AP_GROUPEND
} ;
// Default constructor.
// Note that the Vector/Matrix constructors already implicitly zero
// their values.
//
2017-02-11 04:14:23 -04:00
AC_PosControl : : AC_PosControl ( const 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 ) ,
2018-01-27 02:39:55 -04:00
_pid_accel_z ( POSCONTROL_ACC_Z_P , POSCONTROL_ACC_Z_I , POSCONTROL_ACC_Z_D , POSCONTROL_ACC_Z_IMAX , POSCONTROL_ACC_Z_FILT_HZ , 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-05-02 00:08:18 -03:00
_last_update_xy_ms ( 0 ) ,
_last_update_z_ms ( 0 ) ,
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 ) ,
2015-06-05 03:06:34 -03:00
_accel_last_z_cms ( 0.0f ) ,
2014-04-21 09:34:33 -03:00
_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 ) ,
2014-07-08 06:02:11 -03:00
_roll_target ( 0.0f ) ,
_pitch_target ( 0.0f ) ,
2015-03-10 04:11:26 -03:00
_distance_to_target ( 0.0f ) ,
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 ;
_flags . reset_accel_to_throttle = 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
}
2014-01-23 23:27:06 -04:00
/// set_speed_z - sets 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
/// calc_leash_length_z should be called afterwards
/// speed_down should be a negative number
2014-01-23 23:27:06 -04:00
void AC_PosControl : : set_speed_z ( float speed_down , float speed_up )
{
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
2015-05-02 06:34:54 -03:00
if ( ( fabsf ( _speed_down_cms - speed_down ) > 1.0f ) | | ( fabsf ( _speed_up_cms - speed_up ) > 1.0f ) ) {
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
}
}
/// set_accel_z - set vertical acceleration in cm/s/s
void AC_PosControl : : set_accel_z ( float accel_cmss )
{
2015-05-02 06:34:54 -03:00
if ( fabsf ( _accel_z_cms - accel_cmss ) > 1.0f ) {
2014-01-23 23:27:06 -04:00
_accel_z_cms = accel_cmss ;
_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
}
}
/// 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 )
{
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
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 ) ) {
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
}
2014-01-23 23:27:06 -04:00
}
// do not let target get too far from current altitude
float curr_alt = _inav . get_altitude ( ) ;
_pos_target . z = constrain_float ( _pos_target . z , curr_alt - _leash_down_z , curr_alt + _leash_up_z ) ;
}
2015-10-27 10:04:24 -03:00
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?
if ( ( climb_rate_cms < 0 & & ( ! _motors . limit . throttle_lower | | force_descend ) ) | | ( climb_rate_cms > 0 & & ! _motors . limit . throttle_upper & & ! _limit . pos_up ) ) {
_pos_target . z + = climb_rate_cms * dt ;
}
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
2015-11-27 13:11:58 -04: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 ;
_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?
2015-04-13 06:22:52 -03:00
if ( ( _vel_desired . z < 0 & & ( ! _motors . limit . throttle_lower | | force_descend ) ) | | ( _vel_desired . z > 0 & & ! _motors . limit . throttle_upper & & ! _limit . pos_up ) ) {
_pos_target . z + = _vel_desired . z * dt ;
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 ;
_accel_target . z = - ( _ahrs . get_accel_ef_blended ( ) . z + GRAVITY_MSS ) * 100.0f ;
_flags . reset_accel_to_throttle = true ;
2016-07-11 23:26:10 -03:00
_pid_accel_z . set_integrator ( ( throttle_setting - _motors . get_throttle_hover ( ) ) * 1000.0f ) ;
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
2014-03-16 21:40:11 -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
2015-01-31 02:50:32 -04: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
2015-01-31 02:50:32 -04:00
stopping_point . z = curr_pos_z + curr_vel_z / _p_pos_z . kP ( ) ;
2013-12-28 07:15:29 -04:00
} else {
2015-01-31 02:50:32 -04:00
linear_distance = _accel_z_cms / ( 2.0f * _p_pos_z . kP ( ) * _p_pos_z . kP ( ) ) ;
2014-01-25 04:23:55 -04:00
if ( curr_vel_z > 0 ) {
2014-04-21 09:34:33 -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
} else {
2014-04-21 09:34:33 -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
2016-06-21 10:23:31 -03:00
_pid_accel_z . set_integrator ( ( _motors . get_throttle ( ) - _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
{
2015-11-19 23:05:22 -04:00
return ( ( AP_HAL : : millis ( ) - _last_update_z_ms ) < = POSCONTROL_ACTIVE_TIMEOUT_MS ) ;
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
2015-11-19 23:05:22 -04:00
uint32_t now = AP_HAL : : millis ( ) ;
2014-05-02 00:08:18 -03:00
if ( now - _last_update_z_ms > POSCONTROL_ACTIVE_TIMEOUT_MS ) {
_flags . reset_rate_to_accel_z = true ;
_flags . reset_accel_to_throttle = true ;
}
_last_update_z_ms = now ;
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 ) {
2014-06-06 01:30:45 -03:00
if ( ! _flags . freeze_ff_z ) {
2017-11-16 08:29:02 -04:00
_accel_desired . z = ( _vel_target . z - _vel_last . z ) / _dt ;
2014-05-31 10:35:14 -03:00
} else {
2014-05-31 22:48:43 -03:00
// stop the feed forward being calculated during a known discontinuity
2014-06-06 01:30:45 -03:00
_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
2013-12-28 07:15:29 -04: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
2015-03-26 18:42:42 -03:00
float p , i , d ; // used to capture pid values for logging
2013-12-28 10:04:45 -04:00
// 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
// reset target altitude if this controller has just been engaged
2014-05-02 00:08:18 -03:00
if ( _flags . reset_accel_to_throttle ) {
2013-12-28 10:04:45 -04:00
// Reset Filter
_accel_error . z = 0 ;
2014-05-02 00:08:18 -03:00
_flags . reset_accel_to_throttle = false ;
2013-12-28 10:04:45 -04:00
} else {
2015-04-16 01:56:48 -03:00
// calculate accel error
2017-11-12 08:50:19 -04:00
_accel_error . z = _accel_target . z - z_accel_meas ;
2013-12-28 10:04:45 -04:00
}
2015-01-23 06:06:17 -04:00
// set input to PID
2016-10-23 04:18:55 -03:00
_pid_accel_z . set_input_filter_all ( _accel_error . z ) ;
2017-11-12 08:50:19 -04:00
_pid_accel_z . set_desired_rate ( _accel_target . z ) ;
2015-01-23 06:06:17 -04:00
2013-12-28 10:04:45 -04:00
// separately calculate p, i, d values for logging
2015-01-31 02:50:32 -04:00
p = _pid_accel_z . get_p ( ) ;
2013-12-28 10:04:45 -04:00
// get i term
2015-01-31 02:50:32 -04:00
i = _pid_accel_z . get_integrator ( ) ;
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 ) ;
}
2013-12-28 10:04:45 -04:00
// update i term as long as we haven't breached the limits or the I term will certainly reduce
// To-Do: should this be replaced with limits check from attitude_controller?
if ( ( ! _motors . limit . throttle_lower & & ! _motors . limit . throttle_upper ) | | ( i > 0 & & _accel_error . z < 0 ) | | ( i < 0 & & _accel_error . z > 0 ) ) {
2015-01-31 02:50:32 -04:00
i = _pid_accel_z . get_i ( ) ;
2013-12-28 10:04:45 -04:00
}
// get d term
2015-01-31 02:50:32 -04:00
d = _pid_accel_z . get_d ( ) ;
2013-12-28 10:04:45 -04:00
2017-10-16 15:33:23 -03:00
float thr_out = ( p + i + d ) * 0.001f + _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 ) ;
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
2014-01-23 23:27:06 -04:00
/// set_accel_xy - set horizontal acceleration in cm/s/s
void AC_PosControl : : set_accel_xy ( float accel_cmss )
2013-12-28 07:15:29 -04:00
{
2015-05-02 06:34:54 -03:00
if ( fabsf ( _accel_cms - accel_cmss ) > 1.0f ) {
2014-01-23 23:27:06 -04:00
_accel_cms = accel_cmss ;
_flags . recalc_leash_xy = true ;
2015-05-19 06:15:37 -03:00
calc_leash_length_xy ( ) ;
2013-12-28 07:15:29 -04:00
}
2014-01-23 23:27:06 -04:00
}
2013-12-28 07:15:29 -04:00
2014-01-23 23:27:06 -04:00
/// set_speed_xy - set horizontal speed maximum in cm/s
void AC_PosControl : : set_speed_xy ( float speed_cms )
{
2015-05-02 06:34:54 -03:00
if ( fabsf ( _speed_cms - speed_cms ) > 1.0f ) {
2014-01-23 23:27:06 -04:00
_speed_cms = speed_cms ;
_flags . recalc_leash_xy = true ;
2015-05-19 06:15:37 -03:00
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
/// set_accel_xy() should be called before this method to set vehicle acceleration
/// 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
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
2014-01-17 22:53:46 -04: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 ) {
2014-01-17 22:53:46 -04:00
stopping_dist = vel_total / kP ;
2013-12-28 07:15:29 -04:00
} else {
2014-01-17 22:53:46 -04: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
{
return _distance_to_target ;
}
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 ) ;
}
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
{
2015-11-19 23:05:22 -04:00
return ( ( AP_HAL : : millis ( ) - _last_update_xy_ms ) < = POSCONTROL_ACTIVE_TIMEOUT_MS ) ;
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
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
2014-12-12 17:34:10 -04:00
void AC_PosControl : : init_xy_controller ( bool reset_I )
2014-05-06 05:32:30 -03:00
{
// set roll, pitch lean angle targets to current attitude
_roll_target = _ahrs . roll_sensor ;
_pitch_target = _ahrs . pitch_sensor ;
2014-05-06 23:46:39 -03:00
// initialise I terms from lean angles
2014-12-12 17:34:10 -04:00
if ( reset_I ) {
2014-05-06 23:46:39 -03:00
// reset last velocity if this controller has just been engaged or dt is zero
lean_angles_to_accel ( _accel_target . x , _accel_target . y ) ;
2017-11-16 08:29:02 -04: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
}
2014-04-19 04:40:47 -03:00
/// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher
2017-06-27 10:07:14 -03:00
void AC_PosControl : : update_xy_controller ( float ekfNavVelGainScaler )
2013-12-28 07:15:29 -04:00
{
2014-12-17 17:24:35 -04:00
// compute dt
2015-11-19 23:05:22 -04:00
uint32_t now = AP_HAL : : millis ( ) ;
2017-10-16 15:33:23 -03:00
float dt = ( now - _last_update_xy_ms ) * 0.001f ;
2014-12-17 17:24:35 -04:00
2018-03-01 03:57:29 -04:00
// sanity check dt
if ( dt > = POSCONTROL_ACTIVE_TIMEOUT_MS * 1.0e-3 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
run_xy_controller ( dt , ekfNavVelGainScaler ) ;
2018-03-01 03:57:29 -04:00
// update xy update time
_last_update_xy_ms = now ;
2014-12-17 17:24:35 -04:00
}
float AC_PosControl : : time_since_last_xy_update ( ) const
{
2015-11-19 23:05:22 -04:00
uint32_t now = AP_HAL : : millis ( ) ;
2014-12-17 17:24:35 -04:00
return ( now - _last_update_xy_ms ) * 0.001f ;
2013-12-28 07:15:29 -04:00
}
2018-03-01 01:42:52 -04:00
// write log to dataflash
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 ) ;
DataFlash_Class : : instance ( ) - > Log_Write ( " PSC " , " TimeUS,TPX,TPY,PX,PY,TVX,TVY,VX,VY,TAX,TAY,AX,AY " ,
" smmmmnnnnoooo " , " FBBBBBBBBBBBB " , " Qffffffffffff " ,
AP_HAL : : micros64 ( ) ,
( double ) pos_target . x ,
( double ) pos_target . y ,
( double ) position . x ,
( double ) position . y ,
( double ) vel_target . x ,
( double ) vel_target . y ,
( double ) velocity . x ,
( double ) velocity . y ,
( double ) accel_target . x ,
( double ) accel_target . y ,
( double ) accel_x ,
( double ) accel_y ) ;
}
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 ;
// reset last velocity if this controller has just been engaged or dt is zero
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
set_desired_accel_xy ( 0.0f , 0.0f ) ;
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_xy - run the velocity controller - should be called at 100hz or higher
/// velocity targets should we set using set_desired_velocity_xy() method
2014-06-02 06:02:44 -03:00
/// callers should use get_roll() and get_pitch() methods and sent to the attitude controller
/// throttle targets will be sent directly to the motors
2017-09-08 02:58:59 -03:00
void AC_PosControl : : update_vel_controller_xy ( float ekfNavVelGainScaler )
2014-06-02 06:02:44 -03:00
{
// capture time since last iteration
2015-11-19 23:05:22 -04:00
uint32_t now = AP_HAL : : millis ( ) ;
2015-11-17 02:48:40 -04:00
float dt = ( now - _last_update_xy_ms ) * 0.001f ;
2014-12-17 17:24:35 -04:00
2018-03-01 03:57:29 -04:00
// sanity check dt
if ( dt > = 0.2f ) {
dt = 0.0f ;
}
2014-06-02 06:02:44 -03:00
2018-03-01 03:57:29 -04:00
// check for ekf xy position reset
check_for_ekf_xy_reset ( ) ;
2016-11-22 01:36:17 -04:00
2018-03-01 03:57:29 -04:00
// check if xy leash needs to be recalculated
calc_leash_length_xy ( ) ;
2014-06-02 06:02:44 -03:00
2018-03-01 03:57:29 -04:00
// apply desired velocity request to position target
// TODO: this will need to be removed and added to the calling function.
desired_vel_to_pos ( dt ) ;
2014-06-02 06:02:44 -03:00
2018-03-01 03:57:29 -04:00
// run position controller
run_xy_controller ( dt , ekfNavVelGainScaler ) ;
2015-11-17 02:48:40 -04:00
2018-03-01 03:57:29 -04:00
// update xy update time
_last_update_xy_ms = now ;
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
void AC_PosControl : : update_vel_controller_xyz ( float ekfNavVelGainScaler )
{
update_vel_controller_xy ( ekfNavVelGainScaler ) ;
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
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
void AC_PosControl : : run_xy_controller ( float dt , float ekfNavVelGainScaler )
2013-12-28 07:15:29 -04:00
{
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 ;
2013-12-28 07:15:29 -04:00
} else {
// 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
if ( limit_vector_length ( _pos_error . x , _pos_error . y , _leash ) )
{
_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
}
2017-11-16 08:29:02 -04:00
_distance_to_target = norm ( _pos_error . x , _pos_error . y ) ;
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
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 ;
}
// filter correction acceleration
_accel_target_filter . set_cutoff_frequency ( MIN ( _accel_xy_filt_hz , 5.0f * ekfNavVelGainScaler ) ) ;
_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
2017-12-16 09:06:51 -04: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
2018-02-07 22:23:27 -04: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-01-19 03:27:10 -04: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
if ( speed_cms < = accel_cms / kP ) {
// linear leash length based on speed close in
leash_length = speed_cms / kP ;
} else {
// leash length grows at sqrt of speed further out
leash_length = ( accel_cms / ( 2.0f * kP * kP ) ) + ( speed_cms * speed_cms / ( 2.0f * accel_cms ) ) ;
}
// ensure leash is at least 1m long
if ( leash_length < POSCONTROL_LEASH_LENGTH_MIN ) {
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 ) ) {
return Vector3f ( error . x * p , error . y * p , error . z ) ;
}
float linear_dist = second_ord_lim / sq ( p ) ;
float error_length = norm ( error . x , error . y ) ;
if ( error_length > linear_dist ) {
2018-02-07 22:23:27 -04:00
float first_order_scale = safe_sqrt ( 2.0f * second_ord_lim * ( error_length - ( linear_dist * 0.5f ) ) ) / error_length ;
2017-11-16 08:29:02 -04:00
return Vector3f ( error . x * first_order_scale , error . y * first_order_scale , error . z ) ;
} else {
return Vector3f ( error . x * p , error . y * p , error . z ) ;
}
}