FlgithTaskTransition: refactored backtransition deceleration/pitch se…

…tpoint computation

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

@@ -36,38 +36,17 @@
  */
 
 #include "FlightTaskTransition.hpp"
+#include <lib/geo/geo.h>
 
 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 +55,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 +66,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 +76,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 +94,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(math::radians(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 +109,126 @@ 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_i != 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()
+{
+	return _sub_vehicle_status.get().in_transition_mode
+	       && _sub_vehicle_status.get().in_transition_to_fw;
+
+}
+
+bool FlightTaskTransition::isVtolBackTransition()
+{
+	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
+	const float default_deceleration_sp = _param_vt_b_dec_mss;
+	// Maximum allowed deceleration setpoint as a function of the nominal deceleration setpoint
+	const float max_deceleration_sp = 2.5f * default_deceleration_sp;
+
+	float deceleration_sp = default_deceleration_sp;
+
+	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;
+	const float vel_body_forward = cosf(track) * local_pos.vx + sinf(track) * local_pos.vy;
+
+
+	if (current_pos_sp.valid && local_pos.xy_valid) {
+
+		// Compute backtransition end-point in local reference frame body x-direction -> dist_body_forward
+		MapProjection map_proj{local_pos.ref_lat, local_pos.ref_lon};
+
+		float pos_sp_x, pos_sp_y = 0.f;
+		map_proj.project(current_pos_sp.lat, current_pos_sp.lon, pos_sp_x,
+				 pos_sp_y);
+
+		// Compute backtransition end-point w.r.t. vehicle
+		const float pos_sp_dx = pos_sp_x - local_pos.x;
+		const float pos_sp_dy = pos_sp_y - local_pos.y;
+
+		// Compute the deceleration setpoint if the backtransition end-point is ahead of the vehicle
+		const float vel_proj = local_pos.vx * pos_sp_dx + local_pos.vy * pos_sp_dy;
+
+		if (vel_proj > 0.0f) {
+			const float dist_body_forward = cosf(track) * pos_sp_dx + sinf(track) * pos_sp_dy;
+
+			if (fabsf(dist_body_forward) > FLT_EPSILON) {
+				// Compute deceleration setpoint
+				// Note this is deceleration (i.e. negative acceleration), and therefore the minus sign is skipped
+				deceleration_sp = vel_body_forward * vel_body_forward / (2.f * dist_body_forward);
+
+				// Check if the deceleration setpoint is finite and within limits
+				if (!PX4_ISFINITE(deceleration_sp)) {
+					deceleration_sp = default_deceleration_sp;
+
+				} else {
+					// Limit the deceleration setpoint
+					deceleration_sp = math::constrain(deceleration_sp, 0.0f, max_deceleration_sp);
+				}
+			}
+		}
+	}
+
+	// get accel error, positive means decelerating too slow, need to pitch up (must reverse accel_body_forward, as it is a positive number)
+	float deceleration_error = deceleration_sp - (-accel_body_forward);
+
+	float pitch_sp_new = _decel_error_bt_int;
+
+	updateBackTransitioDecelerationErrorIntegrator(deceleration_error);
+
+	return math::constrain(pitch_sp_new, .0f, _pitch_limit_bt);;
+}
+
+void FlightTaskTransition::updateBackTransitioDecelerationErrorIntegrator(const float deceleration_error)
+{
+	float integrator_input = _param_vt_b_dec_i * deceleration_error;
+	_decel_error_bt_int += integrator_input * math::constrain(_deltatime, 0.01f, 0.1f);
+	// Saturate the integrator value
+	_decel_error_bt_int = math::constrain(_decel_error_bt_int, .0f, _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 @@ class FlightTaskTransition : public FlightTask
 	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.}; ///< Backtransition deceleration error integrator value
+
+	const float _pitch_limit_bt = .3f; ///< Bactransition pitch limit
 
 	void updateParameters();
+	void updateParametersFromStorage();
+
+	void updateSubscribers();
+
+	bool isVtolFrontTransition();
+	bool isVtolBackTransition();
+
+	float computeBackTranstionPitchSetpoint();
+	void updateBackTransitioDecelerationErrorIntegrator(const float deceleration_error);
 
 };

src/modules/vtol_att_control/standard.cpp

@@ -239,11 +239,6 @@ void Standard::update_transition_state()
 
 	} else if (_vtol_mode == vtol_mode::TRANSITION_TO_MC) {
 
-		if (_v_control_mode->flag_control_climb_rate_enabled) {
-			// control backtransition deceleration using pitch.
-			pitch_body = update_and_get_backtransition_pitch_sp();
-		}
-
 		const Quatf q_sp(Eulerf(roll_body, pitch_body, yaw_body));
 		q_sp.copyTo(_v_att_sp->q_d);
 

src/modules/vtol_att_control/tiltrotor.cpp

@@ -293,10 +293,6 @@ void Tiltrotor::update_transition_state()
 
 		_mc_yaw_weight = 1.0f;
 
-		// control backtransition deceleration using pitch.
-		if (_v_control_mode->flag_control_climb_rate_enabled) {
-			pitch_body = update_and_get_backtransition_pitch_sp();
-		}
 
 		if (_time_since_trans_start < BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) {
 			// blend throttle from FW value to 0

src/modules/vtol_att_control/vtol_type.cpp

@@ -46,7 +46,6 @@
 #include <px4_platform_common/defines.h>
 #include <matrix/math.hpp>
 #include <lib/atmosphere/atmosphere.h>
-#include <lib/geo/geo.h>
 
 using namespace matrix;
 
@@ -169,77 +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;
-	const float vel_body_forward = cosf(track) * _local_pos->vx + sinf(track) * _local_pos->vy;
-
-	const float default_dec_sp = _param_vt_b_dec_mss.get();
-	// Maximum allowed deceleration setpoint as a function of the nominal deceleration setpoint
-	const float max_dec_sp = 2.5f * default_dec_sp;
-
-	float dec_sp = default_dec_sp;
-
-	if (_attc->get_pos_sp_triplet()->current.valid && _attc->get_local_pos()->xy_valid) {
-		// Add position feedback to the deceleration setpoint calculation
-
-		// Compute backtransition end-point in local reference frame body x-direction -> dist_body_forward
-		MapProjection map_proj{_attc->get_local_pos()->ref_lat, _attc->get_local_pos()->ref_lon};
-
-		float pos_sp_x, pos_sp_y = 0.f;
-		map_proj.project(_attc->get_pos_sp_triplet()->current.lat, _attc->get_pos_sp_triplet()->current.lon, pos_sp_x,
-				 pos_sp_y);
-
-		// Compute backtransition end-point w.r.t. vehicle
-		const float pos_sp_dx = pos_sp_x - _local_pos->x;
-		const float pos_sp_dy = pos_sp_y - _local_pos->y;
-
-		// Compute the deceleration setpoint if the backtransition end-point is ahead of the vehicle
-		const float vel_proj = _local_pos->vx * pos_sp_dx + _local_pos->vy * pos_sp_dy;
-
-		if (vel_proj > 0.0f) {
-			const float dist_body_forward = cosf(track) * pos_sp_dx + sinf(track) * pos_sp_dy;
-
-			if (fabsf(dist_body_forward) > FLT_EPSILON) {
-				// Compute deceleration setpoint
-				// Note this is deceleration (i.e. negative acceleration), and therefore the minus sign is skipped
-				dec_sp = vel_body_forward * vel_body_forward / (2.f * dist_body_forward);
-
-				// Check if the deceleration setpoint is finite and within limits
-				if (!PX4_ISFINITE(dec_sp)) {
-					dec_sp = default_dec_sp;
-
-				} else {
-					// Limit the deceleration setpoint
-					dec_sp = math::constrain(dec_sp, 0.0f, max_dec_sp);
-				}
-			}
-		}
-	}
-
-	// 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 @@ class VtolType : public ModuleParams
 
 	bool _quadchute_command_treated{false};
 
-	float update_and_get_backtransition_pitch_sp();
 	bool isFrontTransitionCompleted();
 	virtual bool isFrontTransitionCompletedBase();