Navigator: Takeoff mode: add state 2-state takeoff for fixed-wing

Signed-off-by: Silvan <silvan@auterion.com>

src/modules/navigator/navigator_main.cpp

@@ -640,6 +640,10 @@ void Navigator::run()
 
 				rep->next.valid = false;
 
+				// after the straight climbout the vehicle will establish on a loiter at this position
+				_takeoff.setLoiterPosition(matrix::Vector2d(cmd.param5, cmd.param6));
+				_takeoff.setLoiterAltitudeLocalZ(cmd.param7);
+
 				// CMD_NAV_TAKEOFF is acknowledged by commander
 
 #if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF

src/modules/navigator/takeoff.cpp

@@ -43,7 +43,8 @@
 #include <px4_platform_common/events.h>
 
 Takeoff::Takeoff(Navigator *navigator) :
-	MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF)
+	MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF),
+	ModuleParams(navigator)
 {
 }
 
@@ -54,41 +55,109 @@ Takeoff::on_activation()
 
 	// reset cruising speed to default
 	_navigator->reset_cruising_speed();
+
+	_fw_takeoff_state = fw_takeoff_state::CLIMBOUT; // only used for fixed-wing takeoff
 }
 
 void
 Takeoff::on_active()
 {
-	struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet();
+	if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
 
-	if (rep->current.valid) {
-		// reset the position
-		set_takeoff_position();
+		switch (_fw_takeoff_state) {
+		case fw_takeoff_state::CLIMBOUT: {
+				if (_navigator->get_local_position()->z < _climbout_alt_z) {
 
-	} else if (is_mission_item_reached_or_completed() && !_navigator->get_mission_result()->finished) {
-		_navigator->get_mission_result()->finished = true;
-		_navigator->set_mission_result_updated();
-		_navigator->mode_completed(getNavigatorStateId());
+					setLoiterItemCommonFields(&_mission_item);
 
-		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+					_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
 
-		// set loiter item so position controllers stop doing takeoff logic
-		if (_navigator->get_land_detected()->landed) {
-			_mission_item.nav_cmd = NAV_CMD_IDLE;
+					position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
-		} else {
-			if (pos_sp_triplet->current.valid
-			    && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-				setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
+					// we need the vehicle to loiter indefinitely but also we want this mission item to be reached as soon
+					// as the loiter is established. therefore, set a small loiter time so that the mission item will be reached quickly,
+					// however it will just continue loitering as there is no next mission item
+					_mission_item.time_inside = 1.f;
+					_mission_item.loiter_radius = _navigator->get_loiter_radius();
+					_mission_item.acceptance_radius  = _navigator->get_acceptance_radius();
+					_mission_item.altitude = _navigator->get_local_position()->ref_alt - _loiter_height_z;
 
-			} else {
-				setLoiterItemFromCurrentPosition(&_mission_item);
+					mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+					pos_sp_triplet->current.lat = _loiter_position_lat_lon(0) > DBL_EPSILON ?
+								      _loiter_position_lat_lon(0) : _navigator->get_global_position()->lat;
+					pos_sp_triplet->current.lon = _loiter_position_lat_lon(1) > DBL_EPSILON ?
+								      _loiter_position_lat_lon(1) : _navigator->get_global_position()->lon;
+					pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
+					pos_sp_triplet->current.cruising_speed = -1.f;
+					pos_sp_triplet->current.cruising_throttle = -1.f;
+
+					_mission_item.lat = pos_sp_triplet->current.lat;
+					_mission_item.lon = pos_sp_triplet->current.lon;
+
+					_navigator->set_position_setpoint_triplet_updated();
+
+					reset_mission_item_reached();
+
+					_fw_takeoff_state = fw_takeoff_state::GO_TO_LOITER;
+					_climbout_alt_z = NAN; // reset for next takeoff command
+				}
+
+				break;
 			}
+
+		case fw_takeoff_state::GO_TO_LOITER: {
+
+				// consider reached once above the altitude acceptance radius of the loiter altitude
+				if (_navigator->get_local_position()->z < _loiter_height_z + _navigator->get_altitude_acceptance_radius()) {
+
+					position_setpoint_triplet_s *reposition_triplet = _navigator->get_reposition_triplet();
+					_navigator->reset_position_setpoint(reposition_triplet->previous);
+					_navigator->reset_position_setpoint(reposition_triplet->current);
+					_navigator->reset_position_setpoint(reposition_triplet->next);
+
+					// the FW takeoff mode is completed, exit to Hold (handled by Commander)
+					_navigator->get_mission_result()->finished = true;
+					_navigator->set_mission_result_updated();
+					_navigator->mode_completed(getNavigatorStateId());
+
+					_loiter_height_z = NAN; // reset for next takeoff command
+				}
+
+				break;
+			}
+
+		default:
+			break;
 		}
 
-		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+	} else { // rotary-wing takeoff
+		struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet();
 
-		_navigator->set_position_setpoint_triplet_updated();
+		if (rep->current.valid) {
+			// reset the position
+			set_takeoff_position();
+
+		} else if (is_mission_item_reached_or_completed() && !_navigator->get_mission_result()->finished) {
+			_navigator->get_mission_result()->finished = true;
+			_navigator->set_mission_result_updated();
+			_navigator->mode_completed(getNavigatorStateId());
+
+			position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+			// set loiter item so position controllers stop doing takeoff logic
+			if (_navigator->get_land_detected()->landed) {
+				_mission_item.nav_cmd = NAV_CMD_IDLE;
+
+			} else {
+				if (pos_sp_triplet->current.valid
+				    && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+					setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
+				}
+			}
+
+			mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+			_navigator->set_position_setpoint_triplet_updated();
+		}
 	}
 }
 
@@ -97,13 +166,21 @@ Takeoff::set_takeoff_position()
 {
 	struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet();
 
-	float takeoff_altitude_amsl = 0.f;
+	_takeoff_init_z = _navigator->get_local_position()->z;
+	const float alt_ref = PX4_ISFINITE(_navigator->get_local_position()->ref_alt) ?
+			      _navigator->get_local_position()->ref_alt : 0.f;
 
-	if (rep->current.valid && PX4_ISFINITE(rep->current.alt)) {
-		takeoff_altitude_amsl = rep->current.alt;
+	float takeoff_setpoint_z = 0.f;
+
+	if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
+	    && _param_tko_clmb_out_alt.get() > FLT_EPSILON) {
+		takeoff_setpoint_z = - (_param_tko_clmb_out_alt.get() - _takeoff_init_z);
+
+	} else if (rep->current.valid && PX4_ISFINITE(rep->current.alt)) {
+		takeoff_setpoint_z = - (rep->current.alt - alt_ref);
 
 	} else {
-		takeoff_altitude_amsl = _navigator->get_global_position()->alt + _navigator->get_param_mis_takeoff_alt();
+		takeoff_setpoint_z = - (_navigator->get_param_mis_takeoff_alt() - _takeoff_init_z);
 		mavlink_log_info(_navigator->get_mavlink_log_pub(),
 				 "Using default takeoff altitude: %.1f m\t", (double)_navigator->get_param_mis_takeoff_alt());
 
@@ -112,16 +189,27 @@ Takeoff::set_takeoff_position()
 				    _navigator->get_param_mis_takeoff_alt());
 	}
 
-	if (takeoff_altitude_amsl < _navigator->get_global_position()->alt) {
+	if (takeoff_setpoint_z > _takeoff_init_z) {
 		// If the suggestion is lower than our current alt, let's not go down.
-		takeoff_altitude_amsl = _navigator->get_global_position()->alt;
+		takeoff_setpoint_z = _takeoff_init_z;
 		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already higher than takeoff altitude\t");
 		events::send(events::ID("navigator_takeoff_already_higher"), {events::Log::Error, events::LogInternal::Info},
 			     "Already higher than takeoff altitude (not descending)");
 	}
 
+	_climbout_alt_z = takeoff_setpoint_z; // only for FW takeoff
+
+	if (!PX4_ISFINITE(_loiter_height_z)) {
+		if (_navigator->get_loiter_min_alt() > FLT_EPSILON) {
+			_loiter_height_z = - (_navigator->get_loiter_min_alt() - _takeoff_init_z);
+
+		} else {
+			_loiter_height_z = takeoff_setpoint_z;
+		}
+	}
+
 	// set current mission item to takeoff
-	set_takeoff_item(&_mission_item, takeoff_altitude_amsl);
+	set_takeoff_item(&_mission_item, alt_ref - takeoff_setpoint_z);
 	_navigator->get_mission_result()->finished = false;
 	_navigator->set_mission_result_updated();
 	reset_mission_item_reached();
@@ -151,3 +239,14 @@ Takeoff::set_takeoff_position()
 
 	_navigator->set_position_setpoint_triplet_updated();
 }
+
+void
+Takeoff::setLoiterAltitudeLocalZ(const float height_m)
+{
+	if (PX4_ISFINITE(_navigator->get_local_position()->ref_alt)) {
+		_loiter_height_z = - (height_m - _navigator->get_local_position()->ref_alt);
+
+	} else {
+		_loiter_height_z = - height_m;
+	}
+}

src/modules/navigator/takeoff.h

@@ -42,8 +42,10 @@
 
 #include "navigator_mode.h"
 #include "mission_block.h"
+#include <lib/mathlib/mathlib.h>
+#include <px4_platform_common/module_params.h>
 
-class Takeoff : public MissionBlock
+class Takeoff : public MissionBlock,  public ModuleParams
 {
 public:
 	Takeoff(Navigator *navigator);
@@ -52,7 +54,23 @@ public:
 	void on_activation() override;
 	void on_active() override;
 
-private:
+	void setLoiterPosition(matrix::Vector2d loiter_location) { _loiter_position_lat_lon = loiter_location; }
+	void setLoiterAltitudeLocalZ(const float height_m);
 
+private:
 	void set_takeoff_position();
+
+	enum class fw_takeoff_state {
+		CLIMBOUT = 0,
+		GO_TO_LOITER
+	} _fw_takeoff_state;
+
+	float _takeoff_init_z{NAN};
+	float _climbout_alt_z{NAN};
+	matrix::Vector2d _loiter_position_lat_lon{static_cast<double>(NAN), static_cast<double>(NAN)};
+	float _loiter_height_z{NAN};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::TKO_CLMB_OUT_ALT>) _param_tko_clmb_out_alt
+	)
 };

src/modules/navigator/takeoff_params.c

@@ -0,0 +1,57 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file takeoff_params.c
+ *
+ * Parameters for the takeoff navigation mode.
+ *
+ */
+
+/**
+ * Takeoff relative climbout altitude
+ *
+ * Altitude relative to home at which the vehicle will stop with the straight
+ * climbout and start heading in nominal configuration to the defined Hold position.
+ * Only applies to fixed-wing takeoff mode (excluding Mission takeoffs).
+ *
+ * Not used if set to negative value, in which case the climbout altitude
+ * is set equal to the Hold altitude.
+ *
+ * @unit m
+ * @min -1
+ * @decimal 1
+ * @increment 1
+ * @group Takeoff
+ */
+PARAM_DEFINE_FLOAT(TKO_CLMB_OUT_ALT, -1);