VTOL: Added position feedback to backward transition braking controller(#23731)

Instead of tracking a fixed deceleration setpoint during the backtransition we added here position feedback,
such that the vehicle comes to a stop latest at the current position setpoint. This reduces the risk of 
overshooting the landing point.
If no position feedback/position setpoint is present the old logic still applies.
It further moves the braking controller to the FlightTaskTransition instad of doing it in
the VTOL attitude mode.

* vtol_type: added position feedback to backward transition

* FlgithTaskTransition: refactored backtransition deceleration/pitch setpoint computation

* FlightTaskTransition: minor improvements

* FlightTaskTransition: use .dot() consistently and remove unnecessary comments

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

---------

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Co-authored-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp

@@ -42,32 +42,10 @@ using namespace matrix;
 FlightTaskTransition::FlightTaskTransition()
 {
 	_param_handle_pitch_cruise_degrees = param_find("FW_PSP_OFF");
+	_param_handle_vt_b_dec_i = param_find("VT_B_DEC_I");
+	_param_handle_vt_b_dec_mss = param_find("VT_B_DEC_MSS");
 
-	if (_param_handle_pitch_cruise_degrees != PARAM_INVALID) {
-		param_get(_param_handle_pitch_cruise_degrees, &_param_pitch_cruise_degrees);
-	}
-
-}
-
-bool FlightTaskTransition::updateInitialize()
-{
-	updateParameters();
-	return FlightTask::updateInitialize();
-}
-
-void FlightTaskTransition::updateParameters()
-{
-	// check for parameter updates
-	if (_parameter_update_sub.updated()) {
-		// clear update
-		parameter_update_s pupdate;
-		_parameter_update_sub.copy(&pupdate);
-
-		// update parameters from storage
-		if (_param_handle_pitch_cruise_degrees != PARAM_INVALID) {
-			param_get(_param_handle_pitch_cruise_degrees, &_param_pitch_cruise_degrees);
-		}
-	}
+	updateParametersFromStorage();
 }
 
 bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint)
@@ -76,6 +54,8 @@ bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint)
 
 	_vel_z_filter.setParameters(math::constrain(_deltatime, 0.01f, 0.1f), _vel_z_filter_time_const);
 
+	_decel_error_bt_int = 0.f;
+
 	if (PX4_ISFINITE(last_setpoint.velocity[2])) {
 		_vel_z_filter.reset(last_setpoint.velocity[2]);
 
@@ -85,12 +65,7 @@ bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint)
 
 	_velocity_setpoint(2) = _vel_z_filter.getState();
 
-	_sub_vehicle_status.update();
-
-	const bool is_vtol_front_transition = _sub_vehicle_status.get().in_transition_mode
-					      && _sub_vehicle_status.get().in_transition_to_fw;
-
-	if (is_vtol_front_transition) {
+	if (isVtolFrontTransition()) {
 		_gear.landing_gear = landing_gear_s::GEAR_UP;
 
 	} else {
@@ -100,6 +75,13 @@ bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint)
 	return ret;
 }
 
+bool FlightTaskTransition::updateInitialize()
+{
+	updateParameters();
+	updateSubscribers();
+	return FlightTask::updateInitialize();
+}
+
 bool FlightTaskTransition::update()
 {
 	// tailsitters will override attitude and thrust setpoint
@@ -111,7 +93,13 @@ bool FlightTaskTransition::update()
 	// calculate a horizontal acceleration vector which corresponds to an attitude composed of pitch up by _param_pitch_cruise_degrees
 	// and zero roll angle
 	const Vector2f tmp = Dcm2f(_yaw) * Vector2f(-1.0f, 0.0f);
-	_acceleration_setpoint.xy() = tmp * tanf(math::radians(_param_pitch_cruise_degrees)) * CONSTANTS_ONE_G;
+	float pitch_setpoint = math::radians(_param_pitch_cruise_degrees);
+
+	if (isVtolBackTransition()) {
+		pitch_setpoint = computeBackTranstionPitchSetpoint();
+	}
+
+	_acceleration_setpoint.xy() = tmp * tanf(pitch_setpoint) * CONSTANTS_ONE_G;
 
 	// slowly move vertical velocity setpoint to zero
 	_vel_z_filter.setParameters(math::constrain(_deltatime, 0.01f, 0.1f), _vel_z_filter_time_const);
@@ -120,3 +108,102 @@ bool FlightTaskTransition::update()
 	_yaw_setpoint = NAN;
 	return ret;
 }
