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

APM_Control: update for new AC_PID settings 

enable filters with AP_FILTER_ENABLED
This commit is contained in:
Andy Piper 2023-10-01 18:21:01 +01:00 committed by Andrew Tridgell
parent fc76312fc3
commit cfebae5857
4 changed files with 50 additions and 204 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/APM_Control/AP_PitchController.cpp
View File

@ -137,45 +137,23 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] = {
// @Range: 0 1 // @Range: 0 1
// @Increment: 0.01 // @Increment: 0.01
// @Param: _RATE_ADV
// @DisplayName: Pitch Advanced parameters enable
// @Description: Pitch Advanced parameters enable
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
// @Param: _RATE_D_FF // @Param: _RATE_D_FF
// @DisplayName: Pitch Derivative FeedForward Gain // @DisplayName: Pitch Derivative FeedForward Gain
// @Description: FF D Gain which produces an output that is proportional to the rate of change of the target // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
// @Range: 0.001 0.03 // @Range: 0 0.03
// @Increment: 0.001 // @Increment: 0.001
// @User: Advanced // @User: Advanced
// @Param: _RATE_NTF // @Param: _RATE_NTF
// @DisplayName: Pitch Target notch Filter center frequency // @DisplayName: Pitch Target notch filter index
// @Description: Pitch Target notch Filter center frequency in Hz. // @Description: Pitch Target notch filter index
// @Range: 10 495 // @Range: 1 8
// @Units: Hz
// @User: Advanced // @User: Advanced
// @Param: _RATE_NEF // @Param: _RATE_NEF
// @DisplayName: Pitch Error notch Filter center frequency // @DisplayName: Pitch Error notch filter index
// @Description: Pitch Error notch Filter center frequency in Hz. // @Description: Pitch Error notch filter index
// @Range: 10 495 // @Range: 1 8
// @Units: Hz
// @User: Advanced
// @Param: _RATE_NBW
// @DisplayName: Pitch notch Filter bandwidth
// @Description: Pitch notch Filter bandwidth in Hz.
// @Range: 5 250
// @Units: Hz
// @User: Advanced
// @Param: _RATE_NATT
// @DisplayName: Pitch notch Filter attenuation
// @Description: Pitch notch Filter attenuation in dB.
// @Range: 5 50
// @Units: dB
// @User: Advanced // @User: Advanced
AP_SUBGROUPINFO(rate_pid, "_RATE_", 11, AP_PitchController, AC_PID), AP_SUBGROUPINFO(rate_pid, "_RATE_", 11, AP_PitchController, AC_PID),

36
libraries/APM_Control/AP_RollController.cpp
View File

@ -120,45 +120,23 @@ const AP_Param::GroupInfo AP_RollController::var_info[] = {
// @Range: 0 1 // @Range: 0 1
// @Increment: 0.01 // @Increment: 0.01
// @Param: _RATE_ADV
// @DisplayName: Roll Advanced parameters enable
// @Description: Roll Advanced parameters enable
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
// @Param: _RATE_D_FF // @Param: _RATE_D_FF
// @DisplayName: Roll Derivative FeedForward Gain // @DisplayName: Roll Derivative FeedForward Gain
// @Description: FF D Gain which produces an output that is proportional to the rate of change of the target // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
// @Range: 0.001 0.03 // @Range: 0 0.03
// @Increment: 0.001 // @Increment: 0.001
// @User: Advanced // @User: Advanced
// @Param: _RATE_NTF // @Param: _RATE_NTF
// @DisplayName: Roll Target notch Filter center frequency // @DisplayName: Roll Target notch filter index
// @Description: Roll Target notch Filter center frequency in Hz. // @Description: Roll Target notch filter index
// @Range: 10 495 // @Range: 1 8
// @Units: Hz
// @User: Advanced // @User: Advanced
// @Param: _RATE_NEF // @Param: _RATE_NEF
// @DisplayName: Roll Error notch Filter center frequency // @DisplayName: Roll Error notch filter index
// @Description: Roll Error notch Filter center frequency in Hz. // @Description: Roll Error notch filter index
// @Range: 10 495 // @Range: 1 8
// @Units: Hz
// @User: Advanced
// @Param: _RATE_NBW
// @DisplayName: Roll notch Filter bandwidth
// @Description: Roll notch Filter bandwidth in Hz.
// @Range: 5 250
// @Units: Hz
// @User: Advanced
// @Param: _RATE_NATT
// @DisplayName: Roll notch Filter attenuation
// @Description: Roll notch Filter attenuation in dB.
// @Range: 5 50
// @Units: dB
// @User: Advanced // @User: Advanced
AP_SUBGROUPINFO(rate_pid, "_RATE_", 9, AP_RollController, AC_PID), AP_SUBGROUPINFO(rate_pid, "_RATE_", 9, AP_RollController, AC_PID),

36
libraries/APM_Control/AP_YawController.cpp
View File

@ -151,45 +151,23 @@ const AP_Param::GroupInfo AP_YawController::var_info[] = {
// @Range: 0 1 // @Range: 0 1
// @Increment: 0.01 // @Increment: 0.01
// @Param: _RATE_ADV
// @DisplayName: Yaw Advanced parameters enable
// @Description: Yaw Advanced parameters enable
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
// @Param: _RATE_D_FF // @Param: _RATE_D_FF
// @DisplayName: Yaw Derivative FeedForward Gain // @DisplayName: Yaw Derivative FeedForward Gain
// @Description: FF D Gain which produces an output that is proportional to the rate of change of the target // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
// @Range: 0.001 0.03 // @Range: 0 0.03
// @Increment: 0.001 // @Increment: 0.001
// @User: Advanced // @User: Advanced
// @Param: _RATE_NTF // @Param: _RATE_NTF
// @DisplayName: Yaw Target notch Filter center frequency // @DisplayName: Yaw Target notch filter index
// @Description: Yaw Target notch Filter center frequency in Hz. // @Description: Yaw Target notch filter index
// @Range: 10 495 // @Range: 1 8
// @Units: Hz
// @User: Advanced // @User: Advanced
// @Param: _RATE_NEF // @Param: _RATE_NEF
// @DisplayName: Yaw Error notch Filter center frequency // @DisplayName: Yaw Error notch filter index
// @Description: Yaw Error notch Filter center frequency in Hz. // @Description: Yaw Error notch filter index
// @Range: 10 495 // @Range: 1 8
// @Units: Hz
// @User: Advanced
// @Param: _RATE_NBW
// @DisplayName: Yaw notch Filter bandwidth
// @Description: Yaw notch Filter bandwidth in Hz.
// @Range: 5 250
// @Units: Hz
// @User: Advanced
// @Param: _RATE_NATT
// @DisplayName: Yaw notch Filter attenuation
// @Description: Yaw notch Filter attenuation in dB.
// @Range: 5 50
// @Units: dB
// @User: Advanced // @User: Advanced
AP_SUBGROUPINFO(rate_pid, "_RATE_", 9, AP_YawController, AC_PID), AP_SUBGROUPINFO(rate_pid, "_RATE_", 9, AP_YawController, AC_PID),

146
libraries/APM_Control/AR_AttitudeControl.cpp
View File

@ -147,45 +147,23 @@ const AP_Param::GroupInfo AR_AttitudeControl::var_info[] = {
// @Range: 0.000 1.000 // @Range: 0.000 1.000
// @Increment: 0.01 // @Increment: 0.01
// @Param: _STR_RAT_ADV
// @DisplayName: Steering control Advanced parameters enable
// @Description: Steering control Advanced parameters enable
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
// @Param: _STR_RAT_D_FF // @Param: _STR_RAT_D_FF
// @DisplayName: Steering control Derivative FeedForward Gain // @DisplayName: Steering control Derivative FeedForward Gain
// @Description: FF D Gain which produces an output that is proportional to the rate of change of the target // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
// @Range: 0.001 0.03 // @Range: 0 0.03
// @Increment: 0.001 // @Increment: 0.001
// @User: Advanced // @User: Advanced
// @Param: _STR_RAT_NTF // @Param: _STR_RAT_NTF
// @DisplayName: Steering control Target notch Filter center frequency // @DisplayName: Steering control Target notch filter index
// @Description: Steering control Target notch Filter center frequency in Hz. // @Description: Steering control Target notch filter index
// @Range: 10 495 // @Range: 1 8
// @Units: Hz
// @User: Advanced // @User: Advanced
// @Param: _STR_RAT_NEF // @Param: _STR_RAT_NEF
// @DisplayName: Steering control Error notch Filter center frequency // @DisplayName: Steering control Error notch filter index
// @Description: Steering control Error notch Filter center frequency in Hz. // @Description: Steering control Error notch filter index
// @Range: 10 495 // @Range: 1 8
// @Units: Hz
// @User: Advanced
// @Param: _STR_RAT_NBW
// @DisplayName: Steering control notch Filter bandwidth
// @Description: Steering control notch Filter bandwidth in Hz.
// @Range: 5 250
// @Units: Hz
// @User: Advanced
// @Param: _STR_RAT_NATT
// @DisplayName: Steering control notch Filter attenuation
// @Description: Steering control notch Filter attenuation in dB.
// @Range: 5 50
// @Units: dB
// @User: Advanced // @User: Advanced
AP_SUBGROUPINFO(_steer_rate_pid, "_STR_RAT_", 1, AR_AttitudeControl, AC_PID), AP_SUBGROUPINFO(_steer_rate_pid, "_STR_RAT_", 1, AR_AttitudeControl, AC_PID),
@ -270,45 +248,23 @@ const AP_Param::GroupInfo AR_AttitudeControl::var_info[] = {
// @Range: 0.000 1.000 // @Range: 0.000 1.000
// @Increment: 0.01 // @Increment: 0.01
// @Param: _SPEED_ADV
// @DisplayName: Speed control Advanced parameters enable
// @Description: Speed control Advanced parameters enable
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
// @Param: _SPEED_D_FF // @Param: _SPEED_D_FF
// @DisplayName: Speed control Derivative FeedForward Gain // @DisplayName: Speed control Derivative FeedForward Gain
// @Description: FF D Gain which produces an output that is proportional to the rate of change of the target // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
// @Range: 0.001 0.03 // @Range: 0 0.03
// @Increment: 0.001 // @Increment: 0.001
// @User: Advanced // @User: Advanced
// @Param: _SPEED_NTF // @Param: _SPEED_NTF
// @DisplayName: Speed control Target notch Filter center frequency // @DisplayName: Speed control Target notch filter index
// @Description: Speed control Target notch Filter center frequency in Hz. // @Description: Speed control Target notch filter index
// @Range: 10 495 // @Range: 1 8
// @Units: Hz
// @User: Advanced // @User: Advanced
// @Param: _SPEED_NEF // @Param: _SPEED_NEF
// @DisplayName: Speed control Error notch Filter center frequency // @DisplayName: Speed control Error notch filter index
// @Description: Speed control Error notch Filter center frequency in Hz. // @Description: Speed control Error notch filter index
// @Range: 10 495 // @Range: 1 8
// @Units: Hz
// @User: Advanced
// @Param: _SPEED_NBW
// @DisplayName: Speed control notch Filter bandwidth
// @Description: Speed control notch Filter bandwidth in Hz.
// @Range: 5 250
// @Units: Hz
// @User: Advanced
// @Param: _SPEED_NATT
// @DisplayName: Speed control notch Filter attenuation
// @Description: Speed control notch Filter attenuation in dB.
// @Range: 5 50
// @Units: dB
// @User: Advanced // @User: Advanced
AP_SUBGROUPINFO(_throttle_speed_pid, "_SPEED_", 2, AR_AttitudeControl, AC_PID), AP_SUBGROUPINFO(_throttle_speed_pid, "_SPEED_", 2, AR_AttitudeControl, AC_PID),
@ -453,45 +409,23 @@ const AP_Param::GroupInfo AR_AttitudeControl::var_info[] = {
// @Range: 0.000 1.000 // @Range: 0.000 1.000
// @Increment: 0.01 // @Increment: 0.01
// @Param: _BAL_ADV
// @DisplayName: Pitch control Advanced parameters enable
// @Description: Pitch control Advanced parameters enable
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
// @Param: _BAL_D_FF // @Param: _BAL_D_FF
// @DisplayName: Pitch control Derivative FeedForward Gain // @DisplayName: Pitch control Derivative FeedForward Gain
// @Description: FF D Gain which produces an output that is proportional to the rate of change of the target // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
// @Range: 0.001 0.03 // @Range: 0 0.03
// @Increment: 0.001 // @Increment: 0.001
// @User: Advanced // @User: Advanced
// @Param: _BAL_NTF // @Param: _BAL_NTF
// @DisplayName: Pitch control Target notch Filter center frequency // @DisplayName: Pitch control Target notch filter index
// @Description: Pitch control Target notch Filter center frequency in Hz. // @Description: Pitch control Target notch filter index
// @Range: 10 495 // @Range: 1 8
// @Units: Hz
// @User: Advanced // @User: Advanced
// @Param: _BAL_NEF // @Param: _BAL_NEF
// @DisplayName: Pitch control Error notch Filter center frequency // @DisplayName: Pitch control Error notch filter index
// @Description: Pitch control Error notch Filter center frequency in Hz. // @Description: Pitch control Error notch filter index
// @Range: 10 495 // @Range: 1 8
// @Units: Hz
// @User: Advanced
// @Param: _BAL_NBW
// @DisplayName: Pitch control notch Filter bandwidth
// @Description: Pitch control notch Filter bandwidth in Hz.
// @Range: 5 250
// @Units: Hz
// @User: Advanced
// @Param: _BAL_NATT
// @DisplayName: Pitch control notch Filter attenuation
// @Description: Pitch control notch Filter attenuation in dB.
// @Range: 5 50
// @Units: dB
// @User: Advanced // @User: Advanced
AP_SUBGROUPINFO(_pitch_to_throttle_pid, "_BAL_", 10, AR_AttitudeControl, AC_PID), AP_SUBGROUPINFO(_pitch_to_throttle_pid, "_BAL_", 10, AR_AttitudeControl, AC_PID),
@ -584,45 +518,23 @@ const AP_Param::GroupInfo AR_AttitudeControl::var_info[] = {
// @Range: 0.000 1.000 // @Range: 0.000 1.000
// @Increment: 0.01 // @Increment: 0.01
// @Param: _SAIL_ADV
// @DisplayName: Sail Heel Advanced parameters enable
// @Description: Sail Heel Advanced parameters enable
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
// @Param: _SAIL_D_FF // @Param: _SAIL_D_FF
// @DisplayName: Sail Heel Derivative FeedForward Gain // @DisplayName: Sail Heel Derivative FeedForward Gain
// @Description: FF D Gain which produces an output that is proportional to the rate of change of the target // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
// @Range: 0.001 0.03 // @Range: 0 0.03
// @Increment: 0.001 // @Increment: 0.001
// @User: Advanced // @User: Advanced
// @Param: _SAIL_NTF // @Param: _SAIL_NTF
// @DisplayName: Sail Heel Target notch Filter center frequency // @DisplayName: Sail Heel Target notch filter index
// @Description: Sail Heel Target notch Filter center frequency in Hz. // @Description: Sail Heel Target notch filter index
// @Range: 10 495 // @Range: 1 8
// @Units: Hz
// @User: Advanced // @User: Advanced
// @Param: _SAIL_NEF // @Param: _SAIL_NEF
// @DisplayName: Sail Heel Error notch Filter center frequency // @DisplayName: Sail Heel Error notch filter index
// @Description: Sail Heel Error notch Filter center frequency in Hz. // @Description: Sail Heel Error notch filter index
// @Range: 10 495 // @Range: 1 8
// @Units: Hz
// @User: Advanced
// @Param: _SAIL_NBW
// @DisplayName: Sail Heel notch Filter bandwidth
// @Description: Sail Heel notch Filter bandwidth in Hz.
// @Range: 5 250
// @Units: Hz
// @User: Advanced
// @Param: _SAIL_NATT
// @DisplayName: Sail Heel notch Filter attenuation
// @Description: Sail Heel notch Filter attenuation in dB.
// @Range: 5 50
// @Units: dB
// @User: Advanced // @User: Advanced
AP_SUBGROUPINFO(_sailboat_heel_pid, "_SAIL_", 12, AR_AttitudeControl, AC_PID), AP_SUBGROUPINFO(_sailboat_heel_pid, "_SAIL_", 12, AR_AttitudeControl, AC_PID),
@ -1167,7 +1079,7 @@ void AR_AttitudeControl::relax_I()
void AR_AttitudeControl::set_notch_sample_rate(float sample_rate) void AR_AttitudeControl::set_notch_sample_rate(float sample_rate)
{ {
#if AC_PID_ADVANCED_ENABLED #if AP_FILTER_ENABLED
_steer_rate_pid.set_notch_sample_rate(sample_rate); _steer_rate_pid.set_notch_sample_rate(sample_rate);
_throttle_speed_pid.set_notch_sample_rate(sample_rate); _throttle_speed_pid.set_notch_sample_rate(sample_rate);
_pitch_to_throttle_pid.set_notch_sample_rate(sample_rate); _pitch_to_throttle_pid.set_notch_sample_rate(sample_rate);