support for pitch control via TECS during front transitions

- only applicable for standard VTOL and tiltrotor

Signed-off-by: Roman <[email protected]>

msg/tecs_status.msg

@@ -6,6 +6,7 @@ uint8 TECS_MODE_LAND = 3
 uint8 TECS_MODE_LAND_THROTTLELIM = 4
 uint8 TECS_MODE_BAD_DESCENT = 5
 uint8 TECS_MODE_CLIMBOUT = 6
+uint8 TECS_MODE_VTOL_FRONT_TRANSITION = 7
 
 
 float32 altitude_sp

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -561,6 +561,10 @@ FixedwingPositionControl::tecs_status_publish()
 	case TECS::ECL_TECS_MODE_CLIMBOUT:
 		t.mode = tecs_status_s::TECS_MODE_CLIMBOUT;
 		break;
+
+	case TECS::ECL_TECS_MODE_VTOL_FRONT_TRANSITION:
+		t.mode = tecs_status_s::TECS_MODE_VTOL_FRONT_TRANSITION;
+		break;
 	}
 
 	t.altitude_sp = _tecs.hgt_setpoint_adj();
@@ -1925,12 +1929,13 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
 	// do not run TECS if we are not in air
 	bool run_tecs = !_vehicle_land_detected.landed;
 
-	// do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition
-	// (it should also not run during VTOL blending because airspeed is too low still)
+
 	if (_vehicle_status.is_vtol) {
-		if (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode) {
-			run_tecs = false;
-		}
+		// run TECS during transitions
+		run_tecs &= (!_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode);
+
+		// for tailsitters we don't want TECS do run during transitions
+		run_tecs &= !(_parameters.vtol_type == vtol_type::TAILSITTER && _vehicle_status.in_transition_mode);
 
 		if (_vehicle_status.in_transition_mode) {
 			// we're in transition
@@ -2029,6 +2034,18 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
 		}
 	}
 
+	if (_vehicle_status.in_transition_to_fw && !_tecs.in_front_transition()) {
+		_tecs.activate_front_transition();
+		_transition_alt_sp_tecs = alt_sp;
+
+	} else if (!_vehicle_status.in_transition_to_fw && _tecs.in_front_transition()) {
+		_tecs.deactivate_front_transition();
+	}
+
+	if (_tecs.in_front_transition()) {
+		alt_sp = _transition_alt_sp_tecs;
+	}
+
 	_tecs.update_pitch_throttle(_R_nb, pitch_for_tecs,
 				    _global_pos.alt, alt_sp,
 				    airspeed_sp, _airspeed, _eas2tas,

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -250,6 +250,7 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 
 	float _asp_after_transition{0.0f};
 	bool _was_in_transition{false};
+	float _transition_alt_sp_tecs{0.0f}; 		///< altitude sp for TECS during front transition, applies to standard vtol and tiltrotors
 
 	// estimator reset counters
 	uint8_t _pos_reset_counter{0};				///< captures the number of times the estimator has reset the horizontal position

src/modules/vtol_att_control/standard.cpp

@@ -231,15 +231,29 @@ void Standard::update_transition_state()
 	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
 
 	if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
-		if (_params_standard.pusher_ramp_dt <= 0.0f) {
-			// just set the final target throttle value
-			_pusher_throttle = _params->front_trans_throttle;
 
-		} else if (_pusher_throttle <= _params->front_trans_throttle) {
-			// ramp up throttle to the target throttle value
-			_pusher_throttle = _params->front_trans_throttle * time_since_trans_start / _params_standard.pusher_ramp_dt;
+		if (_v_control_mode->flag_control_altitude_enabled) {
+			matrix::Quatf q_sp(matrix::Eulerf(_fw_virtual_att_sp->roll_body, _fw_virtual_att_sp->pitch_body,
+							  _mc_virtual_att_sp->yaw_body));
+			q_sp.copyTo(_v_att_sp->q_d);
+			_v_att_sp->yaw_body = _mc_virtual_att_sp->yaw_body;
+			_pusher_throttle = _fw_virtual_att_sp->thrust_body[0];
+
+		} else {
+			if (_params_standard.pusher_ramp_dt <= 0.0f) {
+				// just set the final target throttle value
+				_pusher_throttle = _params->front_trans_throttle;
+
+			} else if (_pusher_throttle <= _params->front_trans_throttle) {
+				// ramp up throttle to the target throttle value
+				_pusher_throttle = _params->front_trans_throttle * time_since_trans_start / _params_standard.pusher_ramp_dt;
+			}
 		}
 
+		_v_att_sp->thrust_body[0] = _pusher_throttle;
+		_v_att_sp->thrust_body[1] = 0.0f;
+		_v_att_sp->thrust_body[2] = _mc_virtual_att_sp->thrust_body[2];
+
 		// do blending of mc and fw controls if a blending airspeed has been provided and the minimum transition time has passed
 		if (_airspeed_trans_blend_margin > 0.0f &&
 		    _airspeed->indicated_airspeed_m_s > 0.0f &&
@@ -256,12 +270,6 @@ void Standard::update_transition_state()
 
 		}
 
-		// ramp up FW_PSP_OFF
-		_v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset * (1.0f - mc_weight);
-		const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
-		q_sp.copyTo(_v_att_sp->q_d);
-		_v_att_sp->q_d_valid = true;
-
 		// check front transition timeout
 		if (_params->front_trans_timeout > FLT_EPSILON) {
 			if (time_since_trans_start > _params->front_trans_timeout) {
@@ -297,6 +305,8 @@ void Standard::update_transition_state()
 		if (_motor_state != ENABLED) {
 			_motor_state = set_motor_state(_motor_state, ENABLED);
 		}
+
+		_v_att_sp->thrust_body[0] = _pusher_throttle;
 	}
 
 	mc_weight = math::constrain(mc_weight, 0.0f, 1.0f);
@@ -425,8 +435,7 @@ void Standard::fill_actuator_outputs()
 		_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
 			_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH];
 		// yaw
-		_actuators_out_1->control[actuator_controls_s::INDEX_YAW] =
-			_actuators_fw_in->control[actuator_controls_s::INDEX_YAW];
+		_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW];
 
 		_actuators_out_1->control[actuator_controls_s::INDEX_AIRBRAKES] = _reverse_output;
 
@@ -467,10 +476,3 @@ void Standard::fill_actuator_outputs()
 
 
 }
-
-void
-Standard::waiting_on_tecs()
-{
-	// keep thrust from transition
-	_v_att_sp->thrust_body[0] = _pusher_throttle;
-};

src/modules/vtol_att_control/standard.h

@@ -62,7 +62,6 @@ class Standard : public VtolType
 	void update_fw_state() override;
 	void update_mc_state() override;
 	void fill_actuator_outputs() override;
-	void waiting_on_tecs() override;
 
 private:
 

src/modules/vtol_att_control/tailsitter.cpp

@@ -200,7 +200,7 @@ void Tailsitter::update_transition_state()
 
 		} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
 			// initial attitude setpoint for the transition should be with wings level
-			_q_trans_start = Eulerf(0.0f, _mc_virtual_att_sp->pitch_body, _mc_virtual_att_sp->yaw_body);
+			_q_trans_start = Eulerf(0.0f, Eulerf(Quatf(_v_att->q)).theta(), Eulerf(Quatf(_v_att->q)).psi());
 			Vector3f x = Dcmf(Quatf(_v_att->q)) * Vector3f(1, 0, 0);
 			_trans_rot_axis = -x.cross(Vector3f(0, 0, -1));
 		}

src/modules/vtol_att_control/tailsitter.h

@@ -59,7 +59,7 @@ class Tailsitter : public VtolType
 	void update_transition_state() override;
 	void update_fw_state() override;
 	void fill_actuator_outputs() override;
-	void waiting_on_tecs() override;
+	void waiting_on_tecs();
 
 private:
 

src/modules/vtol_att_control/tiltrotor.cpp

@@ -226,6 +226,16 @@ void Tiltrotor::update_transition_state()
 	}
 
 	if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
+		// copy virtual attitude setpoint to real attitude setpoint (we use multicopter att sp)
+		memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
+		matrix::Quatf q_sp(matrix::Eulerf(_fw_virtual_att_sp->roll_body, _fw_virtual_att_sp->pitch_body,
+						  _mc_virtual_att_sp->yaw_body));
+		_v_att_sp->yaw_body = _mc_virtual_att_sp->yaw_body;
+		q_sp.copyTo(_v_att_sp->q_d);
+
+		_v_att_sp->thrust_body[0] = _fw_virtual_att_sp->thrust_body[0];
+		_v_att_sp->thrust_body[2] = _mc_virtual_att_sp->thrust_body[2];
+
 		// for the first part of the transition the rear rotors are enabled
 		if (_motor_state != ENABLED) {
 			_motor_state = set_motor_state(_motor_state, ENABLED);
@@ -264,6 +274,15 @@ void Tiltrotor::update_transition_state()
 		_thrust_transition = -_mc_virtual_att_sp->thrust_body[2];
 
 	} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) {
+		// copy virtual attitude setpoint to real attitude setpoint (we use multicopter att sp)
+		memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
+		matrix::Quatf q_sp(matrix::Eulerf(_fw_virtual_att_sp->roll_body, _fw_virtual_att_sp->pitch_body,
+						  _mc_virtual_att_sp->yaw_body));
+		_v_att_sp->yaw_body = _mc_virtual_att_sp->yaw_body;
+		q_sp.copyTo(_v_att_sp->q_d);
+
+		_v_att_sp->thrust_body[0] = _fw_virtual_att_sp->thrust_body[0];
+		_v_att_sp->thrust_body[2] = _mc_virtual_att_sp->thrust_body[2];
 		// the plane is ready to go into fixed wing mode, tilt the rotors forward completely
 		_tilt_control = _params_tiltrotor.tilt_transition +
 				fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * time_since_trans_start /
@@ -283,6 +302,8 @@ void Tiltrotor::update_transition_state()
 		_thrust_transition = -_mc_virtual_att_sp->thrust_body[2];
 
 	} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
+		memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
+
 		if (_motor_state != ENABLED) {
 			_motor_state = set_motor_state(_motor_state, ENABLED);
 		}
@@ -318,15 +339,6 @@ void Tiltrotor::update_transition_state()
 	_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);
 	_mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f);
 	_mc_throttle_weight = math::constrain(_mc_throttle_weight, 0.0f, 1.0f);
-
-	// copy virtual attitude setpoint to real attitude setpoint (we use multicopter att sp)
-	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
-}
-
-void Tiltrotor::waiting_on_tecs()
-{
-	// keep multicopter thrust until we get data from TECS
-	_v_att_sp->thrust_body[0] = _thrust_transition;
 }
 
 /**

src/modules/vtol_att_control/tiltrotor.h

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

src/modules/vtol_att_control/vtol_type.h

@@ -150,9 +150,9 @@ class VtolType
 
 	/**
 	 * Special handling opportunity for the time right after transition to FW
-	 * before TECS is running.
+	 * before TECS is running. By default this does nothing, vtol sub-classes can override it.
 	 */
-	virtual void waiting_on_tecs() {}
+	void waiting_on_tecs() {}
 
 	/**
 	 * Checks for fixed-wing failsafe condition and issues abort request if needed.