From 89113c8fff4b51bf37fc5ec8fca9cf7749b39667 Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Sat, 10 Jan 2026 07:21:33 -0800 Subject: [PATCH] Remove more stuff --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 1 + ROMFS/px4fmu_common/init.d/rc.vtol_apps | 1 + .../FixedWingGuidanceControl.cpp | 26 ------------------- .../FixedWingGuidanceControl.hpp | 17 ------------ .../fw_mode_manager/FixedWingModeManager.cpp | 20 +------------- .../fw_mode_manager/FixedWingModeManager.hpp | 1 - 6 files changed, 3 insertions(+), 63 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 7597c989b3..41c8a2a429 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -16,6 +16,7 @@ control_allocator start fw_rate_control start fw_att_control start fw_mode_manager start +fw_guidance_control start fw_lat_lon_control start airspeed_selector start diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index 16fb6ee5d6..e24cffc28d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -28,6 +28,7 @@ fi fw_rate_control start vtol fw_att_control start vtol fw_mode_manager start +fw_guidance_control start fw_lat_lon_control start vtol fw_autotune_attitude_control start vtol diff --git a/src/modules/fw_guidance_control/FixedWingGuidanceControl.cpp b/src/modules/fw_guidance_control/FixedWingGuidanceControl.cpp index 247ecb53c0..af74245086 100644 --- a/src/modules/fw_guidance_control/FixedWingGuidanceControl.cpp +++ b/src/modules/fw_guidance_control/FixedWingGuidanceControl.cpp @@ -101,31 +101,6 @@ FixedWingGuidanceControl::vehicle_control_mode_poll() { } -void -FixedWingGuidanceControl::vehicle_command_poll() -{ - vehicle_command_s vehicle_command; - - while (_vehicle_command_sub.update(&vehicle_command)) { - if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) { - - if ((static_cast(vehicle_command.param1 + .5f) == vehicle_command_s::SPEED_TYPE_AIRSPEED)) { - if (vehicle_command.param2 > FLT_EPSILON) { // param2 is an equivalent airspeed setpoint - if (_control_mode_current == FW_POSCTRL_MODE_AUTO) { - _pos_sp_triplet.current.cruising_speed = vehicle_command.param2; - - } else if (_control_mode_current == FW_POSCTRL_MODE_MANUAL_ALTITUDE - || _control_mode_current == FW_POSCTRL_MODE_MANUAL_POSITION) { - _commanded_manual_airspeed_setpoint = vehicle_command.param2; - } - - } - } - - } - } -} - void FixedWingGuidanceControl::airspeed_poll() { @@ -423,7 +398,6 @@ FixedWingGuidanceControl::Run() airspeed_poll(); manual_control_setpoint_poll(); vehicle_attitude_poll(); - vehicle_command_poll(); vehicle_control_mode_poll(); wind_poll(now); diff --git a/src/modules/fw_guidance_control/FixedWingGuidanceControl.hpp b/src/modules/fw_guidance_control/FixedWingGuidanceControl.hpp index 2bfe5c0208..3beece5a80 100644 --- a/src/modules/fw_guidance_control/FixedWingGuidanceControl.hpp +++ b/src/modules/fw_guidance_control/FixedWingGuidanceControl.hpp @@ -204,22 +204,6 @@ private: uint8_t _position_sp_type{0}; - enum FW_POSCTRL_MODE { - FW_POSCTRL_MODE_AUTO, - FW_POSCTRL_MODE_AUTO_ALTITUDE, - FW_POSCTRL_MODE_AUTO_CLIMBRATE, - FW_POSCTRL_MODE_AUTO_TAKEOFF, - FW_POSCTRL_MODE_AUTO_TAKEOFF_NO_NAV, - FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT, - FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR, - FW_POSCTRL_MODE_AUTO_PATH, - FW_POSCTRL_MODE_MANUAL_POSITION, - FW_POSCTRL_MODE_MANUAL_ALTITUDE, - FW_POSCTRL_MODE_TRANSITION_TO_HOVER_LINE_FOLLOW, - FW_POSCTRL_MODE_TRANSITION_TO_HOVER_HEADING_HOLD, - FW_POSCTRL_MODE_OTHER - } _control_mode_current{FW_POSCTRL_MODE_OTHER}; // used to check if the mode has changed - enum StickConfig { STICK_CONFIG_SWAP_STICKS_BIT = (1 << 0), STICK_CONFIG_ENABLE_AIRSPEED_SP_MANUAL_BIT = (1 << 1) @@ -338,7 +322,6 @@ private: void manual_control_setpoint_poll(); void vehicle_attitude_poll(); - void vehicle_command_poll(); void vehicle_control_mode_poll(); void wind_poll(const hrt_abstime now); diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.cpp b/src/modules/fw_mode_manager/FixedWingModeManager.cpp index e930a4a5ce..e9cac23eea 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.cpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.cpp @@ -373,21 +373,7 @@ FixedWingModeManager::set_control_mode_current(const hrt_abstime &now) _skipping_takeoff_detection = false; const bool doing_backtransition = _vehicle_status.in_transition_mode && !_vehicle_status.in_transition_to_fw; - if (_control_mode.flag_control_offboard_enabled && _position_setpoint_current_valid - && _control_mode.flag_control_position_enabled) { - if (PX4_ISFINITE(_pos_sp_triplet.current.vx) && PX4_ISFINITE(_pos_sp_triplet.current.vy) - && PX4_ISFINITE(_pos_sp_triplet.current.vz)) { - // Offboard position with velocity setpoints - _control_mode_current = FW_POSCTRL_MODE_AUTO_PATH; - return; - - } else { - // Offboard position setpoint only - _control_mode_current = FW_POSCTRL_MODE_AUTO; - return; - } - - } else if ((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled) + if ((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled) && _position_setpoint_current_valid) { // Enter this mode only if the current waypoint has valid 3D position setpoints. @@ -2111,10 +2097,6 @@ FixedWingModeManager::Run() break; } - case FW_POSCTRL_MODE_AUTO_PATH: { - break; - } - case FW_POSCTRL_MODE_AUTO_TAKEOFF: { control_auto_takeoff(now, control_interval, curr_pos, ground_speed, _pos_sp_triplet.current); break; diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.hpp b/src/modules/fw_mode_manager/FixedWingModeManager.hpp index 6a1926a495..418d0b8a94 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.hpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.hpp @@ -226,7 +226,6 @@ private: FW_POSCTRL_MODE_AUTO_TAKEOFF_NO_NAV, FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT, FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR, - FW_POSCTRL_MODE_AUTO_PATH, FW_POSCTRL_MODE_MANUAL_POSITION, FW_POSCTRL_MODE_MANUAL_ALTITUDE, FW_POSCTRL_MODE_TRANSITION_TO_HOVER_LINE_FOLLOW,