fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-14 03:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

fw-atune: limit max test signal

Browse Source 
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.
This commit is contained in:
bresch
2023-08-18 13:32:43 +02:00
committed by Daniel Agar
parent 74a54b3b12
commit 759f91ba52
2 changed files with 10 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp
+5 -3
View File
@@ -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();
}
src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp
+5 -1
View File
@@ -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;