zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

use MC auto rates always in AUTO 

removed duplicate weathervaning limit
sbg
Andreas Antener 9 years ago
parent
a2b1269b27
commit
25e749de77
1 changed files with 6 additions and 12 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 18
      src/modules/mc_att_control/mc_att_control_main.cpp

18
src/modules/mc_att_control/mc_att_control_main.cpp
Unescape View File

@ -724,32 +724,26 @@ MulticopterAttitudeControl::control_attitude(float dt)
/* limit rates */ /* limit rates */
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
if (_v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) { if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
!_v_control_mode.flag_control_manual_enabled) {
_rates_sp(i) = math::constrain(_rates_sp(i), -_params.auto_rate_max(i), _params.auto_rate_max(i)); _rates_sp(i) = math::constrain(_rates_sp(i), -_params.auto_rate_max(i), _params.auto_rate_max(i));
} else { } else {
_rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i)); _rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));
} }
} }
/* weather-vane mode, dampen yaw rate */
if (_v_att_sp.disable_mc_yaw_control == true && _v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) {
float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale;
_rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max);
// prevent integrator winding up in weathervane mode
_rates_int(2) = 0.0f;
}
/* feed forward yaw setpoint rate */ /* feed forward yaw setpoint rate */
_rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff; _rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff;
/* weather-vane mode, scale down yaw rate */ /* weather-vane mode, dampen yaw rate */
if (_v_att_sp.disable_mc_yaw_control == true && _v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) { if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
_v_att_sp.disable_mc_yaw_control == true && !_v_control_mode.flag_control_manual_enabled) {
float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale; float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale;
_rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max); _rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max);
// prevent integrator winding up in weathervane mode // prevent integrator winding up in weathervane mode
_rates_int(2) = 0.0f; _rates_int(2) = 0.0f;
} }
} }
/* /*

Write Preview
Loading…
Cancel
Save