Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 21:55:34 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

mc_position_control: avoid calculating arw if not needed

Browse Source
This commit is contained in:
bazooka joe
2024-06-20 15:20:15 +03:00
committed by Mathieu Bresciani
parent 397ff4a102
commit b48aca10a0
1 changed files with 6 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+6 -6
View File
@@ -185,15 +185,15 @@ void PositionControl::_velocityControl(const float dt)
// Use tracking Anti-Windup for horizontal direction: during saturation, the integrator is used to unsaturate the output
// see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990
const Vector2f acc_sp_xy_produced = Vector2f(_thr_sp) * (CONSTANTS_ONE_G / _hover_thrust);
const float arw_gain = 2.f / _gain_vel_p(0);
// The produced acceleration can be greater or smaller than the desired acceleration due to the saturations and the actual vertical thrust (computed independently).
// The ARW loop needs to run if the signal is saturated only.
const Vector2f acc_sp_xy = _acc_sp.xy();
const Vector2f acc_limited_xy = (acc_sp_xy.norm_squared() > acc_sp_xy_produced.norm_squared())
? acc_sp_xy_produced
: acc_sp_xy;
vel_error.xy() = Vector2f(vel_error) - arw_gain * (acc_sp_xy - acc_limited_xy);
if (_acc_sp.xy().norm_squared() > acc_sp_xy_produced.norm_squared()) {
const float arw_gain = 2.f / _gain_vel_p(0);
const Vector2f acc_sp_xy = _acc_sp.xy();
vel_error.xy() = Vector2f(vel_error) - arw_gain * (acc_sp_xy - acc_sp_xy_produced);
}
// Make sure integral doesn't get NAN
ControlMath::setZeroIfNanVector3f(vel_error);