fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 07:31:36 +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
-9
View File
@@ -304,14 +304,6 @@ 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))) {
// 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;
} else {
// PID-velocity controller for NE-direction. // PID-velocity controller for NE-direction.
Vector2f thrust_desired_NE; 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(0) = _gain_vel_p(0) * vel_err(0) + _gain_vel_d(0) * _vel_dot(0) + _vel_int(0);
@@ -344,7 +336,6 @@ void PositionControl::_velocityControl(const float dt)
_vel_int(0) += _gain_vel_i(0) * vel_err_lim(0) * dt; _vel_int(0) += _gain_vel_i(0) * vel_err_lim(0) * dt;
_vel_int(1) += _gain_vel_i(1) * vel_err_lim(1) * dt; _vel_int(1) += _gain_vel_i(1) * vel_err_lim(1) * dt;
} }
}
void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint) const void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint) const