fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 00:03:33 +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
2020-01-30 07:28:18 -05:00
committed by Kabir Mohammed
parent e53ae45188
commit 5bd8da2286
2 changed files with 28 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
+1 -3
View File
@@ -343,9 +343,7 @@ void FlightTaskManualAltitude::_updateSetpoints()
sp.normalize(); sp.normalize();
} }
_thrust_setpoint(0) = sp(0); _thrust_setpoint.xy() = sp;
_thrust_setpoint(1) = sp(1);
_thrust_setpoint(2) = NAN;
_updateAltitudeLock(); _updateAltitudeLock();
_respectGroundSlowdown(); _respectGroundSlowdown();
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+27 -36
View File
@@ -304,46 +304,37 @@ void PositionControl::_velocityControl(const float dt)
// Saturate thrust setpoint in D-direction. // Saturate thrust setpoint in D-direction.
_thr_sp(2) = math::constrain(thrust_desired_D, uMin, uMax); _thr_sp(2) = math::constrain(thrust_desired_D, uMin, uMax);
if (PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1))) { // PID-velocity controller for NE-direction.
// Thrust set-point in NE-direction is already provided. Only Vector2f thrust_desired_NE;
// scaling by the maximum tilt is required. thrust_desired_NE(0) = _gain_vel_p(0) * vel_err(0) + _gain_vel_d(0) * _vel_dot(0) + _vel_int(0);
float thr_xy_max = fabsf(_thr_sp(2)) * tanf(_constraints.tilt); thrust_desired_NE(1) = _gain_vel_p(1) * vel_err(1) + _gain_vel_d(1) * _vel_dot(1) + _vel_int(1);
_thr_sp(0) *= thr_xy_max;
_thr_sp(1) *= thr_xy_max;
} else { // Get maximum allowed thrust in NE based on tilt and excess thrust.
// PID-velocity controller for NE-direction. float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_constraints.tilt);
Vector2f thrust_desired_NE; float thrust_max_NE = sqrtf(_lim_thr_max * _lim_thr_max - _thr_sp(2) * _thr_sp(2));
thrust_desired_NE(0) = _gain_vel_p(0) * vel_err(0) + _gain_vel_d(0) * _vel_dot(0) + _vel_int(0); thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE);
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. // Saturate thrust in NE-direction.
float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_constraints.tilt); _thr_sp(0) = thrust_desired_NE(0);
float thrust_max_NE = sqrtf(_lim_thr_max * _lim_thr_max - _thr_sp(2) * _thr_sp(2)); _thr_sp(1) = thrust_desired_NE(1);
thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE);
// Saturate thrust in NE-direction. if (thrust_desired_NE * thrust_desired_NE > thrust_max_NE * thrust_max_NE) {
_thr_sp(0) = thrust_desired_NE(0); float mag = thrust_desired_NE.length();
_thr_sp(1) = thrust_desired_NE(1); _thr_sp(0) = thrust_desired_NE(0) / mag * thrust_max_NE;
_thr_sp(1) = thrust_desired_NE(1) / mag * thrust_max_NE;
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;
} }
// 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;
} }