From 759f91ba523eb24613b2b1171a2dd090ea9b6781 Mon Sep 17 00:00:00 2001
From: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)>
Date: Fri, 18 Aug 2023 13:32:43 +0200
Subject: [PATCH] fw-atune: limit max test signal

Whe the P gain is high, the test signal is large too. However, it should
not exceed what is allowed from the max rate parameter.
---
 .../fw_autotune_attitude_control.cpp                      | 8 +++++---
 .../fw_autotune_attitude_control.hpp                      | 6 +++++-
 2 files changed, 10 insertions(+), 4 deletions(-)

diff --git a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp
index ec274f28da..f5832ea2af 100644
--- a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp
+++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp
@@ -638,19 +638,21 @@ const Vector3f FwAutotuneAttitudeControl::getIdentificationSignal()
 	if (_state == state::roll || _state == state::test) {
 		// Scale the signal such that the attitude controller is
 		// able to cancel it completely at an attitude error of pi/8
-		signal_scaled = signal * M_PI_F / (8.f * _param_fw_r_tc.get());
+		signal_scaled = math::min(signal * M_PI_F / (8.f * _param_fw_r_tc.get()), math::radians(_param_fw_r_rmax.get()));
 		rate_sp(0) = signal_scaled - _signal_filter.getState();
 	}
 
 	if (_state ==  state::pitch || _state == state::test) {
-		signal_scaled = signal * M_PI_F / (8.f * _param_fw_p_tc.get());
+		const float pitch_rate_max_deg = math::min(_param_fw_p_rmax_pos.get(), _param_fw_p_rmax_neg.get());
+		signal_scaled = math::min(signal * M_PI_F / (8.f * _param_fw_p_tc.get()), math::radians(pitch_rate_max_deg));
 		rate_sp(1) = signal_scaled - _signal_filter.getState();
 
 	}
 
 	if (_state ==  state::yaw) {
 		// Do not send a signal that produces more than a full deflection of the rudder
-		signal_scaled = math::min(signal, 1.f / (_param_fw_yr_ff.get() + _param_fw_yr_p.get()));
+		signal_scaled = math::min(signal, 1.f / (_param_fw_yr_ff.get() + _param_fw_yr_p.get()),
+					  math::radians(_param_fw_y_rmax.get()));
 		rate_sp(2) = signal_scaled - _signal_filter.getState();
 	}
 
diff --git a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp
index 213518ef72..9be5f55596 100644
--- a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp
+++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp
@@ -193,14 +193,18 @@ private:
 		(ParamFloat<px4::params::FW_RR_P>) _param_fw_rr_p,
 		(ParamFloat<px4::params::FW_RR_I>) _param_fw_rr_i,
 		(ParamFloat<px4::params::FW_RR_FF>) _param_fw_rr_ff,
+		(ParamFloat<px4::params::FW_R_RMAX>) _param_fw_r_rmax,
 		(ParamFloat<px4::params::FW_R_TC>) _param_fw_r_tc,
 		(ParamFloat<px4::params::FW_PR_P>) _param_fw_pr_p,
 		(ParamFloat<px4::params::FW_PR_I>) _param_fw_pr_i,
 		(ParamFloat<px4::params::FW_PR_FF>) _param_fw_pr_ff,
+		(ParamFloat<px4::params::FW_P_RMAX_POS>) _param_fw_p_rmax_pos,
+		(ParamFloat<px4::params::FW_P_RMAX_NEG>) _param_fw_p_rmax_neg,
 		(ParamFloat<px4::params::FW_P_TC>) _param_fw_p_tc,
 		(ParamFloat<px4::params::FW_YR_P>) _param_fw_yr_p,
 		(ParamFloat<px4::params::FW_YR_I>) _param_fw_yr_i,
-		(ParamFloat<px4::params::FW_YR_FF>) _param_fw_yr_ff
+		(ParamFloat<px4::params::FW_YR_FF>) _param_fw_yr_ff,
+		(ParamFloat<px4::params::FW_Y_RMAX>) _param_fw_y_rmax
 	)
 
 	static constexpr float _publishing_dt_s = 100e-3f;