Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Allow global scaling of PD terms in multicopters #26620

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
+34 −6
Open

Allow global scaling of PD terms in multicopters #26620

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
f8f9806
AC_AttitudeControl: seed PD scaling with default
andyp1per May 11, 2022
e8089d1
AC_PID: cleanup comment and scaling variable name
andyp1per May 11, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 25 additions & 1 deletion libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -317,6 +317,27 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = {
// @User: Advanced
AP_GROUPINFO("THR_G_BOOST", 7, AC_AttitudeControl_Multi, _throttle_gain_boost, 0.0f),

// @Param: PD_RLL_SCALE
// @DisplayName: PD scale factor for Roll
// @Description: Scale factor that is to be applied to Roll PDs
// @Range: 0.1 1.0
// @User: Advanced
AP_GROUPINFO("PD_RLL_SCALE", 8, AC_AttitudeControl_Multi, _pd_scale_roll, 1.0f),

// @Param: PD_PIT_SCALE
// @DisplayName: PD scale factor for Pitch
// @Description: Scale factor that is to be applied to Pitch PDs
// @Range: 0.1 1.0
// @User: Advanced
AP_GROUPINFO("PD_PIT_SCALE", 9, AC_AttitudeControl_Multi, _pd_scale_pitch, 1.0f),

// @Param: PD_YAW_SCALE
// @DisplayName: PD scale factor for Yaw
// @Description: Scale factor that is to be applied to Yaw PDs
// @Range: 0.1 1.0
// @User: Advanced
AP_GROUPINFO("PD_YAW_SCALE", 10, AC_AttitudeControl_Multi, _pd_scale_yaw, 1.0f),

AP_GROUPEND
};

Expand Down Expand Up @@ -464,7 +485,10 @@ void AC_AttitudeControl_Multi::rate_controller_run()
_actuator_sysid.zero();

_pd_scale_used = _pd_scale;
IamPete1 marked this conversation as resolved.
Show resolved Hide resolved
_pd_scale = VECTORF_111;
// reset the scale back to the default
_pd_scale.x = _pd_scale_roll;
_pd_scale.y = _pd_scale_pitch;
_pd_scale.z = _pd_scale_yaw;

control_monitor_update();
}
Expand Down
3 changes: 3 additions & 0 deletions libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -149,4 +149,7 @@ class AC_AttitudeControl_Multi : public AC_AttitudeControl {

// angle_p/pd boost multiplier
AP_Float _throttle_gain_boost;
AP_Float _pd_scale_roll; // PD roll default scale factor
AP_Float _pd_scale_pitch; // PD pitch default scale factor
AP_Float _pd_scale_yaw; // PD yaw default scale factor
};
9 changes: 5 additions & 4 deletions libraries/AC_PID/AC_PID.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -190,7 +190,7 @@ void AC_PID::set_notch_sample_rate(float sample_rate)
// target and error are filtered
// the derivative is then calculated and filtered
// the integral is then updated based on the setting of the limit flag
float AC_PID::update_all(float target, float measurement, float dt, bool limit, float boost)
float AC_PID::update_all(float target, float measurement, float dt, bool limit, float scale)
{
// don't process inf or NaN
if (!isfinite(target) || !isfinite(measurement)) {
Expand Down Expand Up @@ -252,9 +252,9 @@ float AC_PID::update_all(float target, float measurement, float dt, bool limit,
P_out *= _pid_info.Dmod;
D_out *= _pid_info.Dmod;

// boost output if required
P_out *= boost;
D_out *= boost;
// scale output if required
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If this is a user exposed parameter should we consider adding protection against negative values or 0.

P_out *= scale;
D_out *= scale;

_pid_info.PD_limit = false;
// Apply PD sum limit if enabled
Expand Down Expand Up @@ -318,6 +318,7 @@ void AC_PID::update_i(float dt, bool limit)
} else {
_integrator = 0.0f;
}

_pid_info.I = _integrator;
_pid_info.limit = limit;

Expand Down
2 changes: 1 addition & 1 deletion libraries/AC_PID/AC_PID.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,7 @@ class AC_PID {
// target and error are filtered
// the derivative is then calculated and filtered
// the integral is then updated based on the setting of the limit flag
float update_all(float target, float measurement, float dt, bool limit = false, float boost = 1.0f);
float update_all(float target, float measurement, float dt, bool limit = false, float scale = 1.0f);

// update_error - set error input to PID controller and calculate outputs
// target is set to zero and error is set and filtered
Expand Down
Loading