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()