Towards controlled VTOL transitions

src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp

@@ -60,10 +60,13 @@ void FlightTaskTransition::checkSetpoints(vehicle_local_position_setpoint_s &set
 
 bool FlightTaskTransition::update()
 {
-	// level wings during the transition, altitude should be controlled
-	_position_setpoint(2) = _transition_altitude;
 	_acceleration_setpoint.xy() = matrix::Vector2f(0.f, 0.f);
+	// demand zero vertical velocity and level attitude
+	// tailsitters will override attitude and thrust setpoint
+	// tiltrotors and standard vtol will overrride roll and pitch setpoint but keep vertical thrust setpoint
+	_position_setpoint.setAll(NAN);
+	_velocity_setpoint(2) = 0.0f;
 
-	_yaw_setpoint = _transition_yaw;
+	_yaw_setpoint = NAN;
 	return true;
 }

src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.hpp

@@ -53,7 +53,6 @@ class FlightTaskTransition : public FlightTask
 
 private:
 	void checkSetpoints(vehicle_local_position_setpoint_s &setpoints);
-	void updateAccelerationEstimate();
 
 	float _transition_altitude = 0.0f;
 	float _transition_yaw = 0.0f;

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -1578,8 +1578,9 @@ FixedwingPositionControl::Run()
 
 				// only publish status in full FW mode
 				if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
-				    && !_vehicle_status.in_transition_mode) {
+				    || _vehicle_status.in_transition_mode) {
 					status_publish();
+
 				}
 			}
 		}

src/modules/vtol_att_control/standard.cpp

@@ -264,6 +264,9 @@ 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;
+
 		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);
 
@@ -277,8 +280,11 @@ void Standard::update_transition_state()
 
 	} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) {
 
-		// maintain FW_PSP_OFF
-		_v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset;
+		_v_att_sp->roll_body = _fw_virtual_att_sp->roll_body;
+
+		// control backtransition deceleration using pitch.
+		_v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp();
+
 		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);
 
@@ -408,4 +414,4 @@ Standard::waiting_on_tecs()
 {
 	// keep thrust from transition
 	_v_att_sp->thrust_body[0] = _pusher_throttle;
-};
+};

src/modules/vtol_att_control/tiltrotor.cpp

@@ -42,6 +42,8 @@
 #include "tiltrotor.h"
 #include "vtol_att_control_main.h"
 
+using namespace matrix;
+
 #define ARSP_YAW_CTRL_DISABLE 7.0f	// airspeed at which we stop controlling yaw during a front transition
 
 Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
@@ -224,6 +226,11 @@ void Tiltrotor::update_transition_state()
 {
 	VtolType::update_transition_state();
 
+	// 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));
+
+	_v_att_sp->roll_body = _fw_virtual_att_sp->roll_body;
+
 	float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
 
 	if (!_flag_was_in_trans_mode) {
@@ -310,6 +317,9 @@ void Tiltrotor::update_transition_state()
 
 		_mc_yaw_weight = 1.0f;
 
+		// control backtransition deceleration using pitch.
+		_v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp();
+
 		// while we quickly rotate back the motors keep throttle at idle
 		if (time_since_trans_start < 1.0f) {
 			_mc_throttle_weight = 0.0f;
@@ -324,12 +334,15 @@ void Tiltrotor::update_transition_state()
 		}
 	}
 
+	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);
+
+
 	_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()

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -85,6 +85,10 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	_params_handles.fw_motors_off = param_find("VT_FW_MOT_OFFID");
 	_params_handles.diff_thrust = param_find("VT_FW_DIFTHR_EN");
 	_params_handles.diff_thrust_scale = param_find("VT_FW_DIFTHR_SC");
+	_params_handles.dec_to_pitch_ff = param_find("VT_B_DEC_FF");
+	_params_handles.dec_to_pitch_i = param_find("VT_B_DEC_I");
+	_params_handles.back_trans_dec_sp = param_find("VT_B_DEC_MSS");
+
 
 	_params_handles.down_pitch_max = param_find("VT_DWN_PITCH_MAX");
 	_params_handles.forward_thrust_scale = param_find("VT_FWD_THRUST_SC");
@@ -285,6 +289,14 @@ VtolAttitudeControl::parameters_update()
 	// make sure parameters are feasible, require at least 1 m/s difference between transition and blend airspeed
 	_params.airspeed_blend = math::min(_params.airspeed_blend, _params.transition_airspeed - 1.0f);
 
