2016-02-17 21:24:42 -04:00
# pragma once
2013-12-28 07:15:29 -04:00
2015-08-11 03:28:41 -03:00
# include <AP_Common/AP_Common.h>
# include <AP_Param/AP_Param.h>
# include <AP_Math/AP_Math.h>
# include <AC_PID/AC_PID.h> // PID library
# include <AC_PID/AC_PI_2D.h> // PID library (2-axis)
# include <AC_PID/AC_P.h> // P library
# include <AP_InertialNav/AP_InertialNav.h> // Inertial Navigation library
# include "AC_AttitudeControl.h" // Attitude control library
# include <AP_Motors/AP_Motors.h> // motors library
# include <AP_Vehicle/AP_Vehicle.h> // common vehicle parameters
2013-12-28 07:15:29 -04:00
2013-12-28 10:04:45 -04:00
// position controller default definitions
2014-04-21 09:34:33 -03:00
# define POSCONTROL_ACCELERATION_MIN 50.0f // minimum horizontal acceleration in cm/s/s - used for sanity checking acceleration in leash length calculation
# define POSCONTROL_ACCEL_XY 100.0f // default horizontal acceleration in cm/s/s. This is overwritten by waypoint and loiter controllers
2013-12-28 10:04:45 -04:00
# define POSCONTROL_ACCEL_XY_MAX 980.0f // max horizontal acceleration in cm/s/s that the position velocity controller will ask from the lower accel controller
2013-12-28 07:15:29 -04:00
// should be 1.5 times larger than POSCONTROL_ACCELERATION.
// max acceleration = max lean angle * 980 * pi / 180. i.e. 23deg * 980 * 3.141 / 180 = 393 cm/s/s
2017-04-27 05:30:40 -03:00
# define POSCONTROL_STOPPING_DIST_UP_MAX 300.0f // max stopping distance (in cm) vertically while climbing
# define POSCONTROL_STOPPING_DIST_DOWN_MAX 200.0f // max stopping distance (in cm) vertically while descending
2015-10-28 23:46:34 -03:00
# define POSCONTROL_JERK_LIMIT_CMSSS 1700.0f // default jerk limit on horizontal acceleration (unit: m/s/s/s)
2013-12-28 07:15:29 -04:00
2014-04-21 09:34:33 -03:00
# define POSCONTROL_SPEED 500.0f // default horizontal speed in cm/s
# define POSCONTROL_SPEED_DOWN -150.0f // default descent rate in cm/s
# define POSCONTROL_SPEED_UP 250.0f // default climb rate in cm/s
2014-01-17 22:53:46 -04:00
# define POSCONTROL_VEL_XY_MAX_FROM_POS_ERR 200.0f // max speed output from pos_to_vel controller when feed forward is used
2013-12-28 07:15:29 -04:00
2014-04-21 09:34:33 -03:00
# define POSCONTROL_ACCEL_Z 250.0f // default vertical acceleration in cm/s/s.
2013-12-28 07:15:29 -04:00
2013-12-28 10:04:45 -04:00
# define POSCONTROL_LEASH_LENGTH_MIN 100.0f // minimum leash lengths in cm
2013-12-28 07:15:29 -04:00
2014-12-17 17:24:35 -04:00
# define POSCONTROL_DT_50HZ 0.02f // time difference in seconds for 50hz update rate
2015-11-17 02:48:05 -04:00
# define POSCONTROL_DT_400HZ 0.0025f // time difference in seconds for 400hz update rate
2013-12-28 07:15:29 -04:00
2015-04-16 16:50:53 -03:00
# define POSCONTROL_ACTIVE_TIMEOUT_MS 200 // position controller is considered active if it has been called within the past 0.2 seconds
2014-05-02 00:08:18 -03:00
2015-04-16 16:50:53 -03:00
# define POSCONTROL_VEL_ERROR_CUTOFF_FREQ 4.0f // low-pass filter on velocity error (unit: hz)
# define POSCONTROL_THROTTLE_CUTOFF_FREQ 2.0f // low-pass filter on accel error (unit: hz)
# define POSCONTROL_ACCEL_FILTER_HZ 2.0f // low-pass filter on acceleration (unit: hz)
2015-04-16 07:07:33 -03:00
# define POSCONTROL_JERK_RATIO 1.0f // Defines the time it takes to reach the requested acceleration
2014-09-21 05:52:14 -03:00
2015-10-27 10:45:16 -03:00
# define POSCONTROL_OVERSPEED_GAIN_Z 2.0f // gain controlling rate at which z-axis speed is brought back within SPEED_UP and SPEED_DOWN range
2013-12-28 07:15:29 -04:00
class AC_PosControl
{
public :
/// Constructor
2017-02-11 04:14:23 -04:00
AC_PosControl ( const AP_AHRS_View & ahrs , const AP_InertialNav & inav ,
2013-12-28 10:04:45 -04:00
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 ) ;
2015-02-06 03:59:00 -04:00
// xy_mode - specifies behavior of xy position controller
2015-01-12 18:46:50 -04:00
enum xy_mode {
2015-02-06 03:59:00 -04:00
XY_MODE_POS_ONLY = 0 , // position correction only (i.e. no velocity feed-forward)
XY_MODE_POS_LIMITED_AND_VEL_FF , // for loiter - rate-limiting the position correction, velocity feed-forward
2017-01-05 14:07:14 -04:00
XY_MODE_POS_AND_VEL_FF // for velocity controller - unlimited position correction, velocity feed-forward
2015-01-12 18:46:50 -04:00
} ;
2013-12-28 07:15:29 -04:00
///
/// initialisation functions
///
/// set_dt - sets time delta in seconds for all controllers (i.e. 100hz = 0.01, 400hz = 0.0025)
2014-05-27 10:44:57 -03:00
/// updates z axis accel controller's D term filter
void set_dt ( float delta_sec ) ;
2013-12-30 09:12:59 -04:00
float get_dt ( ) const { return _dt ; }
2013-12-28 07:15:29 -04:00
2014-12-31 01:02:46 -04:00
/// set_dt_xy - sets time delta in seconds for horizontal controller (i.e. 50hz = 0.02)
2015-01-23 06:06:17 -04:00
void set_dt_xy ( float dt_xy ) ;
2014-12-17 17:24:35 -04:00
float get_dt_xy ( ) const { return _dt_xy ; }
2014-01-17 22:53:46 -04:00
///
/// z position controller
///
/// set_speed_z - sets maximum climb and descent rates
2014-04-29 23:14:48 -03:00
/// speed_down can be positive or negative but will always be interpreted as a descent speed
2014-03-16 21:40:11 -03:00
/// leash length will be recalculated the next time update_z_controller() is called
2014-01-23 23:27:06 -04:00
void set_speed_z ( float speed_down , float speed_up ) ;
2013-12-28 10:04:45 -04:00
2014-01-24 02:46:45 -04:00
/// get_speed_up - accessor for current up speed in cm/s
2014-05-06 08:40:45 -03:00
float get_speed_up ( ) const { return _speed_up_cms ; }
2014-01-24 02:46:45 -04:00
/// get_speed_down - accessors for current down speed in cm/s. Will be a negative number
2014-05-06 08:40:45 -03:00
float get_speed_down ( ) const { return _speed_down_cms ; }
2014-01-24 02:46:45 -04:00
2015-04-05 04:23:11 -03:00
/// get_vel_target_z - returns current vertical speed in cm/s
float get_vel_target_z ( ) const { return _vel_target . z ; }
2014-01-17 22:53:46 -04:00
/// set_accel_z - set vertical acceleration in cm/s/s
2014-03-16 21:40:11 -03:00
/// leash length will be recalculated the next time update_z_controller() is called
2014-01-23 23:27:06 -04:00
void set_accel_z ( float accel_cmss ) ;
2014-01-17 22:53:46 -04:00
2014-04-22 10:34:07 -03:00
/// get_accel_z - returns current vertical acceleration in cm/s/s
float get_accel_z ( ) const { return _accel_z_cms ; }
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 calc_leash_length_z ( ) ;
/// set_alt_target - set altitude target in cm above home
2013-12-30 09:12:59 -04:00
void set_alt_target ( float alt_cm ) { _pos_target . z = alt_cm ; }
2014-01-05 23:30:51 -04:00
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 set_alt_target_with_slew ( float alt_cm , float dt ) ;
/// 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 22:38:55 -04:00
/// set force_descend to true during landing to allow target to move low enough to slow the motors
2017-02-07 20:49:55 -04:00
virtual void set_alt_target_from_climb_rate ( float climb_rate_cms , float dt , bool force_descend ) ;
2014-01-23 23:27:06 -04:00
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
2017-02-07 20:49:55 -04:00
virtual void set_alt_target_from_climb_rate_ff ( float climb_rate_cms , float dt , bool force_descend ) ;
2015-10-27 10:04:24 -03: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 add_takeoff_climb_rate ( float climb_rate_cms , float dt ) ;
2014-08-04 04:54:04 -03:00
/// set_alt_target_to_current_alt - set altitude target to current altitude
void set_alt_target_to_current_alt ( ) { _pos_target . z = _inav . get_altitude ( ) ; }
2016-11-22 01:36:17 -04:00
/// shift altitude target (positive means move altitude up)
void shift_alt_target ( float z_cm ) ;
2015-04-14 11:26:33 -03:00
/// relax_alt_hold_controllers - set all desired and targets to measured
void relax_alt_hold_controllers ( float throttle_setting ) ;
2014-01-05 23:30:51 -04:00
/// get_alt_target, get_desired_alt - get desired altitude (in cm above home) from loiter or wp controller which should be fed into throttle controller
/// To-Do: remove one of the two functions below
2013-12-30 09:12:59 -04:00
float get_alt_target ( ) const { return _pos_target . z ; }
2014-01-05 23:30:51 -04:00
2014-01-23 23:27:06 -04:00
/// get_alt_error - returns altitude error in cm
2014-01-05 23:30:51 -04:00
float get_alt_error ( ) const ;
2016-07-07 21:36:01 -03:00
// returns horizontal error in cm
float get_horizontal_error ( ) const ;
2013-12-30 09:12:59 -04:00
/// set_target_to_stopping_point_z - sets altitude target to reasonable stopping altitude in cm above home
void set_target_to_stopping_point_z ( ) ;
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 get_stopping_point_z ( Vector3f & stopping_point ) const ;
2014-01-25 04:23:55 -04:00
2013-12-30 09:12:59 -04:00
/// init_takeoff - initialises target altitude if we are taking off
void init_takeoff ( ) ;
2013-12-28 07:15:29 -04:00
2014-05-02 00:08:18 -03:00
// is_active - returns true if the z-axis position controller has been run very recently
bool is_active_z ( ) const ;
2014-01-17 22:53:46 -04:00
/// update_z_controller - fly to altitude in cm above home
void update_z_controller ( ) ;
2013-12-28 07:15:29 -04:00
2014-01-23 23:27:06 -04:00
// get_leash_down_z, get_leash_up_z - returns vertical leash lengths in cm
2014-01-27 10:42:49 -04:00
float get_leash_down_z ( ) const { return _leash_down_z ; }
float get_leash_up_z ( ) const { return _leash_up_z ; }
2013-12-28 07:15:29 -04:00
2017-01-05 02:23:04 -04:00
/// get_pos_z_kP - returns z position controller's kP gain
float get_pos_z_kP ( ) const { return _p_pos_z . kP ( ) ; }
2013-12-28 07:15:29 -04:00
///
/// xy position controller
///
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 init_xy_controller ( bool reset_I = true ) ;
2014-05-06 05:32:30 -03:00
2014-01-17 22:53:46 -04:00
/// set_accel_xy - set horizontal acceleration in cm/s/s
2014-04-19 04:40:47 -03:00
/// leash length will be recalculated the next time update_xy_controller() is called
2014-01-23 23:27:06 -04:00
void set_accel_xy ( float accel_cmss ) ;
2014-01-27 10:42:49 -04:00
float get_accel_xy ( ) const { return _accel_cms ; }
2014-01-17 22:53:46 -04:00
/// set_speed_xy - set horizontal speed maximum in cm/s
2014-04-19 04:40:47 -03:00
/// leash length will be recalculated the next time update_xy_controller() is called
2014-01-23 23:27:06 -04:00
void set_speed_xy ( float speed_cms ) ;
2014-01-27 10:42:49 -04:00
float get_speed_xy ( ) const { return _speed_cms ; }
2014-01-17 22:53:46 -04:00
2015-10-28 23:46:34 -03:00
/// set_jerk_xy - set max horizontal jerk in cm/s/s/s
void set_jerk_xy ( float jerk_cmsss ) { _jerk_cmsss = jerk_cmsss ; }
void set_jerk_xy_to_default ( ) { _jerk_cmsss = POSCONTROL_JERK_LIMIT_CMSSS ; }
2016-04-10 08:54:05 -03:00
/// set_limit_accel_xy - mark that accel has been limited
/// this prevents integrator buildup
void set_limit_accel_xy ( void ) { _limit . accel_xy = true ; }
2014-01-17 22:53:46 -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
2014-01-23 23:27:06 -04:00
void calc_leash_length_xy ( ) ;
2014-01-17 22:53:46 -04:00
2013-12-28 07:15:29 -04:00
/// get_pos_target - get target as position vector (from home in cm)
2014-01-17 22:53:46 -04:00
const Vector3f & get_pos_target ( ) const { return _pos_target ; }
2013-12-28 07:15:29 -04:00
/// set_pos_target in cm from home
void set_pos_target ( const Vector3f & position ) ;
2014-05-06 05:32:30 -03:00
/// set_xy_target in cm from home
void set_xy_target ( float x , float y ) ;
2015-10-29 04:30:14 -03:00
/// shift position target target in x, y axis
void shift_pos_xy_target ( float x_cm , float y_cm ) ;
2014-01-17 22:53:46 -04:00
/// get_desired_velocity - returns xy desired velocity (i.e. feed forward) in cm/s in lat and lon direction
2014-06-02 06:02:44 -03:00
const Vector3f & get_desired_velocity ( ) { return _vel_desired ; }
2013-12-28 07:15:29 -04:00
2015-10-27 10:04:24 -03:00
/// set_desired_velocity_z - sets desired velocity in cm/s in z axis
void set_desired_velocity_z ( float vel_z_cms ) { _vel_desired . z = vel_z_cms ; }
2017-04-27 03:58:13 -03:00
// clear desired velocity feed-forward in z axis
void clear_desired_velocity_ff_z ( ) { _flags . use_desvel_ff_z = false ; }
2014-06-02 06:02:44 -03:00
/// set_desired_velocity_xy - sets desired velocity in cm/s in lat and lon directions
2014-04-19 04:40:47 -03:00
/// when update_xy_controller is next called the position target is moved based on the desired velocity and
2014-01-17 22:53:46 -04:00
/// the desired velocities are fed forward into the rate_to_accel step
2014-06-02 06:02:44 -03:00
void set_desired_velocity_xy ( float vel_lat_cms , float vel_lon_cms ) { _vel_desired . x = vel_lat_cms ; _vel_desired . y = vel_lon_cms ; }
/// set_desired_velocity - sets desired velocity in cm/s in all 3 axis
/// when update_vel_controller_xyz is next called the position target is moved based on the desired velocity
void set_desired_velocity ( const Vector3f & des_vel ) { _vel_desired = des_vel ; freeze_ff_xy ( ) ; }
2013-12-28 07:15:29 -04:00
2016-07-20 15:38:37 -03:00
// overrides the velocity process variable for one timestep
void override_vehicle_velocity_xy ( const Vector2f & vel_xy ) { _vehicle_horiz_vel = vel_xy ; _flags . vehicle_horiz_vel_override = true ; }
2014-05-31 22:48:43 -03:00
/// freeze_ff_z - used to stop the feed forward being calculated during a known discontinuity
2014-06-06 01:30:45 -03:00
void freeze_ff_z ( ) { _flags . freeze_ff_z = true ; }
2014-05-31 22:48:43 -03:00
/// freeze_ff_xy - used to stop the feed forward being calculated during a known discontinuity
2014-06-06 01:30:45 -03:00
void freeze_ff_xy ( ) { _flags . freeze_ff_xy = true ; }
2014-05-31 22:48:43 -03:00
2014-05-06 05:32:30 -03:00
// is_active_xy - returns true if the xy position controller has been run very recently
bool is_active_xy ( ) const ;
2014-04-19 04:40:47 -03:00
/// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher
2014-01-17 22:53:46 -04:00
/// when use_desired_velocity is true the desired velocity (i.e. feed forward) is incorporated at the pos_to_rate step
2015-06-28 04:16:42 -03:00
void update_xy_controller ( xy_mode mode , float ekfNavVelGainScaler , bool use_althold_lean_angle ) ;
2013-12-28 07:15:29 -04:00
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 set_target_to_stopping_point_xy ( ) ;
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
2016-03-22 10:57:41 -03:00
/// results placed in stopping_position vector
2014-01-17 22:53:46 -04:00
/// set_accel_xy() should be called before this method to set vehicle acceleration
/// set_leash_length() should have been called before this method
void get_stopping_point_xy ( Vector3f & stopping_point ) const ;
2013-12-28 07:15:29 -04:00
2014-01-17 22:53:46 -04:00
/// get_distance_to_target - get horizontal distance to position target in cm (used for reporting)
float get_distance_to_target ( ) const ;
2013-12-28 07:15:29 -04:00
2014-06-02 06:02:44 -03:00
/// xyz velocity controller
/// init_vel_controller_xyz - initialise the velocity controller - should be called once before the caller attempts to use the controller
void init_vel_controller_xyz ( ) ;
2017-09-08 02:58:59 -03:00
/// update_velocity_controller_xy - run the XY velocity controller - should be called at 100hz or higher
/// velocity targets should we set using set_desired_velocity_xy() 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 update_vel_controller_xy ( float ekfNavVelGainScaler ) ;
2014-06-02 06:02:44 -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
2014-11-15 22:03:21 -04:00
void update_vel_controller_xyz ( float ekfNavVelGainScaler ) ;
2014-06-02 06:02:44 -03:00
2013-12-28 07:15:29 -04:00
/// get desired roll, pitch which should be fed into stabilize controllers
2014-01-17 22:53:46 -04:00
float get_roll ( ) const { return _roll_target ; }
float get_pitch ( ) const { return _pitch_target ; }
2014-01-23 23:27:06 -04:00
// get_leash_xy - returns horizontal leash length in cm
2014-04-11 05:14:51 -03:00
float get_leash_xy ( ) const { return _leash ; }
2014-01-23 23:27:06 -04:00
2014-02-10 10:39:25 -04:00
/// get_pos_xy_kP - returns xy position controller's kP gain
2014-02-14 03:05:29 -04:00
float get_pos_xy_kP ( ) const { return _p_pos_xy . kP ( ) ; }
2014-02-10 10:39:25 -04:00
2014-01-17 22:53:46 -04:00
/// accessors for reporting
2014-04-11 05:14:51 -03:00
const Vector3f & get_vel_target ( ) const { return _vel_target ; }
const Vector3f & get_accel_target ( ) const { return _accel_target ; }
2013-12-28 07:15:29 -04:00
2014-05-06 05:32:30 -03:00
// lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s
2014-05-06 08:40:45 -03:00
void lean_angles_to_accel ( float & accel_x_cmss , float & accel_y_cmss ) const ;
2014-05-06 05:32:30 -03:00
2014-12-31 01:02:46 -04:00
// time_since_last_xy_update - returns time in seconds since the horizontal position controller was last run
2014-12-17 17:24:35 -04:00
float time_since_last_xy_update ( ) const ;
2013-12-28 07:15:29 -04:00
static const struct AP_Param : : GroupInfo var_info [ ] ;
2017-02-07 20:49:55 -04:00
protected :
2014-01-17 22:53:46 -04:00
// general purpose flags
struct poscontrol_flags {
2015-03-13 08:46:21 -03:00
uint16_t recalc_leash_z : 1 ; // 1 if we should recalculate the z axis leash length
uint16_t recalc_leash_xy : 1 ; // 1 if we should recalculate the xy axis leash length
uint16_t reset_desired_vel_to_pos : 1 ; // 1 if we should reset the rate_to_accel_xy step
uint16_t reset_rate_to_accel_xy : 1 ; // 1 if we should reset the rate_to_accel_xy step
uint16_t reset_accel_to_lean_xy : 1 ; // 1 if we should reset the accel to lean angle step
uint16_t reset_rate_to_accel_z : 1 ; // 1 if we should reset the rate_to_accel_z step
uint16_t reset_accel_to_throttle : 1 ; // 1 if we should reset the accel_to_throttle step of the z-axis controller
uint16_t freeze_ff_xy : 1 ; // 1 use to freeze feed forward during step updates
2015-10-28 03:42:44 -03:00
uint16_t freeze_ff_z : 1 ; // 1 used to freeze velocity to accel feed forward for one iteration
2015-10-28 03:42:17 -03:00
uint16_t use_desvel_ff_z : 1 ; // 1 to use z-axis desired velocity as feed forward into velocity step
2016-07-20 15:38:37 -03:00
uint16_t vehicle_horiz_vel_override : 1 ; // 1 if we should use _vehicle_horiz_vel as our velocity process variable for one timestep
2014-01-17 22:53:46 -04:00
} _flags ;
// limit flags structure
struct poscontrol_limit_flags {
2013-12-28 10:04:45 -04:00
uint8_t pos_up : 1 ; // 1 if we have hit the vertical position leash limit while going up
uint8_t pos_down : 1 ; // 1 if we have hit the vertical position leash limit while going down
uint8_t vel_up : 1 ; // 1 if we have hit the vertical velocity limit going up
uint8_t vel_down : 1 ; // 1 if we have hit the vertical velocity limit going down
2014-01-17 22:53:46 -04:00
uint8_t accel_xy : 1 ; // 1 if we have hit the horizontal accel limit
2013-12-28 10:04:45 -04:00
} _limit ;
2014-01-23 23:27:06 -04:00
///
/// z controller private methods
///
2013-12-28 10:04:45 -04:00
// pos_to_rate_z - position to rate controller for Z axis
2013-12-30 09:12:59 -04:00
// target altitude should be placed into _pos_target.z using or set with one of these functions
// set_alt_target
// set_target_to_stopping_point_z
// init_takeoff
void pos_to_rate_z ( ) ;
2013-12-28 10:04:45 -04:00
// rate_to_accel_z - calculates desired accel required to achieve the velocity target
2014-05-31 10:35:14 -03:00
void rate_to_accel_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
void accel_to_throttle ( float accel_target_z ) ;
2014-01-23 23:27:06 -04:00
///
/// xy controller private methods
///
2014-01-17 22:53:46 -04:00
/// desired_vel_to_pos - move position target using desired velocities
void desired_vel_to_pos ( float 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 pos_to_rate_xy ( xy_mode mode , float dt , float ekfNavVelGainScaler ) ;
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 rate_to_accel_xy ( float dt , float ekfNavVelGainScaler ) ;
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
2015-06-28 04:16:42 -03:00
void accel_to_lean_angles ( float dt_xy , float ekfNavVelGainScaler , bool use_althold_lean_angle ) ;
2013-12-28 07:15:29 -04: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 calc_leash_length ( float speed_cms , float accel_cms , float kP ) const ;
2013-12-28 07:15:29 -04:00
2016-11-22 01:36:17 -04:00
/// initialise and check for ekf position resets
void init_ekf_xy_reset ( ) ;
void check_for_ekf_xy_reset ( ) ;
void init_ekf_z_reset ( ) ;
void check_for_ekf_z_reset ( ) ;
2013-12-28 07:15:29 -04:00
// references to inertial nav and ahrs libraries
2017-02-11 04:14:23 -04:00
const AP_AHRS_View & _ahrs ;
2013-12-28 10:04:45 -04:00
const AP_InertialNav & _inav ;
const AP_Motors & _motors ;
AC_AttitudeControl & _attitude_control ;
2013-12-28 07:15:29 -04:00
2015-01-31 02:50:32 -04:00
// references to pid controllers
AC_P & _p_pos_z ;
AC_P & _p_vel_z ;
AC_PID & _pid_accel_z ;
2016-03-22 10:57:41 -03:00
AC_P & _p_pos_xy ;
2015-01-29 02:50:14 -04:00
AC_PI_2D & _pi_vel_xy ;
2013-12-28 07:15:29 -04:00
2015-04-16 07:07:33 -03:00
// parameters
AP_Float _accel_xy_filt_hz ; // XY acceleration filter cutoff frequency
2013-12-28 10:04:45 -04:00
// internal variables
float _dt ; // time difference (in seconds) between calls from the main program
2014-12-17 17:24:35 -04:00
float _dt_xy ; // time difference (in seconds) between update_xy_controller and update_vel_controller_xyz calls
2014-05-02 00:08:18 -03:00
uint32_t _last_update_xy_ms ; // system time of last update_xy_controller call
uint32_t _last_update_z_ms ; // system time of last update_z_controller call
2014-01-17 22:53:46 -04:00
float _speed_down_cms ; // max descent rate in cm/s
float _speed_up_cms ; // max climb rate in cm/s
2013-12-28 10:04:45 -04:00
float _speed_cms ; // max horizontal speed in cm/s
2014-01-17 22:53:46 -04:00
float _accel_z_cms ; // max vertical acceleration in cm/s/s
2015-04-13 06:22:52 -03:00
float _accel_last_z_cms ; // max vertical acceleration in cm/s/s
2013-12-28 10:04:45 -04:00
float _accel_cms ; // max horizontal acceleration in cm/s/s
2015-10-28 23:46:34 -03:00
float _jerk_cmsss ; // max horizontal jerk in cm/s/s/s
2014-01-17 22:53:46 -04:00
float _leash ; // horizontal leash length in cm. target will never be further than this distance from the vehicle
2014-01-23 23:27:06 -04:00
float _leash_down_z ; // vertical leash down in cm. target will never be further than this distance below the vehicle
float _leash_up_z ; // vertical leash up in cm. target will never be further than this distance above the vehicle
2013-12-28 07:15:29 -04:00
// output from controller
2014-01-17 22:53:46 -04:00
float _roll_target ; // desired roll angle in centi-degrees calculated by position controller
float _pitch_target ; // desired roll pitch in centi-degrees calculated by position controller
2013-12-28 07:15:29 -04:00
// position controller internal variables
2013-12-28 10:04:45 -04:00
Vector3f _pos_target ; // target location in cm from home
Vector3f _pos_error ; // error between desired and actual position in cm
2014-06-02 06:02:44 -03:00
Vector3f _vel_desired ; // desired velocity in cm/s
2014-01-17 22:53:46 -04:00
Vector3f _vel_target ; // velocity target in cm/s calculated by pos_to_rate step
Vector3f _vel_error ; // error between desired and actual acceleration in cm/s
2014-05-31 10:35:14 -03:00
Vector3f _vel_last ; // previous iterations velocity in cm/s
Vector3f _accel_feedforward ; // feedforward acceleration in cm/s/s
2017-06-27 10:14:23 -03:00
Vector3f _accel_target ; // acceleration target in cm/s/s
Vector3f _accel_error ; // acceleration error in cm/s/s
2016-07-20 15:38:37 -03:00
Vector2f _vehicle_horiz_vel ; // velocity to use if _flags.vehicle_horiz_vel_override is set
2014-01-17 22:53:46 -04:00
float _distance_to_target ; // distance to position target - for reporting only
2014-09-21 05:53:10 -03:00
LowPassFilterFloat _vel_error_filter ; // low-pass-filter on z-axis velocity error
2015-03-10 04:11:26 -03:00
2015-03-10 10:04:31 -03:00
Vector2f _accel_target_jerk_limited ; // acceleration target jerk limited to 100deg/s/s
2015-04-16 16:57:28 -03:00
LowPassFilterVector2f _accel_target_filter ; // acceleration target filter
2016-11-22 01:36:17 -04:00
// ekf reset handling
uint32_t _ekf_xy_reset_ms ; // system time of last recorded ekf xy position reset
uint32_t _ekf_z_reset_ms ; // system time of last recorded ekf altitude reset
2013-12-28 07:15:29 -04:00
} ;