VTOL pusher/tilt support: addressed review comments:

- remove gloabl variable and instead give back forward_thrust from function
 - give back increased vertical thrust from fct thrust_compensation_for_tilt()

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

src/modules/vtol_att_control/standard.cpp

@@ -317,9 +317,7 @@ void Standard::update_mc_state()
 {
 	VtolType::update_mc_state();
 
-	VtolType::pusher_assist();
+	_pusher_throttle = VtolType::pusher_assist();
-
-	_pusher_throttle = _forward_thrust;
 }
 
 void Standard::update_fw_state()

src/modules/vtol_att_control/tiltrotor.cpp

@@ -207,11 +207,9 @@ void Tiltrotor::update_mc_state()
 {
 	VtolType::update_mc_state();
 
-	VtolType::pusher_assist();
+	_tilt_control = VtolType::pusher_assist();
 
-	_tilt_control = _forward_thrust;
+	_v_att_sp->thrust_body[2] = Tiltrotor::thrust_compensation_for_tilt();
-
-	Tiltrotor::thrust_compensation_for_tilt();
 }
 
 void Tiltrotor::update_fw_state()
@@ -395,7 +393,7 @@ void Tiltrotor::fill_actuator_outputs()
  * Increase combined thrust of MC propellers if motors are tilted. Assumes that all MC motors are tilted equally.
  */
 
-void Tiltrotor::thrust_compensation_for_tilt()
+float Tiltrotor::thrust_compensation_for_tilt()
 {
 
 	// only compensate for tilt angle up to 0.5 * max tilt
@@ -403,7 +401,6 @@ void Tiltrotor::thrust_compensation_for_tilt()
 	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);
+	return _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,7 +58,7 @@ public:
 	void update_mc_state() override;
 	void update_fw_state() override;
 	void waiting_on_tecs() override;
-	void thrust_compensation_for_tilt();
+	float thrust_compensation_for_tilt();
 
 private:
 

src/modules/vtol_att_control/vtol_type.cpp

@@ -385,28 +385,24 @@ bool VtolType::is_channel_set(const int channel, const int target)
 }
 
 
-void VtolType::pusher_assist()
+float VtolType::pusher_assist()
 {
-
-	// normalized pusher support throttle (standard VTOL) or tilt (tiltrotor), initialize to 0
-	_forward_thrust = 0.0f;
-
 	// if the thrust scale param is zero or the drone is not in some position or altitude control mode,
 	// then the pusher-for-pitch strategy is disabled and we can return
 	if (_params->forward_thrust_scale < FLT_EPSILON || !(_v_control_mode->flag_control_position_enabled
 			|| _v_control_mode->flag_control_altitude_enabled)) {
-		return;
+		return 0.0f;
 	}
 
 	// Do not engage pusher assist during a failsafe event (could be a problem with the fixed wing drive)
 	if (_attc->get_vtol_vehicle_status()->vtol_transition_failsafe) {
-		return;
+		return 0.0f;
 	}
 
 	// disable pusher assist during landing
 	if (_attc->get_pos_sp_triplet()->current.valid
 	    && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
-		return;
+		return 0.0f;
 	}
 
 	const Dcmf R(Quatf(_v_att->q));
@@ -427,16 +423,19 @@ void VtolType::pusher_assist()
 	// this value corresponds to the amount the vehicle would try to pitch down
 	float pitch_down = atan2f(body_z_sp(0), body_z_sp(2));
 
+	// normalized pusher support throttle (standard VTOL) or tilt (tiltrotor), initialize to 0
+	float forward_thrust = 0.0f;
+
 	// only allow pitching down up to threshold, the rest of the desired
 	// forward acceleration will be compensated by the pusher/tilt
 	if (pitch_down < -_params->down_pitch_max) {
 		// desired roll angle in heading frame stays the same
 		float roll_new = -asinf(body_z_sp(1));
 
-		_forward_thrust = (sinf(-pitch_down) - sinf(_params->down_pitch_max)) * _params->forward_thrust_scale;
+		forward_thrust = (sinf(-pitch_down) - sinf(_params->down_pitch_max)) * _params->forward_thrust_scale;
 		// limit forward actuation to [0, 0.9]
-		_forward_thrust = _forward_thrust < 0.0f ? 0.0f : _forward_thrust;
+		forward_thrust = forward_thrust < 0.0f ? 0.0f : forward_thrust;
-		_forward_thrust = _forward_thrust > 0.9f ? 0.9f : _forward_thrust;
+		forward_thrust = forward_thrust > 0.9f ? 0.9f : forward_thrust;
 
 		// return the vehicle to level position
 		float pitch_new = 0.0f;
@@ -460,4 +459,6 @@ void VtolType::pusher_assist()
 		_v_att_sp->q_d_valid = true;
 	}
 
+	return forward_thrust;
+
 }

src/modules/vtol_att_control/vtol_type.h

@@ -168,7 +168,7 @@ public:
 	/**
 	 * Pusher assist in hover (pusher/pull for standard VTOL, motor tilt for tiltrotor)
 	 */
-	void pusher_assist();
+	float pusher_assist();
 
 
 	mode get_mode() {return _vtol_mode;}
@@ -222,8 +222,6 @@ protected:
 
 	motor_state _motor_state = motor_state::DISABLED;
 
-	float _forward_thrust = 0.0f; // normalized pusher throttle (standard VTOL) or tilt (tiltrotor)
-
 
 
 	/**