VTOL: Make pusher assist generic to enable also for tiltrotor

-move pusher assist functionality into vtol_type class and adapt it to also
work for tiltrotor VTOLs (pitch rotors down to accelerate forward)
-for tiltrotor: compensate for lost lift due to tilt by increasing the throttle
-enable pusher / tiltassist also in altitude mode

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

src/modules/vtol_att_control/tiltrotor.cpp

@@ -207,8 +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 = _forward_thrust;
+
+	Tiltrotor::thrust_compensation_for_tilt();
 }
 
 void Tiltrotor::update_fw_state()
@@ -387,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;
+}