From 02a06df044a0fbe312edf5441e6c3f1aa67f2244 Mon Sep 17 00:00:00 2001
From: Konrad <konrad@auterion.com>
Date: Tue, 7 Feb 2023 10:24:35 +0100
Subject: [PATCH] FixedWingPositionControl: remove get_nav_speed_2d function as
 npfg can handle this internally.

---
 .../FixedwingPositionControl.cpp              | 35 ++++---------------
 .../FixedwingPositionControl.hpp              | 11 ------
 2 files changed, 6 insertions(+), 40 deletions(-)

diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp
index f98113646b0d..9c7287067be2 100644
--- a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp
+++ b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp
@@ -827,7 +827,7 @@ FixedwingPositionControl::move_position_setpoint_for_vtol_transition(position_se
 		if (!PX4_ISFINITE(_transition_waypoint(0))) {
 			double lat_transition, lon_transition;
 
-			// Create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the L1 controller can track
+			// Create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the path navigation controller can track
 			// during the transition. Use the current yaw setpoint to determine the transition heading, as that one in turn
 			// is set to the transition heading by Navigator, or current yaw if setpoint is not valid.
 			const float transition_heading = PX4_ISFINITE(current_sp.yaw) ? current_sp.yaw : _yaw;
@@ -1036,7 +1036,7 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 
 	} else {
 		// No valid previous waypoint, go for the current wp.
-		// This is automatically handled by the L1/NPFG libraries.
+		// This is automatically handled by the NPFG libraries.
 		prev_wp(0) = pos_sp_curr.lat;
 		prev_wp(1) = pos_sp_curr.lon;
 	}
@@ -1105,11 +1105,11 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 		matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy);
 		float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 / _pos_sp_triplet.current.loiter_radius :
 				  0.0f;
-		_npfg.navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), get_nav_speed_2d(ground_speed),
+		_npfg.navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), ground_speed,
 					  _wind_vel, curvature);
 
 	} else {
-		_npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, get_nav_speed_2d(ground_speed), _wind_vel);
+		_npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
 	}
 
 	_att_sp.roll_body = _npfg.getRollSetpoint();
@@ -1201,7 +1201,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 
 	} else {
 		// No valid previous waypoint, go for the current wp.
-		// This is automatically handled by the L1/NPFG libraries.
+		// This is automatically handled by the NPFG libraries.
 		prev_wp(0) = pos_sp_curr.lat;
 		prev_wp(1) = pos_sp_curr.lon;
 	}
@@ -1261,7 +1261,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 	_npfg.setAirspeedNom(target_airspeed * _eas2tas);
 	_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
 	_npfg.navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise,
-			     get_nav_speed_2d(ground_speed),
+			     ground_speed,
 			     _wind_vel);
 	_att_sp.roll_body = _npfg.getRollSetpoint();
 	target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
@@ -2235,29 +2235,6 @@ FixedwingPositionControl::reset_landing_state()
 	}
 }
 
-Vector2f
-FixedwingPositionControl::get_nav_speed_2d(const Vector2f &ground_speed)
-{
-
-	Vector2f nav_speed_2d{ground_speed};
-
-	if (_airspeed_valid && !_wind_valid) {
-		// l1 navigation logic breaks down when wind speed exceeds max airspeed
-		// compute 2D groundspeed from airspeed-heading projection
-		const Vector2f air_speed_2d{_airspeed * cosf(_yaw), _airspeed * sinf(_yaw)};
-
-		// angle between air_speed_2d and ground_speed
-		const float air_gnd_angle = acosf((air_speed_2d * ground_speed) / (air_speed_2d.length() * ground_speed.length()));
-
-		// if angle > 90 degrees or groundspeed is less than threshold, replace groundspeed with airspeed projection
-		if ((fabsf(air_gnd_angle) > M_PI_2_F) || (ground_speed.length() < 3.0f)) {
-			nav_speed_2d = air_speed_2d;
-		}
-	}
-
-	return nav_speed_2d;
-}
-
 float FixedwingPositionControl::compensateTrimThrottleForDensityAndWeight(float throttle_trim, float throttle_min,
 		float throttle_max)
 {
diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.hpp b/src/modules/fw_path_navigation/FixedwingPositionControl.hpp
index 3ed78862702f..000cb21c3446 100644
--- a/src/modules/fw_path_navigation/FixedwingPositionControl.hpp
+++ b/src/modules/fw_path_navigation/FixedwingPositionControl.hpp
@@ -647,17 +647,6 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 	void reset_takeoff_state();
 	void reset_landing_state();
 
-	/**
-	 * @brief Returns the velocity vector to be input in the lateral-directional guidance laws.
-	 *
-	 * Replaces the ground velocity vector with an air velocity vector depending on the wind condition and whether
-	 * NPFG or L1 are being used for horizontal position control.
-	 *
-	 * @param ground_speed Vehicle ground velocity vector [m/s]
-	 * @return Velocity vector to control with lateral-directional guidance [m/s]
-	 */
-	Vector2f get_nav_speed_2d(const Vector2f &ground_speed);
-
 	/**
 	 * @brief Decides which control mode to execute.
 	 *