FlightTaskTransition: refined implementation

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

@@ -127,16 +127,13 @@ void FlightTaskTransition::updateParametersFromStorage()
 		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()
@@ -171,60 +168,57 @@ float FlightTaskTransition::computeBackTranstionPitchSetpoint()
 
 	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;
-
+	const float track_angle = atan2f(local_pos.vy, local_pos.vx);
+	const float accel_forward = local_pos.ax * cosf(track_angle) + local_pos.ay * sinf(track_angle);
 
-	if (current_pos_sp.valid && local_pos.xy_valid) {
+	if (current_pos_sp.valid && local_pos.xy_valid && local_pos.xy_global) {
 
-		float pos_sp_x, pos_sp_y = 0.f;
-		_geo_projection.project(current_pos_sp.lat, current_pos_sp.lon, pos_sp_x,
-					pos_sp_y);
+		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));
 
-		// 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 distance to backtransition end-point w.r.t. vehicle
+		Vector2f pos_sp_dist = position_setpoint_local - Vector2f(local_pos.x, 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;
+		const float vel_proj = pos_sp_dist * Vector2f(local_pos.vx, local_pos.vy);
 
-		if (vel_proj > 0.0f) {
-			const float dist_body_forward = cosf(track) * pos_sp_dx + sinf(track) * pos_sp_dy;
+		if (vel_proj > FLT_EPSILON) {
+			const float dist_body_forward = pos_sp_dist(0) * cosf(track_angle) + pos_sp_dist(1) * sinf(track_angle);
 
 			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);
+				deceleration_sp = Vector2f(local_pos.vx, local_pos.vy).norm_squared() / (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);
+					deceleration_sp = math::constrain(deceleration_sp, 0.f, max_deceleration_sp);
 				}
 			}
 		}
 	}
 
-	float pitch_sp_new = _decel_error_bt_int;
+	const float pitch_sp_new = _decel_error_bt_int;
 
 	// Update deceleration error integrator
 	// 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);
+	const float deceleration_error = deceleration_sp - (-accel_forward);
 
 	updateBackTransitioDecelerationErrorIntegrator(deceleration_error);
 
-	return math::constrain(pitch_sp_new, 0.f, _pitch_limit_bt);;
+	return pitch_sp_new;
 }
 
 void FlightTaskTransition::updateBackTransitioDecelerationErrorIntegrator(const float deceleration_error)
 {
-	float integrator_input = _param_vt_b_dec_i * 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);
 	// Saturate the integrator value
-	_decel_error_bt_int = math::constrain(_decel_error_bt_int, .0f, _pitch_limit_bt);
+	_decel_error_bt_int = math::constrain(_decel_error_bt_int, 0.f, _pitch_limit_bt);
 }

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

@@ -77,7 +77,7 @@ class FlightTaskTransition : public FlightTask
 	float _param_vt_b_dec_mss{0.f};
 
 	AlphaFilter<float> _vel_z_filter;
-	float _decel_error_bt_int{0.}; ///< Backtransition deceleration error integrator value
+	float _decel_error_bt_int{0.f}; ///< Backtransition deceleration error integrator value
 
 	static constexpr float _pitch_limit_bt = .3f; ///< Bactransition pitch limit