From 22da2cb891fcbd396d86379d5145aa1d4147e1c1 Mon Sep 17 00:00:00 2001 From: mahima-yoga Date: Fri, 28 Aug 2026 10:10:54 +0200 Subject: [PATCH] refactor(navigator): end the fixed-wing climbout on the reported takeoff status Both the Navigator and the mode manager decided when the climbout was over, each by comparing an altitude of its own. They only agreed because they read the same number. Report the end of the climbout from the mode manager and act on it in the Navigator, so that it is decided in one place. No behaviour change with the default parameters. Signed-off-by: mahima-yoga --- msg/CMakeLists.txt | 1 + msg/FixedWingTakeoffStatus.msg | 6 ++++++ .../fw_mode_manager/FixedWingModeManager.cpp | 20 +++++++++++++++++++ .../fw_mode_manager/FixedWingModeManager.hpp | 4 ++++ src/modules/logger/logged_topics.cpp | 1 + src/modules/navigator/mission_block.cpp | 4 ++-- src/modules/navigator/navigator.h | 8 ++++++++ src/modules/navigator/navigator_main.cpp | 19 ++++++++++++++++++ src/modules/navigator/takeoff.cpp | 2 +- 9 files changed, 62 insertions(+), 3 deletions(-) create mode 100644 msg/FixedWingTakeoffStatus.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 375d53bc8c8..5520f16e41a 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -100,6 +100,7 @@ set(msg_files FixedWingLateralGuidanceStatus.msg FixedWingLateralStatus.msg FixedWingRunwayControl.msg + FixedWingTakeoffStatus.msg GeneratorStatus.msg GeofenceResult.msg GeofenceStatus.msg diff --git a/msg/FixedWingTakeoffStatus.msg b/msg/FixedWingTakeoffStatus.msg new file mode 100644 index 00000000000..43e59a5da4e --- /dev/null +++ b/msg/FixedWingTakeoffStatus.msg @@ -0,0 +1,6 @@ +# Status of a fixed-wing takeoff +# Passes information from the FixedWingModeManager to the Navigator. + +uint64 timestamp # [us] time since system start + +bool climbout_completed # Whether the takeoff climbout is finished (altitude, or time if FW_TKO_CLMB_T is set) diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.cpp b/src/modules/fw_mode_manager/FixedWingModeManager.cpp index 0f32fc0dcfc..a4928b06bcf 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.cpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.cpp @@ -1323,6 +1323,11 @@ FixedWingModeManager::control_auto_takeoff(const hrt_abstime &now, const float c _flaps_setpoint = _param_fw_flaps_to_scl.get(); + const bool waiting_for_launch = _runway_takeoff.runwayTakeoffEnabled() + ? _runway_takeoff.getState() < RunwayTakeoffState::CLIMBOUT + : _launchDetector.getLaunchDetected() == launch_detection_status_s::STATE_WAITING_FOR_LAUNCH; + publishTakeoffStatus(waiting_for_launch, clearance_altitude_amsl); + if (!_vehicle_status.in_transition_to_fw) { publishLocalPositionSetpoint(pos_sp_curr); } @@ -1454,6 +1459,11 @@ FixedWingModeManager::control_auto_takeoff_no_nav(const hrt_abstime &now, const } _flaps_setpoint = _param_fw_flaps_to_scl.get(); + + const bool waiting_for_launch = _runway_takeoff.runwayTakeoffEnabled() + ? _runway_takeoff.getState() < RunwayTakeoffState::CLIMBOUT + : _launchDetector.getLaunchDetected() == launch_detection_status_s::STATE_WAITING_FOR_LAUNCH; + publishTakeoffStatus(waiting_for_launch, current_setpoint_altitude_amsl); } void @@ -2453,6 +2463,16 @@ FixedWingModeManager::reset_takeoff_state() _takeoff_ground_alt = _current_altitude; } +void +FixedWingModeManager::publishTakeoffStatus(const bool waiting_for_launch, const float clearance_altitude_amsl) +{ + fixed_wing_takeoff_status_s fixed_wing_takeoff_status{}; + fixed_wing_takeoff_status.timestamp = hrt_absolute_time(); + fixed_wing_takeoff_status.climbout_completed = !waiting_for_launch && _current_altitude >= clearance_altitude_amsl; + + _fixed_wing_takeoff_status_pub.publish(fixed_wing_takeoff_status); +} + void FixedWingModeManager::terminateForParachuteLanding(const hrt_abstime &now) { diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.hpp b/src/modules/fw_mode_manager/FixedWingModeManager.hpp index af18e53a548..344bcef7a0e 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.hpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.hpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -204,6 +205,7 @@ private: uORB::PublicationData _longitudinal_ctrl_sp_pub{ORB_ID(fixed_wing_longitudinal_setpoint)}; uORB::Publication _fixed_wing_lateral_guidance_status_pub{ORB_ID(fixed_wing_lateral_guidance_status)}; uORB::Publication _fixed_wing_runway_control_pub{ORB_ID(fixed_wing_runway_control)}; + uORB::Publication _fixed_wing_takeoff_status_pub{ORB_ID(fixed_wing_takeoff_status)}; position_setpoint_triplet_s _pos_sp_triplet{}; vehicle_control_mode_s _control_mode{}; @@ -671,6 +673,8 @@ private: void reset_takeoff_state(); void reset_landing_state(); + void publishTakeoffStatus(const bool waiting_for_launch, const float clearance_altitude_amsl); + /** * @brief Releases the parachute by triggering flight termination. * diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 11e186d2e9e..5da12ea40bf 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -163,6 +163,7 @@ void LoggedTopics::add_default_topics() add_optional_topic("fixed_wing_lateral_guidance_status", 100); add_optional_topic("fixed_wing_lateral_status", 100); add_optional_topic("fixed_wing_runway_control", 100); + add_optional_topic("fixed_wing_takeoff_status", 100); add_optional_topic("ranging_beacon", 100); // multi topics diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 20d3e671202..88d5b7c072c 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -212,8 +212,8 @@ MissionBlock::is_mission_item_reached_or_completed() } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - /* fixed-wing takeoff is reached once the vehicle has exceeded the takeoff altitude */ - if (_navigator->get_global_position()->alt > mission_item_altitude_amsl) { + /* fixed-wing takeoff is reached once the mode manager has finished the climbout */ + if (_navigator->fw_climbout_completed(mission_item_altitude_amsl)) { _waypoint_position_reached = true; } diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 928e50b2d6e..cb6898b6841 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -76,6 +76,7 @@ #include #include #include +#include #include #include #include @@ -202,6 +203,12 @@ public: bool home_global_position_valid() { return (_home_pos.valid_alt && _home_pos.valid_hpos); } + /** + * Whether the fixed-wing mode manager has finished the climbout of the current takeoff. + * Falls back to the given altitude when no takeoff is being flown. + */ + bool fw_climbout_completed(float fallback_altitude_amsl); + Geofence &get_geofence() { return _geofence; } #if CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE @@ -368,6 +375,7 @@ private: uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */ uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ uORB::Subscription _pos_ctrl_landing_status_sub{ORB_ID(position_controller_landing_status)}; /**< position controller landing status subscription */ + uORB::Subscription _fw_takeoff_status_sub{ORB_ID(fixed_wing_takeoff_status)}; /**< fixed-wing takeoff status subscription */ uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; /**< vehicle commands (onboard and offboard) */ uORB::Publication _geofence_result_pub{ORB_ID(geofence_result)}; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index fe469adfd2a..12831a73114 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1475,6 +1475,25 @@ void Navigator::check_traffic() } #endif // CONFIG_NAVIGATOR_ADSB +bool Navigator::fw_climbout_completed(float fallback_altitude_amsl) +{ + if (_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + // no takeoff is being flown, for example because the mode was entered while already in air, + // so the mode manager does not report anything and the altitude decides as it did before + return _global_pos.alt >= fallback_altitude_amsl; + } + + fixed_wing_takeoff_status_s fixed_wing_takeoff_status; + + if (_fw_takeoff_status_sub.copy(&fixed_wing_takeoff_status)) { + // the report has to be newer than the setpoint, as it could otherwise still refer to a previous takeoff + return fixed_wing_takeoff_status.climbout_completed + && fixed_wing_takeoff_status.timestamp > _pos_sp_triplet.timestamp; + } + + return false; +} + bool Navigator::abort_landing() { // only abort if currently landing and position controller status updated diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index 30079e576a6..40b97f5f871 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -68,7 +68,7 @@ Takeoff::on_active() switch (_fw_takeoff_state) { case fw_takeoff_state::CLIMBOUT: { - if (_navigator->get_global_position()->alt >= _loiter_altitude_msl) { + if (_navigator->fw_climbout_completed(_loiter_altitude_msl)) { setLoiterItemCommonFields(&_mission_item);