VTOL tiltrotor: move compensation in thrust for tilt into seperate fu…

…nction

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/vtol_att_control/tiltrotor.cpp

@@ -207,12 +207,11 @@ void Tiltrotor::update_mc_state()
 {
 	VtolType::update_mc_state();
 
-	// make sure motors are not tilted
-	_tilt_control = _params_tiltrotor.tilt_mc;
-
 	VtolType::pusher_assist();
 
 	_tilt_control = _hover_pusher_or_tilt_forward_actuation;
+
+	Tiltrotor::thrust_compensation_for_tilt();
 }
 
 void Tiltrotor::update_fw_state()
@@ -391,3 +390,20 @@ void Tiltrotor::fill_actuator_outputs()
 			_actuators_fw_in->control[actuator_controls_s::INDEX_YAW];
 	}
 }
+
+/*
+ * Increase combined thrust of MC propellers if motors are tilted. Assumes that all MC motors are tilted equally.
+ */
+
+void Tiltrotor::thrust_compensation_for_tilt()
+{
+
+	// only compensate for tilt angle up to 0.5 * max tilt
+	float compensated_tilt = _tilt_control;
+	compensated_tilt = compensated_tilt < 0.0f ? 0.0f : compensated_tilt;
+	compensated_tilt = compensated_tilt > 0.5f ? 0.5f : compensated_tilt;
+
+	float thrust_new = _v_att_sp->thrust_body[2] / cosf(compensated_tilt * M_PI_2_F);
+
+	_v_att_sp->thrust_body[2] = thrust_new;
+}

src/modules/vtol_att_control/tiltrotor.h

@@ -58,6 +58,7 @@ class Tiltrotor : public VtolType
 	void update_mc_state() override;
 	void update_fw_state() override;
 	void waiting_on_tecs() override;
+	void thrust_compensation_for_tilt();
 
 private:
 

src/modules/vtol_att_control/vtol_type.cpp

@@ -442,12 +442,6 @@ void VtolType::pusher_assist()
 		_hover_pusher_or_tilt_forward_actuation = _hover_pusher_or_tilt_forward_actuation > 0.9f ? 0.9f :
 				_hover_pusher_or_tilt_forward_actuation;
 
-		// compensate in combined thrust for tilt (increase thrust with tilt)
-		if (static_cast<vtol_type>(_params->vtol_type) == vtol_type::TILTROTOR) {
-			float thrust_new = _v_att_sp->thrust_body[2] / cosf(_hover_pusher_or_tilt_forward_actuation * M_PI_2_F);
-			_v_att_sp->thrust_body[2] = thrust_new;
-		}
-
 		// return the vehicle to level position
 		float pitch_new = 0.0f;