2015-07-13 02:59:48 -03:00
# include "AC_AttitudeControl_Multi.h"
2015-08-11 03:28:41 -03:00
# include <AP_HAL/AP_HAL.h>
# include <AP_Math/AP_Math.h>
2023-08-29 02:17:09 -03:00
# include <AC_PID/AC_PID.h>
2023-07-27 10:59:16 -03:00
# include <AP_Scheduler/AP_Scheduler.h>
2015-07-13 02:59:48 -03:00
2016-02-15 02:25:41 -04:00
// table of user settable parameters
const AP_Param : : GroupInfo AC_AttitudeControl_Multi : : var_info [ ] = {
// parameters from parent vehicle
AP_NESTEDGROUPINFO ( AC_AttitudeControl , 0 ) ,
// @Param: RAT_RLL_P
// @DisplayName: Roll axis rate controller P gain
2022-11-20 20:45:06 -04:00
// @Description: Roll axis rate controller P gain. Corrects in proportion to the difference between the desired roll rate vs actual roll rate
2019-07-16 03:02:58 -03:00
// @Range: 0.01 0.5
2016-02-15 02:25:41 -04:00
// @Increment: 0.005
// @User: Standard
// @Param: RAT_RLL_I
// @DisplayName: Roll axis rate controller I gain
// @Description: Roll axis rate controller I gain. Corrects long-term difference in desired roll rate vs actual roll rate
2018-01-04 20:20:18 -04:00
// @Range: 0.01 2.0
2016-02-15 02:25:41 -04:00
// @Increment: 0.01
// @User: Standard
// @Param: RAT_RLL_IMAX
// @DisplayName: Roll axis rate controller I gain maximum
2022-11-20 20:45:06 -04:00
// @Description: Roll axis rate controller I gain maximum. Constrains the maximum that the I term will output
2016-02-17 22:10:05 -04:00
// @Range: 0 1
// @Increment: 0.01
2016-02-15 02:25:41 -04:00
// @User: Standard
// @Param: RAT_RLL_D
// @DisplayName: Roll axis rate controller D gain
// @Description: Roll axis rate controller D gain. Compensates for short-term change in desired roll rate vs actual roll rate
2019-07-16 03:02:58 -03:00
// @Range: 0.0 0.05
2016-02-15 02:25:41 -04:00
// @Increment: 0.001
// @User: Standard
2017-03-01 01:59:30 -04:00
// @Param: RAT_RLL_FF
// @DisplayName: Roll axis rate controller feed forward
// @Description: Roll axis rate controller feed forward
// @Range: 0 0.5
// @Increment: 0.001
// @User: Standard
2019-07-16 03:02:58 -03:00
// @Param: RAT_RLL_FLTT
// @DisplayName: Roll axis rate controller target frequency in Hz
// @Description: Roll axis rate controller target frequency in Hz
2020-02-20 22:10:22 -04:00
// @Range: 5 100
2019-07-16 03:02:58 -03:00
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_RLL_FLTE
// @DisplayName: Roll axis rate controller error frequency in Hz
// @Description: Roll axis rate controller error frequency in Hz
2020-02-20 22:10:22 -04:00
// @Range: 0 100
2019-07-16 03:02:58 -03:00
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_RLL_FLTD
// @DisplayName: Roll axis rate controller derivative frequency in Hz
// @Description: Roll axis rate controller derivative frequency in Hz
2020-02-20 22:10:22 -04:00
// @Range: 5 100
2016-02-17 22:10:05 -04:00
// @Increment: 1
// @Units: Hz
2016-07-26 02:35:15 -03:00
// @User: Standard
2020-09-24 04:26:05 -03:00
// @Param: RAT_RLL_SMAX
// @DisplayName: Roll slew rate limit
// @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
// @Range: 0 200
// @Increment: 0.5
// @User: Advanced
2023-09-18 22:44:54 -03:00
// @Param: RAT_RLL_PDMX
// @DisplayName: Roll axis rate controller PD sum maximum
// @Description: Roll axis rate controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output
// @Range: 0 1
// @Increment: 0.01
2023-07-27 10:59:16 -03:00
// @Param: RAT_RLL_D_FF
// @DisplayName: Roll Derivative FeedForward Gain
// @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
// @Range: 0 0.02
// @Increment: 0.0001
// @User: Advanced
// @Param: RAT_RLL_NTF
2023-10-01 14:21:00 -03:00
// @DisplayName: Roll Target notch filter index
// @Description: Roll Target notch filter index
// @Range: 1 8
2023-07-27 10:59:16 -03:00
// @User: Advanced
// @Param: RAT_RLL_NEF
2023-10-01 14:21:00 -03:00
// @DisplayName: Roll Error notch filter index
// @Description: Roll Error notch filter index
// @Range: 1 8
2023-09-18 22:44:54 -03:00
// @User: Advanced
2016-02-15 02:25:41 -04:00
AP_SUBGROUPINFO ( _pid_rate_roll , " RAT_RLL_ " , 1 , AC_AttitudeControl_Multi , AC_PID ) ,
// @Param: RAT_PIT_P
// @DisplayName: Pitch axis rate controller P gain
2022-11-20 20:45:06 -04:00
// @Description: Pitch axis rate controller P gain. Corrects in proportion to the difference between the desired pitch rate vs actual pitch rate output
2019-07-16 03:02:58 -03:00
// @Range: 0.01 0.50
2016-02-15 02:25:41 -04:00
// @Increment: 0.005
// @User: Standard
// @Param: RAT_PIT_I
// @DisplayName: Pitch axis rate controller I gain
// @Description: Pitch axis rate controller I gain. Corrects long-term difference in desired pitch rate vs actual pitch rate
2018-01-04 20:20:18 -04:00
// @Range: 0.01 2.0
2016-02-15 02:25:41 -04:00
// @Increment: 0.01
// @User: Standard
// @Param: RAT_PIT_IMAX
// @DisplayName: Pitch axis rate controller I gain maximum
2022-11-20 20:45:06 -04:00
// @Description: Pitch axis rate controller I gain maximum. Constrains the maximum that the I term will output
2016-02-17 22:10:05 -04:00
// @Range: 0 1
// @Increment: 0.01
2016-02-15 02:25:41 -04:00
// @User: Standard
// @Param: RAT_PIT_D
// @DisplayName: Pitch axis rate controller D gain
// @Description: Pitch axis rate controller D gain. Compensates for short-term change in desired pitch rate vs actual pitch rate
2019-07-16 03:02:58 -03:00
// @Range: 0.0 0.05
2016-02-15 02:25:41 -04:00
// @Increment: 0.001
// @User: Standard
2017-03-01 01:59:30 -04:00
// @Param: RAT_PIT_FF
// @DisplayName: Pitch axis rate controller feed forward
// @Description: Pitch axis rate controller feed forward
// @Range: 0 0.5
// @Increment: 0.001
// @User: Standard
2019-07-16 03:02:58 -03:00
// @Param: RAT_PIT_FLTT
// @DisplayName: Pitch axis rate controller target frequency in Hz
// @Description: Pitch axis rate controller target frequency in Hz
2020-02-20 22:10:22 -04:00
// @Range: 5 100
2019-07-16 03:02:58 -03:00
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_PIT_FLTE
// @DisplayName: Pitch axis rate controller error frequency in Hz
// @Description: Pitch axis rate controller error frequency in Hz
2020-02-20 22:10:22 -04:00
// @Range: 0 100
2019-07-16 03:02:58 -03:00
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_PIT_FLTD
// @DisplayName: Pitch axis rate controller derivative frequency in Hz
// @Description: Pitch axis rate controller derivative frequency in Hz
2020-02-20 22:10:22 -04:00
// @Range: 5 100
2016-02-17 22:10:05 -04:00
// @Increment: 1
// @Units: Hz
2016-07-26 02:35:15 -03:00
// @User: Standard
2020-09-24 04:26:05 -03:00
// @Param: RAT_PIT_SMAX
// @DisplayName: Pitch slew rate limit
// @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
// @Range: 0 200
// @Increment: 0.5
// @User: Advanced
2023-09-18 22:44:54 -03:00
// @Param: RAT_PIT_PDMX
// @DisplayName: Pitch axis rate controller PD sum maximum
// @Description: Pitch axis rate controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output
// @Range: 0 1
// @Increment: 0.01
2023-07-27 10:59:16 -03:00
// @Param: RAT_PIT_D_FF
// @DisplayName: Pitch Derivative FeedForward Gain
// @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
// @Range: 0 0.02
// @Increment: 0.0001
// @User: Advanced
// @Param: RAT_PIT_NTF
2023-10-01 14:21:00 -03:00
// @DisplayName: Pitch Target notch filter index
// @Description: Pitch Target notch filter index
// @Range: 1 8
2023-07-27 10:59:16 -03:00
// @User: Advanced
// @Param: RAT_PIT_NEF
2023-10-01 14:21:00 -03:00
// @DisplayName: Pitch Error notch filter index
// @Description: Pitch Error notch filter index
// @Range: 1 8
2023-09-18 22:44:54 -03:00
// @User: Advanced
2016-02-15 02:25:41 -04:00
AP_SUBGROUPINFO ( _pid_rate_pitch , " RAT_PIT_ " , 2 , AC_AttitudeControl_Multi , AC_PID ) ,
// @Param: RAT_YAW_P
// @DisplayName: Yaw axis rate controller P gain
2022-11-20 20:45:06 -04:00
// @Description: Yaw axis rate controller P gain. Corrects in proportion to the difference between the desired yaw rate vs actual yaw rate
2018-01-04 20:20:18 -04:00
// @Range: 0.10 2.50
2016-02-15 02:25:41 -04:00
// @Increment: 0.005
// @User: Standard
// @Param: RAT_YAW_I
// @DisplayName: Yaw axis rate controller I gain
// @Description: Yaw axis rate controller I gain. Corrects long-term difference in desired yaw rate vs actual yaw rate
2018-01-04 20:20:18 -04:00
// @Range: 0.010 1.0
2016-02-15 02:25:41 -04:00
// @Increment: 0.01
// @User: Standard
// @Param: RAT_YAW_IMAX
// @DisplayName: Yaw axis rate controller I gain maximum
2022-11-20 20:45:06 -04:00
// @Description: Yaw axis rate controller I gain maximum. Constrains the maximum that the I term will output
2016-02-17 22:10:05 -04:00
// @Range: 0 1
// @Increment: 0.01
2016-02-15 02:25:41 -04:00
// @User: Standard
// @Param: RAT_YAW_D
// @DisplayName: Yaw axis rate controller D gain
// @Description: Yaw axis rate controller D gain. Compensates for short-term change in desired yaw rate vs actual yaw rate
// @Range: 0.000 0.02
// @Increment: 0.001
// @User: Standard
2017-03-01 01:59:30 -04:00
// @Param: RAT_YAW_FF
// @DisplayName: Yaw axis rate controller feed forward
// @Description: Yaw axis rate controller feed forward
// @Range: 0 0.5
// @Increment: 0.001
// @User: Standard
2019-07-16 03:02:58 -03:00
// @Param: RAT_YAW_FLTT
// @DisplayName: Yaw axis rate controller target frequency in Hz
// @Description: Yaw axis rate controller target frequency in Hz
2020-02-20 22:10:22 -04:00
// @Range: 1 50
2019-07-16 03:02:58 -03:00
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_YAW_FLTE
// @DisplayName: Yaw axis rate controller error frequency in Hz
// @Description: Yaw axis rate controller error frequency in Hz
2020-02-20 22:10:22 -04:00
// @Range: 0 20
2016-02-17 22:10:05 -04:00
// @Increment: 1
// @Units: Hz
2016-07-26 02:35:15 -03:00
// @User: Standard
2019-07-16 03:02:58 -03:00
// @Param: RAT_YAW_FLTD
// @DisplayName: Yaw axis rate controller derivative frequency in Hz
// @Description: Yaw axis rate controller derivative frequency in Hz
2020-02-20 22:10:22 -04:00
// @Range: 5 50
2019-07-16 03:02:58 -03:00
// @Increment: 1
// @Units: Hz
// @User: Standard
2020-09-24 04:26:05 -03:00
// @Param: RAT_YAW_SMAX
// @DisplayName: Yaw slew rate limit
// @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
// @Range: 0 200
// @Increment: 0.5
// @User: Advanced
2023-09-18 22:44:54 -03:00
// @Param: RAT_YAW_PDMX
// @DisplayName: Yaw axis rate controller PD sum maximum
// @Description: Yaw axis rate controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output
// @Range: 0 1
// @Increment: 0.01
2023-07-27 10:59:16 -03:00
// @Param: RAT_YAW_D_FF
// @DisplayName: Yaw Derivative FeedForward Gain
// @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
// @Range: 0 0.02
// @Increment: 0.0001
// @User: Advanced
// @Param: RAT_YAW_NTF
2023-10-01 14:21:00 -03:00
// @DisplayName: Yaw Target notch filter index
// @Description: Yaw Target notch filter index
// @Range: 1 8
2023-07-27 10:59:16 -03:00
// @Units: Hz
// @User: Advanced
// @Param: RAT_YAW_NEF
2023-10-01 14:21:00 -03:00
// @DisplayName: Yaw Error notch filter index
// @Description: Yaw Error notch filter index
// @Range: 1 8
2023-09-18 22:44:54 -03:00
// @User: Advanced
2016-02-15 02:25:41 -04:00
AP_SUBGROUPINFO ( _pid_rate_yaw , " RAT_YAW_ " , 3 , AC_AttitudeControl_Multi , AC_PID ) ,
2016-06-09 06:39:29 -03:00
// @Param: THR_MIX_MIN
// @DisplayName: Throttle Mix Minimum
// @Description: Throttle vs attitude control prioritisation used when landing (higher values mean we prioritise attitude control over throttle)
// @Range: 0.1 0.25
// @User: Advanced
AP_GROUPINFO ( " THR_MIX_MIN " , 4 , AC_AttitudeControl_Multi , _thr_mix_min , AC_ATTITUDE_CONTROL_MIN_DEFAULT ) ,
// @Param: THR_MIX_MAX
// @DisplayName: Throttle Mix Maximum
// @Description: Throttle vs attitude control prioritisation used during active flight (higher values mean we prioritise attitude control over throttle)
// @Range: 0.5 0.9
// @User: Advanced
AP_GROUPINFO ( " THR_MIX_MAX " , 5 , AC_AttitudeControl_Multi , _thr_mix_max , AC_ATTITUDE_CONTROL_MAX_DEFAULT ) ,
2017-01-04 01:19:00 -04:00
// @Param: THR_MIX_MAN
// @DisplayName: Throttle Mix Manual
// @Description: Throttle vs attitude control prioritisation used during manual flight (higher values mean we prioritise attitude control over throttle)
2018-01-04 20:20:18 -04:00
// @Range: 0.1 0.9
2017-01-04 01:19:00 -04:00
// @User: Advanced
AP_GROUPINFO ( " THR_MIX_MAN " , 6 , AC_AttitudeControl_Multi , _thr_mix_man , AC_ATTITUDE_CONTROL_MAN_DEFAULT ) ,
2023-01-29 06:14:36 -04:00
// @Param: THR_G_BOOST
// @DisplayName: Throttle-gain boost
// @Description: Throttle-gain boost ratio. A value of 0 means no boosting is applied, a value of 1 means full boosting is applied. Describes the ratio increase that is applied to angle P and PD on pitch and roll.
// @Range: 0 1
// @User: Advanced
AP_GROUPINFO ( " THR_G_BOOST " , 7 , AC_AttitudeControl_Multi , _throttle_gain_boost , 0.0f ) ,
2016-02-15 02:25:41 -04:00
AP_GROUPEND
} ;
2022-12-05 08:21:43 -04:00
AC_AttitudeControl_Multi : : AC_AttitudeControl_Multi ( AP_AHRS_View & ahrs , const AP_MultiCopter & aparm , AP_MotorsMulticopter & motors ) :
AC_AttitudeControl ( ahrs , aparm , motors ) ,
2023-08-29 02:17:09 -03:00
_motors_multi ( motors )
2016-02-15 02:25:41 -04:00
{
AP_Param : : setup_object_defaults ( this , var_info ) ;
2023-07-27 10:59:16 -03:00
2023-10-01 14:21:00 -03:00
# if AP_FILTER_ENABLED
2023-07-27 10:59:16 -03:00
set_notch_sample_rate ( AP : : scheduler ( ) . get_loop_rate_hz ( ) ) ;
# endif
2016-02-15 02:25:41 -04:00
}
2016-05-23 12:12:14 -03:00
// Update Alt_Hold angle maximum
void AC_AttitudeControl_Multi : : update_althold_lean_angle_max ( float throttle_in )
2015-06-23 10:41:47 -03:00
{
// calc maximum tilt angle based on throttle
2016-01-21 02:08:56 -04:00
float thr_max = _motors_multi . get_throttle_thrust_max ( ) ;
2015-12-19 00:47:51 -04:00
// divide by zero check
if ( is_zero ( thr_max ) ) {
2016-05-23 12:12:14 -03:00
_althold_lean_angle_max = 0.0f ;
return ;
2015-12-19 00:47:51 -04:00
}
2020-01-28 16:08:22 -04:00
float althold_lean_angle_max = acosf ( constrain_float ( throttle_in / ( AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX * thr_max ) , 0.0f , 1.0f ) ) ;
2019-04-19 08:36:42 -03:00
_althold_lean_angle_max = _althold_lean_angle_max + ( _dt / ( _dt + _angle_limit_tc ) ) * ( althold_lean_angle_max - _althold_lean_angle_max ) ;
2015-06-23 10:41:47 -03:00
}
2016-03-21 07:57:39 -03:00
void AC_AttitudeControl_Multi : : set_throttle_out ( float throttle_in , bool apply_angle_boost , float filter_cutoff )
{
_throttle_in = throttle_in ;
2016-05-23 12:12:14 -03:00
update_althold_lean_angle_max ( throttle_in ) ;
2016-03-21 07:57:39 -03:00
_motors . set_throttle_filter_cutoff ( filter_cutoff ) ;
if ( apply_angle_boost ) {
2016-06-09 06:39:29 -03:00
// Apply angle boost
throttle_in = get_throttle_boosted ( throttle_in ) ;
2019-04-19 08:36:42 -03:00
} else {
2016-03-21 07:57:39 -03:00
// Clear angle_boost for logging purposes
_angle_boost = 0.0f ;
}
2016-06-09 06:39:29 -03:00
_motors . set_throttle ( throttle_in ) ;
2016-06-08 05:49:05 -03:00
_motors . set_throttle_avg_max ( get_throttle_avg_max ( MAX ( throttle_in , _throttle_in ) ) ) ;
2016-03-21 07:57:39 -03:00
}
2019-06-17 12:36:22 -03:00
void AC_AttitudeControl_Multi : : set_throttle_mix_max ( float ratio )
{
ratio = constrain_float ( ratio , 0.0f , 1.0f ) ;
_throttle_rpy_mix_desired = ( 1.0f - ratio ) * _thr_mix_min + ratio * _thr_mix_max ;
}
2015-07-13 02:59:48 -03:00
// returns a throttle including compensation for roll/pitch angle
2015-12-20 07:11:51 -04:00
// throttle value should be 0 ~ 1
2016-03-21 08:01:17 -03:00
float AC_AttitudeControl_Multi : : get_throttle_boosted ( float throttle_in )
2015-07-13 02:59:48 -03:00
{
2016-01-06 23:11:27 -04:00
if ( ! _angle_boost_enabled ) {
_angle_boost = 0 ;
2016-01-07 21:47:20 -04:00
return throttle_in ;
2016-01-06 23:11:27 -04:00
}
2015-07-13 02:59:48 -03:00
// inverted_factor is 1 for tilt angles below 60 degrees
2015-12-20 07:11:51 -04:00
// inverted_factor reduces from 1 to 0 for tilt angles between 60 and 90 degrees
2015-07-13 02:59:48 -03:00
float cos_tilt = _ahrs . cos_pitch ( ) * _ahrs . cos_roll ( ) ;
2021-07-30 10:08:19 -03:00
float inverted_factor = constrain_float ( 10.0f * cos_tilt , 0.0f , 1.0f ) ;
float cos_tilt_target = cosf ( _thrust_angle ) ;
float boost_factor = 1.0f / constrain_float ( cos_tilt_target , 0.1f , 1.0f ) ;
2015-07-13 02:59:48 -03:00
2019-04-19 08:36:42 -03:00
float throttle_out = throttle_in * inverted_factor * boost_factor ;
_angle_boost = constrain_float ( throttle_out - throttle_in , - 1.0f , 1.0f ) ;
2015-07-13 02:59:48 -03:00
return throttle_out ;
}
2016-06-09 06:39:29 -03:00
// returns a throttle including compensation for roll/pitch angle
// throttle value should be 0 ~ 1
2016-06-08 05:49:05 -03:00
float AC_AttitudeControl_Multi : : get_throttle_avg_max ( float throttle_in )
2016-06-09 06:39:29 -03:00
{
2016-08-02 23:44:22 -03:00
throttle_in = constrain_float ( throttle_in , 0.0f , 1.0f ) ;
2019-04-19 08:36:42 -03:00
return MAX ( throttle_in , throttle_in * MAX ( 0.0f , 1.0f - _throttle_rpy_mix ) + _motors . get_throttle_hover ( ) * _throttle_rpy_mix ) ;
2016-06-09 06:39:29 -03:00
}
2023-01-29 06:14:36 -04:00
// update_throttle_gain_boost - boost angle_p/pd each cycle on high throttle slew
void AC_AttitudeControl_Multi : : update_throttle_gain_boost ( )
{
// Boost PD and Angle P on very rapid throttle changes
if ( _motors . get_throttle_slew_rate ( ) > AC_ATTITUDE_CONTROL_THR_G_BOOST_THRESH ) {
const float pd_boost = constrain_float ( _throttle_gain_boost + 1.0f , 1.0 , 2.0 ) ;
set_PD_scale_mult ( Vector3f ( pd_boost , pd_boost , 1.0f ) ) ;
const float angle_p_boost = constrain_float ( ( _throttle_gain_boost + 1.0f ) * ( _throttle_gain_boost + 1.0f ) , 1.0 , 4.0 ) ;
set_angle_P_scale_mult ( Vector3f ( angle_p_boost , angle_p_boost , 1.0f ) ) ;
}
}
2016-06-09 06:39:29 -03:00
// update_throttle_rpy_mix - slew set_throttle_rpy_mix to requested value
void AC_AttitudeControl_Multi : : update_throttle_rpy_mix ( )
{
// slew _throttle_rpy_mix to _throttle_rpy_mix_desired
if ( _throttle_rpy_mix < _throttle_rpy_mix_desired ) {
// increase quickly (i.e. from 0.1 to 0.9 in 0.4 seconds)
2019-04-19 08:36:42 -03:00
_throttle_rpy_mix + = MIN ( 2.0f * _dt , _throttle_rpy_mix_desired - _throttle_rpy_mix ) ;
2016-06-09 06:39:29 -03:00
} else if ( _throttle_rpy_mix > _throttle_rpy_mix_desired ) {
// reduce more slowly (from 0.9 to 0.1 in 1.6 seconds)
2019-04-19 08:36:42 -03:00
_throttle_rpy_mix - = MIN ( 0.5f * _dt , _throttle_rpy_mix - _throttle_rpy_mix_desired ) ;
2022-06-17 17:59:38 -03:00
// if the mix is still higher than that being used, reset immediately
const float throttle_hover = _motors . get_throttle_hover ( ) ;
const float throttle_in = _motors . get_throttle ( ) ;
const float throttle_out = MAX ( _motors . get_throttle_out ( ) , throttle_in ) ;
float mix_used ;
// since throttle_out >= throttle_in at this point we don't need to check throttle_in < throttle_hover
if ( throttle_out < throttle_hover ) {
mix_used = ( throttle_out - throttle_in ) / ( throttle_hover - throttle_in ) ;
} else {
mix_used = throttle_out / throttle_hover ;
}
_throttle_rpy_mix = MIN ( _throttle_rpy_mix , MAX ( mix_used , _throttle_rpy_mix_desired ) ) ;
2016-06-09 06:39:29 -03:00
}
2017-01-04 01:19:00 -04:00
_throttle_rpy_mix = constrain_float ( _throttle_rpy_mix , 0.1f , AC_ATTITUDE_CONTROL_MAX ) ;
2016-06-09 06:39:29 -03:00
}
2016-05-23 02:03:38 -03:00
void AC_AttitudeControl_Multi : : rate_controller_run ( )
{
2023-01-29 06:14:36 -04:00
// boost angle_p/pd each cycle on high throttle slew
update_throttle_gain_boost ( ) ;
2016-05-23 02:03:38 -03:00
// move throttle vs attitude mixing towards desired (called from here because this is conveniently called on every iteration)
update_throttle_rpy_mix ( ) ;
2021-04-14 12:42:42 -03:00
_ang_vel_body + = _sysid_ang_vel_body ;
2019-07-29 04:56:03 -03:00
2024-08-22 16:14:14 -03:00
_rate_gyro = _ahrs . get_gyro_latest ( ) ;
_rate_gyro_time_us = AP_HAL : : micros64 ( ) ;
2019-07-16 03:02:58 -03:00
2024-08-22 16:14:14 -03:00
_motors . set_roll ( get_rate_roll_pid ( ) . update_all ( _ang_vel_body . x , _rate_gyro . x , _dt , _motors . limit . roll , _pd_scale . x ) + _actuator_sysid . x ) ;
2019-07-16 03:02:58 -03:00
_motors . set_roll_ff ( get_rate_roll_pid ( ) . get_ff ( ) ) ;
2024-08-22 16:14:14 -03:00
_motors . set_pitch ( get_rate_pitch_pid ( ) . update_all ( _ang_vel_body . y , _rate_gyro . y , _dt , _motors . limit . pitch , _pd_scale . y ) + _actuator_sysid . y ) ;
2019-07-16 03:02:58 -03:00
_motors . set_pitch_ff ( get_rate_pitch_pid ( ) . get_ff ( ) ) ;
2024-08-22 16:14:14 -03:00
_motors . set_yaw ( get_rate_yaw_pid ( ) . update_all ( _ang_vel_body . z , _rate_gyro . z , _dt , _motors . limit . yaw , _pd_scale . z ) + _actuator_sysid . z ) ;
2019-07-09 09:42:16 -03:00
_motors . set_yaw_ff ( get_rate_yaw_pid ( ) . get_ff ( ) * _feedforward_scalar ) ;
2016-05-31 03:04:49 -03:00
2021-04-14 12:42:42 -03:00
_sysid_ang_vel_body . zero ( ) ;
2019-07-29 04:56:03 -03:00
_actuator_sysid . zero ( ) ;
2022-05-11 08:37:03 -03:00
_pd_scale_used = _pd_scale ;
_pd_scale = VECTORF_111 ;
2016-05-31 03:04:49 -03:00
control_monitor_update ( ) ;
2016-05-23 02:03:38 -03:00
}
2016-09-01 08:31:49 -03:00
// sanity check parameters. should be called once before takeoff
void AC_AttitudeControl_Multi : : parameter_sanity_check ( )
{
// sanity check throttle mix parameters
2021-11-23 00:51:01 -04:00
if ( _thr_mix_man < 0.1f | | _thr_mix_man > AC_ATTITUDE_CONTROL_MAN_LIMIT ) {
2017-01-04 01:19:00 -04:00
// parameter description recommends thr-mix-man be no higher than 0.9 but we allow up to 4.0
// which can be useful for very high powered copters with very low hover throttle
2021-11-23 00:51:01 -04:00
_thr_mix_man . set_and_save ( constrain_float ( _thr_mix_man , 0.1 , AC_ATTITUDE_CONTROL_MAN_LIMIT ) ) ;
2017-01-04 01:19:00 -04:00
}
2021-11-23 00:51:01 -04:00
if ( _thr_mix_min < 0.1f | | _thr_mix_min > AC_ATTITUDE_CONTROL_MIN_LIMIT ) {
_thr_mix_min . set_and_save ( constrain_float ( _thr_mix_min , 0.1 , AC_ATTITUDE_CONTROL_MIN_LIMIT ) ) ;
2016-09-01 08:31:49 -03:00
}
2017-01-04 01:19:00 -04:00
if ( _thr_mix_max < 0.5f | | _thr_mix_max > AC_ATTITUDE_CONTROL_MAX ) {
// parameter description recommends thr-mix-max be no higher than 0.9 but we allow up to 5.0
2016-11-29 23:22:13 -04:00
// which can be useful for very high powered copters with very low hover throttle
2021-11-23 00:51:01 -04:00
_thr_mix_max . set_and_save ( constrain_float ( _thr_mix_max , 0.5 , AC_ATTITUDE_CONTROL_MAX ) ) ;
2017-01-04 01:19:00 -04:00
}
if ( _thr_mix_min > _thr_mix_max ) {
_thr_mix_min . set_and_save ( AC_ATTITUDE_CONTROL_MIN_DEFAULT ) ;
_thr_mix_max . set_and_save ( AC_ATTITUDE_CONTROL_MAX_DEFAULT ) ;
2016-09-01 08:31:49 -03:00
}
}
2023-07-27 10:59:16 -03:00
void AC_AttitudeControl_Multi : : set_notch_sample_rate ( float sample_rate )
{
2023-10-01 14:21:00 -03:00
# if AP_FILTER_ENABLED
2023-07-27 10:59:16 -03:00
_pid_rate_roll . set_notch_sample_rate ( sample_rate ) ;
_pid_rate_pitch . set_notch_sample_rate ( sample_rate ) ;
_pid_rate_yaw . set_notch_sample_rate ( sample_rate ) ;
# endif
}