vtol: implement throttle blending out and into transition

- blend into TECS throttle after front transition
- blend out of TECS throttle during backtransition

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/modules/vtol_att_control/standard.cpp

@@ -420,3 +420,9 @@ Standard::waiting_on_tecs()
 	// keep thrust from transition
 	_v_att_sp->thrust_body[0] = _pusher_throttle;
 };
+
+void Standard::blendThrottleAfterFrontTransition(float scale)
+{
+	const float tecs_throttle = _v_att_sp->thrust_body[0];
+	_v_att_sp->thrust_body[0] = scale * tecs_throttle + (1.0f - scale) * _pusher_throttle;
+}

src/modules/vtol_att_control/standard.h

@@ -63,6 +63,7 @@ public:
 	void update_mc_state() override;
 	void fill_actuator_outputs() override;
 	void waiting_on_tecs() override;
+	void blendThrottleAfterFrontTransition(float scale) override;
 
 private:
 

src/modules/vtol_att_control/tiltrotor.cpp

@@ -46,6 +46,9 @@ using namespace matrix;
 using namespace time_literals;
 
 #define ARSP_YAW_CTRL_DISABLE 7.0f	// airspeed at which we stop controlling yaw during a front transition
+#define BACKTRANS_THROTTLE_DOWNRAMP_DUR_S 1.0f
+#define BACKTRANS_THROTTLE_UPRAMP_DUR_S 1.0f;
+#define BACKTRANS_MOTORS_UPTILT_DUR_S 1.0f;
 
 Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
 	VtolType(attc)
@@ -281,6 +284,10 @@ void Tiltrotor::update_fw_state()
 {
 	VtolType::update_fw_state();
 
+	// this is needed to avoid a race condition when entering backtransition when the mc rate controller publishes
+	// a zero throttle value
+	_v_att_sp->thrust_body[2] = -_v_att_sp->thrust_body[0];
+
 	// make sure motors are tilted forward
 	_tilt_control = _params_tiltrotor.tilt_fw;
 }
@@ -362,6 +369,10 @@ void Tiltrotor::update_transition_state()
 
 		set_alternate_motor_state(motor_state::VALUE, ramp_down_value);
 
+
+		_thrust_transition = -_mc_virtual_att_sp->thrust_body[2];
+		_v_att_sp->thrust_body[0] = _thrust_transition;
+
 	} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) {
 		// turn on all MC motors
 		set_all_motor_state(motor_state::ENABLED);
@@ -371,10 +382,12 @@ void Tiltrotor::update_transition_state()
 			_flag_idle_mc = set_idle_mc();
 		}
 
-		// tilt rotors back
-		if (_tilt_control > _params_tiltrotor.tilt_mc) {
-			_tilt_control = _params_tiltrotor.tilt_fw -
-					fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * time_since_trans_start / 1.0f;
+		// tilt rotors back once motors are idle
+		if (time_since_trans_start > BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) {
+
+			float progress = (time_since_trans_start - BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) / BACKTRANS_MOTORS_UPTILT_DUR_S;
+			progress = math::constrain(progress, 0.0f, 1.0f);
+			_tilt_control = moveLinear(_params_tiltrotor.tilt_fw, _params_tiltrotor.tilt_mc, progress);
 		}
 
 		_mc_yaw_weight = 1.0f;
@@ -385,7 +398,12 @@ void Tiltrotor::update_transition_state()
 		}
 
 		// while we quickly rotate back the motors keep throttle at idle
-		if (time_since_trans_start < 1.0f) {
+		if (time_since_trans_start < BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) {
+			_mc_throttle_weight = 1.0f;
+			const float target_throttle = 0.0f;
+			blendThrottleDuringBacktransition(time_since_trans_start, target_throttle);
+
+		} else if (time_since_trans_start < timeUntilMotorsAreUp()) {
 			_mc_throttle_weight = 0.0f;
 			_mc_roll_weight = 0.0f;
 			_mc_pitch_weight = 0.0f;
@@ -394,7 +412,9 @@ void Tiltrotor::update_transition_state()
 			_mc_roll_weight = 1.0f;
 			_mc_pitch_weight = 1.0f;
 			// slowly ramp up throttle to avoid step inputs
-			_mc_throttle_weight = (time_since_trans_start - 1.0f) / 1.0f;
+			float progress = (time_since_trans_start - timeUntilMotorsAreUp()) / BACKTRANS_THROTTLE_UPRAMP_DUR_S;
+			progress = math::constrain(progress, 0.0f, 1.0f);
+			_mc_throttle_weight = moveLinear(0.0f, 1.0f, progress);
 		}
 	}
 
@@ -472,3 +492,26 @@ float Tiltrotor::thrust_compensation_for_tilt()
 	// increase vertical thrust by 1/cos(tilt), limit to [-1,0]
 	return math::constrain(_v_att_sp->thrust_body[2] / cosf(compensated_tilt * M_PI_2_F), -1.0f, 0.0f);
 }
