fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-30 01:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

PositionControl: deconflict hover thrust estimator, acceleration control

Browse Source 
- Avoid constantly adjusting the velocity gains with the HTE
- Make sure the hover thrust integral update doesn't break
  even though its unit is acceleration and not unit thrust anymore

We need to convert the velocity gains to not contain/depend on the
hover thrust. In horizontal direction it doesn't make sense to scale
them with the hover thrust and in vertical direction the adjustments are
already done in the acceleration to collective thrust conversion.
This commit is contained in:
Matthias Grob
2020-03-20 11:50:25 +01:00
parent 466b5db36f
commit cdf37ca557
4 changed files with 11 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+3 -5
View File
@@ -67,7 +67,7 @@ void PositionControl::setThrustLimits(const float min, const float max)
void PositionControl::updateHoverThrust(const float hover_thrust_new)
{
_vel_int(2) += hover_thrust_new - _hover_thrust;
_vel_int(2) += (hover_thrust_new - _hover_thrust) * (CONSTANTS_ONE_G / hover_thrust_new);
setHoverThrust(hover_thrust_new);
}
@@ -147,8 +147,6 @@ void PositionControl::_velocityControl(const float dt)
Vector3f vel_error = _vel_sp - _vel;
Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int - _vel_dot.emult(_gain_vel_d);
// For backwards compatibility of the gains to non-acceleration based control, needs to be overcome with configuration conversion
acc_sp_velocity *= CONSTANTS_ONE_G / _hover_thrust;
// No control input from setpoints or corresponding states which are NAN
ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity);
@@ -179,7 +177,7 @@ 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_limited = Vector2f(_thr_sp) * (CONSTANTS_ONE_G / _hover_thrust);
const float arw_gain = 2.f / (_gain_vel_p(0) * (CONSTANTS_ONE_G / _hover_thrust));
const float arw_gain = 2.f / _gain_vel_p(0);
vel_error.xy() = Vector2f(vel_error) - (arw_gain * (Vector2f(_acc_sp) - acc_sp_xy_limited));
// Make sure integral doesn't get NAN
@@ -188,7 +186,7 @@ void PositionControl::_velocityControl(const float dt)
_vel_int += vel_error.emult(_gain_vel_i) * dt;
// limit thrust integral
_vel_int(2) = math::min(fabsf(_vel_int(2)), _lim_thr_max) * sign(_vel_int(2));
_vel_int(2) = math::min(fabsf(_vel_int(2)), CONSTANTS_ONE_G) * sign(_vel_int(2));
}
void PositionControl::_accelerationControl()