+
+void FlightTaskTransition::updateParameters()
+{
+	// check for parameter updates
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s pupdate;
+		_parameter_update_sub.copy(&pupdate);
+
+		updateParametersFromStorage();
+	}
+}
+
+void FlightTaskTransition::updateParametersFromStorage()
+{
+	if (_param_handle_pitch_cruise_degrees != PARAM_INVALID) {
+		param_get(_param_handle_pitch_cruise_degrees, &_param_pitch_cruise_degrees);
+	}
+
+	if (_param_handle_vt_b_dec_i != PARAM_INVALID) {
+		param_get(_param_handle_vt_b_dec_i, &_param_vt_b_dec_i);
+	}
+
+	if (_param_handle_vt_b_dec_mss != PARAM_INVALID) {
+		param_get(_param_handle_vt_b_dec_mss, &_param_vt_b_dec_mss);
+	}
+}
+
+void FlightTaskTransition::updateSubscribers()
+{
+	_sub_vehicle_status.update();
+	_sub_position_sp_triplet.update();
+	_sub_vehicle_local_position.update();
+}
+
+bool FlightTaskTransition::isVtolFrontTransition() const
+{
+	return _sub_vehicle_status.get().in_transition_mode
+	       && _sub_vehicle_status.get().in_transition_to_fw;
+
+}
+
+bool FlightTaskTransition::isVtolBackTransition() const
+{
+	return _sub_vehicle_status.get().in_transition_mode
+	       && !_sub_vehicle_status.get().in_transition_to_fw;
+}
+
+float FlightTaskTransition::computeBackTranstionPitchSetpoint()
+{
+	const vehicle_local_position_s &local_pos = _sub_vehicle_local_position.get();
+	const position_setpoint_s &current_pos_sp = _sub_position_sp_triplet.get().current;
+
+	// Retrieve default decelaration setpoint from param
+	const float default_deceleration_sp = _param_vt_b_dec_mss;
+
+	float deceleration_sp = default_deceleration_sp;
+
+	const Vector2f position_local{local_pos.x, local_pos.y};
+	const Vector2f velocity_local{local_pos.vx, local_pos.vy};
+	const Vector2f acceleration_local{local_pos.ax, local_pos.ay};
+
+	const float accel_in_flight_direction = acceleration_local.dot(velocity_local.unit_or_zero());
+
+	if (current_pos_sp.valid && local_pos.xy_valid && local_pos.xy_global) {
+
+		Vector2f position_setpoint_local {0.f, 0.f};
+		_geo_projection.project(current_pos_sp.lat, current_pos_sp.lon, position_setpoint_local(0),
+					position_setpoint_local(1));
+
+		const Vector2f pos_sp_dist = position_setpoint_local - position_local; // backtransition end-point w.r.t. vehicle
+
+		const float dist_body_forward = pos_sp_dist.dot(velocity_local.unit_or_zero());
+
+		// Compute the deceleration setpoint if the backtransition end-point is ahead of the vehicle
+		if (dist_body_forward > FLT_EPSILON && PX4_ISFINITE(velocity_local.norm())) {
+
+			// Note this is deceleration (i.e. negative acceleration), and therefore the minus sign is skipped
+			deceleration_sp = velocity_local.norm_squared() / (2.f * dist_body_forward);
+			// Limit the deceleration setpoint to maximal twice the default deceleration setpoint (param)
+			deceleration_sp = math::constrain(deceleration_sp, 0.f, 2.f * default_deceleration_sp);
+		}
+	}
+
+	// positive means decelerating too slow, need to pitch up (must reverse accel_body_forward, as it is a positive number)
+	const float deceleration_error = deceleration_sp - (-accel_in_flight_direction);
+
+	updateBackTransitioDecelerationErrorIntegrator(deceleration_error);
+
+	return _decel_error_bt_int;
+}
+
+void FlightTaskTransition::updateBackTransitioDecelerationErrorIntegrator(const float deceleration_error)
+{
+	const float integrator_input = _param_vt_b_dec_i * deceleration_error;
+
+	_decel_error_bt_int += integrator_input * math::constrain(_deltatime, 0.001f, 0.1f);
+	_decel_error_bt_int = math::constrain(_decel_error_bt_int, 0.f, _pitch_limit_bt);
+}

