2016-02-17 21:24:42 -04:00
# pragma once
2013-10-12 09:28:18 -03:00
/// @file AC_AttitudeControl.h
/// @brief ArduCopter attitude control library
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 <AP_InertialSensor/AP_InertialSensor.h>
2017-02-11 04:14:23 -04:00
# include <AP_AHRS/AP_AHRS_View.h>
2015-08-11 03:28:41 -03:00
# include <AP_Motors/AP_Motors.h>
# include <AC_PID/AC_PID.h>
# include <AC_PID/AC_P.h>
2013-10-12 09:28:18 -03:00
2016-06-24 04:20:21 -03:00
# define AC_ATTITUDE_CONTROL_ANGLE_P 4.5f // default angle P gain for roll, pitch and yaw
2015-11-24 23:28:42 -04:00
2016-06-24 04:20:21 -03:00
# define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS radians(40.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(10.0f) // minimum body-frame acceleration limit for the stability controller (for yaw axis)
# define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS radians(120.0f) // maximum body-frame acceleration limit for the stability controller (for yaw axis)
2017-01-05 14:07:14 -04:00
# define AC_ATTITUDE_CONTROL_SLEW_YAW_DEFAULT_CDS 6000 // constraint on yaw angle error in degrees. This should lead to maximum turn rate of 10deg/sec * Stab Rate P so by default will be 45deg/sec.
2016-06-24 04:20:21 -03:00
# 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
2014-02-14 21:07:49 -04:00
2014-02-03 00:29:17 -04:00
# define AC_ATTITUDE_RATE_CONTROLLER_TIMEOUT 1.0f // body-frame rate controller timeout in seconds
2016-02-02 08:09:26 -04:00
# define AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX 1.0f // body-frame rate controller maximum output (for roll-pitch axis)
# define AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX 1.0f // body-frame rate controller maximum output (for yaw axis)
2013-10-12 09:28:18 -03:00
2016-06-24 04:20:21 -03:00
# define AC_ATTITUDE_THRUST_ERROR_ANGLE radians(30.0f) // Thrust angle error above which yaw corrections are limited
2013-10-12 09:28:18 -03:00
# define AC_ATTITUDE_400HZ_DT 0.0025f // delta time in seconds for 400hz update rate
2015-05-03 19:05:15 -03:00
# define AC_ATTITUDE_CONTROL_RATE_BF_FF_DEFAULT 1 // body-frame rate feedforward enabled by default
2014-06-06 02:04:06 -03:00
2016-05-26 10:40:35 -03:00
# define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_TC_DEFAULT 1.0f // Time constant used to limit lean angle so that vehicle does not lose altitude
# define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX 0.8f // Max throttle used to limit lean angle so that vehicle does not lose altitude
2015-06-23 10:41:47 -03:00
2017-01-04 01:19:00 -04:00
# define AC_ATTITUDE_CONTROL_MIN_DEFAULT 0.1f // minimum throttle mix default
# define AC_ATTITUDE_CONTROL_MAN_DEFAULT 0.5f // manual throttle mix default
2016-06-24 04:20:21 -03:00
# define AC_ATTITUDE_CONTROL_MAX_DEFAULT 0.5f // maximum throttle mix default
2017-01-04 01:19:00 -04:00
# define AC_ATTITUDE_CONTROL_MAX 5.0f // maximum throttle mix default
2016-06-09 06:39:29 -03:00
2016-06-24 04:20:21 -03:00
# define AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT 0.5f // ratio controlling the max throttle output during competing requests of low throttle from the pilot (or autopilot) and higher throttle for attitude control. Higher favours Attitude over pilot input
2016-06-09 06:39:29 -03:00
2013-10-12 09:28:18 -03:00
class AC_AttitudeControl {
public :
2017-02-11 04:14:23 -04:00
AC_AttitudeControl ( AP_AHRS_View & ahrs ,
2014-01-17 22:54:23 -04:00
const AP_Vehicle : : MultiCopter & aparm ,
2013-10-12 09:28:18 -03:00
AP_Motors & motors ,
2016-02-15 02:26:24 -04:00
float dt ) :
_p_angle_roll ( AC_ATTITUDE_CONTROL_ANGLE_P ) ,
_p_angle_pitch ( AC_ATTITUDE_CONTROL_ANGLE_P ) ,
_p_angle_yaw ( AC_ATTITUDE_CONTROL_ANGLE_P ) ,
_dt ( dt ) ,
2015-12-08 02:48:59 -04:00
_angle_boost ( 0 ) ,
2017-12-31 19:29:28 -04:00
_use_sqrt_controller ( true ) ,
2016-06-09 06:39:29 -03:00
_throttle_rpy_mix_desired ( AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT ) ,
_throttle_rpy_mix ( AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT ) ,
2015-11-24 23:28:42 -04:00
_ahrs ( ahrs ) ,
2014-01-17 22:54:23 -04:00
_aparm ( aparm ) ,
2016-02-15 02:21:38 -04:00
_motors ( motors )
2015-11-24 23:28:42 -04:00
{
AP_Param : : setup_object_defaults ( this , var_info ) ;
}
2014-07-14 10:37:31 -03:00
2015-11-24 23:28:42 -04:00
// Empty destructor to suppress compiler warning
virtual ~ AC_AttitudeControl ( ) { }
2013-10-12 09:28:18 -03:00
2016-02-15 02:26:24 -04:00
// pid accessors
AC_P & get_angle_roll_p ( ) { return _p_angle_roll ; }
AC_P & get_angle_pitch_p ( ) { return _p_angle_pitch ; }
AC_P & get_angle_yaw_p ( ) { return _p_angle_yaw ; }
virtual AC_PID & get_rate_roll_pid ( ) = 0 ;
virtual AC_PID & get_rate_pitch_pid ( ) = 0 ;
virtual AC_PID & get_rate_yaw_pid ( ) = 0 ;
2013-10-12 09:28:18 -03:00
2017-06-26 07:22:02 -03:00
// get the roll acceleration limit in centidegrees/s/s or radians/s/s
float get_accel_roll_max ( ) const { return _accel_roll_max ; }
float get_accel_roll_max_radss ( ) const { return radians ( _accel_roll_max * 0.01f ) ; }
2015-05-06 04:11:48 -03:00
2015-11-24 23:28:42 -04:00
// Sets the roll acceleration limit in centidegrees/s/s
2015-06-05 05:21:56 -03:00
void set_accel_roll_max ( float accel_roll_max ) { _accel_roll_max = accel_roll_max ; }
2015-11-24 23:28:42 -04:00
// Sets and saves the roll acceleration limit in centidegrees/s/s
2017-06-21 06:07:12 -03:00
void save_accel_roll_max ( float accel_roll_max ) { _accel_roll_max . set_and_save ( accel_roll_max ) ; }
2015-02-11 08:10:56 -04:00
2017-06-26 07:22:02 -03:00
// get the pitch acceleration limit in centidegrees/s/s or radians/s/s
float get_accel_pitch_max ( ) const { return _accel_pitch_max ; }
float get_accel_pitch_max_radss ( ) const { return radians ( _accel_pitch_max * 0.01f ) ; }
2015-05-06 04:11:48 -03:00
2015-11-24 23:28:42 -04:00
// Sets the pitch acceleration limit in centidegrees/s/s
2015-06-05 05:21:56 -03:00
void set_accel_pitch_max ( float accel_pitch_max ) { _accel_pitch_max = accel_pitch_max ; }
2015-11-24 23:28:42 -04:00
// Sets and saves the pitch acceleration limit in centidegrees/s/s
2017-06-21 06:07:12 -03:00
void save_accel_pitch_max ( float accel_pitch_max ) { _accel_pitch_max . set_and_save ( accel_pitch_max ) ; }
2015-02-11 08:10:56 -04:00
2017-06-26 07:22:02 -03:00
// get the yaw acceleration limit in centidegrees/s/s or radians/s/s
float get_accel_yaw_max ( ) const { return _accel_yaw_max ; }
float get_accel_yaw_max_radss ( ) const { return radians ( _accel_yaw_max * 0.01f ) ; }
2015-05-06 04:11:48 -03:00
2015-11-24 23:28:42 -04:00
// Sets the yaw acceleration limit in centidegrees/s/s
2015-06-05 05:21:56 -03:00
void set_accel_yaw_max ( float accel_yaw_max ) { _accel_yaw_max = accel_yaw_max ; }
2015-11-24 23:28:42 -04:00
// Sets and saves the yaw acceleration limit in centidegrees/s/s
2017-06-21 06:07:12 -03:00
void save_accel_yaw_max ( float accel_yaw_max ) { _accel_yaw_max . set_and_save ( accel_yaw_max ) ; }
2015-02-11 08:10:56 -04:00
2018-01-26 01:56:54 -04:00
// set the rate control input smoothing time constant
void set_input_tc ( float input_tc ) { _input_tc = constrain_float ( input_tc , 0.0f , 1.0f ) ; }
2017-06-25 11:33:16 -03:00
2016-06-24 04:20:21 -03:00
// Ensure attitude controller have zero errors to relax rate controller output
void relax_attitude_controllers ( ) ;
2014-01-11 04:02:42 -04:00
2016-07-11 23:24:27 -03:00
// reset rate controller I terms
void reset_rate_controller_I_terms ( ) ;
2017-04-15 21:34:13 -03:00
// Sets attitude target to vehicle attitude
void set_attitude_target_to_current_attitude ( ) { _attitude_target_quat . from_rotation_matrix ( _ahrs . get_rotation_body_to_ned ( ) ) ; }
2015-11-24 23:28:42 -04:00
// Sets yaw target to vehicle heading
2016-07-11 23:24:51 -03:00
void set_yaw_target_to_current_heading ( ) { shift_ef_yaw_target ( degrees ( _ahrs . yaw - _attitude_target_euler_angle . z ) * 100.0f ) ; }
2014-06-06 02:45:18 -03:00
2015-11-24 23:28:42 -04:00
// Shifts earth frame yaw target by yaw_shift_cd. yaw_shift_cd should be in centidegrees and is added to the current target heading
2015-06-23 04:49:12 -03:00
void shift_ef_yaw_target ( float yaw_shift_cd ) ;
2017-12-31 19:53:29 -04:00
// handle reset of attitude from EKF since the last iteration
void inertial_frame_reset ( ) ;
2016-06-24 04:20:21 -03:00
// Command a Quaternion attitude with feedforward and smoothing
2017-06-25 11:33:16 -03:00
void input_quaternion ( Quaternion attitude_desired_quat ) ;
2014-02-19 02:55:45 -04:00
2016-06-24 04:20:21 -03:00
// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
2017-06-25 11:33:16 -03:00
virtual void input_euler_angle_roll_pitch_euler_rate_yaw ( float euler_roll_angle_cd , float euler_pitch_angle_cd , float euler_yaw_rate_cds ) ;
2014-01-11 04:02:42 -04:00
2016-06-24 04:20:21 -03:00
// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
2017-06-25 11:33:16 -03:00
virtual void input_euler_angle_roll_pitch_yaw ( float euler_roll_angle_cd , float euler_pitch_angle_cd , float euler_yaw_angle_cd , bool slew_yaw ) ;
2014-01-11 04:02:42 -04:00
2016-06-24 04:20:21 -03:00
// Command an euler roll, pitch, and yaw rate with angular velocity feedforward and smoothing
2015-11-28 13:56:25 -04:00
void input_euler_rate_roll_pitch_yaw ( float euler_roll_rate_cds , float euler_pitch_rate_cds , float euler_yaw_rate_cds ) ;
2014-01-28 04:30:33 -04:00
2016-06-24 04:20:21 -03:00
// Command an angular velocity with angular velocity feedforward and smoothing
2015-11-28 13:56:25 -04:00
virtual void input_rate_bf_roll_pitch_yaw ( float roll_rate_bf_cds , float pitch_rate_bf_cds , float yaw_rate_bf_cds ) ;
2014-01-24 22:59:48 -04:00
2017-12-31 19:53:29 -04:00
// Command an angular velocity with angular velocity feedforward and smoothing
void input_rate_bf_roll_pitch_yaw_2 ( float roll_rate_bf_cds , float pitch_rate_bf_cds , float yaw_rate_bf_cds ) ;
// Command an angular velocity with angular velocity smoothing using rate loops only with integrated rate error stabilization
void input_rate_bf_roll_pitch_yaw_3 ( float roll_rate_bf_cds , float pitch_rate_bf_cds , float yaw_rate_bf_cds ) ;
2017-06-15 10:10:01 -03:00
// Command an angular step (i.e change) in body frame angle
virtual void input_angle_step_bf_roll_pitch_yaw ( float roll_angle_step_bf_cd , float pitch_angle_step_bf_cd , float yaw_angle_step_bf_cd ) ;
2015-11-24 23:28:42 -04:00
// Run angular velocity controller and send outputs to the motors
2016-05-23 02:03:38 -03:00
virtual void rate_controller_run ( ) = 0 ;
2013-10-12 09:28:18 -03:00
2015-11-24 23:28:42 -04:00
// Convert a 321-intrinsic euler angle derivative to an angular velocity vector
2015-12-08 15:15:10 -04:00
void euler_rate_to_ang_vel ( const Vector3f & euler_rad , const Vector3f & euler_rate_rads , Vector3f & ang_vel_rads ) ;
2013-10-12 09:28:18 -03:00
2015-11-24 23:28:42 -04:00
// Convert an angular velocity vector to a 321-intrinsic euler angle derivative
// Returns false if the vehicle is pitched 90 degrees up or down
2015-12-08 15:15:10 -04:00
bool ang_vel_to_euler_rate ( const Vector3f & euler_rad , const Vector3f & ang_vel_rads , Vector3f & euler_rate_rads ) ;
2013-10-12 09:28:18 -03:00
2017-12-31 19:29:28 -04:00
// Specifies whether the attitude controller should use the square root controller in the attitude correction.
// This is used during Autotune to ensure the P term is tuned without being influenced by the acceleration limit of the square root controller.
void use_sqrt_controller ( bool use_sqrt_cont ) { _use_sqrt_controller = use_sqrt_cont ; }
2013-10-12 09:28:18 -03:00
2015-11-24 23:28:42 -04:00
// Return 321-intrinsic euler angles in centidegrees representing the rotation from NED earth frame to the
2016-06-24 04:20:21 -03:00
// attitude controller's target attitude.
2016-09-27 21:04:53 -03:00
// **NOTE** Using vector3f*deg(100) is more efficient than deg(vector3f)*100 or deg(vector3d*100) because it gives the
2017-01-05 14:07:14 -04:00
// same result with the fewest multiplications. Even though it may look like a bug, it is intentional. See issue 4895.
2016-06-24 04:20:21 -03:00
Vector3f get_att_target_euler_cd ( ) const { return _attitude_target_euler_angle * degrees ( 100.0f ) ; }
2013-10-12 09:28:18 -03:00
2016-06-24 04:20:21 -03:00
// Return the angle between the target thrust vector and the current thrust vector.
float get_att_error_angle_deg ( ) const { return degrees ( _thrust_error_angle ) ; }
2013-10-12 09:28:18 -03:00
2016-06-24 04:20:21 -03:00
// Set x-axis angular velocity in centidegrees/s
void rate_bf_roll_target ( float rate_cds ) { _rate_target_ang_vel . x = radians ( rate_cds * 0.01f ) ; }
2015-04-14 10:00:44 -03:00
2016-06-24 04:20:21 -03:00
// Set y-axis angular velocity in centidegrees/s
void rate_bf_pitch_target ( float rate_cds ) { _rate_target_ang_vel . y = radians ( rate_cds * 0.01f ) ; }
2013-10-12 09:28:18 -03:00
2016-06-24 04:20:21 -03:00
// Set z-axis angular velocity in centidegrees/s
void rate_bf_yaw_target ( float rate_cds ) { _rate_target_ang_vel . z = radians ( rate_cds * 0.01f ) ; }
2015-11-24 23:28:42 -04:00
2016-08-05 23:54:19 -03:00
// Return roll rate step size in radians/s that results in maximum output after 4 time steps
2015-02-11 09:03:36 -04:00
float max_rate_step_bf_roll ( ) ;
2015-11-24 23:28:42 -04:00
2016-08-05 23:54:19 -03:00
// Return pitch rate step size in radians/s that results in maximum output after 4 time steps
2015-02-11 09:03:36 -04:00
float max_rate_step_bf_pitch ( ) ;
2015-11-24 23:28:42 -04:00
2016-08-05 23:54:19 -03:00
// Return yaw rate step size in radians/s that results in maximum output after 4 time steps
2015-02-11 09:03:36 -04:00
float max_rate_step_bf_yaw ( ) ;
2016-08-05 23:54:19 -03:00
// Return roll step size in radians that results in maximum output after 4 time steps
2015-02-11 09:03:36 -04:00
float max_angle_step_bf_roll ( ) { return max_rate_step_bf_roll ( ) / _p_angle_roll . kP ( ) ; }
2015-11-24 23:28:42 -04:00
2016-08-05 23:54:19 -03:00
// Return pitch step size in radians that results in maximum output after 4 time steps
2015-02-11 09:03:36 -04:00
float max_angle_step_bf_pitch ( ) { return max_rate_step_bf_pitch ( ) / _p_angle_pitch . kP ( ) ; }
2015-11-24 23:28:42 -04:00
2016-08-05 23:54:19 -03:00
// Return yaw step size in radians that results in maximum output after 4 time steps
2015-02-11 09:03:36 -04:00
float max_angle_step_bf_yaw ( ) { return max_rate_step_bf_yaw ( ) / _p_angle_yaw . kP ( ) ; }
2016-06-24 04:20:21 -03:00
// Return angular velocity in radians used in the angular velocity controller
Vector3f rate_bf_targets ( ) const { return _rate_target_ang_vel ; }
2015-02-11 09:01:11 -04:00
2015-11-24 23:28:42 -04:00
// Enable or disable body-frame feed forward
2014-06-10 03:49:49 -03:00
void bf_feedforward ( bool enable_or_disable ) { _rate_bf_ff_enabled = enable_or_disable ; }
2015-11-24 23:28:42 -04:00
// Enable or disable body-frame feed forward and save
2015-05-08 02:28:41 -03:00
void bf_feedforward_save ( bool enable_or_disable ) { _rate_bf_ff_enabled . set_and_save ( enable_or_disable ) ; }
2015-05-06 04:11:48 -03:00
2015-11-24 23:28:42 -04:00
// Return body-frame feed forward setting
2015-02-11 07:07:29 -04:00
bool get_bf_feedforward ( ) { return _rate_bf_ff_enabled ; }
2015-11-24 23:28:42 -04:00
// Enable or disable body-frame feed forward
2014-06-10 03:49:49 -03:00
void accel_limiting ( bool enable_or_disable ) ;
2016-05-23 12:12:14 -03:00
// Update Alt_Hold angle maximum
virtual void update_althold_lean_angle_max ( float throttle_in ) = 0 ;
2015-11-24 23:28:42 -04:00
// Set output throttle
2016-03-21 07:57:39 -03:00
virtual void set_throttle_out ( float throttle_in , bool apply_angle_boost , float filt_cutoff ) = 0 ;
2015-03-25 05:18:40 -03:00
2015-11-24 23:28:42 -04:00
// Set output throttle and disable stabilization
void set_throttle_out_unstabilized ( float throttle_in , bool reset_attitude_control , float filt_cutoff ) ;
2014-01-05 23:28:35 -04:00
2016-01-04 04:14:01 -04:00
// get throttle passed into attitude controller (i.e. throttle_in provided to set_throttle_out)
float get_throttle_in ( ) const { return _throttle_in ; }
2015-11-24 23:28:42 -04:00
// Return throttle increase applied for tilt compensation
2016-01-03 23:44:45 -04:00
float angle_boost ( ) const { return _angle_boost ; }
2015-06-23 10:41:47 -03:00
2015-11-24 23:28:42 -04:00
// Return tilt angle limit for pilot input that prioritises altitude hold over lean angle
2016-05-26 10:11:58 -03:00
float get_althold_lean_angle_max ( ) const ;
2013-10-12 09:28:18 -03:00
2016-04-28 04:46:58 -03:00
// Return configured tilt angle limit in centidegrees
2015-11-27 19:31:16 -04:00
float lean_angle_max ( ) const { return _aparm . angle_max ; }
2014-01-17 22:54:23 -04:00
2015-11-24 23:28:42 -04:00
// Proportional controller with piecewise sqrt sections to constrain second derivative
2017-06-24 23:35:13 -03:00
static float sqrt_controller ( float error , float p , float second_ord_lim , float dt ) ;
2014-10-27 02:58:56 -03:00
2016-06-24 04:20:21 -03:00
// Inverse proportional controller with piecewise sqrt sections to constrain second derivative
static float stopping_point ( float first_ord_mag , float p , float second_ord_lim ) ;
// calculates the velocity correction from an angle error. The angular velocity has acceleration and
// deceleration limits including basic jerk limiting using smoothing_gain
2017-06-24 23:35:13 -03:00
static float input_shaping_angle ( float error_angle , float smoothing_gain , float accel_max , float target_ang_vel , float dt ) ;
2015-12-02 19:47:52 -04:00
2017-11-15 09:45:34 -04:00
// limits the acceleration and deceleration of a velocity request
static float input_shaping_ang_vel ( float target_ang_vel , float desired_ang_vel , float accel_max , float dt ) ;
2017-11-10 11:22:57 -04:00
// calculates the expected angular velocity correction from an angle error based on the AC_AttitudeControl settings.
// This function can be used to predict the delay associated with angle requests.
void input_shaping_rate_predictor ( Vector2f error_angle , Vector2f & target_ang_vel , float dt ) const ;
2017-11-15 09:45:34 -04:00
// translates body frame acceleration limits to the euler axis
void ang_vel_limit ( Vector3f & euler_rad , float ang_vel_roll_max , float ang_vel_pitch_max , float ang_vel_yaw_max ) const ;
2015-12-02 19:47:52 -04:00
2016-06-24 04:20:21 -03:00
// translates body frame acceleration limits to the euler axis
Vector3f euler_accel_limit ( Vector3f euler_rad , Vector3f euler_accel ) ;
2015-12-02 19:47:52 -04:00
2016-06-24 04:20:21 -03:00
// thrust_heading_rotation_angles - calculates two ordered rotations to move the att_from_quat quaternion to the att_to_quat quaternion.
// The first rotation corrects the thrust vector and the second rotation corrects the heading vector.
void thrust_heading_rotation_angles ( Quaternion & att_to_quat , const Quaternion & att_from_quat , Vector3f & att_diff_angle , float & thrust_vec_dot ) ;
2015-12-02 19:47:52 -04:00
2016-06-24 04:20:21 -03:00
// Calculates the body frame angular velocities to follow the target attitude
void attitude_controller_run_quat ( ) ;
2016-01-20 14:41:32 -04:00
2016-09-01 08:31:49 -03:00
// sanity check parameters. should be called once before take-off
virtual void parameter_sanity_check ( ) { }
2017-01-09 03:30:34 -04:00
// return true if the rpy mix is at lowest value
virtual bool is_throttle_mix_min ( ) const { return true ; }
// control rpy throttle mix
virtual void set_throttle_mix_min ( ) { }
2017-01-04 01:19:00 -04:00
virtual void set_throttle_mix_man ( ) { }
2017-01-09 03:30:34 -04:00
virtual void set_throttle_mix_max ( ) { }
2017-01-28 19:40:34 -04:00
virtual void set_throttle_mix_value ( float value ) { }
2017-04-18 09:24:48 -03:00
virtual float get_throttle_mix ( void ) const { return 0 ; }
2017-01-09 03:30:34 -04:00
// enable use of flybass passthrough on heli
virtual void use_flybar_passthrough ( bool passthrough , bool tail_passthrough ) { }
// use_leaky_i - controls whether we use leaky i term for body-frame to motor output stage on heli
virtual void use_leaky_i ( bool leaky_i ) { }
// set_hover_roll_scalar - scales Hover Roll Trim parameter. To be used by vehicle code according to vehicle condition.
virtual void set_hover_roll_trim_scalar ( float scalar ) { }
// passthrough_bf_roll_pitch_rate_yaw - roll and pitch are passed through directly, body-frame rate target for yaw
virtual void passthrough_bf_roll_pitch_rate_yaw ( float roll_passthrough , float pitch_passthrough , float yaw_rate_bf_cds ) { } ;
2017-08-27 20:48:36 -03:00
// enable inverted flight on backends that support it
virtual void set_inverted_flight ( bool inverted ) { }
2017-01-09 03:30:34 -04:00
2016-09-01 08:30:21 -03:00
// User settable parameters
static const struct AP_Param : : GroupInfo var_info [ ] ;
2016-01-20 14:41:32 -04:00
2016-06-24 04:20:21 -03:00
protected :
2014-02-09 22:59:51 -04:00
2016-06-24 04:20:21 -03:00
// Update rate_target_ang_vel using attitude_error_rot_vec_rad
Vector3f update_ang_vel_target_from_att_error ( Vector3f attitude_error_rot_vec_rad ) ;
2014-02-09 22:59:51 -04:00
2015-11-24 23:28:42 -04:00
// Run the roll angular velocity PID controller and return the output
2017-03-02 07:49:25 -04:00
float rate_target_to_motor_roll ( float rate_actual_rads , float rate_target_rads ) ;
2015-11-24 23:28:42 -04:00
// Run the pitch angular velocity PID controller and return the output
2017-03-02 07:49:25 -04:00
float rate_target_to_motor_pitch ( float rate_actual_rads , float rate_target_rads ) ;
2013-10-12 09:28:18 -03:00
2015-11-24 23:28:42 -04:00
// Run the yaw angular velocity PID controller and return the output
2017-03-02 07:49:25 -04:00
virtual float rate_target_to_motor_yaw ( float rate_actual_rads , float rate_target_rads ) ;
2013-10-12 09:28:18 -03:00
2015-11-24 23:28:42 -04:00
// 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.
2015-11-27 19:31:16 -04:00
virtual float get_roll_trim_rad ( ) { return 0 ; }
2015-11-24 17:11:50 -04:00
2015-11-24 23:28:42 -04:00
// Return the yaw slew rate limit in radians/s
2015-11-24 17:11:50 -04:00
float get_slew_yaw_rads ( ) { return radians ( _slew_yaw * 0.01f ) ; }
2015-11-24 23:28:42 -04:00
// Maximum rate the yaw target can be updated in Loiter, RTL, Auto flight modes
AP_Float _slew_yaw ;
2017-11-15 09:45:34 -04:00
// Maximum angular velocity (in degrees/second) for earth-frame roll, pitch and yaw axis
AP_Float _ang_vel_roll_max ;
AP_Float _ang_vel_pitch_max ;
AP_Float _ang_vel_yaw_max ;
2015-11-24 23:28:42 -04:00
// 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 ;
2016-01-06 23:11:27 -04:00
// Enable/Disable angle boost
AP_Int8 _angle_boost_enabled ;
2016-02-15 02:26:24 -04:00
// angle controller P objects
AC_P _p_angle_roll ;
AC_P _p_angle_pitch ;
AC_P _p_angle_yaw ;
2016-05-26 10:40:35 -03:00
// Angle limit time constant (to maintain altitude)
AP_Float _angle_limit_tc ;
2018-01-26 01:56:54 -04:00
// rate controller input smoothing time constant
AP_Float _input_tc ;
2015-11-24 23:28:42 -04:00
// Intersampling period in seconds
float _dt ;
2018-01-19 02:26:41 -04:00
// This represents a 321-intrinsic rotation in NED frame to the target (setpoint)
2016-06-24 04:20:21 -03:00
// attitude used in the attitude controller, in radians.
Vector3f _attitude_target_euler_angle ;
2015-11-24 23:28:42 -04:00
2016-06-24 04:20:21 -03:00
// This represents the angular velocity of the target (setpoint) attitude used in
2015-12-02 18:05:13 -04:00
// the attitude controller as 321-intrinsic euler angle derivatives, in radians per
2016-06-24 04:20:21 -03:00
// second.
Vector3f _attitude_target_euler_rate ;
2018-01-19 02:26:41 -04:00
// This represents a quaternion rotation in NED frame to the target (setpoint)
2016-06-24 04:20:21 -03:00
// attitude used in the attitude controller.
Quaternion _attitude_target_quat ;
2015-11-24 23:28:42 -04:00
2016-06-24 04:20:21 -03:00
// This represents the angular velocity of the target (setpoint) attitude used in
2015-12-02 18:05:13 -04:00
// the attitude controller as an angular velocity vector, in radians per second in
2016-06-24 04:20:21 -03:00
// the target attitude frame.
Vector3f _attitude_target_ang_vel ;
2015-11-24 23:28:42 -04:00
2018-01-19 02:26:41 -04:00
// This represents the angular velocity in radians per second in the body frame, used in the angular
2016-06-24 04:20:21 -03:00
// velocity controller.
Vector3f _rate_target_ang_vel ;
2017-12-31 19:53:29 -04:00
// This represents a quaternion attitude error in the body frame, used for inertial frame reset handling.
Quaternion _attitude_ang_error ;
2016-06-24 04:20:21 -03:00
// The angle between the target thrust vector and the current thrust vector.
float _thrust_error_angle ;
2015-11-24 23:28:42 -04:00
2016-01-04 04:14:01 -04:00
// throttle provided as input to attitude controller. This does not include angle boost.
float _throttle_in = 0.0f ;
2015-11-24 23:28:42 -04:00
// This represents the throttle increase applied for tilt compensation.
// Used only for logging.
2016-01-03 23:44:45 -04:00
float _angle_boost ;
2015-11-24 23:28:42 -04:00
2017-12-31 19:29:28 -04:00
// Specifies whether the attitude controller should use the square root controller in the attitude correction.
// This is used during Autotune to ensure the P term is tuned without being influenced by the acceleration limit of the square root controller.
bool _use_sqrt_controller ;
2015-11-24 23:28:42 -04:00
2016-05-23 12:12:14 -03:00
// Filtered Alt_Hold lean angle max - used to limit lean angle when throttle is saturated using Alt_Hold
2016-05-26 10:11:58 -03:00
float _althold_lean_angle_max = 0.0f ;
2015-11-24 23:28:42 -04:00
2016-06-24 04:20:21 -03:00
// desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds
float _throttle_rpy_mix_desired ;
// mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1
float _throttle_rpy_mix ;
2016-06-09 06:39:29 -03:00
2015-11-24 23:28:42 -04:00
// References to external libraries
2017-02-11 04:14:23 -04:00
const AP_AHRS_View & _ahrs ;
2014-01-17 22:54:23 -04:00
const AP_Vehicle : : MultiCopter & _aparm ;
2013-10-12 09:28:18 -03:00
AP_Motors & _motors ;
2016-05-27 01:13:02 -03:00
2016-05-31 04:07:33 -03:00
protected :
2016-05-27 01:13:02 -03:00
/*
state of control monitoring
*/
struct {
2016-06-09 03:00:57 -03:00
float rms_roll_P ;
float rms_roll_D ;
float rms_pitch_P ;
float rms_pitch_D ;
2016-05-27 01:13:02 -03:00
float rms_yaw ;
} _control_monitor ;
// update state in ControlMonitor
2016-06-09 03:00:57 -03:00
void control_monitor_filter_pid ( float value , float & rms_P ) ;
2016-05-27 01:13:02 -03:00
void control_monitor_update ( void ) ;
2017-08-27 20:48:36 -03:00
// true in inverted flight mode
bool _inverted_flight ;
2016-05-27 01:13:02 -03:00
public :
// log a CTRL message
void control_monitor_log ( void ) ;
2016-06-01 04:18:58 -03:00
// return current RMS controller filter for each axis
float control_monitor_rms_output_roll ( void ) const ;
2016-06-09 03:25:19 -03:00
float control_monitor_rms_output_roll_P ( void ) const ;
float control_monitor_rms_output_roll_D ( void ) const ;
float control_monitor_rms_output_pitch_P ( void ) const ;
float control_monitor_rms_output_pitch_D ( void ) const ;
2016-06-01 04:18:58 -03:00
float control_monitor_rms_output_pitch ( void ) const ;
float control_monitor_rms_output_yaw ( void ) const ;
2013-10-12 09:28:18 -03:00
} ;
# define AC_ATTITUDE_CONTROL_LOG_FORMAT(msg) { msg, sizeof(AC_AttitudeControl::log_Attitude), \
" ATT " , " cccccCC " , " RollIn,Roll,PitchIn,Pitch,YawIn,Yaw,NavYaw " }