Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: move THR_G_BOOST to Multicopter only 

refactor PD and Angle P boosting into update_throttle_gain_boost()
call update_throttle_gain_boost() from rate_controller_run()
This commit is contained in:
Andy Piper 2023-01-29 11:14:36 +01:00 committed by Randy Mackay
parent 060d582d28
commit 8ecd1d9a37
4 changed files with 29 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -150,13 +150,6 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = {
// @User: Standard
AP_GROUPINFO("INPUT_TC", 20, AC_AttitudeControl, _input_tc, AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT),
// @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", 21, AC_AttitudeControl, _throttle_gain_boost, 0.0f),
AP_GROUPEND
};
@ -1016,12 +1009,6 @@ Vector3f AC_AttitudeControl::update_ang_vel_target_from_att_error(const Vector3f
{
Vector3f rate_target_ang_vel;
// Boost Angle P one very rapid throttle changes
if (_motors.get_throttle_slew_rate() > AC_ATTITUDE_CONTROL_THR_G_BOOST_THRESH) {
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));
}
// Compute the roll angular velocity demand from the roll angle error
const float angleP_roll = _p_angle_roll.kP() * _angle_P_scale.x;
if (_use_sqrt_controller && !is_zero(get_accel_roll_max_radss())) {

3
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -457,9 +457,6 @@ protected:
// rate controller input smoothing time constant
AP_Float _input_tc;
// angle_p/pd boost multiplier
AP_Float _throttle_gain_boost;
// Intersampling period in seconds
float _dt;

29
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
View File

@ -232,6 +232,13 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = {
// @User: Advanced
AP_GROUPINFO("THR_MIX_MAN", 6, AC_AttitudeControl_Multi, _thr_mix_man, AC_ATTITUDE_CONTROL_MAN_DEFAULT),
// @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),
AP_GROUPEND
};
@ -312,6 +319,19 @@ float AC_AttitudeControl_Multi::get_throttle_avg_max(float throttle_in)
return MAX(throttle_in, throttle_in * MAX(0.0f, 1.0f - _throttle_rpy_mix) + _motors.get_throttle_hover() * _throttle_rpy_mix);
}
// 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));
}
}
// update_throttle_rpy_mix - slew set_throttle_rpy_mix to requested value
void AC_AttitudeControl_Multi::update_throttle_rpy_mix()
{
@ -342,6 +362,9 @@ void AC_AttitudeControl_Multi::update_throttle_rpy_mix()
void AC_AttitudeControl_Multi::rate_controller_run()
{
// boost angle_p/pd each cycle on high throttle slew
update_throttle_gain_boost();
// move throttle vs attitude mixing towards desired (called from here because this is conveniently called on every iteration)
update_throttle_rpy_mix();
@ -349,12 +372,6 @@ void AC_AttitudeControl_Multi::rate_controller_run()
Vector3f gyro_latest = _ahrs.get_gyro_latest();
// Boost PD 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));
}
_motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro_latest.x, _dt, _motors.limit.roll, _pd_scale.x) + _actuator_sysid.x);
_motors.set_roll_ff(get_rate_roll_pid().get_ff());

6
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h
View File

@ -83,6 +83,9 @@ public:
protected:
// boost angle_p/pd each cycle on high throttle slew
void update_throttle_gain_boost();
// update_throttle_rpy_mix - updates thr_low_comp value towards the target
void update_throttle_rpy_mix();
@ -97,4 +100,7 @@ protected:
AP_Float _thr_mix_man; // throttle vs attitude control prioritisation used when using manual throttle (higher values mean we prioritise attitude control over throttle)
AP_Float _thr_mix_min; // throttle vs attitude control prioritisation used when landing (higher values mean we prioritise attitude control over throttle)
AP_Float _thr_mix_max; // throttle vs attitude control prioritisation used during active flight (higher values mean we prioritise attitude control over throttle)
// angle_p/pd boost multiplier
AP_Float _throttle_gain_boost;
};