src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp

@@ -44,6 +44,7 @@
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/position_setpoint_triplet.h>
 #include <drivers/drv_hrt.h>
 
 using namespace time_literals;
@@ -66,12 +67,29 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	uORB::SubscriptionData<vehicle_status_s> _sub_vehicle_status{ORB_ID(vehicle_status)};
+	uORB::SubscriptionData<position_setpoint_triplet_s> _sub_position_sp_triplet{ORB_ID(position_setpoint_triplet)};
 
 	param_t _param_handle_pitch_cruise_degrees{PARAM_INVALID};
 	float _param_pitch_cruise_degrees{0.f};
+	param_t _param_handle_vt_b_dec_i{PARAM_INVALID};
+	float _param_vt_b_dec_i{0.f};
+	param_t _param_handle_vt_b_dec_mss{PARAM_INVALID};
+	float _param_vt_b_dec_mss{0.f};
 
 	AlphaFilter<float> _vel_z_filter;
+	float _decel_error_bt_int{0.f}; ///< Backtransition deceleration error integrator value
+
+	static constexpr float _pitch_limit_bt = .3f; ///< Bactransition pitch limit
 
 	void updateParameters();
+	void updateParametersFromStorage();
+
+	void updateSubscribers();
+
+	bool isVtolFrontTransition() const;
+	bool isVtolBackTransition() const;
+
+	float computeBackTranstionPitchSetpoint();
+	void updateBackTransitioDecelerationErrorIntegrator(const float deceleration_error);
 
 };

src/modules/vtol_att_control/standard.cpp

@@ -241,10 +241,11 @@ void Standard::update_transition_state()
 
 		if (_v_control_mode->flag_control_climb_rate_enabled) {
 			// control backtransition deceleration using pitch.
-			pitch_body = update_and_get_backtransition_pitch_sp();
+			pitch_body = Eulerf(Quatf(_mc_virtual_att_sp->q_d)).theta();
 		}
 
 		const Quatf q_sp(Eulerf(roll_body, pitch_body, yaw_body));
+
 		q_sp.copyTo(_v_att_sp->q_d);
 
 		_pusher_throttle = 0.0f;

src/modules/vtol_att_control/tiltrotor.cpp

@@ -295,7 +295,7 @@ void Tiltrotor::update_transition_state()
 
 		// control backtransition deceleration using pitch.
 		if (_v_control_mode->flag_control_climb_rate_enabled) {
-			pitch_body = update_and_get_backtransition_pitch_sp();
+			pitch_body = Eulerf(Quatf(_mc_virtual_att_sp->q_d)).theta();
 		}
 
 		if (_time_since_trans_start < BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) {

src/modules/vtol_att_control/vtol_type.cpp

@@ -168,37 +168,6 @@ void VtolType::update_transition_state()
 	_last_thr_in_mc = _vehicle_thrust_setpoint_virtual_mc->xyz[2];
 }
 
-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;
-
-	// 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
-	const float dec_sp = _param_vt_b_dec_mss.get() * 1.2f;
-
-	// 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 = dec_sp + accel_body_forward;
-
-	const float pitch_sp_new = _accel_to_pitch_integ;
-
-	float integrator_input = _param_vt_b_dec_i.get() * accel_error_forward;
-
-	if ((pitch_sp_new >= pitch_lim && accel_error_forward > 0.0f) ||
-	    (pitch_sp_new <= 0.f && accel_error_forward < 0.0f)) {
-		integrator_input = 0.0f;
-	}
-
-	_accel_to_pitch_integ += integrator_input * _transition_dt;
-
-	// only allow positive (pitch up) pitch setpoint
-	return math::constrain(pitch_sp_new, 0.f, pitch_lim);
-}
-
 bool VtolType::isFrontTransitionCompleted()
 {
 	bool ret = isFrontTransitionCompletedBase();

src/modules/vtol_att_control/vtol_type.h

@@ -317,7 +317,6 @@ protected:
 
 	bool _quadchute_command_treated{false};
 
-	float update_and_get_backtransition_pitch_sp();
 	bool isFrontTransitionCompleted();
 	virtual bool isFrontTransitionCompletedBase();