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);