2013-12-28 07:15:29 -04:00
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
# include <AP_HAL.h>
# include <AC_PosControl.h>
2015-05-02 06:34:54 -03:00
# include <AP_Math.h>
2013-12-28 07:15:29 -04:00
extern const AP_HAL : : HAL & hal ;
const AP_Param : : GroupInfo AC_PosControl : : var_info [ ] PROGMEM = {
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
AP_GROUPEND
} ;
// Default constructor.
// Note that the Vector/Matrix constructors already implicitly zero
// their values.
//
2013-12-28 10:04:45 -04:00
AC_PosControl : : AC_PosControl ( const AP_AHRS & ahrs , const AP_InertialNav & inav ,
const AP_Motors & motors , AC_AttitudeControl & attitude_control ,
2015-01-31 02:50:32 -04:00
AC_P & p_pos_z , AC_P & p_vel_z , AC_PID & pid_accel_z ,
2015-01-29 02:50:14 -04:00
AC_P & p_pos_xy , AC_PI_2D & pi_vel_xy ) :
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 ) ,
2015-01-31 02:50:32 -04:00
_p_pos_z ( p_pos_z ) ,
_p_vel_z ( p_vel_z ) ,
_pid_accel_z ( pid_accel_z ) ,
2014-02-14 03:05:29 -04:00
_p_pos_xy ( p_pos_xy ) ,
2015-01-29 02:50:14 -04:00
_pi_vel_xy ( pi_vel_xy ) ,
2013-12-28 10:04:45 -04:00
_dt ( POSCONTROL_DT_10HZ ) ,
2014-12-17 17:24:35 -04:00
_dt_xy ( POSCONTROL_DT_50HZ ) ,
2014-05-02 00:08:18 -03:00
_last_update_xy_ms ( 0 ) ,
_last_update_z_ms ( 0 ) ,
2015-03-10 09:55:15 -03:00
_throttle_hover ( POSCONTROL_THROTTLE_HOVER ) ,
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 ) ,
2014-07-08 06:02:11 -03:00
_roll_target ( 0.0f ) ,
_pitch_target ( 0.0f ) ,
2014-07-14 11:30:23 -03:00
_alt_max ( 0.0f ) ,
2015-03-10 04:11:26 -03:00
_distance_to_target ( 0.0f ) ,
_accel_target_jerk_limited ( 0.0f , 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_xy = true ;
_flags . recalc_leash_z = true ;
2014-05-06 05:32:30 -03:00
_flags . reset_desired_vel_to_pos = true ;
_flags . reset_rate_to_accel_xy = 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 ;
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 ;
2015-01-23 06:06:17 -04:00
// update rate controller's dt
2015-01-31 02:50:32 -04:00
_pid_accel_z . 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
}
2015-01-23 06:06:17 -04:00
/// set_dt_xy - sets time delta in seconds for horizontal controller (i.e. 50hz = 0.02)
void AC_PosControl : : set_dt_xy ( float dt_xy )
{
_dt_xy = dt_xy ;
2015-01-29 02:50:14 -04:00
_pi_vel_xy . set_dt ( dt_xy ) ;
2015-01-23 06:06:17 -04: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-01-12 19:24:46 -04:00
2015-04-13 06:22:52 -03:00
_vel_desired . z = 0.0f ;
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 ) ) {
_pos_target . z + = constrain_float ( alt_change , _speed_down_cms * dt , _speed_up_cms * dt ) ;
}
// 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 ) ;
}
/// 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 )
2014-01-23 23:27:06 -04:00
{
2015-04-16 07:07:33 -03:00
// jerk_z is calculated to reach full acceleration in 1000ms.
float jerk_z = _accel_z_cms * POSCONTROL_JERK_RATIO ;
2015-04-13 06:22:52 -03:00
2015-05-02 06:34:54 -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-04-13 06:22:52 -03:00
_accel_last_z_cms = min ( accel_z_max , _accel_last_z_cms ) ;
float vel_change_limit = _accel_last_z_cms * dt ;
_vel_desired . z = constrain_float ( climb_rate_cms , _vel_desired . z - vel_change_limit , _vel_desired . z + vel_change_limit ) ;
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-01-30 00:57:37 -04:00
// do not let target alt get above limit
if ( _alt_max > 0 & & _pos_target . z > _alt_max ) {
_pos_target . z = _alt_max ;
_limit . pos_up = true ;
2015-04-13 06:22:52 -03:00
// decelerate feed forward to zero
_vel_desired . z = constrain_float ( 0.0f , _vel_desired . z - vel_change_limit , _vel_desired . z + vel_change_limit ) ;
2015-01-30 00:57:37 -04:00
}
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 ;
}
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 ;
_vel_target . z = _inav . get_velocity_z ( ) ;
_vel_last . z = _inav . get_velocity_z ( ) ;
_accel_feedforward . z = 0.0f ;
_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 ;
_pid_accel_z . set_integrator ( throttle_setting ) ;
}
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-04-18 00:38:21 -03:00
curr_vel_z - = _vel_desired . z ;
2014-05-02 03:26:04 -03:00
}
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
}
}
2014-01-25 04:23:55 -04:00
stopping_point . z = constrain_float ( stopping_point . z , curr_pos_z - POSCONTROL_STOPPING_DIST_Z_MAX , curr_pos_z + POSCONTROL_STOPPING_DIST_Z_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 ( ) ;
_pos_target . z = curr_pos . z + POSCONTROL_TAKEOFF_JUMP_CM ;
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
2015-05-14 22:02:40 -03:00
_pid_accel_z . set_integrator ( _motors . get_throttle ( ) - _throttle_hover ) ;
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
{
return ( ( hal . scheduler - > millis ( ) - _last_update_z_ms ) < = POSCONTROL_ACTIVE_TIMEOUT_MS ) ;
}
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
uint32_t now = hal . scheduler - > millis ( ) ;
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 ;
2014-01-23 23:27:06 -04:00
// check if leash lengths need to be recalculated
calc_leash_length_z ( ) ;
2013-12-28 10:04:45 -04:00
// call position controller
2013-12-30 09:12:59 -04:00
pos_to_rate_z ( ) ;
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
/// called by pos_to_rate_z if z-axis speed or accelerations are changed
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 ;
}
}
// pos_to_rate_z - position to rate controller for Z axis
// 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
void AC_PosControl : : pos_to_rate_z ( )
{
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
2015-01-31 02:50:32 -04:00
_vel_target . z = AC_AttitudeControl : : sqrt_controller ( _pos_error . z , _p_pos_z . kP ( ) , _accel_z_cms ) ;
2013-12-28 07:15:29 -04:00
2015-04-13 06:22:52 -03:00
// add feed forward component
_vel_target . z + = _vel_desired . z ;
2013-12-28 07:15:29 -04:00
// call rate based throttle controller which will update accel based throttle controller targets
2014-05-31 10:35:14 -03:00
rate_to_accel_z ( ) ;
2013-12-28 10:04:45 -04:00
}
2013-12-28 07:15:29 -04:00
2013-12-28 10:04:45 -04:00
// rate_to_accel_z - calculates desired accel required to achieve the velocity target
// calculates desired acceleration and calls accel throttle controller
2014-05-31 10:35:14 -03:00
void AC_PosControl : : rate_to_accel_z ( )
2013-12-28 10:04:45 -04:00
{
const Vector3f & curr_vel = _inav . get_velocity ( ) ;
float p ; // used to capture pid values for logging
// check speed limits
// To-Do: check these speed limits here or in the pos->rate controller
_limit . vel_up = false ;
_limit . vel_down = false ;
2014-01-17 22:53:46 -04:00
if ( _vel_target . z < _speed_down_cms ) {
_vel_target . z = _speed_down_cms ;
2013-12-28 10:04:45 -04:00
_limit . vel_down = true ;
}
2014-01-17 22:53:46 -04:00
if ( _vel_target . z > _speed_up_cms ) {
_vel_target . z = _speed_up_cms ;
2013-12-28 10:04:45 -04:00
_limit . vel_up = true ;
}
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 ) {
2014-05-31 10:35:14 -03:00
_accel_feedforward . z = ( _vel_target . z - _vel_last . z ) / _dt ;
} 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 {
_accel_feedforward . z = 0.0f ;
}
// 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
}
// calculate p
2015-01-31 02:50:32 -04:00
p = _p_vel_z . kP ( ) * _vel_error . z ;
2013-12-28 10:04:45 -04:00
// consolidate and constrain target acceleration
2015-04-05 04:23:11 -03:00
_accel_target . z = _accel_feedforward . z + p ;
2013-12-28 10:04:45 -04:00
// set target for accel based throttle controller
2015-04-05 04:23:11 -03:00
accel_to_throttle ( _accel_target . z ) ;
2013-12-28 07:15:29 -04:00
}
2013-12-28 10:04:45 -04:00
// accel_to_throttle - alt hold's acceleration controller
// calculates a desired throttle which is sent directly to the motors
void AC_PosControl : : accel_to_throttle ( float accel_target_z )
{
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
_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
2015-01-31 02:50:32 -04:00
_pid_accel_z . set_input_filter_d ( _accel_error . 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
// 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
2015-03-26 18:42:42 -03:00
float thr_out = p + i + d + _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
///
/// position controller
///
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
/// calc_leash_length_xy should be called afterwards
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
/// calc_leash_length_xy should be called afterwards
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-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 ;
}
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
{
2014-05-06 08:40:45 -03:00
const Vector3f curr_pos = _inav . get_position ( ) ;
2014-01-17 22:53:46 -04:00
Vector3f curr_vel = _inav . get_velocity ( ) ;
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
2014-04-01 08:40:08 -03:00
float vel_total = pythagorous2 ( 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
}
/// get_distance_to_target - get horizontal distance to loiter target in cm
float AC_PosControl : : get_distance_to_target ( ) const
{
return _distance_to_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
{
return ( ( hal . scheduler - > millis ( ) - _last_update_xy_ms ) < = POSCONTROL_ACTIVE_TIMEOUT_MS ) ;
}
/// 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 ) ;
2015-01-29 02:50:14 -04:00
_pi_vel_xy . set_integrator ( _accel_target ) ;
2014-05-06 05:32:30 -03:00
}
// flag reset required in rate to accel step
_flags . reset_desired_vel_to_pos = true ;
_flags . reset_rate_to_accel_xy = true ;
2015-03-13 08:46:21 -03:00
_flags . reset_accel_to_lean_xy = true ;
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
2015-01-12 18:46:50 -04:00
void AC_PosControl : : update_xy_controller ( xy_mode mode , float ekfNavVelGainScaler )
2013-12-28 07:15:29 -04:00
{
2014-12-17 17:24:35 -04:00
// compute dt
2014-01-17 22:53:46 -04:00
uint32_t now = hal . scheduler - > millis ( ) ;
2014-12-17 17:24:35 -04:00
float dt = ( now - _last_update_xy_ms ) / 1000.0f ;
_last_update_xy_ms = now ;
// sanity check dt - expect to be called faster than ~5hz
2015-03-10 18:57:03 -03:00
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
}
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 ) ;
// run position controller's position error to desired velocity step
2015-01-12 18:46:50 -04:00
pos_to_rate_xy ( mode , dt , ekfNavVelGainScaler ) ;
2014-12-17 17:24:35 -04:00
// run position controller's velocity to acceleration step
rate_to_accel_xy ( dt , ekfNavVelGainScaler ) ;
// run position controller's acceleration to lean angle step
accel_to_lean_angles ( dt , ekfNavVelGainScaler ) ;
}
float AC_PosControl : : time_since_last_xy_update ( ) const
{
uint32_t now = hal . scheduler - > millis ( ) ;
return ( now - _last_update_xy_ms ) * 0.001f ;
2013-12-28 07:15:29 -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 ;
// reset last velocity if this controller has just been engaged or dt is zero
lean_angles_to_accel ( _accel_target . x , _accel_target . y ) ;
2015-01-29 02:50:14 -04:00
_pi_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 ;
_flags . reset_rate_to_accel_xy = true ;
2015-03-13 08:46:21 -03:00
_flags . reset_accel_to_lean_xy = true ;
2014-06-02 06:02:44 -03:00
// set target position in xy axis
const Vector3f & curr_pos = _inav . get_position ( ) ;
set_xy_target ( curr_pos . x , curr_pos . y ) ;
// move current vehicle velocity into feed forward velocity
const Vector3f & curr_vel = _inav . get_velocity ( ) ;
set_desired_velocity_xy ( curr_vel . x , curr_vel . y ) ;
}
/// 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
2014-11-15 22:03:21 -04:00
void AC_PosControl : : update_vel_controller_xyz ( float ekfNavVelGainScaler )
2014-06-02 06:02:44 -03:00
{
// capture time since last iteration
uint32_t now = hal . scheduler - > millis ( ) ;
2014-12-17 17:24:35 -04:00
float dt = ( now - _last_update_xy_ms ) / 1000.0f ;
// sanity check dt - expect to be called faster than ~5hz
2015-03-10 18:57:03 -03:00
if ( dt > = POSCONTROL_ACTIVE_TIMEOUT_MS * 1.0e-3 f ) {
2014-12-17 17:24:35 -04:00
dt = 0.0f ;
}
2014-06-02 06:02:44 -03:00
// check if xy leash needs to be recalculated
calc_leash_length_xy ( ) ;
2014-12-17 17:24:35 -04:00
// apply desired velocity request to position target
desired_vel_to_pos ( dt ) ;
2014-06-02 06:02:44 -03:00
2014-12-17 17:24:35 -04:00
// run position controller's position error to desired velocity step
2015-02-06 03:59:00 -04:00
pos_to_rate_xy ( XY_MODE_POS_LIMITED_AND_VEL_FF , dt , ekfNavVelGainScaler ) ;
2014-06-02 06:02:44 -03:00
2014-12-17 17:24:35 -04:00
// run velocity to acceleration step
rate_to_accel_xy ( dt , ekfNavVelGainScaler ) ;
2014-06-02 06:02:44 -03:00
2014-12-17 17:24:35 -04:00
// run acceleration to lean angle step
accel_to_lean_angles ( dt , ekfNavVelGainScaler ) ;
2014-06-02 06:02:44 -03:00
// update altitude target
2014-12-17 17:24:35 -04:00
set_alt_target_from_climb_rate ( _vel_desired . z , dt ) ;
2014-06-02 06:02:44 -03:00
// run z-axis position controller
update_z_controller ( ) ;
2014-12-17 17:24:35 -04:00
// record update time
_last_update_xy_ms = now ;
2014-06-02 06:02:44 -03:00
}
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 ( )
{
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 ;
}
}
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
}
2014-01-17 22:53:46 -04:00
/// pos_to_rate_xy - horizontal position error to velocity controller
/// converts position (_pos_target) to target velocity (_vel_target)
/// when use_desired_rate is set to true:
/// desired velocity (_vel_desired) is combined into final target velocity and
/// velocity due to position error is reduce to a maximum of 1m/s
2015-01-12 18:46:50 -04:00
void AC_PosControl : : pos_to_rate_xy ( xy_mode mode , 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 ( ) ;
float linear_distance ; // the distance we swap between linear and sqrt velocity response
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 ;
// constrain target position to within reasonable distance of current location
2014-04-01 08:40:08 -03:00
_distance_to_target = pythagorous2 ( _pos_error . x , _pos_error . y ) ;
2014-01-17 22:53:46 -04:00
if ( _distance_to_target > _leash & & _distance_to_target > 0.0f ) {
_pos_target . x = curr_pos . x + _leash * _pos_error . x / _distance_to_target ;
_pos_target . y = curr_pos . y + _leash * _pos_error . y / _distance_to_target ;
// re-calculate distance error
_pos_error . x = _pos_target . x - curr_pos . x ;
_pos_error . y = _pos_target . y - curr_pos . y ;
_distance_to_target = _leash ;
}
2013-12-28 07:15:29 -04:00
2014-01-17 22:53:46 -04:00
// calculate the distance at which we swap between linear and sqrt velocity response
linear_distance = _accel_cms / ( 2.0f * kP * kP ) ;
2013-12-28 07:15:29 -04:00
2014-01-17 22:53:46 -04:00
if ( _distance_to_target > 2.0f * linear_distance ) {
// velocity response grows with the square root of the distance
float vel_sqrt = safe_sqrt ( 2.0f * _accel_cms * ( _distance_to_target - linear_distance ) ) ;
_vel_target . x = vel_sqrt * _pos_error . x / _distance_to_target ;
_vel_target . y = vel_sqrt * _pos_error . y / _distance_to_target ;
} else {
// velocity response grows linearly with the distance
2014-02-14 03:05:29 -04:00
_vel_target . x = _p_pos_xy . kP ( ) * _pos_error . x ;
_vel_target . y = _p_pos_xy . kP ( ) * _pos_error . y ;
2014-01-17 22:53:46 -04:00
}
2015-02-06 03:59:00 -04:00
if ( mode = = XY_MODE_POS_LIMITED_AND_VEL_FF ) {
2015-01-12 18:46:50 -04:00
// this mode is for loiter - rate-limiting the position correction
// allows the pilot to always override the position correction in
// the event of a disturbance
2015-02-06 03:59:00 -04:00
2015-01-12 18:46:50 -04:00
// scale velocity within limit
float vel_total = pythagorous2 ( _vel_target . x , _vel_target . y ) ;
if ( vel_total > POSCONTROL_VEL_XY_MAX_FROM_POS_ERR ) {
_vel_target . x = POSCONTROL_VEL_XY_MAX_FROM_POS_ERR * _vel_target . x / vel_total ;
_vel_target . y = POSCONTROL_VEL_XY_MAX_FROM_POS_ERR * _vel_target . y / vel_total ;
}
2015-02-06 03:59:00 -04:00
// add velocity feed-forward
2014-01-17 22:53:46 -04:00
_vel_target . x + = _vel_desired . x ;
_vel_target . y + = _vel_desired . y ;
2015-01-12 18:46:50 -04:00
} else {
2015-02-06 03:59:00 -04:00
if ( mode = = XY_MODE_POS_AND_VEL_FF ) {
// add velocity feed-forward
2015-01-12 18:46:50 -04:00
_vel_target . x + = _vel_desired . x ;
_vel_target . y + = _vel_desired . y ;
}
2015-02-06 03:59:00 -04:00
// scale velocity within speed limit
2015-01-12 18:46:50 -04:00
float vel_total = pythagorous2 ( _vel_target . x , _vel_target . y ) ;
if ( vel_total > _speed_cms ) {
_vel_target . x = _speed_cms * _vel_target . x / vel_total ;
_vel_target . y = _speed_cms * _vel_target . y / vel_total ;
}
2014-01-17 22:53:46 -04:00
}
2013-12-28 07:15:29 -04:00
}
}
2014-01-17 22:53:46 -04:00
/// rate_to_accel_xy - horizontal desired rate to desired acceleration
2013-12-28 07:15:29 -04:00
/// converts desired velocities in lat/lon directions to accelerations in lat/lon frame
2014-11-15 22:03:21 -04:00
void AC_PosControl : : rate_to_accel_xy ( float dt , float ekfNavVelGainScaler )
2013-12-28 07:15:29 -04:00
{
2014-01-17 22:53:46 -04:00
const Vector3f & vel_curr = _inav . get_velocity ( ) ; // current velocity in cm/s
2013-12-28 07:15:29 -04:00
float accel_total ; // total acceleration in cm/s/s
2015-01-29 02:50:14 -04:00
Vector2f vel_xy_p , vel_xy_i ;
2014-01-17 22:53:46 -04:00
2014-05-06 05:32:30 -03:00
// reset last velocity target to current target
if ( _flags . reset_rate_to_accel_xy ) {
_vel_last . x = _vel_target . x ;
_vel_last . y = _vel_target . y ;
_flags . reset_rate_to_accel_xy = false ;
}
// feed forward desired acceleration calculation
if ( dt > 0.0f ) {
2014-06-06 01:30:45 -03:00
if ( ! _flags . freeze_ff_xy ) {
2014-05-31 10:35:14 -03:00
_accel_feedforward . x = ( _vel_target . x - _vel_last . x ) / dt ;
_accel_feedforward . y = ( _vel_target . y - _vel_last . y ) / dt ;
} 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_xy = false ;
2014-05-31 10:35:14 -03:00
}
2014-05-06 05:32:30 -03:00
} else {
2014-05-31 10:35:14 -03:00
_accel_feedforward . x = 0.0f ;
_accel_feedforward . y = 0.0f ;
2013-12-28 07:15:29 -04:00
}
// store this iteration's velocities for the next iteration
2014-01-17 22:53:46 -04:00
_vel_last . x = _vel_target . x ;
_vel_last . y = _vel_target . y ;
2013-12-28 07:15:29 -04:00
// calculate velocity error
2014-01-17 22:53:46 -04:00
_vel_error . x = _vel_target . x - vel_curr . x ;
_vel_error . y = _vel_target . y - vel_curr . y ;
2013-12-28 07:15:29 -04:00
2015-01-29 02:50:14 -04:00
// call pi controller
_pi_vel_xy . set_input ( _vel_error ) ;
2015-01-23 06:06:17 -04:00
2015-01-29 02:50:14 -04:00
// get p
vel_xy_p = _pi_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
2015-01-29 02:50:14 -04:00
if ( ( ! _limit . accel_xy & & ! _motors . limit . throttle_upper ) ) {
vel_xy_i = _pi_vel_xy . get_i ( ) ;
} else {
vel_xy_i = _pi_vel_xy . get_i_shrink ( ) ;
2014-04-22 10:33:48 -03:00
}
2014-11-15 22:03:21 -04:00
// combine feed forward accel with PID output from velocity error and scale PID output to compensate for optical flow measurement induced EKF noise
2015-01-29 02:50:14 -04:00
_accel_target . x = _accel_feedforward . x + ( vel_xy_p . x + vel_xy_i . x ) * ekfNavVelGainScaler ;
_accel_target . y = _accel_feedforward . y + ( vel_xy_p . y + vel_xy_i . y ) * ekfNavVelGainScaler ;
2013-12-28 07:15:29 -04:00
// scale desired acceleration if it's beyond acceptable limit
2014-01-17 22:53:46 -04:00
// To-Do: move this check down to the accel_to_lean_angle method?
2014-04-01 08:40:08 -03:00
accel_total = pythagorous2 ( _accel_target . x , _accel_target . y ) ;
2014-05-06 05:32:30 -03:00
if ( accel_total > POSCONTROL_ACCEL_XY_MAX & & accel_total > 0.0f ) {
2014-01-17 22:53:46 -04:00
_accel_target . x = POSCONTROL_ACCEL_XY_MAX * _accel_target . x / accel_total ;
_accel_target . y = POSCONTROL_ACCEL_XY_MAX * _accel_target . y / accel_total ;
_limit . accel_xy = true ; // unused
2014-04-22 10:33:48 -03:00
} else {
// reset accel limit flag
_limit . accel_xy = false ;
2013-12-28 07:15:29 -04:00
}
}
2014-01-17 22:53:46 -04:00
/// accel_to_lean_angles - horizontal desired acceleration to lean angles
2013-12-28 07:15:29 -04:00
/// converts desired accelerations provided in lat/lon frame to roll/pitch angles
2014-12-17 17:24:35 -04:00
void AC_PosControl : : accel_to_lean_angles ( float dt , float ekfNavVelGainScaler )
2013-12-28 07:15:29 -04:00
{
2014-01-17 22:53:46 -04:00
float accel_right , accel_forward ;
float lean_angle_max = _attitude_control . lean_angle_max ( ) ;
2013-12-28 07:15:29 -04:00
2015-03-13 08:46:21 -03:00
// reset accel to current desired acceleration
if ( _flags . reset_accel_to_lean_xy ) {
_accel_target_jerk_limited . x = _accel_target . x ;
_accel_target_jerk_limited . y = _accel_target . y ;
2015-04-16 16:57:28 -03:00
_accel_target_filter . reset ( Vector2f ( _accel_target . x , _accel_target . y ) ) ;
2015-03-13 08:46:21 -03:00
_flags . reset_accel_to_lean_xy = false ;
}
2015-03-10 10:04:31 -03:00
// apply jerk limit of 17 m/s^3 - equates to a worst case of about 100 deg/sec/sec
float max_delta_accel = dt * POSCONTROL_JERK_LIMIT_CMSSS ;
2014-12-08 20:56:53 -04:00
2015-03-10 04:11:26 -03:00
Vector2f accel_in ( _accel_target . x , _accel_target . y ) ;
Vector2f accel_change = accel_in - _accel_target_jerk_limited ;
float accel_change_length = accel_change . length ( ) ;
if ( accel_change_length > max_delta_accel ) {
accel_change * = max_delta_accel / accel_change_length ;
2014-11-07 23:47:33 -04:00
}
2015-03-13 08:46:21 -03:00
_accel_target_jerk_limited + = accel_change ;
2014-11-07 23:47:33 -04:00
2015-04-16 07:07:33 -03:00
// lowpass filter on NE accel
2015-04-16 16:57:28 -03:00
_accel_target_filter . set_cutoff_frequency ( min ( _accel_xy_filt_hz , 5.0f * ekfNavVelGainScaler ) ) ;
Vector2f accel_target_filtered = _accel_target_filter . apply ( _accel_target_jerk_limited , dt ) ;
2014-11-07 23:47:33 -04:00
2014-12-08 20:56:53 -04:00
// rotate accelerations into body forward-right frame
2015-04-16 16:57:28 -03:00
accel_forward = accel_target_filtered . x * _ahrs . cos_yaw ( ) + accel_target_filtered . y * _ahrs . sin_yaw ( ) ;
accel_right = - accel_target_filtered . x * _ahrs . sin_yaw ( ) + accel_target_filtered . y * _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
2015-05-05 00:57:16 -03:00
_pitch_target = constrain_float ( atanf ( - accel_forward / ( GRAVITY_MSS * 100 ) ) * ( 18000 / M_PI_F ) , - lean_angle_max , lean_angle_max ) ;
2015-05-05 00:44:19 -03:00
float cos_pitch_target = cosf ( _pitch_target * M_PI_F / 18000 ) ;
2015-05-05 00:57:16 -03:00
_roll_target = constrain_float ( atanf ( accel_right * cos_pitch_target / ( GRAVITY_MSS * 100 ) ) * ( 18000 / M_PI_F ) , - lean_angle_max , lean_angle_max ) ;
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
2014-07-08 06:02:11 -03:00
accel_x_cmss = ( GRAVITY_MSS * 100 ) * ( - ( _ahrs . cos_yaw ( ) * _ahrs . sin_pitch ( ) / max ( _ahrs . cos_pitch ( ) , 0.5f ) ) - _ahrs . sin_yaw ( ) * _ahrs . sin_roll ( ) / max ( _ahrs . cos_roll ( ) , 0.5f ) ) ;
accel_y_cmss = ( GRAVITY_MSS * 100 ) * ( - ( _ahrs . sin_yaw ( ) * _ahrs . sin_pitch ( ) / max ( _ahrs . cos_pitch ( ) , 0.5f ) ) + _ahrs . cos_yaw ( ) * _ahrs . sin_roll ( ) / max ( _ahrs . cos_roll ( ) , 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
}