Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: update for new AC_PID settings

Browse Source 
enable filters with AP_FILTER_ENABLED
This commit is contained in:
Andy Piper 2023-10-01 18:21:00 +01:00 committed by Andrew Tridgell
parent 13ff23f39d
commit b7a969d462
4 changed files with 65 additions and 283 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

105
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
View File

@ -88,12 +88,6 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = {
// @Increment: 0.5
// @User: Advanced
// @Param: RAT_RLL_ADV
// @DisplayName: Roll Advanced parameters enable
// @Description: Roll Advanced parameters enable
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
// @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
@ -102,31 +96,15 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = {
// @User: Advanced
// @Param: RAT_RLL_NTF
// @DisplayName: Roll Target notch Filter center frequency
// @Description: Roll Target notch Filter center frequency in Hz.
// @Range: 10 495
// @Units: Hz
// @DisplayName: Roll Target notch filter index
// @Description: Roll Target notch filter index
// @Range: 1 8
// @User: Advanced
// @Param: RAT_RLL_NEF
// @DisplayName: Roll Error notch Filter center frequency
// @Description: Roll Error notch Filter center frequency in Hz.
// @Range: 10 495
// @Units: Hz
// @User: Advanced
// @Param: RAT_RLL_NBW
// @DisplayName: Roll notch Filter bandwidth
// @Description: Roll notch Filter bandwidth in Hz.
// @Range: 5 250
// @Units: Hz
// @User: Advanced
// @Param: RAT_RLL_NATT
// @DisplayName: Roll notch Filter attenuation
// @Description: Roll notch Filter attenuation in dB.
// @Range: 5 50
// @Units: dB
// @DisplayName: Roll Error notch filter index
// @Description: Roll Error notch filter index
// @Range: 1 8
// @User: Advanced
AP_SUBGROUPINFO(_pid_rate_roll, "RAT_RLL_", 2, AC_AttitudeControl_Heli, AC_HELI_PID),
@ -203,12 +181,6 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = {
// @Increment: 0.5
// @User: Advanced
// @Param: RAT_PIT_ADV
// @DisplayName: Pitch Advanced parameters enable
// @Description: Pitch Advanced parameters enable
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
// @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
@ -217,31 +189,15 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = {
// @User: Advanced
// @Param: RAT_PIT_NTF
// @DisplayName: Pitch Target notch Filter center frequency
// @Description: Pitch Target notch Filter center frequency in Hz.
// @Range: 10 495
// @Units: Hz
// @DisplayName: Pitch Target notch filter index
// @Description: Pitch Target notch filter index
// @Range: 1 8
// @User: Advanced
// @Param: RAT_PIT_NEF
// @DisplayName: Pitch Error notch Filter center frequency
// @Description: Pitch Error notch Filter center frequency in Hz.
// @Range: 10 495
// @Units: Hz
// @User: Advanced
// @Param: RAT_PIT_NBW
// @DisplayName: Pitch notch Filter bandwidth
// @Description: Pitch notch Filter bandwidth in Hz.
// @Range: 5 250
// @Units: Hz
// @User: Advanced
// @Param: RAT_PIT_NATT
// @DisplayName: Pitch notch Filter attenuation
// @Description: Pitch notch Filter attenuation in dB.
// @Range: 5 50
// @Units: dB
// @DisplayName: Pitch Error notch filter index
// @Description: Pitch Error notch filter index
// @Range: 1 8
// @User: Advanced
AP_SUBGROUPINFO(_pid_rate_pitch, "RAT_PIT_", 3, AC_AttitudeControl_Heli, AC_HELI_PID),
@ -318,12 +274,6 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = {
// @Increment: 0.5
// @User: Advanced
// @Param: RAT_YAW_ADV
// @DisplayName: Yaw Advanced parameters enable
// @Description: Yaw Advanced parameters enable
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
// @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
@ -332,31 +282,16 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = {
// @User: Advanced
// @Param: RAT_YAW_NTF
// @DisplayName: Yaw Target notch Filter center frequency
// @Description: Yaw Target notch Filter center frequency in Hz.
// @Range: 10 495
// @DisplayName: Yaw Target notch filter index
// @Description: Yaw Target notch filter index
// @Range: 1 8
// @Units: Hz
// @User: Advanced
// @Param: RAT_YAW_NEF
// @DisplayName: Yaw Error notch Filter center frequency
// @Description: Yaw Error notch Filter center frequency in Hz.
// @Range: 10 495
// @Units: Hz
// @User: Advanced
// @Param: RAT_YAW_NBW
// @DisplayName: Yaw notch Filter bandwidth
// @Description: Yaw notch Filter bandwidth in Hz.
// @Range: 5 250
// @Units: Hz
// @User: Advanced
// @Param: RAT_YAW_NATT
// @DisplayName: Yaw notch Filter attenuation
// @Description: Yaw notch Filter attenuation in dB.
// @Range: 5 50
// @Units: dB
// @DisplayName: Yaw Error notch filter index
// @Description: Yaw Error notch filter index
// @Range: 1 8
// @User: Advanced
AP_SUBGROUPINFO(_pid_rate_yaw, "RAT_YAW_", 4, AC_AttitudeControl_Heli, AC_HELI_PID),
@ -383,7 +318,7 @@ AC_AttitudeControl_Heli::AC_AttitudeControl_Heli(AP_AHRS_View &ahrs, const AP_Mu
_flags_heli.leaky_i = true;
_flags_heli.flybar_passthrough = false;
_flags_heli.tail_passthrough = false;
#if AC_PID_ADVANCED_ENABLED
#if AP_FILTER_ENABLED
set_notch_sample_rate(AP::scheduler().get_loop_rate_hz());
#endif
}
@ -629,7 +564,7 @@ void AC_AttitudeControl_Heli::input_euler_angle_roll_pitch_yaw(float euler_roll_
void AC_AttitudeControl_Heli::set_notch_sample_rate(float sample_rate)
{
#if AC_PID_ADVANCED_ENABLED
#if AP_FILTER_ENABLED
_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);

105
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
View File

@ -81,12 +81,6 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = {
// @Range: 0 1
// @Increment: 0.01
// @Param: RAT_RLL_ADV
// @DisplayName: Roll Advanced parameters enable
// @Description: Roll Advanced parameters enable
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
// @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
@ -95,31 +89,15 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = {
// @User: Advanced
// @Param: RAT_RLL_NTF
// @DisplayName: Roll Target notch Filter center frequency
// @Description: Roll Target notch Filter center frequency in Hz.
// @Range: 10 495
// @Units: Hz
// @DisplayName: Roll Target notch filter index
// @Description: Roll Target notch filter index
// @Range: 1 8
// @User: Advanced
// @Param: RAT_RLL_NEF
// @DisplayName: Roll Error notch Filter center frequency
// @Description: Roll Error notch Filter center frequency in Hz.
// @Range: 10 495
// @Units: Hz
// @User: Advanced
// @Param: RAT_RLL_NBW
// @DisplayName: Roll notch Filter bandwidth
// @Description: Roll notch Filter bandwidth in Hz.
// @Range: 5 250
// @Units: Hz
// @User: Advanced
// @Param: RAT_RLL_NATT
// @DisplayName: Roll notch Filter attenuation
// @Description: Roll notch Filter attenuation in dB.
// @Range: 5 50
// @Units: dB
// @DisplayName: Roll Error notch filter index
// @Description: Roll Error notch filter index
// @Range: 1 8
// @User: Advanced
AP_SUBGROUPINFO(_pid_rate_roll, "RAT_RLL_", 1, AC_AttitudeControl_Multi, AC_PID),
@ -196,12 +174,6 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = {
// @Range: 0 1
// @Increment: 0.01
// @Param: RAT_PIT_ADV
// @DisplayName: Pitch Advanced parameters enable
// @Description: Pitch Advanced parameters enable
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
// @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
@ -210,31 +182,15 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = {
// @User: Advanced
// @Param: RAT_PIT_NTF
// @DisplayName: Pitch Target notch Filter center frequency
// @Description: Pitch Target notch Filter center frequency in Hz.
// @Range: 10 495
// @Units: Hz
// @DisplayName: Pitch Target notch filter index
// @Description: Pitch Target notch filter index
// @Range: 1 8
// @User: Advanced
// @Param: RAT_PIT_NEF
// @DisplayName: Pitch Error notch Filter center frequency
// @Description: Pitch Error notch Filter center frequency in Hz.
// @Range: 10 495
// @Units: Hz
// @User: Advanced
// @Param: RAT_PIT_NBW
// @DisplayName: Pitch notch Filter bandwidth
// @Description: Pitch notch Filter bandwidth in Hz.
// @Range: 5 250
// @Units: Hz
// @User: Advanced
// @Param: RAT_PIT_NATT
// @DisplayName: Pitch notch Filter attenuation
// @Description: Pitch notch Filter attenuation in dB.
// @Range: 5 50
// @Units: dB
// @DisplayName: Pitch Error notch filter index
// @Description: Pitch Error notch filter index
// @Range: 1 8
// @User: Advanced
AP_SUBGROUPINFO(_pid_rate_pitch, "RAT_PIT_", 2, AC_AttitudeControl_Multi, AC_PID),
@ -311,12 +267,6 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = {
// @Range: 0 1
// @Increment: 0.01
// @Param: RAT_YAW_ADV
// @DisplayName: Yaw Advanced parameters enable
// @Description: Yaw Advanced parameters enable
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
// @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
@ -325,31 +275,16 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = {
// @User: Advanced
// @Param: RAT_YAW_NTF
// @DisplayName: Yaw Target notch Filter center frequency
// @Description: Yaw Target notch Filter center frequency in Hz.
// @Range: 10 495
// @DisplayName: Yaw Target notch filter index
// @Description: Yaw Target notch filter index
// @Range: 1 8
// @Units: Hz
// @User: Advanced
// @Param: RAT_YAW_NEF
// @DisplayName: Yaw Error notch Filter center frequency
// @Description: Yaw Error notch Filter center frequency in Hz.
// @Range: 10 495
// @Units: Hz
// @User: Advanced
// @Param: RAT_YAW_NBW
// @DisplayName: Yaw notch Filter bandwidth
// @Description: Yaw notch Filter bandwidth in Hz.
// @Range: 5 250
// @Units: Hz
// @User: Advanced
// @Param: RAT_YAW_NATT
// @DisplayName: Yaw notch Filter attenuation
// @Description: Yaw notch Filter attenuation in dB.
// @Range: 5 50
// @Units: dB
// @DisplayName: Yaw Error notch filter index
// @Description: Yaw Error notch filter index
// @Range: 1 8
// @User: Advanced
AP_SUBGROUPINFO(_pid_rate_yaw, "RAT_YAW_", 3, AC_AttitudeControl_Multi, AC_PID),
@ -391,7 +326,7 @@ AC_AttitudeControl_Multi::AC_AttitudeControl_Multi(AP_AHRS_View &ahrs, const AP_
{
AP_Param::setup_object_defaults(this, var_info);
#if AC_PID_ADVANCED_ENABLED
#if AP_FILTER_ENABLED
set_notch_sample_rate(AP::scheduler().get_loop_rate_hz());
#endif
}
@ -559,7 +494,7 @@ void AC_AttitudeControl_Multi::parameter_sanity_check()
void AC_AttitudeControl_Multi::set_notch_sample_rate(float sample_rate)
{
#if AC_PID_ADVANCED_ENABLED
#if AP_FILTER_ENABLED
_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);

104
libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp
View File

@ -79,12 +79,6 @@ const AP_Param::GroupInfo AC_AttitudeControl_Sub::var_info[] = {
// @Range: 0 1
// @Increment: 0.01
// @Param: RAT_RLL_ADV
// @DisplayName: Roll Advanced parameters enable
// @Description: Roll Advanced parameters enable
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
// @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
@ -93,31 +87,15 @@ const AP_Param::GroupInfo AC_AttitudeControl_Sub::var_info[] = {
// @User: Advanced
// @Param: RAT_RLL_NTF
// @DisplayName: Roll Target notch Filter center frequency
// @Description: Roll Target notch Filter center frequency in Hz.
// @Range: 10 495
// @Units: Hz
// @DisplayName: Roll Target notch filter index
// @Description: Roll Target notch filter index
// @Range: 1 8
// @User: Advanced
// @Param: RAT_RLL_NEF
// @DisplayName: Roll Error notch Filter center frequency
// @Description: Roll Error notch Filter center frequency in Hz.
// @Range: 10 495
// @Units: Hz
// @User: Advanced
// @Param: RAT_RLL_NBW
// @DisplayName: Roll notch Filter bandwidth
// @Description: Roll notch Filter bandwidth in Hz.
// @Range: 5 250
// @Units: Hz
// @User: Advanced
// @Param: RAT_RLL_NATT
// @DisplayName: Roll notch Filter attenuation
// @Description: Roll notch Filter attenuation in dB.
// @Range: 5 50
// @Units: dB
// @DisplayName: Roll Error notch filter index
// @Description: Roll Error notch filter index
// @Range: 1 8
// @User: Advanced
AP_SUBGROUPINFO(_pid_rate_roll, "RAT_RLL_", 1, AC_AttitudeControl_Sub, AC_PID),
@ -194,12 +172,6 @@ const AP_Param::GroupInfo AC_AttitudeControl_Sub::var_info[] = {
// @Range: 0 1
// @Increment: 0.01
// @Param: RAT_PIT_ADV
// @DisplayName: Pitch Advanced parameters enable
// @Description: Pitch Advanced parameters enable
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
// @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
@ -208,31 +180,15 @@ const AP_Param::GroupInfo AC_AttitudeControl_Sub::var_info[] = {
// @User: Advanced
// @Param: RAT_PIT_NTF
// @DisplayName: Pitch Target notch Filter center frequency
// @Description: Pitch Target notch Filter center frequency in Hz.
// @Range: 10 495
// @Units: Hz
// @DisplayName: Pitch Target notch filter index
// @Description: Pitch Target notch filter index
// @Range: 1 8
// @User: Advanced
// @Param: RAT_PIT_NEF
// @DisplayName: Pitch Error notch Filter center frequency
// @Description: Pitch Error notch Filter center frequency in Hz.
// @Range: 10 495
// @Units: Hz
// @User: Advanced
// @Param: RAT_PIT_NBW
// @DisplayName: Pitch notch Filter bandwidth
// @Description: Pitch notch Filter bandwidth in Hz.
// @Range: 5 250
// @Units: Hz
// @User: Advanced
// @Param: RAT_PIT_NATT
// @DisplayName: Pitch notch Filter attenuation
// @Description: Pitch notch Filter attenuation in dB.
// @Range: 5 50
// @Units: dB
// @DisplayName: Pitch Error notch filter index
// @Description: Pitch Error notch filter index
// @Range: 1 8
// @User: Advanced
AP_SUBGROUPINFO(_pid_rate_pitch, "RAT_PIT_", 2, AC_AttitudeControl_Sub, AC_PID),
@ -309,12 +265,6 @@ const AP_Param::GroupInfo AC_AttitudeControl_Sub::var_info[] = {
// @Range: 0 1
// @Increment: 0.01
// @Param: RAT_YAW_ADV
// @DisplayName: Yaw Advanced parameters enable
// @Description: Yaw Advanced parameters enable
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
// @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
@ -323,31 +273,15 @@ const AP_Param::GroupInfo AC_AttitudeControl_Sub::var_info[] = {
// @User: Advanced
// @Param: RAT_YAW_NTF
// @DisplayName: Yaw Target notch Filter center frequency
// @Description: Yaw Target notch Filter center frequency in Hz.
// @Range: 10 495
// @Units: Hz
// @DisplayName: Yaw Target notch filter index
// @Description: Yaw Target notch filter index
// @Range: 1 8
// @User: Advanced
// @Param: RAT_YAW_NEF
// @DisplayName: Yaw Error notch Filter center frequency
// @Description: Yaw Error notch Filter center frequency in Hz.
// @Range: 10 495
// @Units: Hz
// @User: Advanced
// @Param: RAT_YAW_NBW
// @DisplayName: Yaw notch Filter bandwidth
// @Description: Yaw notch Filter bandwidth in Hz.
// @Range: 5 250
// @Units: Hz
// @User: Advanced
// @Param: RAT_YAW_NATT
// @DisplayName: Yaw notch Filter attenuation
// @Description: Yaw notch Filter attenuation in dB.
// @Range: 5 50
// @Units: dB
// @DisplayName: Yaw Error notch filter index
// @Description: Yaw Error notch filter index
// @Range: 1 8
// @User: Advanced
AP_SUBGROUPINFO(_pid_rate_yaw, "RAT_YAW_", 3, AC_AttitudeControl_Sub, AC_PID),
@ -552,7 +486,7 @@ void AC_AttitudeControl_Sub::input_euler_angle_roll_pitch_slew_yaw(float euler_r
void AC_AttitudeControl_Sub::set_notch_sample_rate(float sample_rate)
{
#if AC_PID_ADVANCED_ENABLED
#if AP_FILTER_ENABLED
_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);

34
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -210,12 +210,6 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] = {
// @Range: 0 1000
// @Units: d%
// @Param: _ACCZ_ADV
// @DisplayName: Accel (vertical) Advanced parameters enable
// @Description: Accel (vertical) Advanced parameters enable
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
// @Param: _ACCZ_D_FF
// @DisplayName: Accel (vertical) Derivative FeedForward Gain
// @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
@ -224,31 +218,15 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] = {
// @User: Advanced
// @Param: _ACCZ_NTF
// @DisplayName: Accel (vertical) Target notch Filter center frequency
// @Description: Accel (vertical) Target notch Filter center frequency in Hz.
// @Range: 10 495
// @Units: Hz
// @DisplayName: Accel (vertical) Target notch filter index
// @Description: Accel (vertical) Target notch filter index
// @Range: 1 8
// @User: Advanced
// @Param: _ACCZ_NEF
// @DisplayName: Accel (vertical) Error notch Filter center frequency
// @Description: Accel (vertical) Error notch Filter center frequency in Hz.
// @Range: 10 495
// @Units: Hz
// @User: Advanced
// @Param: _ACCZ_NBW
// @DisplayName: Accel (vertical) notch Filter bandwidth
// @Description: Accel (vertical) notch Filter bandwidth in Hz.
// @Range: 5 250
// @Units: Hz
// @User: Advanced
// @Param: _ACCZ_NATT
// @DisplayName: Accel (vertical) notch Filter attenuation
// @Description: Accel (vertical) notch Filter attenuation in dB.
// @Range: 5 50
// @Units: dB
// @DisplayName: Accel (vertical) Error notch filter index
// @Description: Accel (vertical) Error notch filter index
// @Range: 1 8
// @User: Advanced
AP_SUBGROUPINFO(_pid_accel_z, "_ACCZ_", 4, AC_PosControl, AC_PID),