From 5bd8da22865d9f1d5daa9a262e4e6d74a8aadb2a Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Sat, 14 Dec 2019 21:04:50 +0100 Subject: [PATCH] PositionControl: temporarily remove direct thrust setpoint support 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. --- .../FlightTaskManualAltitude.cpp | 4 +- .../PositionControl/PositionControl.cpp | 63 ++++++++----------- 2 files changed, 28 insertions(+), 39 deletions(-) diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index cd30f459d5..3880cc00ca 100644 --- a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -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(); diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index ff67df4b61..5f435c406c 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -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; }