2013-10-12 09:28:18 -03:00
# include "AC_AttitudeControl.h"
2015-08-11 03:28:41 -03:00
# include <AP_HAL/AP_HAL.h>
2022-09-29 20:10:39 -03:00
# include <AP_Vehicle/AP_Vehicle_Type.h>
2022-12-05 08:21:43 -04:00
# include <AP_Scheduler/AP_Scheduler.h>
2019-03-04 23:27:31 -04:00
extern const AP_HAL : : HAL & hal ;
2013-10-12 09:28:18 -03:00
2018-01-26 01:56:54 -04:00
# if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
// default gains for Plane
# define AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT 0.2f // Soft
2022-05-25 11:03:36 -03:00
# define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN 5.0 // Min lean angle so that vehicle can maintain limited control
2018-01-26 01:56:54 -04:00
# else
// default gains for Copter and Sub
# define AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT 0.15f // Medium
2022-05-25 11:03:36 -03:00
# define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN 10.0 // Min lean angle so that vehicle can maintain limited control
2018-01-26 01:56:54 -04:00
# endif
2022-04-26 06:07:20 -03:00
AC_AttitudeControl * AC_AttitudeControl : : _singleton ;
2013-10-12 09:28:18 -03:00
// table of user settable parameters
2015-10-25 14:03:46 -03:00
const AP_Param : : GroupInfo AC_AttitudeControl : : var_info [ ] = {
2013-10-12 09:28:18 -03:00
2015-03-29 07:05:23 -03:00
// 0, 1 were RATE_RP_MAX, RATE_Y_MAX
2014-02-03 00:29:17 -04:00
// @Param: SLEW_YAW
// @DisplayName: Yaw target slew rate
2024-04-17 03:48:54 -03:00
// @Description: Maximum rate the yaw target can be updated in RTL and Auto flight modes
2017-05-02 10:37:20 -03:00
// @Units: cdeg/s
2014-02-03 00:29:17 -04:00
// @Range: 500 18000
// @Increment: 100
// @User: Advanced
2019-04-19 08:36:42 -03:00
AP_GROUPINFO ( " SLEW_YAW " , 2 , AC_AttitudeControl , _slew_yaw , AC_ATTITUDE_CONTROL_SLEW_YAW_DEFAULT_CDS ) ,
2013-10-12 09:28:18 -03:00
2015-02-11 08:10:56 -04:00
// 3 was for ACCEL_RP_MAX
2014-02-14 21:07:49 -04:00
// @Param: ACCEL_Y_MAX
// @DisplayName: Acceleration Max for Yaw
// @Description: Maximum acceleration in yaw axis
2017-05-02 10:37:20 -03:00
// @Units: cdeg/s/s
2014-07-17 04:22:32 -03:00
// @Range: 0 72000
2018-01-05 06:49:14 -04:00
// @Values: 0:Disabled, 9000:VerySlow, 18000:Slow, 36000:Medium, 54000:Fast
2014-07-17 04:22:32 -03:00
// @Increment: 1000
2014-02-14 21:07:49 -04:00
// @User: Advanced
2019-04-19 08:36:42 -03:00
AP_GROUPINFO ( " ACCEL_Y_MAX " , 4 , AC_AttitudeControl , _accel_yaw_max , AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT_CDSS ) ,
2014-02-14 21:07:49 -04:00
2014-07-12 22:31:11 -03:00
// @Param: RATE_FF_ENAB
2014-06-06 02:04:06 -03:00
// @DisplayName: Rate Feedforward Enable
2023-10-11 04:41:49 -03:00
// @Description: Controls whether body-frame rate feedforward is enabled or disabled
2014-06-06 02:04:06 -03:00
// @Values: 0:Disabled, 1:Enabled
// @User: Advanced
AP_GROUPINFO ( " RATE_FF_ENAB " , 5 , AC_AttitudeControl , _rate_bf_ff_enabled , AC_ATTITUDE_CONTROL_RATE_BF_FF_DEFAULT ) ,
2015-02-11 08:10:56 -04:00
// @Param: ACCEL_R_MAX
// @DisplayName: Acceleration Max for Roll
// @Description: Maximum acceleration in roll axis
2017-05-02 10:37:20 -03:00
// @Units: cdeg/s/s
2015-02-11 08:10:56 -04:00
// @Range: 0 180000
// @Increment: 1000
2018-01-05 06:49:14 -04:00
// @Values: 0:Disabled, 30000:VerySlow, 72000:Slow, 108000:Medium, 162000:Fast
2015-02-11 08:10:56 -04:00
// @User: Advanced
2015-11-24 17:11:50 -04:00
AP_GROUPINFO ( " ACCEL_R_MAX " , 6 , AC_AttitudeControl , _accel_roll_max , AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT_CDSS ) ,
2015-02-11 08:10:56 -04:00
// @Param: ACCEL_P_MAX
// @DisplayName: Acceleration Max for Pitch
// @Description: Maximum acceleration in pitch axis
2017-05-02 10:37:20 -03:00
// @Units: cdeg/s/s
2015-02-11 08:10:56 -04:00
// @Range: 0 180000
// @Increment: 1000
2018-01-05 06:49:14 -04:00
// @Values: 0:Disabled, 30000:VerySlow, 72000:Slow, 108000:Medium, 162000:Fast
2015-02-11 08:10:56 -04:00
// @User: Advanced
2015-11-24 17:11:50 -04:00
AP_GROUPINFO ( " ACCEL_P_MAX " , 7 , AC_AttitudeControl , _accel_pitch_max , AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT_CDSS ) ,
2015-02-11 08:10:56 -04:00
2015-11-09 22:43:21 -04:00
// IDs 8,9,10,11 RESERVED (in use on Solo)
2016-01-06 23:11:27 -04:00
// @Param: ANGLE_BOOST
// @DisplayName: Angle Boost
// @Description: Angle Boost increases output throttle as the vehicle leans to reduce loss of altitude
// @Values: 0:Disabled, 1:Enabled
// @User: Advanced
AP_GROUPINFO ( " ANGLE_BOOST " , 12 , AC_AttitudeControl , _angle_boost_enabled , 1 ) ,
2016-02-15 02:26:24 -04:00
// @Param: ANG_RLL_P
// @DisplayName: Roll axis angle controller P gain
// @Description: Roll axis angle controller P gain. Converts the error between the desired roll angle and actual angle to a desired roll rate
// @Range: 3.000 12.000
2018-04-11 20:25:52 -03:00
// @Range{Sub}: 0.0 12.000
2016-02-15 02:26:24 -04:00
// @User: Standard
AP_SUBGROUPINFO ( _p_angle_roll , " ANG_RLL_ " , 13 , AC_AttitudeControl , AC_P ) ,
// @Param: ANG_PIT_P
// @DisplayName: Pitch axis angle controller P gain
// @Description: Pitch axis angle controller P gain. Converts the error between the desired pitch angle and actual angle to a desired pitch rate
// @Range: 3.000 12.000
2018-04-11 20:25:52 -03:00
// @Range{Sub}: 0.0 12.000
2016-02-15 02:26:24 -04:00
// @User: Standard
AP_SUBGROUPINFO ( _p_angle_pitch , " ANG_PIT_ " , 14 , AC_AttitudeControl , AC_P ) ,
// @Param: ANG_YAW_P
// @DisplayName: Yaw axis angle controller P gain
// @Description: Yaw axis angle controller P gain. Converts the error between the desired yaw angle and actual angle to a desired yaw rate
2020-02-20 22:10:22 -04:00
// @Range: 3.000 12.000
2018-04-11 20:25:52 -03:00
// @Range{Sub}: 0.0 6.000
2016-02-15 02:26:24 -04:00
// @User: Standard
AP_SUBGROUPINFO ( _p_angle_yaw , " ANG_YAW_ " , 15 , AC_AttitudeControl , AC_P ) ,
2016-05-26 10:40:35 -03:00
// @Param: ANG_LIM_TC
// @DisplayName: Angle Limit (to maintain altitude) Time Constant
// @Description: Angle Limit (to maintain altitude) Time Constant
// @Range: 0.5 10.0
// @User: Advanced
AP_GROUPINFO ( " ANG_LIM_TC " , 16 , AC_AttitudeControl , _angle_limit_tc , AC_ATTITUDE_CONTROL_ANGLE_LIMIT_TC_DEFAULT ) ,
2017-11-15 09:45:34 -04:00
// @Param: RATE_R_MAX
// @DisplayName: Angular Velocity Max for Roll
// @Description: Maximum angular velocity in roll axis
// @Units: deg/s
// @Range: 0 1080
// @Increment: 1
2021-11-09 18:42:12 -04:00
// @Values: 0:Disabled, 60:Slow, 180:Medium, 360:Fast
2017-11-15 09:45:34 -04:00
// @User: Advanced
AP_GROUPINFO ( " RATE_R_MAX " , 17 , AC_AttitudeControl , _ang_vel_roll_max , 0.0f ) ,
// @Param: RATE_P_MAX
// @DisplayName: Angular Velocity Max for Pitch
// @Description: Maximum angular velocity in pitch axis
// @Units: deg/s
// @Range: 0 1080
// @Increment: 1
2021-11-09 18:42:12 -04:00
// @Values: 0:Disabled, 60:Slow, 180:Medium, 360:Fast
2017-11-15 09:45:34 -04:00
// @User: Advanced
AP_GROUPINFO ( " RATE_P_MAX " , 18 , AC_AttitudeControl , _ang_vel_pitch_max , 0.0f ) ,
// @Param: RATE_Y_MAX
2018-10-24 14:28:45 -03:00
// @DisplayName: Angular Velocity Max for Yaw
// @Description: Maximum angular velocity in yaw axis
2017-11-15 09:45:34 -04:00
// @Units: deg/s
// @Range: 0 1080
// @Increment: 1
2021-11-09 18:42:12 -04:00
// @Values: 0:Disabled, 60:Slow, 180:Medium, 360:Fast
2017-11-15 09:45:34 -04:00
// @User: Advanced
AP_GROUPINFO ( " RATE_Y_MAX " , 19 , AC_AttitudeControl , _ang_vel_yaw_max , 0.0f ) ,
2018-01-26 01:56:54 -04:00
// @Param: INPUT_TC
2019-07-16 03:02:58 -03:00
// @DisplayName: Attitude control input time constant
2018-01-26 01:56:54 -04:00
// @Description: Attitude control input time constant. Low numbers lead to sharper response, higher numbers to softer response
2018-02-07 22:22:19 -04:00
// @Units: s
2018-01-26 01:56:54 -04:00
// @Range: 0 1
// @Increment: 0.01
// @Values: 0.5:Very Soft, 0.2:Soft, 0.15:Medium, 0.1:Crisp, 0.05:Very Crisp
// @User: Standard
AP_GROUPINFO ( " INPUT_TC " , 20 , AC_AttitudeControl , _input_tc , AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT ) ,
2024-07-23 04:31:53 -03:00
// @Param: LAND_R_MULT
// @DisplayName: Landed roll gain multiplier
// @Description: Roll gain multiplier active when landed. A factor of 1.0 means no reduction in gain while landed. Reduce this factor to reduce ground oscitation in the roll axis.
// @Range: 0.25 1.0
// @User: Advanced
AP_GROUPINFO ( " LAND_R_MULT " , 21 , AC_AttitudeControl , _land_roll_mult , 1.0 ) ,
// @Param: LAND_P_MULT
// @DisplayName: Landed pitch gain multiplier
// @Description: Pitch gain multiplier active when landed. A factor of 1.0 means no reduction in gain while landed. Reduce this factor to reduce ground oscitation in the pitch axis.
// @Range: 0.25 1.0
// @User: Advanced
AP_GROUPINFO ( " LAND_P_MULT " , 22 , AC_AttitudeControl , _land_pitch_mult , 1.0 ) ,
// @Param: LAND_Y_MULT
// @DisplayName: Landed yaw gain multiplier
// @Description: Yaw gain multiplier active when landed. A factor of 1.0 means no reduction in gain while landed. Reduce this factor to reduce ground oscitation in the yaw axis.
// @Range: 0.25 1.0
// @User: Advanced
AP_GROUPINFO ( " LAND_Y_MULT " , 23 , AC_AttitudeControl , _land_yaw_mult , 1.0 ) ,
2013-10-12 09:28:18 -03:00
AP_GROUPEND
} ;
2022-05-11 08:37:03 -03:00
constexpr Vector3f AC_AttitudeControl : : VECTORF_111 ;
2022-01-27 17:03:32 -04:00
// get the slew yaw rate limit in deg/s
float AC_AttitudeControl : : get_slew_yaw_max_degs ( ) const
{
if ( ! is_positive ( _ang_vel_yaw_max ) ) {
return _slew_yaw * 0.01 ;
}
return MIN ( _ang_vel_yaw_max , _slew_yaw * 0.01 ) ;
}
2016-06-24 04:20:21 -03:00
// Ensure attitude controller have zero errors to relax rate controller output
void AC_AttitudeControl : : relax_attitude_controllers ( )
2014-01-11 04:02:42 -04:00
{
2018-09-14 10:13:47 -03:00
// Initialize the attitude variables to the current attitude
2021-04-14 12:42:42 -03:00
_ahrs . get_quat_body_to_ned ( _attitude_target ) ;
2023-04-15 20:59:20 -03:00
_attitude_target . to_euler ( _euler_angle_target ) ;
2018-09-14 10:13:47 -03:00
_attitude_ang_error . initialise ( ) ;
2016-06-24 04:20:21 -03:00
2018-09-14 10:13:47 -03:00
// Initialize the angular rate variables to the current rate
2021-04-14 12:42:42 -03:00
_ang_vel_target = _ahrs . get_gyro ( ) ;
2024-02-19 19:46:36 -04:00
ang_vel_to_euler_rate ( _attitude_target , _ang_vel_target , _euler_rate_target ) ;
2021-04-14 12:42:42 -03:00
_ang_vel_body = _ahrs . get_gyro ( ) ;
2018-09-14 10:13:47 -03:00
// Initialize remaining variables
_thrust_error_angle = 0.0f ;
// Reset the PID filters
get_rate_roll_pid ( ) . reset_filter ( ) ;
get_rate_pitch_pid ( ) . reset_filter ( ) ;
get_rate_yaw_pid ( ) . reset_filter ( ) ;
// Reset the I terms
2018-09-13 22:27:08 -03:00
reset_rate_controller_I_terms ( ) ;
2014-01-11 04:02:42 -04:00
}
2016-07-11 23:24:27 -03:00
void AC_AttitudeControl : : reset_rate_controller_I_terms ( )
{
get_rate_roll_pid ( ) . reset_I ( ) ;
get_rate_pitch_pid ( ) . reset_I ( ) ;
get_rate_yaw_pid ( ) . reset_I ( ) ;
}
2020-11-17 23:38:18 -04:00
// reset rate controller I terms smoothly to zero in 0.5 seconds
void AC_AttitudeControl : : reset_rate_controller_I_terms_smoothly ( )
{
2022-12-05 08:21:43 -04:00
get_rate_roll_pid ( ) . relax_integrator ( 0.0 , _dt , AC_ATTITUDE_RATE_RELAX_TC ) ;
get_rate_pitch_pid ( ) . relax_integrator ( 0.0 , _dt , AC_ATTITUDE_RATE_RELAX_TC ) ;
get_rate_yaw_pid ( ) . relax_integrator ( 0.0 , _dt , AC_ATTITUDE_RATE_RELAX_TC ) ;
2020-11-17 23:38:18 -04:00
}
2024-07-23 04:31:53 -03:00
// Reduce attitude control gains while landed to stop ground resonance
void AC_AttitudeControl : : landed_gain_reduction ( bool landed )
{
if ( is_positive ( _input_tc ) ) {
// use 2.0 x tc to match the response time to 86% commanded
const float spool_step = _dt / ( 2.0 * _input_tc ) ;
if ( landed ) {
_landed_gain_ratio = MIN ( 1.0 , _landed_gain_ratio + spool_step ) ;
} else {
_landed_gain_ratio = MAX ( 0.0 , _landed_gain_ratio - spool_step ) ;
}
} else {
_landed_gain_ratio = landed ? 1.0 : 0.0 ;
}
Vector3f scale_mult = VECTORF_111 * ( 1.0 - _landed_gain_ratio ) + Vector3f ( _land_roll_mult , _land_pitch_mult , _land_yaw_mult ) * _landed_gain_ratio ;
set_PD_scale_mult ( scale_mult ) ;
set_angle_P_scale_mult ( scale_mult ) ;
}
2016-06-24 04:20:21 -03:00
// The attitude controller works around the concept of the desired attitude, target attitude
// and measured attitude. The desired attitude is the attitude input into the attitude controller
// that expresses where the higher level code would like the aircraft to move to. The target attitude is moved
// to the desired attitude with jerk, acceleration, and velocity limits. The target angular velocities are fed
// directly into the rate controllers. The angular error between the measured attitude and the target attitude is
// fed into the angle controller and the output of the angle controller summed at the input of the rate controllers.
// By feeding the target angular velocity directly into the rate controllers the measured and target attitudes
// remain very close together.
//
// All input functions below follow the same procedure
// 1. define the desired attitude the aircraft should attempt to achieve using the input variables
// 2. using the desired attitude and input variables, define the target angular velocity so that it should
// move the target attitude towards the desired attitude
2017-12-31 19:29:28 -04:00
// 3. if _rate_bf_ff_enabled is not being used then make the target attitude
2016-06-24 04:20:21 -03:00
// and target angular velocities equal to the desired attitude and desired angular velocities.
2021-04-20 21:26:54 -03:00
// 4. ensure _attitude_target, _euler_angle_target, _euler_rate_target and
// _ang_vel_target have been defined. This ensures input modes can be changed without discontinuity.
2016-06-24 04:20:21 -03:00
// 5. attitude_controller_run_quat is then run to pass the target angular velocities to the rate controllers and
// integrate them into the target attitude. Any errors between the target attitude and the measured attitude are
// corrected by first correcting the thrust vector until the angle between the target thrust vector measured
// trust vector drops below 2*AC_ATTITUDE_THRUST_ERROR_ANGLE. At this point the heading is also corrected.
// Command a Quaternion attitude with feedforward and smoothing
2023-08-09 22:57:00 -03:00
// attitude_desired_quat: is updated on each time_step by the integral of the body frame angular velocity
void AC_AttitudeControl : : input_quaternion ( Quaternion & attitude_desired_quat , Vector3f ang_vel_body )
2016-06-24 04:20:21 -03:00
{
2021-04-14 12:42:42 -03:00
Quaternion attitude_error_quat = _attitude_target . inverse ( ) * attitude_desired_quat ;
2016-06-24 04:20:21 -03:00
Vector3f attitude_error_angle ;
attitude_error_quat . to_axis_angle ( attitude_error_angle ) ;
2022-01-11 17:47:09 -04:00
// Limit the angular velocity
2023-08-09 22:57:00 -03:00
ang_vel_limit ( ang_vel_body , radians ( _ang_vel_roll_max ) , radians ( _ang_vel_pitch_max ) , radians ( _ang_vel_yaw_max ) ) ;
Vector3f ang_vel_target = attitude_desired_quat * ang_vel_body ;
2022-01-11 17:47:09 -04:00
2017-12-31 19:29:28 -04:00
if ( _rate_bf_ff_enabled ) {
2016-06-24 04:20:21 -03:00
// When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
// angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
2018-01-26 01:56:54 -04:00
// and an exponential decay specified by _input_tc at the end.
2022-01-11 17:47:09 -04:00
_ang_vel_target . x = input_shaping_angle ( wrap_PI ( attitude_error_angle . x ) , _input_tc , get_accel_roll_max_radss ( ) , _ang_vel_target . x , ang_vel_target . x , radians ( _ang_vel_roll_max ) , _dt ) ;
_ang_vel_target . y = input_shaping_angle ( wrap_PI ( attitude_error_angle . y ) , _input_tc , get_accel_pitch_max_radss ( ) , _ang_vel_target . y , ang_vel_target . y , radians ( _ang_vel_pitch_max ) , _dt ) ;
_ang_vel_target . z = input_shaping_angle ( wrap_PI ( attitude_error_angle . z ) , _input_tc , get_accel_yaw_max_radss ( ) , _ang_vel_target . z , ang_vel_target . z , radians ( _ang_vel_yaw_max ) , _dt ) ;
2016-06-24 04:20:21 -03:00
} else {
2021-04-14 12:42:42 -03:00
_attitude_target = attitude_desired_quat ;
2022-01-11 17:47:09 -04:00
_ang_vel_target = ang_vel_target ;
2016-06-24 04:20:21 -03:00
}
2022-01-11 17:47:09 -04:00
// calculate the attitude target euler angles
2023-04-15 20:59:20 -03:00
_attitude_target . to_euler ( _euler_angle_target ) ;
2022-01-11 17:47:09 -04:00
// Convert body-frame angular velocity into euler angle derivative of desired attitude
2024-02-19 19:46:36 -04:00
ang_vel_to_euler_rate ( _attitude_target , _ang_vel_target , _euler_rate_target ) ;
2022-01-11 17:47:09 -04:00
// rotate target and normalize
Quaternion attitude_desired_update ;
attitude_desired_update . from_axis_angle ( ang_vel_target * _dt ) ;
attitude_desired_quat = attitude_desired_quat * attitude_desired_update ;
attitude_desired_quat . normalize ( ) ;
2016-06-24 04:20:21 -03:00
// Call quaternion attitude controller
attitude_controller_run_quat ( ) ;
2015-06-23 04:49:12 -03: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
void AC_AttitudeControl : : input_euler_angle_roll_pitch_euler_rate_yaw ( float euler_roll_angle_cd , float euler_pitch_angle_cd , float euler_yaw_rate_cds )
2014-02-19 02:55:45 -04:00
{
2015-11-24 23:28:42 -04:00
// Convert from centidegrees on public interface to radians
2019-04-19 08:36:42 -03:00
float euler_roll_angle = radians ( euler_roll_angle_cd * 0.01f ) ;
float euler_pitch_angle = radians ( euler_pitch_angle_cd * 0.01f ) ;
float euler_yaw_rate = radians ( euler_yaw_rate_cds * 0.01f ) ;
2014-02-19 04:05:29 -04:00
2016-06-24 04:20:21 -03:00
// calculate the attitude target euler angles
2023-04-15 20:59:20 -03:00
_attitude_target . to_euler ( _euler_angle_target ) ;
2014-02-19 04:05:29 -04:00
2016-06-24 04:20:21 -03:00
// Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
euler_roll_angle + = get_roll_trim_rad ( ) ;
2014-06-04 11:46:29 -03:00
2017-12-31 19:29:28 -04:00
if ( _rate_bf_ff_enabled ) {
2016-06-24 04:20:21 -03:00
// translate the roll pitch and yaw acceleration limits to the euler axis
2024-02-19 19:46:36 -04:00
const Vector3f euler_accel = euler_accel_limit ( _attitude_target , Vector3f { get_accel_roll_max_radss ( ) , get_accel_pitch_max_radss ( ) , get_accel_yaw_max_radss ( ) } ) ;
2015-02-11 08:10:56 -04:00
2016-06-24 04:20:21 -03:00
// When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
// angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
2015-11-24 23:28:42 -04:00
// and an exponential decay specified by smoothing_gain at the end.
2021-04-14 12:42:42 -03:00
_euler_rate_target . x = input_shaping_angle ( wrap_PI ( euler_roll_angle - _euler_angle_target . x ) , _input_tc , euler_accel . x , _euler_rate_target . x , _dt ) ;
_euler_rate_target . y = input_shaping_angle ( wrap_PI ( euler_pitch_angle - _euler_angle_target . y ) , _input_tc , euler_accel . y , _euler_rate_target . y , _dt ) ;
2014-06-04 11:46:29 -03:00
2015-11-24 23:28:42 -04:00
// When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
// the output rate towards the input rate.
2022-06-24 00:11:32 -03:00
_euler_rate_target . z = input_shaping_ang_vel ( _euler_rate_target . z , euler_yaw_rate , euler_accel . z , _dt , _rate_y_tc ) ;
2014-02-19 02:55:45 -04:00
2016-06-24 04:20:21 -03:00
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
2024-02-19 19:46:36 -04:00
euler_rate_to_ang_vel ( _attitude_target , _euler_rate_target , _ang_vel_target ) ;
2017-11-15 09:45:34 -04:00
// Limit the angular velocity
2021-04-14 12:42:42 -03:00
ang_vel_limit ( _ang_vel_target , radians ( _ang_vel_roll_max ) , radians ( _ang_vel_pitch_max ) , radians ( _ang_vel_yaw_max ) ) ;
2017-11-15 09:45:34 -04:00
// Convert body-frame angular velocity into euler angle derivative of desired attitude
2024-02-19 19:46:36 -04:00
ang_vel_to_euler_rate ( _attitude_target , _ang_vel_target , _euler_rate_target ) ;
2014-02-19 02:55:45 -04:00
} else {
2016-06-24 04:20:21 -03:00
// When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
2021-04-14 12:42:42 -03:00
_euler_angle_target . x = euler_roll_angle ;
_euler_angle_target . y = euler_pitch_angle ;
_euler_angle_target . z + = euler_yaw_rate * _dt ;
2016-06-24 04:20:21 -03:00
// Compute quaternion target attitude
2021-04-14 12:42:42 -03:00
_attitude_target . from_euler ( _euler_angle_target . x , _euler_angle_target . y , _euler_angle_target . z ) ;
2016-06-24 04:20:21 -03:00
2020-01-04 02:19:00 -04:00
// Set rate feedforward requests to zero
2021-04-14 12:42:42 -03:00
_euler_rate_target . zero ( ) ;
_ang_vel_target . zero ( ) ;
2020-01-04 02:19:00 -04:00
}
// Call quaternion attitude controller
attitude_controller_run_quat ( ) ;
}
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
void AC_AttitudeControl : : 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
{
2015-11-24 23:28:42 -04:00
// Convert from centidegrees on public interface to radians
2019-04-19 08:36:42 -03:00
float euler_roll_angle = radians ( euler_roll_angle_cd * 0.01f ) ;
float euler_pitch_angle = radians ( euler_pitch_angle_cd * 0.01f ) ;
float euler_yaw_angle = radians ( euler_yaw_angle_cd * 0.01f ) ;
2015-11-24 17:11:50 -04:00
2016-06-24 04:20:21 -03:00
// calculate the attitude target euler angles
2023-04-15 20:59:20 -03:00
_attitude_target . to_euler ( _euler_angle_target ) ;
2015-10-13 16:02:21 -03:00
2016-06-24 04:20:21 -03:00
// Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
euler_roll_angle + = get_roll_trim_rad ( ) ;
2014-01-11 04:02:42 -04:00
2022-01-27 17:03:32 -04:00
const float slew_yaw_max_rads = get_slew_yaw_max_rads ( ) ;
2017-12-31 19:29:28 -04:00
if ( _rate_bf_ff_enabled ) {
2016-06-24 04:20:21 -03:00
// translate the roll pitch and yaw acceleration limits to the euler axis
2024-02-19 19:46:36 -04:00
const Vector3f euler_accel = euler_accel_limit ( _attitude_target , Vector3f { get_accel_roll_max_radss ( ) , get_accel_pitch_max_radss ( ) , get_accel_yaw_max_radss ( ) } ) ;
2014-02-13 22:52:58 -04:00
2016-06-24 04:20:21 -03:00
// When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
// angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
2018-01-26 01:56:54 -04:00
// and an exponential decay specified by _input_tc at the end.
2021-04-14 12:42:42 -03:00
_euler_rate_target . x = input_shaping_angle ( wrap_PI ( euler_roll_angle - _euler_angle_target . x ) , _input_tc , euler_accel . x , _euler_rate_target . x , _dt ) ;
_euler_rate_target . y = input_shaping_angle ( wrap_PI ( euler_pitch_angle - _euler_angle_target . y ) , _input_tc , euler_accel . y , _euler_rate_target . y , _dt ) ;
_euler_rate_target . z = input_shaping_angle ( wrap_PI ( euler_yaw_angle - _euler_angle_target . z ) , _input_tc , euler_accel . z , _euler_rate_target . z , _dt ) ;
2016-06-24 04:20:21 -03:00
if ( slew_yaw ) {
2022-01-27 17:03:32 -04:00
_euler_rate_target . z = constrain_float ( _euler_rate_target . z , - slew_yaw_max_rads , slew_yaw_max_rads ) ;
2016-06-24 04:20:21 -03:00
}
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
2024-02-19 19:46:36 -04:00
euler_rate_to_ang_vel ( _attitude_target , _euler_rate_target , _ang_vel_target ) ;
2017-11-15 09:45:34 -04:00
// Limit the angular velocity
2021-04-14 12:42:42 -03:00
ang_vel_limit ( _ang_vel_target , radians ( _ang_vel_roll_max ) , radians ( _ang_vel_pitch_max ) , radians ( _ang_vel_yaw_max ) ) ;
2017-11-15 09:45:34 -04:00
// Convert body-frame angular velocity into euler angle derivative of desired attitude
2024-02-19 19:46:36 -04:00
ang_vel_to_euler_rate ( _attitude_target , _ang_vel_target , _euler_rate_target ) ;
2014-06-07 02:27:39 -03:00
} else {
2016-06-24 04:20:21 -03:00
// When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
2021-04-14 12:42:42 -03:00
_euler_angle_target . x = euler_roll_angle ;
_euler_angle_target . y = euler_pitch_angle ;
2016-06-24 04:20:21 -03:00
if ( slew_yaw ) {
// Compute constrained angle error
2022-01-27 17:03:32 -04:00
float angle_error = constrain_float ( wrap_PI ( euler_yaw_angle - _euler_angle_target . z ) , - slew_yaw_max_rads * _dt , slew_yaw_max_rads * _dt ) ;
2016-06-24 04:20:21 -03:00
// Update attitude target from constrained angle error
2021-04-14 12:42:42 -03:00
_euler_angle_target . z = wrap_PI ( angle_error + _euler_angle_target . z ) ;
2016-06-24 04:20:21 -03:00
} else {
2021-04-14 12:42:42 -03:00
_euler_angle_target . z = euler_yaw_angle ;
2016-06-24 04:20:21 -03:00
}
// Compute quaternion target attitude
2021-04-14 12:42:42 -03:00
_attitude_target . from_euler ( _euler_angle_target . x , _euler_angle_target . y , _euler_angle_target . z ) ;
2014-01-11 04:02:42 -04:00
2016-06-24 04:20:21 -03:00
// Set rate feedforward requests to zero
2021-04-14 12:42:42 -03:00
_euler_rate_target . zero ( ) ;
_ang_vel_target . zero ( ) ;
2016-06-24 04:20:21 -03:00
}
2014-01-11 04:02:42 -04:00
2016-06-24 04:20:21 -03:00
// Call quaternion attitude controller
attitude_controller_run_quat ( ) ;
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
void AC_AttitudeControl : : 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
{
2015-11-24 23:28:42 -04:00
// Convert from centidegrees on public interface to radians
2019-04-19 08:36:42 -03:00
float euler_roll_rate = radians ( euler_roll_rate_cds * 0.01f ) ;
float euler_pitch_rate = radians ( euler_pitch_rate_cds * 0.01f ) ;
float euler_yaw_rate = radians ( euler_yaw_rate_cds * 0.01f ) ;
2015-11-24 17:11:50 -04:00
2016-06-24 04:20:21 -03:00
// calculate the attitude target euler angles
2023-04-15 20:59:20 -03:00
_attitude_target . to_euler ( _euler_angle_target ) ;
2015-10-13 16:02:21 -03:00
2017-12-31 19:29:28 -04:00
if ( _rate_bf_ff_enabled ) {
2016-06-24 04:20:21 -03:00
// translate the roll pitch and yaw acceleration limits to the euler axis
2024-02-19 19:46:36 -04:00
const Vector3f euler_accel = euler_accel_limit ( _attitude_target , Vector3f { get_accel_roll_max_radss ( ) , get_accel_pitch_max_radss ( ) , get_accel_yaw_max_radss ( ) } ) ;
2014-01-24 22:59:48 -04:00
2016-06-24 04:20:21 -03:00
// When acceleration limiting is enabled, the input shaper constrains angular acceleration, slewing
// the output rate towards the input rate.
2022-06-24 00:11:32 -03:00
_euler_rate_target . x = input_shaping_ang_vel ( _euler_rate_target . x , euler_roll_rate , euler_accel . x , _dt , _rate_rp_tc ) ;
_euler_rate_target . y = input_shaping_ang_vel ( _euler_rate_target . y , euler_pitch_rate , euler_accel . y , _dt , _rate_rp_tc ) ;
_euler_rate_target . z = input_shaping_ang_vel ( _euler_rate_target . z , euler_yaw_rate , euler_accel . z , _dt , _rate_y_tc ) ;
2014-03-29 08:41:22 -03:00
2016-06-24 04:20:21 -03:00
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
2024-02-19 19:46:36 -04:00
euler_rate_to_ang_vel ( _attitude_target , _euler_rate_target , _ang_vel_target ) ;
2016-06-24 04:20:21 -03:00
} else {
// When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
// Pitch angle is restricted to +- 85.0 degrees to avoid gimbal lock discontinuities.
2021-04-14 12:42:42 -03:00
_euler_angle_target . x = wrap_PI ( _euler_angle_target . x + euler_roll_rate * _dt ) ;
_euler_angle_target . y = constrain_float ( _euler_angle_target . y + euler_pitch_rate * _dt , radians ( - 85.0f ) , radians ( 85.0f ) ) ;
_euler_angle_target . z = wrap_2PI ( _euler_angle_target . z + euler_yaw_rate * _dt ) ;
2016-06-24 04:20:21 -03:00
// Set rate feedforward requests to zero
2021-04-14 12:42:42 -03:00
_euler_rate_target . zero ( ) ;
_ang_vel_target . zero ( ) ;
2016-06-24 04:20:21 -03:00
// Compute quaternion target attitude
2021-04-14 12:42:42 -03:00
_attitude_target . from_euler ( _euler_angle_target . x , _euler_angle_target . y , _euler_angle_target . z ) ;
2016-01-20 14:41:32 -04:00
}
2014-02-01 02:27:58 -04:00
2016-06-24 04:20:21 -03:00
// Call quaternion attitude controller
attitude_controller_run_quat ( ) ;
2013-10-12 09:28:18 -03:00
}
2016-06-24 04:20:21 -03:00
// Command an angular velocity with angular velocity feedforward and smoothing
void AC_AttitudeControl : : input_rate_bf_roll_pitch_yaw ( float roll_rate_bf_cds , float pitch_rate_bf_cds , float yaw_rate_bf_cds )
2013-10-12 09:28:18 -03:00
{
2015-11-24 23:28:42 -04:00
// Convert from centidegrees on public interface to radians
2019-04-19 08:36:42 -03:00
float roll_rate_rads = radians ( roll_rate_bf_cds * 0.01f ) ;
float pitch_rate_rads = radians ( pitch_rate_bf_cds * 0.01f ) ;
float yaw_rate_rads = radians ( yaw_rate_bf_cds * 0.01f ) ;
2015-02-11 08:10:56 -04:00
2016-06-24 04:20:21 -03:00
// calculate the attitude target euler angles
2023-04-15 20:59:20 -03:00
_attitude_target . to_euler ( _euler_angle_target ) ;
2014-07-12 08:46:13 -03:00
2017-12-31 19:29:28 -04:00
if ( _rate_bf_ff_enabled ) {
2018-01-26 23:36:42 -04:00
// Compute acceleration-limited body frame rates
2016-06-24 04:20:21 -03:00
// When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing
// the output rate towards the input rate.
2022-06-24 00:11:32 -03:00
_ang_vel_target . x = input_shaping_ang_vel ( _ang_vel_target . x , roll_rate_rads , get_accel_roll_max_radss ( ) , _dt , _rate_rp_tc ) ;
_ang_vel_target . y = input_shaping_ang_vel ( _ang_vel_target . y , pitch_rate_rads , get_accel_pitch_max_radss ( ) , _dt , _rate_rp_tc ) ;
_ang_vel_target . z = input_shaping_ang_vel ( _ang_vel_target . z , yaw_rate_rads , get_accel_yaw_max_radss ( ) , _dt , _rate_y_tc ) ;
2016-06-24 04:20:21 -03:00
// Convert body-frame angular velocity into euler angle derivative of desired attitude
2024-02-19 19:46:36 -04:00
ang_vel_to_euler_rate ( _attitude_target , _ang_vel_target , _euler_rate_target ) ;
2014-07-12 08:46:13 -03:00
} else {
2016-06-24 04:20:21 -03:00
// When feedforward is not enabled, the quaternion is calculated and is input into the target and the feedforward rate is zeroed.
2021-04-20 21:26:54 -03:00
Quaternion attitude_target_update ;
2021-04-15 09:24:43 -03:00
attitude_target_update . from_axis_angle ( Vector3f { roll_rate_rads * _dt , pitch_rate_rads * _dt , yaw_rate_rads * _dt } ) ;
2021-04-20 21:26:54 -03:00
_attitude_target = _attitude_target * attitude_target_update ;
2021-04-14 12:42:42 -03:00
_attitude_target . normalize ( ) ;
2016-06-24 04:20:21 -03:00
// Set rate feedforward requests to zero
2021-04-14 12:42:42 -03:00
_euler_rate_target . zero ( ) ;
_ang_vel_target . zero ( ) ;
2014-07-12 08:46:13 -03:00
}
2013-10-12 09:28:18 -03:00
2016-06-24 04:20:21 -03:00
// Call quaternion attitude controller
attitude_controller_run_quat ( ) ;
}
2014-06-06 00:59:16 -03:00
2017-12-31 19:53:29 -04:00
// Command an angular velocity with angular velocity smoothing using rate loops only with no attitude loop stabilization
void AC_AttitudeControl : : input_rate_bf_roll_pitch_yaw_2 ( float roll_rate_bf_cds , float pitch_rate_bf_cds , float yaw_rate_bf_cds )
{
// Convert from centidegrees on public interface to radians
2019-04-19 08:36:42 -03:00
float roll_rate_rads = radians ( roll_rate_bf_cds * 0.01f ) ;
float pitch_rate_rads = radians ( pitch_rate_bf_cds * 0.01f ) ;
float yaw_rate_rads = radians ( yaw_rate_bf_cds * 0.01f ) ;
2017-12-31 19:53:29 -04:00
// Compute acceleration-limited body frame rates
// When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing
// the output rate towards the input rate.
2022-06-24 00:11:32 -03:00
_ang_vel_target . x = input_shaping_ang_vel ( _ang_vel_target . x , roll_rate_rads , get_accel_roll_max_radss ( ) , _dt , _rate_rp_tc ) ;
_ang_vel_target . y = input_shaping_ang_vel ( _ang_vel_target . y , pitch_rate_rads , get_accel_pitch_max_radss ( ) , _dt , _rate_rp_tc ) ;
_ang_vel_target . z = input_shaping_ang_vel ( _ang_vel_target . z , yaw_rate_rads , get_accel_yaw_max_radss ( ) , _dt , _rate_y_tc ) ;
2017-12-31 19:53:29 -04:00
// Update the unused targets attitude based on current attitude to condition mode change
2021-04-14 12:42:42 -03:00
_ahrs . get_quat_body_to_ned ( _attitude_target ) ;
2023-04-15 20:59:20 -03:00
_attitude_target . to_euler ( _euler_angle_target ) ;
2017-12-31 19:53:29 -04:00
// Convert body-frame angular velocity into euler angle derivative of desired attitude
2024-02-19 19:46:36 -04:00
ang_vel_to_euler_rate ( _attitude_target , _ang_vel_target , _euler_rate_target ) ;
2021-04-14 12:42:42 -03:00
_ang_vel_body = _ang_vel_target ;
2017-12-31 19:53:29 -04:00
}
// Command an angular velocity with angular velocity smoothing using rate loops only with integrated rate error stabilization
void AC_AttitudeControl : : input_rate_bf_roll_pitch_yaw_3 ( float roll_rate_bf_cds , float pitch_rate_bf_cds , float yaw_rate_bf_cds )
{
// Convert from centidegrees on public interface to radians
2019-04-19 08:36:42 -03:00
float roll_rate_rads = radians ( roll_rate_bf_cds * 0.01f ) ;
float pitch_rate_rads = radians ( pitch_rate_bf_cds * 0.01f ) ;
float yaw_rate_rads = radians ( yaw_rate_bf_cds * 0.01f ) ;
2017-12-31 19:53:29 -04:00
// Update attitude error
2021-04-20 21:26:54 -03:00
Vector3f attitude_error ;
_attitude_ang_error . to_axis_angle ( attitude_error ) ;
2019-04-16 19:58:20 -03:00
2017-12-31 19:53:29 -04:00
Quaternion attitude_ang_error_update_quat ;
2019-04-16 19:58:20 -03:00
// limit the integrated error angle
2021-04-20 21:26:54 -03:00
float err_mag = attitude_error . length ( ) ;
2019-04-16 19:58:20 -03:00
if ( err_mag > AC_ATTITUDE_THRUST_ERROR_ANGLE ) {
2021-04-20 21:26:54 -03:00
attitude_error * = AC_ATTITUDE_THRUST_ERROR_ANGLE / err_mag ;
_attitude_ang_error . from_axis_angle ( attitude_error ) ;
2019-04-16 19:58:20 -03:00
}
Vector3f gyro_latest = _ahrs . get_gyro_latest ( ) ;
2021-04-15 09:24:43 -03:00
attitude_ang_error_update_quat . from_axis_angle ( Vector3f { ( _ang_vel_target . x - gyro_latest . x ) * _dt , ( _ang_vel_target . y - gyro_latest . y ) * _dt , ( _ang_vel_target . z - gyro_latest . z ) * _dt } ) ;
2017-12-31 19:53:29 -04:00
_attitude_ang_error = attitude_ang_error_update_quat * _attitude_ang_error ;
// Compute acceleration-limited body frame rates
// When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing
// the output rate towards the input rate.
2022-06-24 00:11:32 -03:00
_ang_vel_target . x = input_shaping_ang_vel ( _ang_vel_target . x , roll_rate_rads , get_accel_roll_max_radss ( ) , _dt , _rate_rp_tc ) ;
_ang_vel_target . y = input_shaping_ang_vel ( _ang_vel_target . y , pitch_rate_rads , get_accel_pitch_max_radss ( ) , _dt , _rate_rp_tc ) ;
_ang_vel_target . z = input_shaping_ang_vel ( _ang_vel_target . z , yaw_rate_rads , get_accel_yaw_max_radss ( ) , _dt , _rate_y_tc ) ;
2017-12-31 19:53:29 -04:00
2021-04-14 12:42:42 -03:00
// Retrieve quaternion body attitude
Quaternion attitude_body ;
_ahrs . get_quat_body_to_ned ( attitude_body ) ;
2017-12-31 19:53:29 -04:00
// Update the unused targets attitude based on current attitude to condition mode change
2021-04-14 12:42:42 -03:00
_attitude_target = attitude_body * _attitude_ang_error ;
2017-12-31 19:53:29 -04:00
// calculate the attitude target euler angles
2023-04-15 20:59:20 -03:00
_attitude_target . to_euler ( _euler_angle_target ) ;
2017-12-31 19:53:29 -04:00
// Convert body-frame angular velocity into euler angle derivative of desired attitude
2024-02-19 19:46:36 -04:00
ang_vel_to_euler_rate ( _attitude_target , _ang_vel_target , _euler_rate_target ) ;
2017-12-31 19:53:29 -04:00
// Compute the angular velocity target from the integrated rate error
2021-04-20 21:26:54 -03:00
_attitude_ang_error . to_axis_angle ( attitude_error ) ;
_ang_vel_body = update_ang_vel_target_from_att_error ( attitude_error ) ;
2021-04-14 12:42:42 -03:00
_ang_vel_body + = _ang_vel_target ;
2017-12-31 19:53:29 -04:00
// ensure Quaternions stay normalized
_attitude_ang_error . normalize ( ) ;
}
2017-06-15 10:10:01 -03:00
// Command an angular step (i.e change) in body frame angle
// Used to command a step in angle without exciting the orthogonal axis during autotune
void AC_AttitudeControl : : 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 )
{
// Convert from centidegrees on public interface to radians
2019-04-19 08:36:42 -03:00
float roll_step_rads = radians ( roll_angle_step_bf_cd * 0.01f ) ;
float pitch_step_rads = radians ( pitch_angle_step_bf_cd * 0.01f ) ;
float yaw_step_rads = radians ( yaw_angle_step_bf_cd * 0.01f ) ;
2017-06-15 10:10:01 -03:00
// rotate attitude target by desired step
2021-04-20 21:26:54 -03:00
Quaternion attitude_target_update ;
2021-04-15 09:24:43 -03:00
attitude_target_update . from_axis_angle ( Vector3f { roll_step_rads , pitch_step_rads , yaw_step_rads } ) ;
2021-04-20 21:26:54 -03:00
_attitude_target = _attitude_target * attitude_target_update ;
2021-04-14 12:42:42 -03:00
_attitude_target . normalize ( ) ;
2017-06-15 10:10:01 -03:00
// calculate the attitude target euler angles
2023-04-15 20:59:20 -03:00
_attitude_target . to_euler ( _euler_angle_target ) ;
2017-06-15 10:10:01 -03:00
// Set rate feedforward requests to zero
2021-04-14 12:42:42 -03:00
_euler_rate_target . zero ( ) ;
_ang_vel_target . zero ( ) ;
2017-06-15 10:10:01 -03:00
// Call quaternion attitude controller
attitude_controller_run_quat ( ) ;
}
2021-06-20 13:56:43 -03:00
// Command a thrust vector and heading rate
2022-05-14 00:55:34 -03:00
void AC_AttitudeControl : : input_thrust_vector_rate_heading ( const Vector3f & thrust_vector , float heading_rate_cds , bool slew_yaw )
2021-04-15 09:24:43 -03:00
{
// Convert from centidegrees on public interface to radians
2022-05-14 00:55:34 -03:00
float heading_rate = radians ( heading_rate_cds * 0.01f ) ;
if ( slew_yaw ) {
2022-05-14 21:52:44 -03:00
// a zero _angle_vel_yaw_max means that setting is disabled
const float slew_yaw_max_rads = get_slew_yaw_max_rads ( ) ;
2022-05-14 00:55:34 -03:00
heading_rate = constrain_float ( heading_rate , - slew_yaw_max_rads , slew_yaw_max_rads ) ;
}
2021-04-15 09:24:43 -03:00
// calculate the attitude target euler angles
2023-04-15 20:59:20 -03:00
_attitude_target . to_euler ( _euler_angle_target ) ;
2021-04-15 09:24:43 -03:00
// convert thrust vector to a quaternion attitude
Quaternion thrust_vec_quat = attitude_from_thrust_vector ( thrust_vector , 0.0f ) ;
// calculate the angle error in x and y.
float thrust_vector_diff_angle ;
Quaternion thrust_vec_correction_quat ;
Vector3f attitude_error ;
float returned_thrust_vector_angle ;
thrust_vector_rotation_angles ( thrust_vec_quat , _attitude_target , thrust_vec_correction_quat , attitude_error , returned_thrust_vector_angle , thrust_vector_diff_angle ) ;
if ( _rate_bf_ff_enabled ) {
// When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
// the output rate towards the input rate.
_ang_vel_target . x = input_shaping_angle ( attitude_error . x , _input_tc , get_accel_roll_max_radss ( ) , _ang_vel_target . x , _dt ) ;
_ang_vel_target . y = input_shaping_angle ( attitude_error . y , _input_tc , get_accel_pitch_max_radss ( ) , _ang_vel_target . y , _dt ) ;
// When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
// the output rate towards the input rate.
2022-06-24 00:11:32 -03:00
_ang_vel_target . z = input_shaping_ang_vel ( _ang_vel_target . z , heading_rate , get_accel_yaw_max_radss ( ) , _dt , _rate_y_tc ) ;
2021-04-15 09:24:43 -03:00
// Limit the angular velocity
2022-05-14 00:55:34 -03:00
ang_vel_limit ( _ang_vel_target , radians ( _ang_vel_roll_max ) , radians ( _ang_vel_pitch_max ) , radians ( _ang_vel_yaw_max ) ) ;
2021-04-15 09:24:43 -03:00
} else {
Quaternion yaw_quat ;
yaw_quat . from_axis_angle ( Vector3f { 0.0f , 0.0f , heading_rate * _dt } ) ;
_attitude_target = _attitude_target * thrust_vec_correction_quat * yaw_quat ;
// Set rate feedforward requests to zero
_euler_rate_target . zero ( ) ;
_ang_vel_target . zero ( ) ;
}
// Convert body-frame angular velocity into euler angle derivative of desired attitude
2024-02-19 19:46:36 -04:00
ang_vel_to_euler_rate ( _attitude_target , _ang_vel_target , _euler_rate_target ) ;
2021-04-15 09:24:43 -03:00
// Call quaternion attitude controller
attitude_controller_run_quat ( ) ;
}
2021-06-20 13:56:43 -03:00
// Command a thrust vector, heading and heading rate
2021-04-15 09:24:43 -03:00
void AC_AttitudeControl : : input_thrust_vector_heading ( const Vector3f & thrust_vector , float heading_angle_cd , float heading_rate_cds )
{
2021-08-15 19:06:07 -03:00
// a zero _angle_vel_yaw_max means that setting is disabled
2022-01-27 17:03:32 -04:00
const float slew_yaw_max_rads = get_slew_yaw_max_rads ( ) ;
2021-08-15 19:06:07 -03:00
2021-04-15 09:24:43 -03:00
// Convert from centidegrees on public interface to radians
2022-01-27 17:03:32 -04:00
float heading_rate = constrain_float ( radians ( heading_rate_cds * 0.01f ) , - slew_yaw_max_rads , slew_yaw_max_rads ) ;
2021-04-15 09:24:43 -03:00
float heading_angle = radians ( heading_angle_cd * 0.01f ) ;
// calculate the attitude target euler angles
2023-04-15 20:59:20 -03:00
_attitude_target . to_euler ( _euler_angle_target ) ;
2021-04-15 09:24:43 -03:00
// convert thrust vector and heading to a quaternion attitude
const Quaternion desired_attitude_quat = attitude_from_thrust_vector ( thrust_vector , heading_angle ) ;
if ( _rate_bf_ff_enabled ) {
// calculate the angle error in x and y.
Vector3f attitude_error ;
float thrust_vector_diff_angle ;
Quaternion thrust_vec_correction_quat ;
float returned_thrust_vector_angle ;
thrust_vector_rotation_angles ( desired_attitude_quat , _attitude_target , thrust_vec_correction_quat , attitude_error , returned_thrust_vector_angle , thrust_vector_diff_angle ) ;
// When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
// the output rate towards the input rate.
_ang_vel_target . x = input_shaping_angle ( attitude_error . x , _input_tc , get_accel_roll_max_radss ( ) , _ang_vel_target . x , _dt ) ;
_ang_vel_target . y = input_shaping_angle ( attitude_error . y , _input_tc , get_accel_pitch_max_radss ( ) , _ang_vel_target . y , _dt ) ;
2022-01-27 17:03:32 -04:00
_ang_vel_target . z = input_shaping_angle ( attitude_error . z , _input_tc , get_accel_yaw_max_radss ( ) , _ang_vel_target . z , heading_rate , slew_yaw_max_rads , _dt ) ;
2021-04-15 09:24:43 -03:00
// Limit the angular velocity
2022-01-27 17:03:32 -04:00
ang_vel_limit ( _ang_vel_target , radians ( _ang_vel_roll_max ) , radians ( _ang_vel_pitch_max ) , slew_yaw_max_rads ) ;
2021-04-15 09:24:43 -03:00
} else {
// set persisted quaternion target attitude
_attitude_target = desired_attitude_quat ;
// Set rate feedforward requests to zero
_euler_rate_target . zero ( ) ;
_ang_vel_target . zero ( ) ;
}
// Convert body-frame angular velocity into euler angle derivative of desired attitude
2024-02-19 19:46:36 -04:00
ang_vel_to_euler_rate ( _attitude_target , _ang_vel_target , _euler_rate_target ) ;
2021-04-15 09:24:43 -03:00
// Call quaternion attitude controller
attitude_controller_run_quat ( ) ;
}
2022-09-06 05:47:56 -03:00
// Command a thrust vector and heading rate
void AC_AttitudeControl : : input_thrust_vector_heading ( const Vector3f & thrust_vector , HeadingCommand heading )
{
switch ( heading . heading_mode ) {
case HeadingMode : : Rate_Only :
input_thrust_vector_rate_heading ( thrust_vector , heading . yaw_rate_cds ) ;
break ;
case HeadingMode : : Angle_Only :
input_thrust_vector_heading ( thrust_vector , heading . yaw_angle_cd , 0.0 ) ;
break ;
case HeadingMode : : Angle_And_Rate :
input_thrust_vector_heading ( thrust_vector , heading . yaw_angle_cd , heading . yaw_rate_cds ) ;
break ;
}
}
2021-04-15 09:24:43 -03:00
Quaternion AC_AttitudeControl : : attitude_from_thrust_vector ( Vector3f thrust_vector , float heading_angle ) const
{
const Vector3f thrust_vector_up { 0.0f , 0.0f , - 1.0f } ;
if ( is_zero ( thrust_vector . length_squared ( ) ) ) {
thrust_vector = thrust_vector_up ;
} else {
thrust_vector . normalize ( ) ;
}
// the cross product of the desired and target thrust vector defines the rotation vector
Vector3f thrust_vec_cross = thrust_vector_up % thrust_vector ;
// the dot product is used to calculate the angle between the target and desired thrust vectors
const float thrust_vector_angle = acosf ( constrain_float ( thrust_vector_up * thrust_vector , - 1.0f , 1.0f ) ) ;
// Normalize the thrust rotation vector
const float thrust_vector_length = thrust_vec_cross . length ( ) ;
if ( is_zero ( thrust_vector_length ) | | is_zero ( thrust_vector_angle ) ) {
thrust_vec_cross = thrust_vector_up ;
} else {
thrust_vec_cross / = thrust_vector_length ;
}
Quaternion thrust_vec_quat ;
thrust_vec_quat . from_axis_angle ( thrust_vec_cross , thrust_vector_angle ) ;
Quaternion yaw_quat ;
yaw_quat . from_axis_angle ( Vector3f { 0.0f , 0.0f , 1.0f } , heading_angle ) ;
return thrust_vec_quat * yaw_quat ;
}
2016-06-24 04:20:21 -03:00
// Calculates the body frame angular velocities to follow the target attitude
void AC_AttitudeControl : : attitude_controller_run_quat ( )
{
2021-04-14 12:42:42 -03:00
// This represents a quaternion rotation in NED frame to the body
Quaternion attitude_body ;
_ahrs . get_quat_body_to_ned ( attitude_body ) ;
2014-02-09 22:59:51 -04:00
2021-04-14 12:42:42 -03:00
// This vector represents the angular error to rotate the thrust vector using x and y and heading using z
2021-04-20 21:26:54 -03:00
Vector3f attitude_error ;
thrust_heading_rotation_angles ( _attitude_target , attitude_body , attitude_error , _thrust_angle , _thrust_error_angle ) ;
2014-02-09 22:59:51 -04:00
2021-04-14 12:42:42 -03:00
// Compute the angular velocity corrections in the body frame from the attitude error
2021-04-20 21:26:54 -03:00
_ang_vel_body = update_ang_vel_target_from_att_error ( attitude_error ) ;
2013-10-12 09:28:18 -03:00
2020-05-22 06:37:24 -03:00
// ensure angular velocity does not go over configured limits
2021-04-14 12:42:42 -03:00
ang_vel_limit ( _ang_vel_body , radians ( _ang_vel_roll_max ) , radians ( _ang_vel_pitch_max ) , radians ( _ang_vel_yaw_max ) ) ;
2017-11-15 09:45:34 -04:00
2021-04-15 14:53:55 -03:00
// rotation from the target frame to the body frame
2021-04-14 12:42:42 -03:00
Quaternion rotation_target_to_body = attitude_body . inverse ( ) * _attitude_target ;
2021-04-14 12:20:26 -03:00
2021-04-15 14:53:55 -03:00
// target angle velocity vector in the body frame
2021-04-14 12:42:42 -03:00
Vector3f ang_vel_body_feedforward = rotation_target_to_body * _ang_vel_target ;
2016-06-24 04:20:21 -03:00
// Correct the thrust vector and smoothly add feedforward and yaw input
2019-07-09 09:42:16 -03:00
_feedforward_scalar = 1.0f ;
2019-04-19 08:36:42 -03:00
if ( _thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE * 2.0f ) {
2021-04-14 12:42:42 -03:00
_ang_vel_body . z = _ahrs . get_gyro ( ) . z ;
2019-04-19 08:36:42 -03:00
} else if ( _thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE ) {
2019-07-09 09:42:16 -03:00
_feedforward_scalar = ( 1.0f - ( _thrust_error_angle - AC_ATTITUDE_THRUST_ERROR_ANGLE ) / AC_ATTITUDE_THRUST_ERROR_ANGLE ) ;
2021-04-14 12:42:42 -03:00
_ang_vel_body . x + = ang_vel_body_feedforward . x * _feedforward_scalar ;
_ang_vel_body . y + = ang_vel_body_feedforward . y * _feedforward_scalar ;
_ang_vel_body . z + = ang_vel_body_feedforward . z ;
_ang_vel_body . z = _ahrs . get_gyro ( ) . z * ( 1.0 - _feedforward_scalar ) + _ang_vel_body . z * _feedforward_scalar ;
2014-08-05 08:59:25 -03:00
} else {
2021-04-14 12:42:42 -03:00
_ang_vel_body + = ang_vel_body_feedforward ;
2014-08-05 08:59:25 -03:00
}
2017-12-31 19:29:28 -04:00
if ( _rate_bf_ff_enabled ) {
2016-06-24 04:20:21 -03:00
// rotate target and normalize
2021-04-14 12:42:42 -03:00
Quaternion attitude_target_update ;
2021-04-15 09:24:43 -03:00
attitude_target_update . from_axis_angle ( Vector3f { _ang_vel_target . x * _dt , _ang_vel_target . y * _dt , _ang_vel_target . z * _dt } ) ;
2021-04-14 12:42:42 -03:00
_attitude_target = _attitude_target * attitude_target_update ;
2014-08-05 08:59:25 -03:00
}
2017-12-31 19:53:29 -04:00
2021-04-14 12:20:26 -03:00
// ensure Quaternion stay normalised
2021-04-14 12:42:42 -03:00
_attitude_target . normalize ( ) ;
2017-12-31 19:53:29 -04:00
// Record error to handle EKF resets
2021-04-14 12:42:42 -03:00
_attitude_ang_error = attitude_body . inverse ( ) * _attitude_target ;
2016-06-24 04:20:21 -03:00
}
2014-08-05 08:59:25 -03:00
2021-04-14 12:42:42 -03:00
// thrust_heading_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion.
2023-09-11 20:01:11 -03:00
// The maximum error in the yaw axis is limited based on static output saturation.
2021-04-15 09:24:43 -03:00
void AC_AttitudeControl : : thrust_heading_rotation_angles ( Quaternion & attitude_target , const Quaternion & attitude_body , Vector3f & attitude_error , float & thrust_angle , float & thrust_error_angle ) const
2021-04-14 12:20:26 -03:00
{
2021-04-20 21:26:54 -03:00
Quaternion thrust_vector_correction ;
thrust_vector_rotation_angles ( attitude_target , attitude_body , thrust_vector_correction , attitude_error , thrust_angle , thrust_error_angle ) ;
2021-04-14 12:20:26 -03:00
// Todo: Limit roll an pitch error based on output saturation and maximum error.
2023-09-11 20:01:11 -03:00
// Limit Yaw Error based to the maximum that would saturate the output when yaw rate is zero.
Quaternion heading_vec_correction_quat ;
float heading_accel_max = constrain_float ( get_accel_yaw_max_radss ( ) / 2.0f , AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS , AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS ) ;
if ( ! is_zero ( get_rate_yaw_pid ( ) . kP ( ) ) ) {
float heading_error_max = MIN ( inv_sqrt_controller ( 1.0 / get_rate_yaw_pid ( ) . kP ( ) , _p_angle_yaw . kP ( ) , heading_accel_max ) , AC_ATTITUDE_YAW_MAX_ERROR_ANGLE ) ;
if ( ! is_zero ( _p_angle_yaw . kP ( ) ) & & fabsf ( attitude_error . z ) > heading_error_max ) {
attitude_error . z = constrain_float ( wrap_PI ( attitude_error . z ) , - heading_error_max , heading_error_max ) ;
heading_vec_correction_quat . from_axis_angle ( Vector3f { 0.0f , 0.0f , attitude_error . z } ) ;
attitude_target = attitude_body * thrust_vector_correction * heading_vec_correction_quat ;
}
2021-04-14 12:20:26 -03:00
}
}
2021-04-14 12:42:42 -03:00
// thrust_vector_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion.
2016-06-24 04:20:21 -03:00
// The first rotation corrects the thrust vector and the second rotation corrects the heading vector.
2021-04-15 09:24:43 -03:00
void AC_AttitudeControl : : thrust_vector_rotation_angles ( const Quaternion & attitude_target , const Quaternion & attitude_body , Quaternion & thrust_vector_correction , Vector3f & attitude_error , float & thrust_angle , float & thrust_error_angle ) const
2016-06-24 04:20:21 -03:00
{
2021-04-15 14:53:55 -03:00
// The direction of thrust is [0,0,-1] is any body-fixed frame, inc. body frame and target frame.
2021-04-15 09:24:43 -03:00
const Vector3f thrust_vector_up { 0.0f , 0.0f , - 1.0f } ;
2023-10-11 04:41:49 -03:00
// attitude_target and attitude_body are passive rotations from target / body frames to the NED frame
2021-04-15 14:53:55 -03:00
// Rotating [0,0,-1] by attitude_target expresses (gets a view of) the target thrust vector in the inertial frame
2021-04-15 09:24:43 -03:00
Vector3f att_target_thrust_vec = attitude_target * thrust_vector_up ; // target thrust vector
2015-11-25 16:29:27 -04:00
2021-04-15 14:53:55 -03:00
// Rotating [0,0,-1] by attitude_target expresses (gets a view of) the current thrust vector in the inertial frame
2021-04-15 09:24:43 -03:00
Vector3f att_body_thrust_vec = attitude_body * thrust_vector_up ; // current thrust vector
2015-11-25 16:29:27 -04:00
2019-07-29 04:56:03 -03:00
// the dot product is used to calculate the current lean angle for use of external functions
2021-04-15 09:24:43 -03:00
thrust_angle = acosf ( constrain_float ( thrust_vector_up * att_body_thrust_vec , - 1.0f , 1.0f ) ) ;
2019-07-29 04:56:03 -03:00
2016-06-24 04:20:21 -03:00
// the cross product of the desired and target thrust vector defines the rotation vector
2021-04-14 12:42:42 -03:00
Vector3f thrust_vec_cross = att_body_thrust_vec % att_target_thrust_vec ;
2016-01-20 14:41:32 -04:00
2016-06-24 04:20:21 -03:00
// the dot product is used to calculate the angle between the target and desired thrust vectors
2021-04-14 12:42:42 -03:00
thrust_error_angle = acosf ( constrain_float ( att_body_thrust_vec * att_target_thrust_vec , - 1.0f , 1.0f ) ) ;
2016-06-24 04:20:21 -03:00
// Normalize the thrust rotation vector
float thrust_vector_length = thrust_vec_cross . length ( ) ;
2021-02-09 20:30:13 -04:00
if ( is_zero ( thrust_vector_length ) | | is_zero ( thrust_error_angle ) ) {
2021-04-15 09:24:43 -03:00
thrust_vec_cross = thrust_vector_up ;
2019-04-19 08:36:42 -03:00
} else {
2016-06-24 04:20:21 -03:00
thrust_vec_cross / = thrust_vector_length ;
}
2017-12-30 23:44:54 -04:00
2021-04-15 14:53:55 -03:00
// thrust_vector_correction is defined relative to the body frame but its axis `thrust_vec_cross` was computed in
// the inertial frame. First rotate it by the inverse of attitude_body to express it back in the body frame
2021-04-14 12:42:42 -03:00
thrust_vec_cross = attitude_body . inverse ( ) * thrust_vec_cross ;
thrust_vector_correction . from_axis_angle ( thrust_vec_cross , thrust_error_angle ) ;
2016-06-24 04:20:21 -03:00
2017-12-30 23:44:54 -04:00
// calculate the angle error in x and y.
2016-06-24 04:20:21 -03:00
Vector3f rotation ;
2021-04-14 12:42:42 -03:00
thrust_vector_correction . to_axis_angle ( rotation ) ;
attitude_error . x = rotation . x ;
attitude_error . y = rotation . y ;
2016-06-24 04:20:21 -03:00
2021-04-14 12:42:42 -03:00
// calculate the remaining rotation required after thrust vector is rotated transformed to the body frame
// heading_vector_correction
Quaternion heading_vec_correction_quat = thrust_vector_correction . inverse ( ) * attitude_body . inverse ( ) * attitude_target ;
2021-04-14 12:20:26 -03:00
2017-12-30 23:44:54 -04:00
// calculate the angle error in z (x and y should be zero here).
2021-04-14 12:20:26 -03:00
heading_vec_correction_quat . to_axis_angle ( rotation ) ;
2021-04-14 12:42:42 -03:00
attitude_error . z = rotation . z ;
2015-11-30 17:33:07 -04:00
}
2016-06-24 04:20:21 -03:00
// calculates the velocity correction from an angle error. The angular velocity has acceleration and
2018-01-26 01:56:54 -04:00
// deceleration limits including basic jerk limiting using _input_tc
2021-04-15 09:24:43 -03:00
float AC_AttitudeControl : : input_shaping_angle ( float error_angle , float input_tc , float accel_max , float target_ang_vel , float desired_ang_vel , float max_ang_vel , float dt )
2015-11-30 17:33:07 -04:00
{
2016-06-24 04:20:21 -03:00
// Calculate the velocity as error approaches zero with acceleration limited by accel_max_radss
2021-04-15 09:24:43 -03:00
desired_ang_vel + = sqrt_controller ( error_angle , 1.0f / MAX ( input_tc , 0.01f ) , accel_max , dt ) ;
2021-05-12 00:58:20 -03:00
if ( is_positive ( max_ang_vel ) ) {
2021-04-15 09:24:43 -03:00
desired_ang_vel = constrain_float ( desired_ang_vel , - max_ang_vel , max_ang_vel ) ;
}
2017-11-15 09:45:34 -04:00
// Acceleration is limited directly to smooth the beginning of the curve.
2022-06-24 00:11:32 -03:00
return input_shaping_ang_vel ( target_ang_vel , desired_ang_vel , accel_max , dt , 0.0f ) ;
2017-11-15 09:45:34 -04:00
}
2016-01-20 14:41:32 -04:00
2022-06-24 00:11:32 -03:00
// Shapes the velocity request based on a rate time constant. The angular acceleration and deceleration is limited.
float AC_AttitudeControl : : input_shaping_ang_vel ( float target_ang_vel , float desired_ang_vel , float accel_max , float dt , float input_tc )
2017-11-15 09:45:34 -04:00
{
2022-06-24 00:11:32 -03:00
if ( is_positive ( input_tc ) ) {
// Calculate the acceleration to smoothly achieve rate. Jerk is not limited.
float error_rate = desired_ang_vel - target_ang_vel ;
float desired_ang_accel = sqrt_controller ( error_rate , 1.0f / MAX ( input_tc , 0.01f ) , 0.0f , dt ) ;
desired_ang_vel = target_ang_vel + desired_ang_accel * dt ;
}
2016-06-24 04:20:21 -03:00
// Acceleration is limited directly to smooth the beginning of the curve.
2018-02-02 03:00:07 -04:00
if ( is_positive ( accel_max ) ) {
2017-06-24 23:35:13 -03:00
float delta_ang_vel = accel_max * dt ;
2019-04-19 08:36:42 -03:00
return constrain_float ( desired_ang_vel , target_ang_vel - delta_ang_vel , target_ang_vel + delta_ang_vel ) ;
2016-11-23 01:43:19 -04:00
} else {
2017-11-15 09:45:34 -04:00
return desired_ang_vel ;
2016-11-23 01:43:19 -04:00
}
2016-01-20 14:41:32 -04:00
}
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.
2018-12-20 05:10:02 -04:00
void AC_AttitudeControl : : input_shaping_rate_predictor ( const Vector2f & error_angle , Vector2f & target_ang_vel , float dt ) const
2017-11-10 11:22:57 -04:00
{
if ( _rate_bf_ff_enabled ) {
// translate the roll pitch and yaw acceleration limits to the euler axis
2018-01-26 01:56:54 -04:00
target_ang_vel . x = input_shaping_angle ( wrap_PI ( error_angle . x ) , _input_tc , get_accel_roll_max_radss ( ) , target_ang_vel . x , dt ) ;
target_ang_vel . y = input_shaping_angle ( wrap_PI ( error_angle . y ) , _input_tc , get_accel_pitch_max_radss ( ) , target_ang_vel . y , dt ) ;
2017-11-10 11:22:57 -04:00
} else {
2022-10-10 19:14:35 -03:00
const float angleP_roll = _p_angle_roll . kP ( ) * _angle_P_scale . x ;
const float angleP_pitch = _p_angle_pitch . kP ( ) * _angle_P_scale . y ;
target_ang_vel . x = angleP_roll * wrap_PI ( error_angle . x ) ;
target_ang_vel . y = angleP_pitch * wrap_PI ( error_angle . y ) ;
2017-11-10 11:22:57 -04:00
}
2017-11-15 09:45:34 -04:00
// Limit the angular velocity correction
Vector3f ang_vel ( target_ang_vel . x , target_ang_vel . y , 0.0f ) ;
ang_vel_limit ( ang_vel , radians ( _ang_vel_roll_max ) , radians ( _ang_vel_pitch_max ) , 0.0f ) ;
2019-04-19 08:36:42 -03:00
target_ang_vel . x = ang_vel . x ;
target_ang_vel . y = ang_vel . y ;
2017-11-10 11:22:57 -04:00
}
2017-11-15 09:45:34 -04:00
// limits angular velocity
void AC_AttitudeControl : : ang_vel_limit ( Vector3f & euler_rad , float ang_vel_roll_max , float ang_vel_pitch_max , float ang_vel_yaw_max ) const
2016-01-20 14:41:32 -04:00
{
2017-11-15 09:45:34 -04:00
if ( is_zero ( ang_vel_roll_max ) | | is_zero ( ang_vel_pitch_max ) ) {
if ( ! is_zero ( ang_vel_roll_max ) ) {
euler_rad . x = constrain_float ( euler_rad . x , - ang_vel_roll_max , ang_vel_roll_max ) ;
}
if ( ! is_zero ( ang_vel_pitch_max ) ) {
euler_rad . y = constrain_float ( euler_rad . y , - ang_vel_pitch_max , ang_vel_pitch_max ) ;
}
2016-06-24 04:20:21 -03:00
} else {
2019-04-19 08:36:42 -03:00
Vector2f thrust_vector_ang_vel ( euler_rad . x / ang_vel_roll_max , euler_rad . y / ang_vel_pitch_max ) ;
2017-11-15 09:45:34 -04:00
float thrust_vector_length = thrust_vector_ang_vel . length ( ) ;
if ( thrust_vector_length > 1.0f ) {
euler_rad . x = thrust_vector_ang_vel . x * ang_vel_roll_max / thrust_vector_length ;
euler_rad . y = thrust_vector_ang_vel . y * ang_vel_pitch_max / thrust_vector_length ;
}
}
if ( ! is_zero ( ang_vel_yaw_max ) ) {
euler_rad . z = constrain_float ( euler_rad . z , - ang_vel_yaw_max , ang_vel_yaw_max ) ;
2016-06-24 04:20:21 -03:00
}
2016-01-20 14:41:32 -04:00
}
2016-06-24 04:20:21 -03:00
// translates body frame acceleration limits to the euler axis
2024-02-19 19:46:36 -04:00
Vector3f AC_AttitudeControl : : euler_accel_limit ( const Quaternion & att , const Vector3f & euler_accel )
2016-01-20 14:41:32 -04:00
{
2024-02-19 19:46:36 -04:00
if ( ! is_positive ( euler_accel . x ) | | ! is_positive ( euler_accel . y ) | | ! is_positive ( euler_accel . z ) ) {
return Vector3f { euler_accel } ;
2016-06-24 04:20:21 -03:00
}
2024-02-19 19:46:36 -04:00
const float phi = att . get_euler_roll ( ) ;
const float theta = att . get_euler_pitch ( ) ;
const float sin_phi = constrain_float ( fabsf ( sinf ( phi ) ) , 0.1f , 1.0f ) ;
const float cos_phi = constrain_float ( fabsf ( cosf ( phi ) ) , 0.1f , 1.0f ) ;
const float sin_theta = constrain_float ( fabsf ( sinf ( theta ) ) , 0.1f , 1.0f ) ;
const float cos_theta = constrain_float ( fabsf ( cosf ( theta ) ) , 0.1f , 1.0f ) ;
return Vector3f {
euler_accel . x ,
MIN ( euler_accel . y / cos_phi , euler_accel . z / sin_phi ) ,
MIN ( MIN ( euler_accel . x / sin_theta , euler_accel . y / ( sin_phi * cos_theta ) ) , euler_accel . z / ( cos_phi * cos_theta ) )
} ;
2016-06-24 04:20:21 -03:00
}
2013-10-12 09:28:18 -03:00
2021-05-24 20:38:16 -03:00
// Sets attitude target to vehicle attitude and sets all rates to zero
2021-07-09 09:11:33 -03:00
// If reset_rate is false rates are not reset to allow the rate controllers to run
void AC_AttitudeControl : : reset_target_and_rate ( bool reset_rate )
2021-05-24 20:38:16 -03:00
{
// move attitude target to current attitude
_ahrs . get_quat_body_to_ned ( _attitude_target ) ;
2024-02-18 09:00:31 -04:00
_attitude_target . to_euler ( _euler_angle_target ) ;
2021-05-24 20:38:16 -03:00
2021-07-09 09:11:33 -03:00
if ( reset_rate ) {
_ang_vel_target . zero ( ) ;
2024-02-18 09:00:31 -04:00
_euler_rate_target . zero ( ) ;
2021-07-09 09:11:33 -03:00
}
2021-05-24 20:38:16 -03:00
}
2021-05-24 10:31:40 -03:00
// Sets yaw target to vehicle heading and sets yaw rate to zero
2021-07-09 09:11:33 -03:00
// If reset_rate is false rates are not reset to allow the rate controllers to run
void AC_AttitudeControl : : reset_yaw_target_and_rate ( bool reset_rate )
2016-06-24 04:20:21 -03:00
{
2021-05-24 10:31:40 -03:00
// move attitude target to current heading
float yaw_shift = _ahrs . yaw - _euler_angle_target . z ;
2021-04-20 21:26:54 -03:00
Quaternion _attitude_target_update ;
2021-04-15 09:24:43 -03:00
_attitude_target_update . from_axis_angle ( Vector3f { 0.0f , 0.0f , yaw_shift } ) ;
2021-04-20 21:26:54 -03:00
_attitude_target = _attitude_target_update * _attitude_target ;
2021-05-24 10:31:40 -03:00
2021-07-09 09:11:33 -03:00
if ( reset_rate ) {
// set yaw rate to zero
_euler_rate_target . z = 0.0f ;
2021-05-24 10:31:40 -03:00
2021-07-09 09:11:33 -03:00
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
2024-02-19 19:46:36 -04:00
euler_rate_to_ang_vel ( _attitude_target , _euler_rate_target , _ang_vel_target ) ;
2021-07-09 09:11:33 -03:00
}
2014-02-09 22:59:51 -04:00
}
2013-10-12 09:28:18 -03:00
2021-02-13 05:15:45 -04:00
// Shifts the target attitude to maintain the current error in the event of an EKF reset
2017-12-31 19:53:29 -04:00
void AC_AttitudeControl : : inertial_frame_reset ( )
{
2021-04-14 12:42:42 -03:00
// Retrieve quaternion body attitude
Quaternion attitude_body ;
_ahrs . get_quat_body_to_ned ( attitude_body ) ;
2017-12-31 19:53:29 -04:00
// Recalculate the target quaternion
2021-04-14 12:42:42 -03:00
_attitude_target = attitude_body * _attitude_ang_error ;
2017-12-31 19:53:29 -04:00
// calculate the attitude target euler angles
2023-04-15 20:59:20 -03:00
_attitude_target . to_euler ( _euler_angle_target ) ;
2017-12-31 19:53:29 -04:00
}
2016-06-24 04:20:21 -03:00
// Convert a 321-intrinsic euler angle derivative to an angular velocity vector
2024-02-19 19:46:36 -04:00
void AC_AttitudeControl : : euler_rate_to_ang_vel ( const Quaternion & att , const Vector3f & euler_rate_rads , Vector3f & ang_vel_rads )
2014-02-09 22:59:51 -04:00
{
2024-02-19 19:46:36 -04:00
const float theta = att . get_euler_pitch ( ) ;
const float phi = att . get_euler_roll ( ) ;
const float sin_theta = sinf ( theta ) ;
const float cos_theta = cosf ( theta ) ;
const float sin_phi = sinf ( phi ) ;
const float cos_phi = cosf ( phi ) ;
2015-11-24 20:18:35 -04:00
2015-12-08 15:15:10 -04:00
ang_vel_rads . x = euler_rate_rads . x - sin_theta * euler_rate_rads . z ;
2019-04-19 08:36:42 -03:00
ang_vel_rads . y = cos_phi * euler_rate_rads . y + sin_phi * cos_theta * euler_rate_rads . z ;
2015-12-08 15:15:10 -04:00
ang_vel_rads . z = - sin_phi * euler_rate_rads . y + cos_theta * cos_phi * euler_rate_rads . z ;
2014-02-09 22:59:51 -04:00
}
2016-06-24 04:20:21 -03: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
2024-02-19 19:46:36 -04:00
bool AC_AttitudeControl : : ang_vel_to_euler_rate ( const Quaternion & att , const Vector3f & ang_vel_rads , Vector3f & euler_rate_rads )
2014-02-09 22:59:51 -04:00
{
2024-02-19 19:46:36 -04:00
const float theta = att . get_euler_pitch ( ) ;
const float phi = att . get_euler_roll ( ) ;
const float sin_theta = sinf ( theta ) ;
const float cos_theta = cosf ( theta ) ;
const float sin_phi = sinf ( phi ) ;
const float cos_phi = cosf ( phi ) ;
2015-11-24 20:18:35 -04:00
2015-11-24 23:28:42 -04:00
// When the vehicle pitches all the way up or all the way down, the euler angles become discontinuous. In this case, we just return false.
2015-11-24 20:18:35 -04:00
if ( is_zero ( cos_theta ) ) {
2014-08-22 10:50:10 -03:00
return false ;
}
2015-11-24 20:18:35 -04:00
2019-04-19 08:36:42 -03:00
euler_rate_rads . x = ang_vel_rads . x + sin_phi * ( sin_theta / cos_theta ) * ang_vel_rads . y + cos_phi * ( sin_theta / cos_theta ) * ang_vel_rads . z ;
euler_rate_rads . y = cos_phi * ang_vel_rads . y - sin_phi * ang_vel_rads . z ;
2015-12-08 15:15:10 -04:00
euler_rate_rads . z = ( sin_phi / cos_theta ) * ang_vel_rads . y + ( cos_phi / cos_theta ) * ang_vel_rads . z ;
2014-08-22 10:50:10 -03:00
return true ;
2014-02-09 22:59:51 -04:00
}
2013-10-12 09:28:18 -03:00
2016-06-24 04:20:21 -03:00
// Update rate_target_ang_vel using attitude_error_rot_vec_rad
2018-12-20 05:10:02 -04:00
Vector3f AC_AttitudeControl : : update_ang_vel_target_from_att_error ( const Vector3f & attitude_error_rot_vec_rad )
2013-10-12 09:28:18 -03:00
{
2016-06-24 04:20:21 -03:00
Vector3f rate_target_ang_vel ;
2022-05-11 08:37:03 -03:00
2015-11-24 23:28:42 -04:00
// Compute the roll angular velocity demand from the roll angle error
2022-10-10 19:14:35 -03:00
const float angleP_roll = _p_angle_roll . kP ( ) * _angle_P_scale . x ;
2020-06-22 04:40:42 -03:00
if ( _use_sqrt_controller & & ! is_zero ( get_accel_roll_max_radss ( ) ) ) {
2022-10-10 19:14:35 -03:00
rate_target_ang_vel . x = sqrt_controller ( attitude_error_rot_vec_rad . x , angleP_roll , constrain_float ( get_accel_roll_max_radss ( ) / 2.0f , AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS , AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS ) , _dt ) ;
2019-04-19 08:36:42 -03:00
} else {
2022-10-10 19:14:35 -03:00
rate_target_ang_vel . x = angleP_roll * attitude_error_rot_vec_rad . x ;
2014-02-09 22:59:51 -04:00
}
2013-10-12 09:28:18 -03:00
2019-04-27 12:54:43 -03:00
// Compute the pitch angular velocity demand from the pitch angle error
2022-10-10 19:14:35 -03:00
const float angleP_pitch = _p_angle_pitch . kP ( ) * _angle_P_scale . y ;
2020-06-22 04:40:42 -03:00
if ( _use_sqrt_controller & & ! is_zero ( get_accel_pitch_max_radss ( ) ) ) {
2022-10-10 19:14:35 -03:00
rate_target_ang_vel . y = sqrt_controller ( attitude_error_rot_vec_rad . y , angleP_pitch , constrain_float ( get_accel_pitch_max_radss ( ) / 2.0f , AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS , AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS ) , _dt ) ;
2019-04-19 08:36:42 -03:00
} else {
2022-10-10 19:14:35 -03:00
rate_target_ang_vel . y = angleP_pitch * attitude_error_rot_vec_rad . y ;
2014-02-09 22:59:51 -04:00
}
2013-10-12 09:28:18 -03:00
2019-04-27 12:54:43 -03:00
// Compute the yaw angular velocity demand from the yaw angle error
2022-10-10 19:14:35 -03:00
const float angleP_yaw = _p_angle_yaw . kP ( ) * _angle_P_scale . z ;
2020-06-22 04:40:42 -03:00
if ( _use_sqrt_controller & & ! is_zero ( get_accel_yaw_max_radss ( ) ) ) {
2022-10-10 19:14:35 -03:00
rate_target_ang_vel . z = sqrt_controller ( attitude_error_rot_vec_rad . z , angleP_yaw , constrain_float ( get_accel_yaw_max_radss ( ) / 2.0f , AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS , AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS ) , _dt ) ;
2019-04-19 08:36:42 -03:00
} else {
2022-10-10 19:14:35 -03:00
rate_target_ang_vel . z = angleP_yaw * attitude_error_rot_vec_rad . z ;
2014-02-09 22:59:51 -04:00
}
2022-10-10 19:14:35 -03:00
2023-08-09 10:33:55 -03:00
// reset angle P scaling, saving used value
2022-10-10 19:14:35 -03:00
_angle_P_scale_used = _angle_P_scale ;
2022-05-11 08:37:03 -03:00
_angle_P_scale = VECTORF_111 ;
2022-10-10 19:14:35 -03:00
2016-06-24 04:20:21 -03:00
return rate_target_ang_vel ;
2013-10-12 09:28:18 -03:00
}
2016-06-24 04:20:21 -03:00
// Enable or disable body-frame feed forward
2014-06-10 03:49:49 -03:00
void AC_AttitudeControl : : accel_limiting ( bool enable_limits )
{
if ( enable_limits ) {
2015-11-24 23:28:42 -04:00
// If enabling limits, reload from eeprom or set to defaults
2015-05-04 23:34:37 -03:00
if ( is_zero ( _accel_roll_max ) ) {
2015-02-11 08:10:56 -04:00
_accel_roll_max . load ( ) ;
}
2015-05-04 23:34:37 -03:00
if ( is_zero ( _accel_pitch_max ) ) {
2015-02-11 08:10:56 -04:00
_accel_pitch_max . load ( ) ;
2014-06-10 03:49:49 -03:00
}
2015-05-04 23:34:37 -03:00
if ( is_zero ( _accel_yaw_max ) ) {
2015-02-11 08:10:56 -04:00
_accel_yaw_max . load ( ) ;
2014-06-10 03:49:49 -03:00
}
} else {
2022-07-05 00:08:55 -03:00
_accel_roll_max . set ( 0.0f ) ;
_accel_pitch_max . set ( 0.0f ) ;
_accel_yaw_max . set ( 0.0f ) ;
2014-06-10 03:49:49 -03:00
}
}
2013-10-12 09:28:18 -03:00
2016-05-26 10:11:58 -03:00
// Return tilt angle limit for pilot input that prioritises altitude hold over lean angle
2021-09-06 06:45:31 -03:00
float AC_AttitudeControl : : get_althold_lean_angle_max_cd ( ) const
2016-05-26 10:11:58 -03:00
{
// convert to centi-degrees for public interface
2018-09-11 10:51:08 -03:00
return MAX ( ToDeg ( _althold_lean_angle_max ) , AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN ) * 100.0f ;
2016-05-26 10:11:58 -03:00
}
2016-06-24 04:20:21 -03:00
// Return roll rate step size in centidegrees/s that results in maximum output after 4 time steps
2015-02-11 09:03:36 -04:00
float AC_AttitudeControl : : max_rate_step_bf_roll ( )
{
2022-12-05 08:21:43 -04:00
float dt_average = AP : : scheduler ( ) . get_filtered_loop_time ( ) ;
float alpha = MIN ( get_rate_roll_pid ( ) . get_filt_E_alpha ( dt_average ) , get_rate_roll_pid ( ) . get_filt_D_alpha ( dt_average ) ) ;
2019-04-19 08:36:42 -03:00
float alpha_remaining = 1 - alpha ;
2018-12-21 04:49:07 -04:00
// todo: When a thrust_max is available we should replace 0.5f with 0.5f * _motors.thrust_max
float throttle_hover = constrain_float ( _motors . get_throttle_hover ( ) , 0.1f , 0.5f ) ;
2021-12-21 01:25:19 -04:00
float rate_max = 2.0f * throttle_hover * AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX / ( ( alpha_remaining * alpha_remaining * alpha_remaining * alpha * get_rate_roll_pid ( ) . kD ( ) ) / _dt + get_rate_roll_pid ( ) . kP ( ) ) ;
if ( is_positive ( _ang_vel_roll_max ) ) {
rate_max = MIN ( rate_max , get_ang_vel_roll_max_rads ( ) ) ;
}
return rate_max ;
2015-02-11 09:03:36 -04:00
}
2016-06-24 04:20:21 -03:00
// Return pitch rate step size in centidegrees/s that results in maximum output after 4 time steps
2015-02-11 09:03:36 -04:00
float AC_AttitudeControl : : max_rate_step_bf_pitch ( )
{
2022-12-05 08:21:43 -04:00
float dt_average = AP : : scheduler ( ) . get_filtered_loop_time ( ) ;
float alpha = MIN ( get_rate_pitch_pid ( ) . get_filt_E_alpha ( dt_average ) , get_rate_pitch_pid ( ) . get_filt_D_alpha ( dt_average ) ) ;
2019-04-19 08:36:42 -03:00
float alpha_remaining = 1 - alpha ;
2018-12-21 04:49:07 -04:00
// todo: When a thrust_max is available we should replace 0.5f with 0.5f * _motors.thrust_max
float throttle_hover = constrain_float ( _motors . get_throttle_hover ( ) , 0.1f , 0.5f ) ;
2021-12-21 01:25:19 -04:00
float rate_max = 2.0f * throttle_hover * AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX / ( ( alpha_remaining * alpha_remaining * alpha_remaining * alpha * get_rate_pitch_pid ( ) . kD ( ) ) / _dt + get_rate_pitch_pid ( ) . kP ( ) ) ;
if ( is_positive ( _ang_vel_pitch_max ) ) {
rate_max = MIN ( rate_max , get_ang_vel_pitch_max_rads ( ) ) ;
}
return rate_max ;
2015-02-11 09:03:36 -04:00
}
2016-06-24 04:20:21 -03:00
// Return yaw rate step size in centidegrees/s that results in maximum output after 4 time steps
2015-02-11 09:03:36 -04:00
float AC_AttitudeControl : : max_rate_step_bf_yaw ( )
{
2022-12-05 08:21:43 -04:00
float dt_average = AP : : scheduler ( ) . get_filtered_loop_time ( ) ;
float alpha = MIN ( get_rate_yaw_pid ( ) . get_filt_E_alpha ( dt_average ) , get_rate_yaw_pid ( ) . get_filt_D_alpha ( dt_average ) ) ;
2019-04-19 08:36:42 -03:00
float alpha_remaining = 1 - alpha ;
2018-12-21 04:49:07 -04:00
// todo: When a thrust_max is available we should replace 0.5f with 0.5f * _motors.thrust_max
float throttle_hover = constrain_float ( _motors . get_throttle_hover ( ) , 0.1f , 0.5f ) ;
2021-12-21 01:25:19 -04:00
float rate_max = 2.0f * throttle_hover * AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX / ( ( alpha_remaining * alpha_remaining * alpha_remaining * alpha * get_rate_yaw_pid ( ) . kD ( ) ) / _dt + get_rate_yaw_pid ( ) . kP ( ) ) ;
if ( is_positive ( _ang_vel_yaw_max ) ) {
rate_max = MIN ( rate_max , get_ang_vel_yaw_max_rads ( ) ) ;
}
return rate_max ;
2015-02-11 09:03:36 -04:00
}
2019-03-04 23:27:31 -04:00
bool AC_AttitudeControl : : pre_arm_checks ( const char * param_prefix ,
char * failure_msg ,
const uint8_t failure_msg_len )
{
// validate AC_P members:
const struct {
const char * pid_name ;
AC_P & p ;
} ps [ ] = {
{ " ANG_PIT " , get_angle_pitch_p ( ) } ,
{ " ANG_RLL " , get_angle_roll_p ( ) } ,
{ " ANG_YAW " , get_angle_yaw_p ( ) }
} ;
for ( uint8_t i = 0 ; i < ARRAY_SIZE ( ps ) ; i + + ) {
// all AC_P's must have a positive P value:
if ( ! is_positive ( ps [ i ] . p . kP ( ) ) ) {
hal . util - > snprintf ( failure_msg , failure_msg_len , " %s_%s_P must be > 0 " , param_prefix , ps [ i ] . pid_name ) ;
return false ;
}
}
// validate AC_PID members:
const struct {
const char * pid_name ;
AC_PID & pid ;
} pids [ ] = {
{ " RAT_RLL " , get_rate_roll_pid ( ) } ,
{ " RAT_PIT " , get_rate_pitch_pid ( ) } ,
{ " RAT_YAW " , get_rate_yaw_pid ( ) } ,
} ;
for ( uint8_t i = 0 ; i < ARRAY_SIZE ( pids ) ; i + + ) {
// if the PID has a positive FF then we just ensure kP and
// kI aren't negative
AC_PID & pid = pids [ i ] . pid ;
const char * pid_name = pids [ i ] . pid_name ;
if ( is_positive ( pid . ff ( ) ) ) {
// kP and kI must be non-negative:
if ( is_negative ( pid . kP ( ) ) ) {
hal . util - > snprintf ( failure_msg , failure_msg_len , " %s_%s_P must be >= 0 " , param_prefix , pid_name ) ;
return false ;
}
if ( is_negative ( pid . kI ( ) ) ) {
hal . util - > snprintf ( failure_msg , failure_msg_len , " %s_%s_I must be >= 0 " , param_prefix , pid_name ) ;
return false ;
}
} else {
// kP and kI must be positive:
if ( ! is_positive ( pid . kP ( ) ) ) {
hal . util - > snprintf ( failure_msg , failure_msg_len , " %s_%s_P must be > 0 " , param_prefix , pid_name ) ;
return false ;
}
if ( ! is_positive ( pid . kI ( ) ) ) {
hal . util - > snprintf ( failure_msg , failure_msg_len , " %s_%s_I must be > 0 " , param_prefix , pid_name ) ;
return false ;
}
}
// never allow a negative D term (but zero is allowed)
if ( is_negative ( pid . kD ( ) ) ) {
hal . util - > snprintf ( failure_msg , failure_msg_len , " %s_%s_D must be >= 0 " , param_prefix , pid_name ) ;
return false ;
}
}
return true ;
}
2022-04-26 06:07:20 -03:00
/*
2022-04-30 06:25:33 -03:00
get the slew rate for roll , pitch and yaw , for oscillation
detection in lua scripts
2022-04-26 06:07:20 -03:00
*/
2022-04-30 06:25:33 -03:00
void AC_AttitudeControl : : get_rpy_srate ( float & roll_srate , float & pitch_srate , float & yaw_srate )
2022-04-26 06:07:20 -03:00
{
2022-04-30 06:25:33 -03:00
roll_srate = get_rate_roll_pid ( ) . get_pid_info ( ) . slew_rate ;
pitch_srate = get_rate_pitch_pid ( ) . get_pid_info ( ) . slew_rate ;
yaw_srate = get_rate_yaw_pid ( ) . get_pid_info ( ) . slew_rate ;
2022-04-26 06:07:20 -03:00
}