@ -15,16 +15,17 @@
@@ -15,16 +15,17 @@
# include <AC_PID/AC_PID.h>
# include <AC_PID/AC_P.h>
// To-Do: change the name or move to AP_Math?
# define AC_ATTITUDE_CONTROL_DEGX100 5729.57795f // constant to convert from radians to centi-degrees
// TODO: change the name or move to AP_Math? eliminate in favor of degrees(100)?
# define AC_ATTITUDE_CONTROL_DEGX100 5729.57795f // constant to convert from radians to centidegrees
# define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS radians(360.0f) // minimum body-frame acceleration limit for the stability controller (for roll and pitch axis)
# define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS radians(720.0f) // maximum body-frame acceleration limit for the stability controller (for roll and pitch axis)
# define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS radians(90.0f) // minimum body-frame acceleration limit for the stability controller (for yaw axis)
# define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS radians(360.0f) // maximum body-frame acceleration limit for the stability controller (for yaw axis)
// BUG: SLEW_YAW's behavior does not match its parameter documentation
# define AC_ATTITUDE_CONTROL_SLEW_YAW_DEFAULT_CDS 1000 // constraint on yaw angle error in degrees. This should lead to maximum turn rate of 10deg/sed * Stab Rate P so by default will be 45deg/sec.
# define AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT_CDSS 110000.0f // default maximum acceleration for roll/pitch axis in centi- degrees/sec/sec
# define AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT_CDSS 27000.0f // default maximum acceleration for yaw axis in centi- degrees/sec/sec
# define AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT_CDSS 110000.0f // default maximum acceleration for roll/pitch axis in centidegrees/sec/sec
# define AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT_CDSS 27000.0f // default maximum acceleration for yaw axis in centidegrees/sec/sec
# define AC_ATTITUDE_RATE_CONTROLLER_TIMEOUT 1.0f // body-frame rate controller timeout in seconds
# define AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX 5000.0f // body-frame rate controller maximum output (for roll-pitch axis)
@ -46,13 +47,13 @@
@@ -46,13 +47,13 @@
class AC_AttitudeControl {
public :
AC_AttitudeControl ( AP_AHRS & ahrs ,
AC_AttitudeControl ( AP_AHRS & ahrs ,
const AP_Vehicle : : MultiCopter & aparm ,
AP_Motors & motors ,
AC_P & pi_angle_roll , AC_P & pi_angle_pitch , AC_P & pi_angle_yaw ,
AC_PID & pid_rate_roll , AC_PID & pid_rate_pitch , AC_PID & pid_rate_yaw
) :
_ahrs ( ahrs ) ,
_ahrs ( ahrs ) ,
_aparm ( aparm ) ,
_motors ( motors ) ,
_p_angle_roll ( pi_angle_roll ) ,
@ -64,256 +65,268 @@ public:
@@ -64,256 +65,268 @@ public:
_dt ( AC_ATTITUDE_100HZ_DT ) ,
_angle_boost ( 0 ) ,
_acro_angle_switch_rad ( 0 ) ,
_throttle_in_filt ( AC_ATTITUDE_CONTROL_ALTHOLD_LEANANGLE_FILT_HZ )
{
AP_Param : : setup_object_defaults ( this , var_info ) ;
// initialise flags
_flags . limit_angle_to_rate_request = true ;
}
_throttle_in_filt ( AC_ATTITUDE_CONTROL_ALTHOLD_LEANANGLE_FILT_HZ )
{
AP_Param : : setup_object_defaults ( this , var_info ) ;
// empty destructor to suppress compiler warning
virtual ~ AC_AttitudeControl ( ) { }
_att_ctrl_use_accel_limit = true ;
}
//
// initialisation functions
//
// Empty destructor to suppress compiler warning
virtual ~ AC_AttitudeControl ( ) { }
// s et_dt - sets time delta in seconds for all controllers (i.e. 100hz = 0.01, 400hz = 0.0025)
// S et_dt - sets time delta in seconds for all controllers (i.e. 100hz = 0.01, 400hz = 0.0025)
void set_dt ( float delta_sec ) ;
// get_accel_roll_max - g ets the roll acceleration limit in centidegrees/s/s
// G ets the roll acceleration limit in centidegrees/s/s
float get_accel_roll_max ( ) { return _accel_roll_max ; }
// set_accel_roll_max - s ets the roll acceleration limit in centidegrees/s/s
// S ets the roll acceleration limit in centidegrees/s/s
void set_accel_roll_max ( float accel_roll_max ) { _accel_roll_max = accel_roll_max ; }
// save_accel_roll_max - s ets and saves the roll acceleration limit in centidegrees/s/s
// S ets and saves the roll acceleration limit in centidegrees/s/s
void save_accel_roll_max ( float accel_roll_max ) { _accel_roll_max = accel_roll_max ; _accel_roll_max . save ( ) ; }
// get_accel_pitch_max - g ets the pitch acceleration limit in centidegrees/s/s
// S ets the pitch acceleration limit in centidegrees/s/s
float get_accel_pitch_max ( ) { return _accel_pitch_max ; }
// set_accel_pitch_max - s ets the pitch acceleration limit in centidegrees/s/s
// S ets the pitch acceleration limit in centidegrees/s/s
void set_accel_pitch_max ( float accel_pitch_max ) { _accel_pitch_max = accel_pitch_max ; }
// save_accel_pitch_max - s ets and saves the pitch acceleration limit in centidegrees/s/s
// S ets and saves the pitch acceleration limit in centidegrees/s/s
void save_accel_pitch_max ( float accel_pitch_max ) { _accel_pitch_max = accel_pitch_max ; _accel_pitch_max . save ( ) ; }
// get_accel_yaw_max - g ets the yaw acceleration limit in centidegrees/s/s
// G ets the yaw acceleration limit in centidegrees/s/s
float get_accel_yaw_max ( ) { return _accel_yaw_max ; }
// set_accel_yaw_max - s ets the yaw acceleration limit in centidegrees/s/s
// S ets the yaw acceleration limit in centidegrees/s/s
void set_accel_yaw_max ( float accel_yaw_max ) { _accel_yaw_max = accel_yaw_max ; }
// save_accel_yaw_max - s ets and saves the yaw acceleration limit in centidegrees/s/s
// S ets and saves the yaw acceleration limit in centidegrees/s/s
void save_accel_yaw_max ( float accel_yaw_max ) { _accel_yaw_max = accel_yaw_max ; _accel_yaw_max . save ( ) ; }
// relax_bf_rate_controller - e nsure body-frame rate controller has zero errors to relax rate controller output
// E nsure body-frame rate controller has zero errors to relax rate controller output
void relax_bf_rate_controller ( ) ;
// set_yaw_target_to_current_heading - sets yaw target to current heading
void set_yaw_target_to_current_heading ( ) { _angle_ef_target _rad . z = _ahrs . yaw ; }
// Sets yaw target to vehicle heading
void set_yaw_target_to_current_heading ( ) { _att_target_euler _rad . z = _ahrs . yaw ; }
// shifts earth frame yaw target by yaw_shift_cd. yaw_shift_cd should be in centi- degrees and is added to the current target heading
// Shifts earth frame yaw target by yaw_shift_cd. yaw_shift_cd should be in centi degrees and is added to the current target heading
void shift_ef_yaw_target ( float yaw_shift_cd ) ;
//
// methods to be called by upper controllers to request and implement a desired attitude
//
// angle_ef_roll_pitch_rate_ef_yaw_smooth - attempts to maintain a roll and pitch angle and yaw rate (all earth frame) while smoothing the attitude based on the feel parameter
// smoothing_gain : a number from 1 to 50 with 1 being sluggish and 50 being very crisp
void angle_ef_roll_pitch_rate_ef_yaw_smooth ( float roll_angle_ef_cd , float pitch_angle_ef_cd , float yaw_rate_ef_cds , float smoothing_gain ) ;
// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
void euler_angle_roll_pitch_euler_rate_yaw_smooth ( float euler_roll_angle_cd , float euler_pitch_angle_cd , float euler_yaw_rate_cds , float smoothing_gain ) ;
// angle_ef_roll_pitch_rate_ef_yaw - attempts to maintain a roll and pitch angle and yaw rate (all earth frame)
void angle_ef _roll_pitch_rate_ef _yaw ( float roll_angle_ef _cd , float pitch_angle_ef_ cd , float yaw_rate_ef _cds ) ;
// Command an euler roll and pitch angle and an euler yaw rate
void euler_angle_roll_pitch_euler_rate_yaw ( float euler_roll_angle_cd , float euler_pitch_angle_cd , float euler_yaw_rate_cds ) ;
// angle_ef_roll_pitch_yaw - attempts to maintain a roll, pitch and yaw angle (all earth frame)
// if yaw_slew is true then target yaw movement will be gradually moved to the new target based on the YAW_SLEW parameter
void angle_ef_roll_pitch_yaw ( float roll_angle_ef_cd , float pitch_angle_ef_cd , float yaw_angle_ef_cd , bool slew_yaw ) ;
// Command an euler roll, pitch and yaw angle
void euler_angle_roll_pitch_yaw ( float euler_roll_angle_cd , float euler_pitch_angle_ef_cd , float euler_yaw_angle_cd , bool slew_yaw ) ;
// rate_ef_roll_pitch_yaw - attempts to maintain a roll, pitch and yaw rate (all earth frame)
void rate_ef _roll_pitch_yaw ( float roll_rate_ef _cds , float pitch_rate_ef _cds , float yaw_rate_ef _cds ) ;
// Command an euler roll, pitch, and yaw rate
void euler_ rate_roll_pitch_yaw( float euler_ roll_rate_cds, float euler_ pitch_rate_cds, float euler_ yaw_rate_cds) ;
// rate_bf_roll_pitch_yaw - attempts to maintain a roll, pitch and yaw rate (all body frame)
// Command an angular velocity
virtual void rate_bf_roll_pitch_yaw ( float roll_rate_bf_cds , float pitch_rate_bf_cds , float yaw_rate_bf_cds ) ;
//
// rate_controller_run - run lowest level body-frame rate controller and send outputs to the motors
// should be called at 100hz or more
//
// Run angular velocity controller and send outputs to the motors
virtual void rate_controller_run ( ) ;
// converts a 321-intrinsic euler angle derivative to an angular velocity vector
// Convert a 321-intrinsic euler angle derivative to an angular velocity vector
void euler_derivative_to_ang_vel ( const Vector3f & euler_rad , const Vector3f & euler_dot_rads , Vector3f & ang_vel_rads ) ;
// converts an angular velocity vector to a 321-intrinsic euler angle derivative
// returns false if the vehicle is pitched all the way up or all the way down
// Convert an angular velocity vector to a 321-intrinsic euler angle derivative
// Returns false if the vehicle is pitched 90 degrees up or down
bool ang_vel_to_euler_derivative ( const Vector3f & euler_rad , const Vector3f & ang_vel_rads , Vector3f & euler_dot_rads ) ;
//
// public accessor functions
//
// Configures whether the attitude controller should limit the rate demand to constrain angular acceleration
void limit_angle_to_rate_request ( bool limit_request ) { _att_ctrl_use_accel_limit = limit_request ; }
// limit_angle_to_rate_request
void limit_angle_to_rate_request ( bool limit_request ) { _flags . limit_angle_to_rate_request = limit_request ; }
// Return 321-intrinsic euler angles in centidegrees representing the rotation from NED earth frame to the
// attitude controller's reference attitude.
Vector3f angle_ef_targets ( ) const { return _att_target_euler_rad * degrees ( 100.0f ) ; }
// angle_ef_targets - returns angle controller earth-frame targets (for reporting)
// convert from radians to centi-degrees on public interface
Vector3f angle_ef_targets ( ) const { return _angle_ef_target _rad * degrees ( 100.0f ) ; }
// Return a rotation vector in centidegrees representing the rotation from vehicle body frame to the
// attitude controller's reference attitude.
Vector3f angle_bf_error ( ) const { return _att_error_rot_vec _rad * degrees ( 100.0f ) ; }
// angle_bf_error - returns angle controller body-frame errors (for stability checking)
// convert from radians to centi-degrees on public interface
Vector3f angle_bf_error ( ) const { return _angle_bf_error_rad * degrees ( 100.0f ) ; }
// Set x-axis angular velocity reference in centidegrees/s
void rate_bf_roll_target ( float rate_cds ) { _ang_vel_target_rads . x = radians ( rate_cds * 0.01f ) ; }
// rate_bf_targets - sets rate controller body-frame targets
// convert from centi-degrees on public interface to radians
void rate_bf_roll_target ( float rate_cds ) { _rate_bf_target_rads . x = radians ( rate_cds * 0.01f ) ; }
void rate_bf_pitch_target ( float rate_cds ) { _rate_bf_target_rads . y = radians ( rate_cds * 0.01f ) ; }
void rate_bf_yaw_target ( float rate_cds ) { _rate_bf_target_rads . z = radians ( rate_cds * 0.01f ) ; }
// Set y-axis angular velocity reference in centidegrees/s
void rate_bf_pitch_target ( float rate_cds ) { _ang_vel_target_rads . y = radians ( rate_cds * 0.01f ) ; }
// Maximum roll rate step size in centi-degrees/s that results in maximum output after 4 time steps
// Set z-axis angular velocity reference in centidegrees/s
void rate_bf_yaw_target ( float rate_cds ) { _ang_vel_target_rads . z = radians ( rate_cds * 0.01f ) ; }
// Return roll rate step size in centidegrees/s that results in maximum output after 4 time steps
float max_rate_step_bf_roll ( ) ;
// Maximum pitch rate step size in centi-degrees/s that results in maximum output after 4 time steps
// Return pitch rate step size in centidegrees/s that results in maximum output after 4 time steps
float max_rate_step_bf_pitch ( ) ;
// Maximum yaw rate step size in centi-degrees/s that results in maximum output after 4 time steps
// Return yaw rate step size in centidegrees/s that results in maximum output after 4 time steps
float max_rate_step_bf_yaw ( ) ;
// Maximum roll step size in centi- degrees that results in maximum output after 4 time steps
// Return roll step size in centi degrees that results in maximum output after 4 time steps
float max_angle_step_bf_roll ( ) { return max_rate_step_bf_roll ( ) / _p_angle_roll . kP ( ) ; }
// Maximum pitch step size in centi-degrees that results in maximum output after 4 time steps
// Return pitch step size in centidegrees that results in maximum output after 4 time steps
float max_angle_step_bf_pitch ( ) { return max_rate_step_bf_pitch ( ) / _p_angle_pitch . kP ( ) ; }
// Maximum yaw step size in centi-degrees that results in maximum output after 4 time steps
// Return yaw step size in centidegrees that results in maximum output after 4 time steps
float max_angle_step_bf_yaw ( ) { return max_rate_step_bf_yaw ( ) / _p_angle_yaw . kP ( ) ; }
// rate_ef_targets - returns rate controller body-frame targets (for reporting)
// convert from radians/s to centi-degrees/s on public interface
Vector3f rate_bf_targets ( ) const { return _rate_bf_target_rads * degrees ( 100.0f ) ; }
// Return reference angular velocity used in the angular velocity controller
Vector3f rate_bf_targets ( ) const { return _ang_vel_target_rads * degrees ( 100.0f ) ; }
// bf_feedforward - e nable or disable body-frame feed forward
// E nable or disable body-frame feed forward
void bf_feedforward ( bool enable_or_disable ) { _rate_bf_ff_enabled = enable_or_disable ; }
// bf_feedforward - e nable or disable body-frame feed forward and save
// E nable or disable body-frame feed forward and save
void bf_feedforward_save ( bool enable_or_disable ) { _rate_bf_ff_enabled . set_and_save ( enable_or_disable ) ; }
// get_bf_feedforward - r eturn body-frame feed forward setting
// R eturn body-frame feed forward setting
bool get_bf_feedforward ( ) { return _rate_bf_ff_enabled ; }
// enable_bf_feedforward - e nable or disable body-frame feed forward
// E nable or disable body-frame feed forward
void accel_limiting ( bool enable_or_disable ) ;
//
// throttle functions
//
// set_throttle_out - to be called by upper throttle controllers when they wish to provide throttle output directly to motors
void set_throttle_out ( float throttle_in , bool apply_angle_boost , float filt_cutoff ) ;
// outputs a throttle to all motors evenly with no stabilization
void set_throttle_out_unstabilized ( float throttle_in , bool reset_attitude_control , float filt_cutoff ) ;
// Set output throttle
void set_throttle_out ( float throttle_in , bool apply_angle_boost , float filt_cutoff ) ;
// angle_boost - accessor for angle boost so it can be logged
int16_t angle_boost ( ) const { return _angle_boost ; }
// Set output throttle and disable stabilization
void set_throttle_out_unstabilized ( float throttle_in , bool reset_attitude_control , float filt_cutoff ) ;
// get lean angle max for pilot input that prioritises altitude hold over lean angle
virtual float get_althold_lean_angle_max ( ) const = 0 ;
// Return throttle increase applied for tilt compensation
int16_t angle_boost ( ) const { return _angle_boost ; }
//
// helper functions
//
// Return tilt angle limit for pilot input that prioritises altitude hold over lean angle
virtual float get_althold_lean_angle_max ( ) const = 0 ;
// lean_angle_max - maximum lean angle of the copter in centi-degree s
// Return configured tilt angle limit in centidegrees/ s
int16_t lean_angle_max ( ) const { return _aparm . angle_max ; }
// sqrt_controller - response based on the sqrt of the error instead of the more common linear respons e
// Proportional controller with piecewise sqrt sections to constrain second derivativ e
static float sqrt_controller ( float error , float p , float second_ord_lim ) ;
// u ser settable parameters
// U ser settable parameters
static const struct AP_Param : : GroupInfo var_info [ ] ;
protected :
// Update _att_target_euler_rad.x by integrating a 321-intrinsic euler roll angle derivative
void update_att_target_and_error_roll ( float roll_rate_ef_rads , Vector3f & angle_ef_error_rad , float overshoot_max_rad ) ;
// attitude control flags
struct AttControlFlags {
uint8_t limit_angle_to_rate_request : 1 ; // 1 if the earth frame angle controller is limiting it's maximum rate request
} _flags ;
// update_ef_roll_angle_and_error - update _angle_ef_target.x using an earth frame roll rate request
void update_ef_roll_angle_and_error ( float roll_rate_ef_rads , Vector3f & angle_ef_error_rad , float overshoot_max_rad ) ;
// Update _att_target_euler_rad.y by integrating a 321-intrinsic euler pitch angle derivative
void update_att_target_and_error_pitch ( float pitch_rate_ef_rads , Vector3f & angle_ef_error_rad , float overshoot_max_rad ) ;
// update_ef_pitch_angle_and_error - update _angle_ef_target.y using an earth frame pitch rate request
void update_ef_pitch_angle_and_error ( float pitch _rate_ef_rads , Vector3f & angle_ef_error_rad , float overshoot_max_rad ) ;
// Update _att_target_euler_rad.z by integrating a 321-intrinsic euler yaw angle derivative
void update_att_target_and_error_yaw ( float yaw_rate_ef_rads , Vector3f & angle_ef_error_rad , float overshoot_max_rad ) ;
// update_ef_yaw_angle_and_error - update _angle_ef_target.z using an earth frame yaw rate request
void update_ef_yaw_angle_and_error ( float yaw_rate_ef_rads , Vector3f & angle_ef_error_rad , float overshoot_max_rad ) ;
// integrate_bf_rate_error_to_angle_errors - calculates body frame angle errors
// body-frame feed forward rates (radians/s) taken from _angle_bf_error
// angle errors in radians placed in _angle_bf_error
// Integrate vehicle rate into _att_error_rot_vec_rad
void integrate_bf_rate_error_to_angle_errors ( ) ;
// update_rate_bf_targets - converts body-frame angle error to body-frame rate targets for roll, pitch and yaw axis
// targets rates in radians taken from _angle_bf_error
// results in radians/s put into _rate_bf_target
void update_rate_bf_targets ( ) ;
// Update _ang_vel_target_rads using _att_error_rot_vec_rad
void update_ang_vel_target_from_att_error ( ) ;
//
// body-frame rate controller
//
// rate_bf_to_motor_roll - ask the rate controller to calculate the motor outputs to achieve the target body-frame rate (in radians/s) for roll, pitch and yaw
// Run the roll angular velocity PID controller and return the output
float rate_bf_to_motor_roll ( float rate_target_rads ) ;
// Run the pitch angular velocity PID controller and return the output
float rate_bf_to_motor_pitch ( float rate_target_rads ) ;
virtual float rate_bf_to_motor_yaw ( float rate_target_rads ) ;
//
// throttle methods
//
// Run the yaw angular velocity PID controller and return the output
virtual float rate_bf_to_motor_yaw ( float rate_target_rads ) ;
// calculate total body frame throttle required to produce the given earth frame thrott le
// Compute a throttle value that is adjusted for the tilt angle of the vehic le
virtual float get_boosted_throttle ( float throttle_in ) = 0 ;
// get_roll_trim - angle in centi-degrees to be added to roll angle. Used by helicopter to counter tail rotor thrust in hover
// Overloaded by AC_Attitude_Heli to return angle. Should be left to return zero for multirotors .
// Return angle in radians to be added to roll angle. Used by heli to counteract
// tail rotor thrust in hover. Overloaded by AC_Attitude_Heli to return angle.
virtual int16_t get_roll_trim_rad ( ) { return 0 ; }
// Return the roll axis acceleration limit in radians/s/s
float get_accel_roll_max_radss ( ) { return radians ( _accel_roll_max * 0.01f ) ; }
// Return the pitch axis acceleration limit in radians/s/s
float get_accel_pitch_max_radss ( ) { return radians ( _accel_pitch_max * 0.01f ) ; }
// Return the yaw axis acceleration limit in radians/s/s
float get_accel_yaw_max_radss ( ) { return radians ( _accel_yaw_max * 0.01f ) ; }
// Return the yaw slew rate limit in radians/s
float get_slew_yaw_rads ( ) { return radians ( _slew_yaw * 0.01f ) ; }
// Return the tilt angle limit in radians
float get_tilt_limit_rad ( ) { return radians ( _aparm . angle_max * 0.01f ) ; }
// references to external libraries
// Maximum rate the yaw target can be updated in Loiter, RTL, Auto flight modes
// BUG: SLEW_YAW's behavior does not match its parameter documentation
AP_Float _slew_yaw ;
// Maximum rotation acceleration for earth-frame roll axis
AP_Float _accel_roll_max ;
// Maximum rotation acceleration for earth-frame pitch axis
AP_Float _accel_pitch_max ;
// Maximum rotation acceleration for earth-frame yaw axis
AP_Float _accel_yaw_max ;
// Enable/Disable body frame rate feed forward
AP_Int8 _rate_bf_ff_enabled ;
// Intersampling period in seconds
float _dt ;
// This represents a 321-intrinsic rotation from NED frame to the reference
// attitude used in the attitude controller. Formerly _angle_ef_target.
Vector3f _att_target_euler_rad ;
// This represents an euler axis-angle rotation vector from the vehicle’s
// estimated attitude to the reference attitude used in the attitude
// controller. Formerly _angle_bf_error.
Vector3f _att_error_rot_vec_rad ;
// This represents the angular velocity of the reference attitude used in
// the attitude controller as 321-intrinsic euler angle derivatives.
// Formerly _rate_ef_desired.
Vector3f _att_target_euler_deriv_rads ;
// This represents the angular velocity of the reference attitude used in
// the attitude controller as an angular velocity vector in the reference
// attitude frame. Formerly _rate_bf_desired.
Vector3f _att_target_ang_vel_rads ;
// This represents the reference angular velocity used in the angular
// velocity controller. Formerly _rate_bf_target.
Vector3f _ang_vel_target_rads ;
// This represents the throttle increase applied for tilt compensation.
// Used only for logging.
int16_t _angle_boost ;
// This provides hysteresis on the attitude controller switching used to account
// for deficiencies in the euler angle based attitude control.
float _acro_angle_switch_rad ;
// Specifies whether the attitude controller should use the acceleration limit
bool _att_ctrl_use_accel_limit ;
// Filtered throttle input - used to limit lean angle when throttle is saturated
LowPassFilterFloat _throttle_in_filt ;
// References to external libraries
const AP_AHRS & _ahrs ;
const AP_Vehicle : : MultiCopter & _aparm ;
AP_Motors & _motors ;
AC_P & _p_angle_roll ;
AC_P & _p_angle_pitch ;
AC_P & _p_angle_yaw ;
AC_P & _p_angle_roll ;
AC_P & _p_angle_pitch ;
AC_P & _p_angle_yaw ;
AC_PID & _pid_rate_roll ;
AC_PID & _pid_rate_pitch ;
AC_PID & _pid_rate_yaw ;
// parameters
AP_Float _slew_yaw ; // maximum rate the yaw target can be updated in Loiter, RTL, Auto flight modes
AP_Float _accel_roll_max ; // maximum rotation acceleration for earth-frame roll axis
AP_Float _accel_pitch_max ; // maximum rotation acceleration for earth-frame pitch axis
AP_Float _accel_yaw_max ; // maximum rotation acceleration for earth-frame yaw axis
AP_Int8 _rate_bf_ff_enabled ; // Enable/Disable body frame rate feed forward
// internal variables
// To-Do: make rate targets a typedef instead of Vector3f?
float _dt ; // time delta in seconds
Vector3f _angle_ef_target_rad ; // angle controller earth-frame targets
Vector3f _angle_bf_error_rad ; // angle controller body-frame error
Vector3f _rate_bf_target_rads ; // rate controller body-frame targets
Vector3f _rate_ef_desired_rads ; // earth-frame feed forward rates
Vector3f _rate_bf_desired_rads ; // body-frame feed forward rates
int16_t _angle_boost ; // used only for logging
float _acro_angle_switch_rad ;
// throttle based angle limits
LowPassFilterFloat _throttle_in_filt ; // throttle input from pilot or alt hold controller
} ;
# define AC_ATTITUDE_CONTROL_LOG_FORMAT(msg) { msg, sizeof(AC_AttitudeControl::log_Attitude), \
Jonathan Challinger
9 years ago
committed by
Randy Mackay