dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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.
This commit is contained in:
bresch 2023-08-18 14:13:38 +02:00 committed by Daniel Agar
parent 759f91ba52
commit dbebe7d168
1 changed files with 5 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp
View File

@ -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<float, 5> &coeff_var = _sys_id.getVariances();
const Vector3f rate_sp = _sys_id.areFiltersInitialized()