From a6a21bec45b4ee8af5daebf7f081ad82db16e77b Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Sat, 14 Mar 2026 17:01:05 -0700 Subject: [PATCH] Fix format Fix --- .../FixedWingGuidanceControl.cpp | 21 +++++++++++-------- .../FixedWingGuidanceControl.hpp | 3 ++- .../fw_mode_manager/FixedWingModeManager.cpp | 2 +- 3 files changed, 15 insertions(+), 11 deletions(-) diff --git a/src/modules/fw_guidance_control/FixedWingGuidanceControl.cpp b/src/modules/fw_guidance_control/FixedWingGuidanceControl.cpp index f1b2e05b74..d9ed5bedfc 100644 --- a/src/modules/fw_guidance_control/FixedWingGuidanceControl.cpp +++ b/src/modules/fw_guidance_control/FixedWingGuidanceControl.cpp @@ -211,16 +211,16 @@ FixedWingGuidanceControl::update_in_air_states(const hrt_abstime now) } void -FixedWingGuidanceControl::control_auto_path(const float control_interval, const Vector2d &curr_pos, - const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr) +FixedWingGuidanceControl::control_auto_path(const float control_interval, + const Vector2f &ground_speed, const float cruising_speed, const Vector2f curr_wp_local, const float curr_wp_alt, + const Vector2f velocity_2d, bool gliding_enabled, + const position_setpoint_s &pos_sp_curr) { - const float target_airspeed = pos_sp_curr.cruising_speed > FLT_EPSILON ? pos_sp_curr.cruising_speed : NAN; + const float target_airspeed = cruising_speed > FLT_EPSILON ? cruising_speed : NAN; Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; - Vector2f curr_wp_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); // Navigate directly on position setpoint and path tangent - const matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy); const float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 / _pos_sp_triplet.current.loiter_radius : 0.0f; @@ -235,7 +235,7 @@ FixedWingGuidanceControl::control_auto_path(const float control_interval, const const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = { .timestamp = hrt_absolute_time(), - .altitude = pos_sp_curr.alt, + .altitude = curr_wp_alt, .height_rate = NAN, .equivalent_airspeed = target_airspeed, .pitch_direct = NAN, @@ -244,7 +244,7 @@ FixedWingGuidanceControl::control_auto_path(const float control_interval, const _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - if (pos_sp_curr.gliding_enabled) { + if (gliding_enabled) { _ctrl_configuration_handler.setThrottleMin(0.0f); _ctrl_configuration_handler.setThrottleMax(0.0f); _ctrl_configuration_handler.setSpeedWeight(2.0f); @@ -423,8 +423,11 @@ FixedWingGuidanceControl::Run() // by default set speed weight to the param value, can be overwritten inside the methods below _ctrl_configuration_handler.setSpeedWeight(_param_t_spdweight.get()); - - control_auto_path(control_interval, curr_pos, ground_speed, _pos_sp_triplet.current); + Vector2f curr_wp_local = _global_local_proj_ref.project(_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon); + const matrix::Vector2f velocity_2d(_pos_sp_triplet.current.vx, _pos_sp_triplet.current.vy); + control_auto_path(control_interval, ground_speed, _pos_sp_triplet.current.cruising_speed, curr_wp_local, _pos_sp_triplet.current.alt, + velocity_2d, + _pos_sp_triplet.current.gliding_enabled, _pos_sp_triplet.current); _ctrl_configuration_handler.update(now); diff --git a/src/modules/fw_guidance_control/FixedWingGuidanceControl.hpp b/src/modules/fw_guidance_control/FixedWingGuidanceControl.hpp index ed99147fce..22f5f628af 100644 --- a/src/modules/fw_guidance_control/FixedWingGuidanceControl.hpp +++ b/src/modules/fw_guidance_control/FixedWingGuidanceControl.hpp @@ -335,7 +335,8 @@ private: * @param pos_sp_prev previous position setpoint * @param pos_sp_curr current position setpoint */ - void control_auto_path(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed, + void control_auto_path(const float control_interval, const Vector2f &ground_speed, + const float cruising_speed, const Vector2f curr_wp_local, const float curr_wp_alt, const Vector2f velocity_2d, bool gliding_enabled, const position_setpoint_s &pos_sp_curr); void publishLocalPositionSetpoint(const position_setpoint_s ¤t_waypoint); diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.cpp b/src/modules/fw_mode_manager/FixedWingModeManager.cpp index e9cac23eea..92359d00de 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.cpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.cpp @@ -374,7 +374,7 @@ FixedWingModeManager::set_control_mode_current(const hrt_abstime &now) const bool doing_backtransition = _vehicle_status.in_transition_mode && !_vehicle_status.in_transition_to_fw; if ((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled) - && _position_setpoint_current_valid) { + && _position_setpoint_current_valid) { // Enter this mode only if the current waypoint has valid 3D position setpoints.