+	param_get(_params_handles.back_trans_dec_sp, &v);
+	// increase the target deceleration setpoint provided to the controller by 20%
+	// to make overshooting the transition waypoint less likely in the presence of tracking errors
+	_params.back_trans_dec_sp = 1.2f * v;
+
+	param_get(_params_handles.dec_to_pitch_ff, &_params.dec_to_pitch_ff);
+	param_get(_params_handles.dec_to_pitch_i, &_params.dec_to_pitch_i);
+
 	// update the parameters of the instances of base VtolType
 	if (_vtol_type != nullptr) {
 		_vtol_type->parameters_update();

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -203,6 +203,9 @@ class VtolAttitudeControl : public ModuleBase<VtolAttitudeControl>, public px4::
 		param_t diff_thrust_scale;
 		param_t down_pitch_max;
 		param_t forward_thrust_scale;
+		param_t dec_to_pitch_ff;
+		param_t dec_to_pitch_i;
+		param_t back_trans_dec_sp;
 	} _params_handles{};
 
 	/* for multicopters it is usual to have a non-zero idle speed of the engines

src/modules/vtol_att_control/vtol_att_control_params.c

@@ -149,11 +149,12 @@ PARAM_DEFINE_FLOAT(VT_B_TRANS_THR, 0.0f);
  *
  * The approximate deceleration during a back transition in m/s/s
  * Used to calculate back transition distance in mission mode. A lower value will make the VTOL transition further from the destination waypoint.
+ * For standard vtol and tiltrotors a controller is used to track this value during the transition.
  *
  * @unit m/s/s
- * @min 0.00
- * @max 20.00
- * @increment 1
+ * @min 0.5
+ * @max 10
+ * @increment 0.1
  * @decimal 2
  * @group VTOL Attitude Control
  */
@@ -315,3 +316,29 @@ PARAM_DEFINE_INT32(VT_FW_DIFTHR_EN, 0);
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_SC, 0.1f);
+
+/**
+ * Backtransition deceleration setpoint to pitch feedforward gain.
+ *
+ *
+ * @unit rad*s*s/m
+ * @min 0
+ * @max 0.2
+ * @decimal 1
+ * @increment 0.05
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_B_DEC_FF, 0.12f);
+
+/**
+ * Backtransition deceleration setpoint to pitch I gain.
+ *
+ *
+ * @unit rad*s/m
+ * @min 0
+ * @max 0.3
+ * @decimal 1
+ * @increment 0.05
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_B_DEC_I, 0.1f);

src/modules/vtol_att_control/vtol_type.cpp

@@ -177,9 +177,43 @@ void VtolType::update_fw_state()
 
 void VtolType::update_transition_state()
 {
+	hrt_abstime t_now = hrt_absolute_time();
+	_transition_dt = (float)(t_now - _last_loop_ts) / 1e6f;
+	_transition_dt = math::constrain(_transition_dt, 0.0001f, 0.02f);
+	_last_loop_ts = t_now;
+
+
+
 	check_quadchute_condition();
 }
 
+float VtolType::update_and_get_backtransition_pitch_sp()
+{
+	// maximum up or down pitch the controller is allowed to demand
+	const float pitch_lim = 0.3f;
+	const Eulerf euler(Quatf(_v_att->q));
+
+	const float track = atan2f(_local_pos->vy, _local_pos->vx);
+	const float accel_body_forward = cosf(track) * _local_pos->ax + sinf(track) * _local_pos->ay;
+
+	// get accel error, positive means decelerating too slow, need to pitch up (must reverse dec_max, as it is a positive number)
+	const float accel_error_forward = _params->back_trans_dec_sp + accel_body_forward;
+
+	const float pitch_sp_new = _params->dec_to_pitch_ff * _params->back_trans_dec_sp + _accel_to_pitch_integ;
+
+	float integrator_input = _params->dec_to_pitch_i * accel_error_forward;
+
+	if ((pitch_sp_new >= pitch_lim && accel_error_forward > 0.0f) ||
+	    (pitch_sp_new <= -pitch_lim && accel_error_forward < 0.0f)) {
+		integrator_input = 0.0f;
+	}
+
+	_accel_to_pitch_integ += integrator_input * _transition_dt;
+
+
+	return math::constrain(pitch_sp_new, -pitch_lim, pitch_lim);
+}
+
 bool VtolType::can_transition_on_ground()
 {
 	return !_v_control_mode->flag_armed || _land_detected->landed;

src/modules/vtol_att_control/vtol_type.h

@@ -72,6 +72,9 @@ struct Params {
 	float diff_thrust_scale;
 	float down_pitch_max;
 	float forward_thrust_scale;
+	float dec_to_pitch_ff;
+	float dec_to_pitch_i;
+	float back_trans_dec_sp;
 };
 
 // Has to match 1:1 msg/vtol_vehicle_status.msg
@@ -222,6 +225,11 @@ class VtolType
 
 	motor_state _motor_state = motor_state::DISABLED;
 
+	hrt_abstime _last_loop_ts = 0;
+	float _transition_dt = 0;
+
+	float _accel_to_pitch_integ = 0;
+
 
 
 	/**
@@ -253,6 +261,8 @@ class VtolType
 	 */
 	motor_state set_motor_state(const motor_state current_state, const motor_state next_state, const int value = 0);
 
+	float update_and_get_backtransition_pitch_sp();
+
 private: