From d5025810b44d763b3882eec9e569ba1f28368133 Mon Sep 17 00:00:00 2001 From: Konrad 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 470862f5f8..34ced43c84 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; @@ -1027,7 +1027,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; } @@ -1096,11 +1096,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; - navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), get_nav_speed_2d(ground_speed), + navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), ground_speed, _wind_vel, curvature); } else { - navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, get_nav_speed_2d(ground_speed), _wind_vel); + navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); } _att_sp.roll_body = _npfg.getRollSetpoint(); @@ -1187,7 +1187,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; } @@ -1245,7 +1245,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); 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; @@ -2204,29 +2204,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 e79a632b70..ca586fd684 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.hpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.hpp @@ -650,17 +650,6 @@ private: 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. *