diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index 64e5a10478..c093d2d40e 100644 --- a/libraries/APM_Control/AP_PitchController.cpp +++ b/libraries/APM_Control/AP_PitchController.cpp @@ -136,6 +136,46 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] = { // @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 + + // @Param: _RATE_ADV + // @DisplayName: Pitch Advanced parameters enable + // @Description: Pitch Advanced parameters enable + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + + // @Param: _RATE_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.001 0.03 + // @Increment: 0.001 + // @User: Advanced + + // @Param: _RATE_NTF + // @DisplayName: Pitch Target notch Filter center frequency + // @Description: Pitch Target notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: _RATE_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: _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 AP_SUBGROUPINFO(rate_pid, "_RATE_", 11, AP_PitchController, AC_PID), diff --git a/libraries/APM_Control/AP_PitchController.h b/libraries/APM_Control/AP_PitchController.h index 1cf1d76a32..e2900d15ad 100644 --- a/libraries/APM_Control/AP_PitchController.h +++ b/libraries/APM_Control/AP_PitchController.h @@ -40,6 +40,9 @@ public: return _pid_info; } + // set the PID notch sample rates + void set_notch_sample_rate(float sample_rate) { rate_pid.set_notch_sample_rate(sample_rate); } + static const struct AP_Param::GroupInfo var_info[]; AP_Float &kP(void) { return rate_pid.kP(); } diff --git a/libraries/APM_Control/AP_RollController.cpp b/libraries/APM_Control/AP_RollController.cpp index f2cb86cf94..0cd9e7076a 100644 --- a/libraries/APM_Control/AP_RollController.cpp +++ b/libraries/APM_Control/AP_RollController.cpp @@ -119,6 +119,46 @@ const AP_Param::GroupInfo AP_RollController::var_info[] = { // @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 + + // @Param: _RATE_ADV + // @DisplayName: Roll Advanced parameters enable + // @Description: Roll Advanced parameters enable + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + + // @Param: _RATE_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.001 0.03 + // @Increment: 0.001 + // @User: Advanced + + // @Param: _RATE_NTF + // @DisplayName: Roll Target notch Filter center frequency + // @Description: Roll Target notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: _RATE_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: _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 AP_SUBGROUPINFO(rate_pid, "_RATE_", 9, AP_RollController, AC_PID), diff --git a/libraries/APM_Control/AP_RollController.h b/libraries/APM_Control/AP_RollController.h index 85633f1d5f..35a8a331cc 100644 --- a/libraries/APM_Control/AP_RollController.h +++ b/libraries/APM_Control/AP_RollController.h @@ -40,6 +40,9 @@ public: return _pid_info; } + // set the PID notch sample rates + void set_notch_sample_rate(float sample_rate) { rate_pid.set_notch_sample_rate(sample_rate); } + static const struct AP_Param::GroupInfo var_info[]; diff --git a/libraries/APM_Control/AP_YawController.cpp b/libraries/APM_Control/AP_YawController.cpp index 47b04df4e0..04fc1890be 100644 --- a/libraries/APM_Control/AP_YawController.cpp +++ b/libraries/APM_Control/AP_YawController.cpp @@ -150,6 +150,46 @@ const AP_Param::GroupInfo AP_YawController::var_info[] = { // @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 + + // @Param: _RATE_ADV + // @DisplayName: Yaw Advanced parameters enable + // @Description: Yaw Advanced parameters enable + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + + // @Param: _RATE_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.001 0.03 + // @Increment: 0.001 + // @User: Advanced + + // @Param: _RATE_NTF + // @DisplayName: Yaw Target notch Filter center frequency + // @Description: Yaw Target notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: _RATE_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: _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 AP_SUBGROUPINFO(rate_pid, "_RATE_", 9, AP_YawController, AC_PID), diff --git a/libraries/APM_Control/AP_YawController.h b/libraries/APM_Control/AP_YawController.h index 32129dd89c..df33c9a1a6 100644 --- a/libraries/APM_Control/AP_YawController.h +++ b/libraries/APM_Control/AP_YawController.h @@ -43,6 +43,9 @@ public: return _pid_info; } + // set the PID notch sample rates + void set_notch_sample_rate(float sample_rate) { rate_pid.set_notch_sample_rate(sample_rate); } + // start/stop auto tuner void autotune_start(void); void autotune_restore(void); diff --git a/libraries/APM_Control/AR_AttitudeControl.cpp b/libraries/APM_Control/AR_AttitudeControl.cpp index 95d23fdc8f..140f8a832d 100644 --- a/libraries/APM_Control/AR_AttitudeControl.cpp +++ b/libraries/APM_Control/AR_AttitudeControl.cpp @@ -146,6 +146,46 @@ const AP_Param::GroupInfo AR_AttitudeControl::var_info[] = { // @Description: Steering control PD sum maximum. The maximum/minimum value that the sum of the P and D term can output // @Range: 0.000 1.000 // @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 + // @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 + // @Range: 0.001 0.03 + // @Increment: 0.001 + // @User: Advanced + + // @Param: _STR_RAT_NTF + // @DisplayName: Steering control Target notch Filter center frequency + // @Description: Steering control Target notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: _STR_RAT_NEF + // @DisplayName: Steering control Error notch Filter center frequency + // @Description: Steering control Error notch Filter center frequency in Hz. + // @Range: 10 495 + // @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 AP_SUBGROUPINFO(_steer_rate_pid, "_STR_RAT_", 1, AR_AttitudeControl, AC_PID), @@ -229,6 +269,46 @@ const AP_Param::GroupInfo AR_AttitudeControl::var_info[] = { // @Description: Speed control PD sum maximum. The maximum/minimum value that the sum of the P and D term can output // @Range: 0.000 1.000 // @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 + // @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 + // @Range: 0.001 0.03 + // @Increment: 0.001 + // @User: Advanced + + // @Param: _SPEED_NTF + // @DisplayName: Speed control Target notch Filter center frequency + // @Description: Speed control Target notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: _SPEED_NEF + // @DisplayName: Speed control Error notch Filter center frequency + // @Description: Speed control Error notch Filter center frequency in Hz. + // @Range: 10 495 + // @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 AP_SUBGROUPINFO(_throttle_speed_pid, "_SPEED_", 2, AR_AttitudeControl, AC_PID), @@ -372,6 +452,46 @@ const AP_Param::GroupInfo AR_AttitudeControl::var_info[] = { // @Description: Pitch control PD sum maximum. The maximum/minimum value that the sum of the P and D term can output // @Range: 0.000 1.000 // @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 + // @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 + // @Range: 0.001 0.03 + // @Increment: 0.001 + // @User: Advanced + + // @Param: _BAL_NTF + // @DisplayName: Pitch control Target notch Filter center frequency + // @Description: Pitch control Target notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: _BAL_NEF + // @DisplayName: Pitch control Error notch Filter center frequency + // @Description: Pitch control Error notch Filter center frequency in Hz. + // @Range: 10 495 + // @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 AP_SUBGROUPINFO(_pitch_to_throttle_pid, "_BAL_", 10, AR_AttitudeControl, AC_PID), @@ -463,6 +583,46 @@ const AP_Param::GroupInfo AR_AttitudeControl::var_info[] = { // @Description: Sail Heel control PD sum maximum. The maximum/minimum value that the sum of the P and D term can output // @Range: 0.000 1.000 // @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 + // @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 + // @Range: 0.001 0.03 + // @Increment: 0.001 + // @User: Advanced + + // @Param: _SAIL_NTF + // @DisplayName: Sail Heel Target notch Filter center frequency + // @Description: Sail Heel Target notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + + // @Param: _SAIL_NEF + // @DisplayName: Sail Heel Error notch Filter center frequency + // @Description: Sail Heel Error notch Filter center frequency in Hz. + // @Range: 10 495 + // @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 AP_SUBGROUPINFO(_sailboat_heel_pid, "_SAIL_", 12, AR_AttitudeControl, AC_PID), @@ -1004,3 +1164,12 @@ void AR_AttitudeControl::relax_I() _throttle_speed_pid.reset_I(); _pitch_to_throttle_pid.reset_I(); } + +void AR_AttitudeControl::set_notch_sample_rate(float sample_rate) +{ +#if AC_PID_ADVANCED_ENABLED + _steer_rate_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); +#endif +} diff --git a/libraries/APM_Control/AR_AttitudeControl.h b/libraries/APM_Control/AR_AttitudeControl.h index 2a6bc583af..54d956a657 100644 --- a/libraries/APM_Control/AR_AttitudeControl.h +++ b/libraries/APM_Control/AR_AttitudeControl.h @@ -100,6 +100,9 @@ public: AC_PID& get_sailboat_heel_pid() { return _sailboat_heel_pid; } const AP_PIDInfo& get_throttle_speed_pid_info() const { return _throttle_speed_pid_info; } + // set the PID notch sample rates + void set_notch_sample_rate(float sample_rate); + // get the slew rate value for speed and steering for oscillation detection in lua scripts void get_srate(float &steering_srate, float &speed_srate);