2013-10-12 09:28:18 -03:00
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file AC_AttitudeControl.h
/// @brief ArduCopter attitude control library
# ifndef AC_AttitudeControl_H
# define AC_AttitudeControl_H
# include <AP_Common.h>
# include <AP_Param.h>
# include <AP_Math.h>
# include <AP_InertialSensor.h>
# include <AP_AHRS.h>
# include <AP_Motors.h>
# include <DataFlash.h>
# include <AC_PID.h>
2014-02-14 03:06:21 -04:00
# include <AC_P.h>
2013-10-12 09:28:18 -03:00
// To-Do: change the name or move to AP_Math?
2014-02-03 00:29:17 -04:00
# define AC_ATTITUDE_CONTROL_DEGX100 5729.57795f // constant to convert from radians to centi-degrees
# define AC_ATTITUDE_CONTROL_RATE_RP_MAX_DEFAULT 18000 // maximum rotation rate in roll/pitch axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes
# define AC_ATTITUDE_CONTROL_RATE_Y_MAX_DEFAULT 18000 // maximum rotation rate on yaw axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes
# define AC_ATTITUDE_CONTROL_SLEW_YAW_DEFAULT 6000 // default yaw slew rate in centi-degrees/sec (i.e. maximum yaw target change in 1second)
2014-02-14 21:07:49 -04:00
# define AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT 54000 // default maximum acceleration for roll/pitch axis in centi-degrees/sec/sec
# define AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT 18000 // default maximum acceleration for yaw axis in centi-degrees/sec/sec
2014-02-03 00:29:17 -04:00
# 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)
# define AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX 4500.0f // body-frame rate controller maximum output (for yaw axis)
# define AC_ATTITUDE_ANGLE_YAW_CONTROLLER_OUT_MAX 4500.0f // earth-frame angle controller maximum output (for yaw axis)
# define AC_ATTITUDE_ANGLE_CONTROLLER_ANGLE_MAX 4500.0f // earth-frame angle controller maximum input angle (To-Do: replace with reference to aparm.angle_max)
2013-10-12 09:28:18 -03:00
# define AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX 3000.0f // earth-frame rate stabilize controller's maximum overshoot angle
# define AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX 3000.0f // earth-frame rate stabilize controller's maximum overshoot angle
# define AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX 1000.0f // earth-frame rate stabilize controller's maximum overshoot angle
# define AC_ATTITUDE_100HZ_DT 0.0100f // delta time in seconds for 100hz update rate
# define AC_ATTITUDE_400HZ_DT 0.0025f // delta time in seconds for 400hz update rate
class AC_AttitudeControl {
public :
AC_AttitudeControl ( AP_AHRS & ahrs ,
AP_InertialSensor & ins ,
2014-01-17 22:54:23 -04:00
const AP_Vehicle : : MultiCopter & aparm ,
2013-10-12 09:28:18 -03:00
AP_Motors & motors ,
2014-02-14 03:06:21 -04:00
AC_P & pi_angle_roll , AC_P & pi_angle_pitch , AC_P & pi_angle_yaw ,
2014-02-10 00:23:52 -04:00
AC_PID & pid_rate_roll , AC_PID & pid_rate_pitch , AC_PID & pid_rate_yaw
2013-10-12 09:28:18 -03:00
) :
_ahrs ( ahrs ) ,
_ins ( ins ) ,
2014-01-17 22:54:23 -04:00
_aparm ( aparm ) ,
2013-10-12 09:28:18 -03:00
_motors ( motors ) ,
2014-02-14 03:06:21 -04:00
_p_angle_roll ( pi_angle_roll ) ,
_p_angle_pitch ( pi_angle_pitch ) ,
_p_angle_yaw ( pi_angle_yaw ) ,
2013-10-12 09:28:18 -03:00
_pid_rate_roll ( pid_rate_roll ) ,
_pid_rate_pitch ( pid_rate_pitch ) ,
_pid_rate_yaw ( pid_rate_yaw ) ,
2014-01-05 23:28:35 -04:00
_dt ( AC_ATTITUDE_100HZ_DT ) ,
2014-02-08 23:35:08 -04:00
_angle_boost ( 0 )
2013-10-12 09:28:18 -03:00
{
AP_Param : : setup_object_defaults ( this , var_info ) ;
2014-02-01 02:27:58 -04:00
// initialise flags
_flags . limit_angle_to_rate_request = true ;
2013-10-12 09:28:18 -03:00
}
//
// initialisation functions
//
// set_dt - sets time delta in seconds for all controllers (i.e. 100hz = 0.01, 400hz = 0.0025)
void set_dt ( float delta_sec ) { _dt = delta_sec ; }
2014-01-11 04:02:42 -04:00
// init_targets - resets target angles to current angles
void init_targets ( ) ;
2014-02-09 22:59:51 -04:00
//
// methods to be called by upper controllers to request and implement a desired attitude
//
2014-02-19 04:05:29 -04:00
// 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 , float pitch_angle_ef , float yaw_rate_ef , float smoothing_gain ) ;
2014-02-19 02:55:45 -04:00
2014-02-13 22:52:58 -04:00
// 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 , float pitch_angle_ef , float yaw_rate_ef ) ;
2014-01-11 04:02:42 -04:00
2014-02-13 22:52:58 -04:00
// angle_ef_roll_pitch_yaw - attempts to maintain a roll, pitch and yaw angle (all earth frame)
2014-02-03 00:29:17 -04:00
// if yaw_slew is true then target yaw movement will be gradually moved to the new target based on the YAW_SLEW parameter
2014-02-13 22:52:58 -04:00
void angle_ef_roll_pitch_yaw ( float roll_angle_ef , float pitch_angle_ef , float yaw_angle_ef , bool slew_yaw ) ;
2014-01-11 04:02:42 -04:00
2014-02-13 22:52:58 -04:00
// 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 , float pitch_rate_ef , float yaw_rate_ef ) ;
2014-01-28 04:30:33 -04:00
2014-02-13 22:52:58 -04:00
// rate_bf_roll_pitch_yaw - attempts to maintain a roll, pitch and yaw rate (all body frame)
void rate_bf_roll_pitch_yaw ( float roll_rate_bf , float pitch_rate_bf , float yaw_rate_bf ) ;
2014-01-24 22:59:48 -04:00
2013-10-12 09:28:18 -03:00
//
2014-02-09 22:59:51 -04:00
// rate_controller_run - run lowest level body-frame rate controller and send outputs to the motors
// should be called at 100hz or more
2013-10-12 09:28:18 -03:00
//
2014-02-09 22:59:51 -04:00
virtual void rate_controller_run ( ) ;
2013-10-12 09:28:18 -03:00
//
2014-02-09 22:59:51 -04:00
// earth-frame <-> body-frame conversion functions
2013-10-12 09:28:18 -03:00
//
2014-02-13 22:52:58 -04:00
// frame_conversion_ef_to_bf - converts earth frame rate targets to body frame rate targets
void frame_conversion_ef_to_bf ( const Vector3f & ef_vector , Vector3f & bf_vector ) ;
2013-10-12 09:28:18 -03:00
2014-02-13 22:52:58 -04:00
// frame_conversion_bf_to_ef - converts body frame rate targets to earth frame rate targets
void frame_conversion_bf_to_ef ( const Vector3f & bf_vector , Vector3f & ef_vector ) ;
2013-10-12 09:28:18 -03:00
//
2014-02-09 22:59:51 -04:00
// public accessor functions
2013-10-12 09:28:18 -03:00
//
2014-02-09 22:59:51 -04:00
// limit_angle_to_rate_request
void limit_angle_to_rate_request ( bool limit_request ) { _flags . limit_angle_to_rate_request = limit_request ; }
2013-10-12 09:28:18 -03:00
2014-02-09 22:59:51 -04:00
// angle_ef_targets - returns angle controller earth-frame targets (for reporting)
Vector3f angle_ef_targets ( ) const { return _angle_ef_target ; }
2013-10-12 09:28:18 -03:00
2014-02-09 22:59:51 -04:00
// rate_bf_targets - gets rate controller body-frame targets
2014-02-01 02:27:58 -04:00
void rate_bf_roll_target ( float rate_cds ) { _rate_bf_target . x = rate_cds ; }
void rate_bf_pitch_target ( float rate_cds ) { _rate_bf_target . y = rate_cds ; }
void rate_bf_yaw_target ( float rate_cds ) { _rate_bf_target . z = rate_cds ; }
2013-10-12 09:28:18 -03:00
//
// throttle functions
//
// set_throttle_out - to be called by upper throttle controllers when they wish to provide throttle output directly to motors
// provide 0 to cut motors
void set_throttle_out ( int16_t throttle_pwm , bool apply_angle_boost ) ;
2014-01-05 23:28:35 -04:00
// angle_boost - accessor for angle boost so it can be logged
int16_t angle_boost ( ) const { return _angle_boost ; }
2013-10-12 09:28:18 -03:00
//
// helper functions
//
2014-01-17 22:54:23 -04:00
// lean_angle_max - maximum lean angle of the copter in centi-degrees
int16_t lean_angle_max ( ) { return _aparm . angle_max ; }
2013-10-12 09:28:18 -03:00
// user settable parameters
static const struct AP_Param : : GroupInfo var_info [ ] ;
2014-01-29 09:50:32 -04:00
protected :
2013-10-12 09:28:18 -03:00
2014-02-01 02:27:58 -04:00
// 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 ;
2014-02-13 08:34:27 -04:00
2014-02-13 22:52:58 -04:00
// 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 , Vector3f & angle_ef_error ) ;
2014-02-13 08:34:27 -04:00
2014-02-13 22:52:58 -04:00
// 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 , Vector3f & angle_ef_error ) ;
2014-02-13 08:34:27 -04:00
2014-02-13 22:52:58 -04:00
// 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 , Vector3f & angle_ef_error ) ;
2014-02-01 02:27:58 -04:00
2014-02-13 22:52:58 -04:00
// integrate_bf_rate_error_to_angle_errors - calculates body frame angle errors
2014-02-09 22:59:51 -04:00
// body-frame feed forward rates (centi-degrees / second) taken from _angle_bf_error
// angle errors in centi-degrees placed in _angle_bf_error
2014-02-13 22:52:58 -04:00
void integrate_bf_rate_error_to_angle_errors ( ) ;
2014-02-09 22:59:51 -04:00
2014-02-13 22:52:58 -04:00
// update_rate_bf_targets - converts body-frame angle error to body-frame rate targets for roll, pitch and yaw axis
2014-02-09 22:59:51 -04:00
// targets rates in centi-degrees taken from _angle_bf_error
// results in centi-degrees/sec put into _rate_bf_target
2014-02-13 22:52:58 -04:00
void update_rate_bf_targets ( ) ;
2014-02-09 22:59:51 -04:00
2013-10-12 09:28:18 -03:00
//
// 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 centi-degrees/sec) for roll, pitch and yaw
float rate_bf_to_motor_roll ( float rate_target_cds ) ;
float rate_bf_to_motor_pitch ( float rate_target_cds ) ;
2014-01-29 09:50:32 -04:00
virtual float rate_bf_to_motor_yaw ( float rate_target_cds ) ;
2013-10-12 09:28:18 -03:00
//
// throttle methods
//
// get_angle_boost - calculate total body frame throttle required to produce the given earth frame throttle
2014-01-30 03:58:13 -04:00
virtual int16_t get_angle_boost ( int16_t throttle_pwm ) ;
2013-10-12 09:28:18 -03:00
//
// logging
//
// log data on internal state of the controller. Called at 10Hz
void log_data ( DataFlash_Class & dataflash , uint8_t msgid ) ;
// dataflash logging packet
struct PACKED log_Attitude {
LOG_PACKET_HEADER ;
int16_t roll_in ;
int16_t roll ;
int16_t pitch_in ;
int16_t pitch ;
int16_t yaw_in ;
uint16_t yaw ;
uint16_t nav_yaw ;
} log_ACAttControl ;
// references to external libraries
2014-01-27 01:08:59 -04:00
const AP_AHRS & _ahrs ;
const AP_InertialSensor & _ins ;
2014-01-17 22:54:23 -04:00
const AP_Vehicle : : MultiCopter & _aparm ;
2013-10-12 09:28:18 -03:00
AP_Motors & _motors ;
2014-02-14 03:06:21 -04:00
AC_P & _p_angle_roll ;
AC_P & _p_angle_pitch ;
AC_P & _p_angle_yaw ;
2013-10-12 09:28:18 -03:00
AC_PID & _pid_rate_roll ;
AC_PID & _pid_rate_pitch ;
AC_PID & _pid_rate_yaw ;
// parameters
2014-02-01 02:27:58 -04:00
AP_Float _angle_rate_rp_max ; // maximum rate request output from the earth-frame angle controller for roll and pitch axis
AP_Float _angle_rate_y_max ; // maximum rate request output from the earth-frame angle controller for yaw axis
2014-02-03 00:29:17 -04:00
AP_Float _slew_yaw ; // maximum rate the yaw target can be updated in Loiter, RTL, Auto flight modes
2014-02-14 21:07:49 -04:00
AP_Float _accel_rp_max ; // maximum rotation acceleration for earth-frame roll and pitch axis
AP_Float _accel_y_max ; // maximum rotation acceleration for earth-frame yaw axis
2013-10-12 09:28:18 -03:00
// internal variables
// To-Do: make rate targets a typedef instead of Vector3f?
float _dt ; // time delta in seconds
2014-01-05 23:28:35 -04:00
Vector3f _angle_ef_target ; // angle controller earth-frame targets
2014-02-09 22:59:51 -04:00
Vector3f _angle_bf_error ; // angle controller earth-frame targets
2014-01-05 23:28:35 -04:00
Vector3f _rate_bf_target ; // rate controller body-frame targets
2014-02-14 21:07:49 -04:00
Vector3f _rate_ef_desired ; // earth-frame feed forward rates
Vector3f _rate_bf_desired ; // body-frame feed forward rates
2014-01-05 23:28:35 -04:00
int16_t _angle_boost ; // used only for logging
2014-02-13 08:34:27 -04:00
int16_t _acro_angle_switch ; // used only for logging
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 " }
# endif //AC_AttitudeControl_H