From dbebe7d1689e5174490d2e34a6b164a3955eec0b Mon Sep 17 00:00:00 2001 From: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)> Date: Fri, 18 Aug 2023 14:13:38 +0200 Subject: [PATCH] fw-atune: use same attitude P rule as for multirotors The current rule was producing too high gains. Also constrain the value using the prameter's limits. --- .../fw_autotune_attitude_control.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) 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 f5832ea2af..eee05880fd 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 @@ -180,7 +180,11 @@ void FwAutotuneAttitudeControl::Run() Vector3f kid = pid_design::computePidGmvc(num_design, den, _sample_interval_avg, 0.2f, 0.f, 0.4f); _kiff(0) = kid(0); _kiff(1) = kid(1); - _attitude_p = 8.f / (M_PI_F * (_kiff(2) + _kiff(0))); // Maximum control power at an attitude error of pi/8 + + // To compute the attitude gain, use the following empirical rule: + // "An error of 60 degrees should produce the maximum control output" + // or K_att * (K_rate + K_ff) * rad(60) = 1 + _attitude_p = math::constrain(1.f / (math::radians(60.f) * (_kiff(0) + _kiff(2))), 1.f, 5.f); const Vector &coeff_var = _sys_id.getVariances(); const Vector3f rate_sp = _sys_id.areFiltersInitialized()