tiltrotor tilt support: fix thrust compensation constraining

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
PX4 · Feb 7, 2020 · 9ba0d5b · 9ba0d5b
1 parent 4c9dba8
commit 9ba0d5b
Showing 1 changed file with 2 additions and 3 deletions.
diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp
@@ -395,11 +395,10 @@ void Tiltrotor::fill_actuator_outputs()
 
 float Tiltrotor::thrust_compensation_for_tilt()
 {
-
 	// only compensate for tilt angle up to 0.5 * max tilt
 	float compensated_tilt = math::constrain(_tilt_control, 0.0f, 0.5f);
 
-	// increase vertical thrust by 1/cos(tilt), limmit to [0,1]
-	return math::constrain(_v_att_sp->thrust_body[2] / cosf(compensated_tilt * M_PI_2_F), 0.0f, 1.0f);
+	// increase vertical thrust by 1/cos(tilt), limmit to [-1,0]
+	return math::constrain(_v_att_sp->thrust_body[2] / cosf(compensated_tilt * M_PI_2_F), -1.0f, 0.0f);
 
 }