+
+void Tiltrotor::blendThrottleAfterFrontTransition(float scale)
+{
+	const float tecs_throttle = _v_att_sp->thrust_body[0];
+
+	_v_att_sp->thrust_body[0] = scale * tecs_throttle + (1.0f - scale) * _thrust_transition;
+}
+
+void Tiltrotor::blendThrottleDuringBacktransition(float scale, float target_throttle)
+{
+	_v_att_sp->thrust_body[2] = -(scale * target_throttle + (1.0f - scale) * _last_thr_in_fw_mode);
+}
+
+
+float Tiltrotor::timeUntilMotorsAreUp()
+{
+	return BACKTRANS_THROTTLE_DOWNRAMP_DUR_S + BACKTRANS_MOTORS_UPTILT_DUR_S;
+}
+
+float Tiltrotor::moveLinear(float start, float stop, float progress)
+{
+	return start + progress * (stop - start);
+}

src/modules/vtol_att_control/tiltrotor.h

@@ -59,6 +59,7 @@ public:
 	void update_fw_state() override;
 	void waiting_on_tecs() override;
 	float thrust_compensation_for_tilt();
+	void blendThrottleAfterFrontTransition(float scale) override;
 
 private:
 
@@ -101,6 +102,12 @@ private:
 	float _tilt_control{0.0f};		/**< actuator value for the tilt servo */
 
 	void parameters_update() override;
+	float timeUntilMotorsAreUp();
+	float moveLinear(float start, float stop, float progress);
+
+	void blendThrottleDuringBacktransition(const float scale, const float target_throttle);
+
+
 	hrt_abstime _last_timestamp_disarmed{0}; /**< used for calculating time since arming */
 	bool _tilt_motors_for_startup{false};
 

src/modules/vtol_att_control/vtol_type.cpp

@@ -48,6 +48,8 @@
 
 using namespace matrix;
 
+#define THROTTLE_BLENDING_DUR_S 1.0f
+
 
 VtolType::VtolType(VtolAttitudeControl *att_controller) :
 	_attc(att_controller),
@@ -158,6 +160,7 @@ void VtolType::update_fw_state()
 	}
 
 	resetAccelToPitchPitchIntegrator();
+	_last_thr_in_fw_mode =  _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
 
 	VtolType::set_alternate_motor_state(motor_state::DISABLED);
 
@@ -182,6 +185,17 @@ void VtolType::update_fw_state()
 	    && _v_control_mode->flag_control_altitude_enabled) {
 
 		waiting_on_tecs();
+		_throttle_blend_start_ts = hrt_absolute_time();
+
+	} else if (shouldBlendThrottleAfterFrontTransition()) {
+		const float progress = (float)(hrt_absolute_time() - _throttle_blend_start_ts) * 1e-6f / THROTTLE_BLENDING_DUR_S;
+
+		if (progress >= 1.0f) {
+			stopBlendingThrottleAfterFrontTransition();
+
+		} else {
+			blendThrottleAfterFrontTransition(progress);
+		}
 	}
 
 	check_quadchute_condition();
@@ -193,6 +207,7 @@ void VtolType::update_transition_state()
 	_transition_dt = (float)(t_now - _last_loop_ts) / 1e6f;
 	_transition_dt = math::constrain(_transition_dt, 0.0001f, 0.02f);
 	_last_loop_ts = t_now;
+	_throttle_blend_start_ts = t_now;
 
 
 

src/modules/vtol_att_control/vtol_type.h

@@ -188,6 +188,7 @@ public:
 	 */
 	float pusher_assist();
 
+	virtual void blendThrottleAfterFrontTransition(float scale) {};
 
 	mode get_mode() {return _vtol_mode;}
 
@@ -195,7 +196,6 @@ public:
 
 	virtual void parameters_update() = 0;
 
-protected:
 	VtolAttitudeControl *_attc;
 	mode _vtol_mode;
 
@@ -229,6 +229,7 @@ protected:
 
 	// motors spinning up or cutting too fast when doing transitions.
 	float _thrust_transition = 0.0f;	// thrust value applied during a front transition (tailsitter & tiltrotor only)
+	float _last_thr_in_fw_mode = 0.0f;
 
 	float _ra_hrate = 0.0f;			// rolling average on height rate for quadchute condition
 	float _ra_hrate_sp = 0.0f;		// rolling average on height rate setpoint for quadchute condition
@@ -278,6 +279,8 @@ protected:
 private:
 
 
+	hrt_abstime _throttle_blend_start_ts{0};	// time at which we start blending between transition throttle and fixed wing throttle
+
 	/**
 	 * @brief      Stores the max pwm values given by the system.
 	 */
@@ -316,6 +319,9 @@ private:
 	bool set_motor_state(const motor_state target_state, const int32_t channel_bitmap,  const int value);
 
 	void resetAccelToPitchPitchIntegrator() { _accel_to_pitch_integ = 0.f; }
+	bool shouldBlendThrottleAfterFrontTransition() { return _throttle_blend_start_ts != 0; };
+
+	void stopBlendingThrottleAfterFrontTransition() { _throttle_blend_start_ts = 0; }
 
 };