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

PositionControl: readd direct thrust setpoint support

Browse Source 
This restores original horizontal thrust setpoint execution
that I intentionally broke three commits ago. It's necessary
for correct stick to tilt mapping of FlightTaskManualAltitude.
This commit is contained in:
Matthias Grob
2019-12-15 16:43:15 +01:00
committed by Kabir Mohammed
parent 282d46efcb
commit 83653d6581
1 changed files with 6 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+6
View File
@@ -281,6 +281,12 @@ void PositionControl::_velocityControl(const float dt)
Vector3f vel_error = _vel_sp - _vel;
Vector3f thr_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int + _vel_dot.emult(_gain_vel_d);
thr_sp_velocity -= Vector3f(0.f, 0.f, _hover_thrust);
if (PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1)) && PX4_ISFINITE(thr_sp_velocity(2))) {
// Thrust set-point in NE-direction from FlightTaskManualAltitude is provided. Scaling by the maximum tilt is required.
_thr_sp.xy() = Vector2f(_thr_sp) * fabsf(thr_sp_velocity(2)) * tanf(_constraints.tilt);
}
// Velocity and feed-forward thrust setpoints or velocity states being NAN results in them not having an influence
ControlMath::addIfNotNanVector3f(_thr_sp, thr_sp_velocity);