fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

PositionControl: temporarily remove direct thrust setpoint support

Browse Source 
This commit temporarily breaks direct horizontal thrust setpoint execution
which is used by FlightTaskManualAltitude. This is necessary to allow for
PositionControl cleanup namely calculating the whole velocity PID in one
Vector3f formula.

Having this in a separate commit is useful since it reduces indentation
of a bigger code block.
This commit is contained in:
Matthias Grob 2019-12-14 21:04:50 +01:00 committed by Kabir Mohammed
parent e53ae45188
commit 5bd8da2286
2 changed files with 28 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
View File

@ -343,9 +343,7 @@ void FlightTaskManualAltitude::_updateSetpoints()
sp.normalize();
}
_thrust_setpoint(0) = sp(0);
_thrust_setpoint(1) = sp(1);
_thrust_setpoint(2) = NAN;
_thrust_setpoint.xy() = sp;
_updateAltitudeLock();
_respectGroundSlowdown();

63
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
View File

@ -304,46 +304,37 @@ void PositionControl::_velocityControl(const float dt)
// Saturate thrust setpoint in D-direction.
_thr_sp(2) = math::constrain(thrust_desired_D, uMin, uMax);
if (PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1))) {
// Thrust set-point in NE-direction is already provided. Only
// scaling by the maximum tilt is required.
float thr_xy_max = fabsf(_thr_sp(2)) * tanf(_constraints.tilt);
_thr_sp(0) *= thr_xy_max;
_thr_sp(1) *= thr_xy_max;
// PID-velocity controller for NE-direction.
Vector2f thrust_desired_NE;
thrust_desired_NE(0) = _gain_vel_p(0) * vel_err(0) + _gain_vel_d(0) * _vel_dot(0) + _vel_int(0);
thrust_desired_NE(1) = _gain_vel_p(1) * vel_err(1) + _gain_vel_d(1) * _vel_dot(1) + _vel_int(1);
} else {
// PID-velocity controller for NE-direction.
Vector2f thrust_desired_NE;
thrust_desired_NE(0) = _gain_vel_p(0) * vel_err(0) + _gain_vel_d(0) * _vel_dot(0) + _vel_int(0);
thrust_desired_NE(1) = _gain_vel_p(1) * vel_err(1) + _gain_vel_d(1) * _vel_dot(1) + _vel_int(1);
// Get maximum allowed thrust in NE based on tilt and excess thrust.
float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_constraints.tilt);
float thrust_max_NE = sqrtf(_lim_thr_max * _lim_thr_max - _thr_sp(2) * _thr_sp(2));
thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE);
// Get maximum allowed thrust in NE based on tilt and excess thrust.
float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_constraints.tilt);
float thrust_max_NE = sqrtf(_lim_thr_max * _lim_thr_max - _thr_sp(2) * _thr_sp(2));
thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE);
// Saturate thrust in NE-direction.
_thr_sp(0) = thrust_desired_NE(0);
_thr_sp(1) = thrust_desired_NE(1);
// Saturate thrust in NE-direction.
_thr_sp(0) = thrust_desired_NE(0);
_thr_sp(1) = thrust_desired_NE(1);
if (thrust_desired_NE * thrust_desired_NE > thrust_max_NE * thrust_max_NE) {
float mag = thrust_desired_NE.length();
_thr_sp(0) = thrust_desired_NE(0) / mag * thrust_max_NE;
_thr_sp(1) = thrust_desired_NE(1) / mag * thrust_max_NE;
}
// Use tracking Anti-Windup for NE-direction: during saturation, the integrator is used to unsaturate the output
// see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990
float arw_gain = 2.f / _gain_vel_p(0);
Vector2f vel_err_lim;
vel_err_lim(0) = vel_err(0) - (thrust_desired_NE(0) - _thr_sp(0)) * arw_gain;
vel_err_lim(1) = vel_err(1) - (thrust_desired_NE(1) - _thr_sp(1)) * arw_gain;
// Update integral
_vel_int(0) += _gain_vel_i(0) * vel_err_lim(0) * dt;
_vel_int(1) += _gain_vel_i(1) * vel_err_lim(1) * dt;
if (thrust_desired_NE * thrust_desired_NE > thrust_max_NE * thrust_max_NE) {
float mag = thrust_desired_NE.length();
_thr_sp(0) = thrust_desired_NE(0) / mag * thrust_max_NE;
_thr_sp(1) = thrust_desired_NE(1) / mag * thrust_max_NE;
}
// Use tracking Anti-Windup for NE-direction: during saturation, the integrator is used to unsaturate the output
// see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990
float arw_gain = 2.f / _gain_vel_p(0);
Vector2f vel_err_lim;
vel_err_lim(0) = vel_err(0) - (thrust_desired_NE(0) - _thr_sp(0)) * arw_gain;
vel_err_lim(1) = vel_err(1) - (thrust_desired_NE(1) - _thr_sp(1)) * arw_gain;
// Update integral
_vel_int(0) += _gain_vel_i(0) * vel_err_lim(0) * dt;
_vel_int(1) += _gain_vel_i(1) * vel_err_lim(1) * dt;
}