dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mc_pos_control: hotfix for possible thrust below minimum thrust setting 

Hotfix after a crash because of a heavily negative desired thrust that got clipped to 0 by the mixer.
0 thrust meant no more attitude tracking.
This commit is contained in:
Matthias Grob 2017-01-19 17:07:52 +01:00 committed by Lorenz Meier
parent 23eb07ff3e
commit db581fa5e8
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -1923,7 +1923,7 @@ MulticopterPositionControl::control_position(float dt)
float thrust_body_z = F.dot(-R_z); /* recalculate because it might have changed */
/* limit max thrust */
if (thrust_body_z > thr_max) {
if (fabsf(thrust_body_z) > thr_max) {
if (thrust_sp(2) < 0.0f) {
if (-thrust_sp(2) > thr_max) {
/* thrust Z component is too large, limit it */
@ -1946,8 +1946,8 @@ MulticopterPositionControl::control_position(float dt)
}
} else {
/* Z component is negative, going down, simply limit thrust vector */
float k = thr_max / thrust_body_z;
/* Z component is positive, going down (Z is positive down in NED), simply limit thrust vector */
float k = thr_max / fabsf(thrust_body_z);
thrust_sp *= k;
saturation_xy = true;
saturation_z = true;
@ -1956,7 +1956,7 @@ MulticopterPositionControl::control_position(float dt)
thrust_body_z = thr_max;
}
_att_sp.thrust = thrust_body_z;
_att_sp.thrust = math::max(thrust_body_z, thr_min);
/* update integrals */
if (_control_mode.flag_control_velocity_enabled && !saturation_xy) {