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 <mahima@auterion.com>

msg/CMakeLists.txt

@@ -100,6 +100,7 @@ set(msg_files
 	FixedWingLateralGuidanceStatus.msg
 	FixedWingLateralStatus.msg
 	FixedWingRunwayControl.msg
+	FixedWingTakeoffStatus.msg
 	GeneratorStatus.msg
 	GeofenceResult.msg
 	GeofenceStatus.msg

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)

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

src/modules/fw_mode_manager/FixedWingModeManager.hpp

@@ -72,6 +72,7 @@
 #include <uORB/topics/fixed_wing_lateral_guidance_status.h>
 #include <uORB/topics/fixed_wing_longitudinal_setpoint.h>
 #include <uORB/topics/fixed_wing_runway_control.h>
+#include <uORB/topics/fixed_wing_takeoff_status.h>
 #include <uORB/topics/landing_gear.h>
 #include <uORB/topics/launch_detection_status.h>
 #include <uORB/topics/normalized_unsigned_setpoint.h>
@@ -204,6 +205,7 @@ private:
 	uORB::PublicationData<fixed_wing_longitudinal_setpoint_s> _longitudinal_ctrl_sp_pub{ORB_ID(fixed_wing_longitudinal_setpoint)};
 	uORB::Publication<fixed_wing_lateral_guidance_status_s> _fixed_wing_lateral_guidance_status_pub{ORB_ID(fixed_wing_lateral_guidance_status)};
 	uORB::Publication<fixed_wing_runway_control_s> _fixed_wing_runway_control_pub{ORB_ID(fixed_wing_runway_control)};
+	uORB::Publication<fixed_wing_takeoff_status_s> _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.
 	 *

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

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

src/modules/navigator/navigator.h

@@ -76,6 +76,7 @@
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/distance_sensor_mode_change_request.h>
 #include <uORB/topics/fixed_wing_lateral_guidance_status.h>
+#include <uORB/topics/fixed_wing_takeoff_status.h>
 #include <uORB/topics/geofence_result.h>
 #include <uORB/topics/gimbal_manager_set_attitude.h>
 #include <uORB/topics/home_position.h>
@@ -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_s>		_geofence_result_pub{ORB_ID(geofence_result)};

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

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