vtol_type: added explicit control over deceleration during backtransition

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

src/modules/vtol_att_control/standard.cpp

@@ -280,9 +280,9 @@ 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;
+		_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);
 
@@ -412,4 +412,4 @@ Standard::waiting_on_tecs()
 {
 	// keep thrust from transition
 	_v_att_sp->thrust_body[0] = _pusher_throttle;
-};
+};

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_SP");
+
 
 	_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,10 @@ 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, &_params.back_trans_dec_sp);
+	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 @@ private:
 		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

@@ -315,3 +315,44 @@ PARAM_DEFINE_INT32(VT_FW_DIFTHR_EN, 0);
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_SC, 0.1f);
+
+/**
+ * Target deceleration during backtransition.
+ *
+ * The vehicle will use its pitch angle to try and maintain a target deceleration during the backtransition.
+ * This parameter only applies for standard vtol and tiltrotors.
+ *
+ * @unit m/s2
+ * @min 0.5
+ * @max 5
+ * @decimal 1
+ * @increment 0.5
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_B_DEC_SP, 2.0f);
+
+/**
+ * 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,45 @@ 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;
+
+	// calculate acceleration in body x direction
+	const Dcmf R_to_body(Quatf(_v_att->q).inversed());
+	const Vector3f acc = R_to_body * Vector3f(_local_pos->ax, _local_pos->ay, _local_pos->az);
+	float accel_body_x = acc(0);
+	float accel_error_x = 0.0f;
+
+	// get accel error, positive means decelerating too slow, need to pitch up (must reverse dec_max, as it is a positive number)
+	accel_error_x = _params->back_trans_dec_sp + accel_body_x;
+	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_x;
+
+	if ((pitch_sp_new >= pitch_lim && accel_error_x > 0.0f) ||
+	    (pitch_sp_new <= -pitch_lim && accel_error_x < 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 @@ protected:
 
 	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 @@ protected:
 	 */
 	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: