vtol: take fixed wing attitude setpoint during transition if altitude is

not controlled

- required as there is no flightask running if altitude is not controlled

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

src/modules/vtol_att_control/standard.cpp

@@ -230,8 +230,16 @@ void Standard::update_transition_state()
 
 	VtolType::update_transition_state();
 
-	// copy virtual attitude setpoint to real attitude setpoint
+	// we get attitude setpoint from a multirotor flighttask if altitude is controlled.
-	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
+	// in any other case the fixed wing attitude controller publishes attitude setpoint from manual stick input.
+	if (_v_control_mode->flag_control_climb_rate_enabled) {
+		memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
+		_v_att_sp->roll_body = _fw_virtual_att_sp->roll_body;
+
+	} else {
+		memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
+		_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
+	}
 
 	if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) {
 		if (_params_standard.pusher_ramp_dt <= 0.0f) {
@@ -263,13 +271,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);
 
-		_v_att_sp->roll_body = _fw_virtual_att_sp->roll_body;
-
-		// in stabilized, acro or manual mode, set the MC thrust to the throttle stick position (coming from the FW attitude setpoint)
-		if (!_v_control_mode->flag_control_climb_rate_enabled) {
-			_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
-		}
-
 		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);
 
@@ -283,18 +284,11 @@ void Standard::update_transition_state()
 
 	} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) {
 
-		_v_att_sp->roll_body = _fw_virtual_att_sp->roll_body;
-
 		if (_v_control_mode->flag_control_climb_rate_enabled) {
 			// control backtransition deceleration using pitch.
 			_v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp();
 		}
 
-		// in stabilized, acro or manual mode, set the MC thrust to the throttle stick position (coming from the FW attitude setpoint)
-		if (!_v_control_mode->flag_control_climb_rate_enabled) {
-			_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
-		}
-
 		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);
 

src/modules/vtol_att_control/tiltrotor.cpp

@@ -289,10 +289,18 @@ void Tiltrotor::update_transition_state()
 {
 	VtolType::update_transition_state();
 
-	// copy virtual attitude setpoint to real attitude setpoint (we use multicopter att sp)
+	// we get attitude setpoint from a multirotor flighttask if altitude is controlled.
-	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
+	// in any other case the fixed wing attitude controller publishes attitude setpoint from manual stick input.
+	if (_v_control_mode->flag_control_climb_rate_enabled) {
+		memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
+		_v_att_sp->roll_body = _fw_virtual_att_sp->roll_body;
+		_thrust_transition = -_mc_virtual_att_sp->thrust_body[2];
 
-	_v_att_sp->roll_body = _fw_virtual_att_sp->roll_body;
+	} else {
+		memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
+		_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
+		_thrust_transition = _fw_virtual_att_sp->thrust_body[0];
+	}
 
 	float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
 
@@ -339,13 +347,6 @@ void Tiltrotor::update_transition_state()
 			_mc_yaw_weight = _mc_roll_weight;
 		}
 
-		_thrust_transition = -_mc_virtual_att_sp->thrust_body[2];
-
-		// in stabilized, acro or manual mode, set the MC thrust to the throttle stick position (coming from the FW attitude setpoint)
-		if (!_v_control_mode->flag_control_climb_rate_enabled) {
-			_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
-		}
-
 	} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P2) {
 		// the plane is ready to go into fixed wing mode, tilt the rotors forward completely
 		_tilt_control = _params_tiltrotor.tilt_transition +
@@ -359,22 +360,12 @@ void Tiltrotor::update_transition_state()
 		int ramp_down_value = (1.0f - time_since_trans_start / _params_tiltrotor.front_trans_dur_p2) *
 				      (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) + PWM_DEFAULT_MIN;
 
-
 		set_alternate_motor_state(motor_state::VALUE, ramp_down_value);
 
-
-		_thrust_transition = -_mc_virtual_att_sp->thrust_body[2];
-
-		// in stabilized, acro or manual mode, set the MC thrust to the throttle stick position (coming from the FW attitude setpoint)
-		if (!_v_control_mode->flag_control_climb_rate_enabled) {
-			_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
-		}
-
 	} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) {
 		// turn on all MC motors
 		set_all_motor_state(motor_state::ENABLED);
 
-
 		// set idle speed for rotary wing mode
 		if (!_flag_idle_mc) {
 			_flag_idle_mc = set_idle_mc();
@@ -405,11 +396,6 @@ void Tiltrotor::update_transition_state()
 			// slowly ramp up throttle to avoid step inputs
 			_mc_throttle_weight = (time_since_trans_start - 1.0f) / 1.0f;
 		}
-
-		// in stabilized, acro or manual mode, set the MC thrust to the throttle stick position (coming from the FW attitude setpoint)
-		if (!_v_control_mode->flag_control_climb_rate_enabled) {
-			_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
-		}
 	}
 
 	const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));