Added VTOL Takeoff navigation mode (#19027)

* Commander: added support for MAIN_STATE_AUTO_VTOL_TAKEOFF
* navigator: added support for vtol_takeoff navigation mode

msg/commander_state.msg

@@ -16,7 +16,8 @@ uint8 MAIN_STATE_AUTO_LAND          = 11
 uint8 MAIN_STATE_AUTO_FOLLOW_TARGET = 12
 uint8 MAIN_STATE_AUTO_PRECLAND      = 13
 uint8 MAIN_STATE_ORBIT              = 14
-uint8 MAIN_STATE_MAX                = 15
+uint8 MAIN_STATE_AUTO_VTOL_TAKEOFF  = 15
+uint8 MAIN_STATE_MAX                = 16
 
 uint8 main_state
 

msg/vehicle_status.msg

@@ -46,7 +46,8 @@ uint8 NAVIGATION_STATE_AUTO_LAND = 18		# Land
 uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19	# Auto Follow
 uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20	# Precision land with landing target
 uint8 NAVIGATION_STATE_ORBIT = 21       # Orbit in a circle
-uint8 NAVIGATION_STATE_MAX = 22
+uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22   # Takeoff, transition, establish loiter
+uint8 NAVIGATION_STATE_MAX = 23
 
 uint8 VEHICLE_TYPE_UNKNOWN = 0
 uint8 VEHICLE_TYPE_ROTARY_WING = 1

src/lib/events/enums.json

@@ -239,6 +239,10 @@
                             "name": "orbit",
                             "description": "Orbit"
                         },
+                        "15": {
+                            "name": "auto_vtol_takeoff",
+                            "description": "Vtol Takeoff"
+                        },
                         "255": {
                             "name": "unknown",
                             "description": "[Unknown]"

src/modules/commander/Commander.cpp

@@ -629,9 +629,12 @@ static inline navigation_mode_t navigation_mode(uint8_t main_state)
 	case commander_state_s::MAIN_STATE_AUTO_PRECLAND: return navigation_mode_t::auto_precland;
 
 	case commander_state_s::MAIN_STATE_ORBIT: return navigation_mode_t::orbit;
+
+	case commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF: return navigation_mode_t::auto_vtol_takeoff;
 	}
 
-	static_assert(commander_state_s::MAIN_STATE_MAX - 1 == (int)navigation_mode_t::orbit, "enum definition mismatch");
+	static_assert(commander_state_s::MAIN_STATE_MAX - 1 == (int)navigation_mode_t::auto_vtol_takeoff,
+		      "enum definition mismatch");
 
 	return navigation_mode_t::unknown;
 }
@@ -1149,6 +1152,24 @@ Commander::handle_command(const vehicle_command_s &cmd)
 		}
 		break;
 
+	case vehicle_command_s::VEHICLE_CMD_NAV_VTOL_TAKEOFF:
+
+		/* ok, home set, use it to take off */
+		if (TRANSITION_CHANGED == main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF,
+				_status_flags,
+				_internal_state)) {
+			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
+		} else if (_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF) {
+			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
+		} else {
+			mavlink_log_critical(&_mavlink_log_pub, "VTOL Takeoff denied! Please disarm and retry");
+			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+		}
+
+		break;
+
 	case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
 			if (TRANSITION_DENIED != main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LAND, _status_flags,
 					_internal_state)) {
@@ -2393,7 +2414,8 @@ Commander::run()
 
 			// Transition main state to loiter or auto-mission after takeoff is completed.
 			if (_armed.armed && !_land_detector.landed
-			    && (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF)
+			    && (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
+				_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)
 			    && (mission_result.timestamp >= _status.nav_state_timestamp)
 			    && mission_result.finished) {
 
@@ -3332,6 +3354,7 @@ Commander::update_control_mode()
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF:
 		_vehicle_control_mode.flag_control_auto_enabled = true;
 		_vehicle_control_mode.flag_control_rates_enabled = true;
 		_vehicle_control_mode.flag_control_attitude_enabled = true;

src/modules/commander/px4_custom_mode.h

@@ -63,7 +63,8 @@ enum PX4_CUSTOM_SUB_MODE_AUTO {
 	PX4_CUSTOM_SUB_MODE_AUTO_LAND,
 	PX4_CUSTOM_SUB_MODE_AUTO_RESERVED_DO_NOT_USE, // was PX4_CUSTOM_SUB_MODE_AUTO_RTGS, deleted 2020-03-05
 	PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET,
-	PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND
+	PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND,
+	PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF
 };
 
 enum PX4_CUSTOM_SUB_MODE_POSCTL {

src/modules/commander/state_machine_helper.cpp

@@ -368,6 +368,13 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai
 
 		break;
 
+	case commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF:
+		if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+			ret = TRANSITION_CHANGED;
+		}
+
+		break;
+
 	case commander_state_s::MAIN_STATE_AUTO_MISSION:
 
 		/* need global position, home position, and a valid mission */
@@ -735,6 +742,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 		break;
 
 	case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
+	case commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF:
 
 		/* require local position */
 
@@ -769,7 +777,8 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 			set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0);
 
 		} else {
-			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF;
+			status.nav_state = internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF ?
+					   vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF : vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF;
 		}
 
 		break;

src/modules/mavlink/mavlink_messages.cpp

@@ -309,6 +309,11 @@ union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
 		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_POSCTL_ORBIT;
 		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF;
+		break;
 	}
 
 	return custom_mode;

src/modules/navigator/CMakeLists.txt

@@ -50,6 +50,7 @@ px4_add_module(
 		geofence.cpp
 		enginefailure.cpp
 		follow_target.cpp
+		vtol_takeoff.cpp
 	DEPENDS
 		geo
 		landing_slope

src/modules/navigator/navigator.h

@@ -51,6 +51,7 @@
 #include "navigator_mode.h"
 #include "rtl.h"
 #include "takeoff.h"
+#include "vtol_takeoff.h"
 
 #include "navigation.h"
 
@@ -85,7 +86,7 @@ using namespace time_literals;
 /**
  * Number of navigation modes that need on_active/on_inactive calls
  */
-#define NAVIGATOR_MODE_ARRAY_SIZE 8
+#define NAVIGATOR_MODE_ARRAY_SIZE 9
 
 class Navigator : public ModuleBase<Navigator>, public ModuleParams
 {
@@ -386,7 +387,8 @@ private:
 	Mission		_mission;			/**< class that handles the missions */
 	Loiter		_loiter;			/**< class that handles loiter */
 	Takeoff		_takeoff;			/**< class for handling takeoff commands */
-	Land		_land;				/**< class for handling land commands */
+	VtolTakeoff	_vtol_takeoff;			/**< class for handling VEHICLE_CMD_NAV_VTOL_TAKEOFF command */
+	Land		_land;			/**< class for handling land commands */
 	PrecLand	_precland;			/**< class for handling precision land commands */
 	RTL 		_rtl;				/**< class that handles RTL */
 	EngineFailure	_engineFailure;			/**< class that handles the engine failure mode (FW only!) */

src/modules/navigator/navigator_main.cpp

@@ -74,6 +74,7 @@ Navigator::Navigator() :
 	_mission(this),
 	_loiter(this),
 	_takeoff(this),
+	_vtol_takeoff(this),
 	_land(this),
 	_precland(this),
 	_rtl(this),
@@ -88,7 +89,8 @@ Navigator::Navigator() :
 	_navigation_mode_array[4] = &_takeoff;
 	_navigation_mode_array[5] = &_land;
 	_navigation_mode_array[6] = &_precland;
-	_navigation_mode_array[7] = &_follow_target;
+	_navigation_mode_array[7] = &_vtol_takeoff;
+	_navigation_mode_array[8] = &_follow_target;
 
 	_handle_back_trans_dec_mss = param_find("VT_B_DEC_MSS");
 	_handle_reverse_delay = param_find("VT_B_REV_DEL");
@@ -448,6 +450,16 @@ void Navigator::run()
 
 				// CMD_NAV_TAKEOFF is acknowledged by commander
 
+			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_VTOL_TAKEOFF) {
+
+				_vtol_takeoff.setTransitionAltitudeAbsolute(cmd.param7);
+
+				// after the transition the vehicle will establish on a loiter at this position
+				_vtol_takeoff.setLoiterLocation(matrix::Vector2d(cmd.param5, cmd.param6));
+
+				// loiter height is the height above takeoff altitude at which the vehicle will establish on a loiter circle
+				_vtol_takeoff.setLoiterHeight(cmd.param1);
+
 			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_LAND_START) {
 
 				/* find NAV_CMD_DO_LAND_START in the mission and
@@ -669,6 +681,11 @@ void Navigator::run()
 			navigation_mode_new = &_takeoff;
 			break;
 
+		case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF:
+			_pos_sp_triplet_published_invalid_once = false;
+			navigation_mode_new = &_vtol_takeoff;
+			break;
+
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
 			_pos_sp_triplet_published_invalid_once = false;
 			navigation_mode_new = &_land;
@@ -714,17 +731,30 @@ void Navigator::run()
 
 		/* we have a new navigation mode: reset triplet */
 		if (_navigation_mode != navigation_mode_new) {
-			// We don't reset the triplet if we just did an auto-takeoff and are now
+			// We don't reset the triplet in the following two cases:
+			// 1)  if we just did an auto-takeoff and are now
 			// going to loiter. Otherwise, we lose the takeoff altitude and end up lower
 			// than where we wanted to go.
+			// 2) We switch to loiter and the current position setpoint already has a valid loiter point.
+			// In that case we can assume that the vehicle has already established a loiter and we don't need to set a new
+			// loiter position.
 			//
 			// FIXME: a better solution would be to add reset where they are needed and remove
 			//        this general reset here.
-			if (!(_navigation_mode == &_takeoff &&
-			      navigation_mode_new == &_loiter)) {
+
+			const bool current_mode_is_takeoff = _navigation_mode == &_takeoff;
+			const bool new_mode_is_loiter = navigation_mode_new == &_loiter;
+			const bool valid_loiter_setpoint = (_pos_sp_triplet.current.valid
+							    && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
+
+			const bool did_not_switch_takeoff_to_loiter = !(current_mode_is_takeoff && new_mode_is_loiter);
+			const bool did_not_switch_to_loiter_with_valid_loiter_setpoint = !(new_mode_is_loiter && valid_loiter_setpoint);
+
+			if (did_not_switch_takeoff_to_loiter && did_not_switch_to_loiter_with_valid_loiter_setpoint) {
 				reset_triplets();
 			}
 
+
 			// transition to hover in Descend mode
 			if (_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND &&
 			    _vstatus.is_vtol && _vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING &&

src/modules/navigator/vtol_takeoff.cpp

@@ -0,0 +1,165 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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 vtol_takeoff.cpp
+ *
+ * Helper class to do a VTOL takeoff and transition into a loiter.
+ *
+ */
+
+#include "vtol_takeoff.h"
+#include "navigator.h"
+
+using matrix::wrap_pi;
+
+VtolTakeoff::VtolTakeoff(Navigator *navigator) :
+	MissionBlock(navigator),
+	ModuleParams(navigator)
+{
+}
+
+void
+VtolTakeoff::on_activation()
+{
+	if (_navigator->home_position_valid()) {
+		set_takeoff_position();
+		_takeoff_state = vtol_takeoff_state::TAKEOFF_HOVER;
+	}
+}
+
+void
+VtolTakeoff::on_active()
+{
+	if (is_mission_item_reached()) {
+		reset_mission_item_reached();
+
+		switch	(_takeoff_state) {
+		case vtol_takeoff_state::TAKEOFF_HOVER: {
+
+				position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+				_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+				_mission_item.yaw = wrap_pi(get_bearing_to_next_waypoint(_navigator->get_home_position()->lat,
+							    _navigator->get_home_position()->lon, _loiter_location(0), _loiter_location(1)));
+				_mission_item.force_heading = true;
+				mission_apply_limitation(_mission_item);
+				mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+				pos_sp_triplet->current.disable_weather_vane = true;
+
+				_navigator->set_position_setpoint_triplet_updated();
+
+				_takeoff_state = vtol_takeoff_state::ALIGN_HEADING;
+
+				break;
+			}
+
+		case vtol_takeoff_state::ALIGN_HEADING: {
+
+				set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
+				_mission_item.lat = _loiter_location(0);
+				_mission_item.lon = _loiter_location(1);
+				position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+				pos_sp_triplet->previous = pos_sp_triplet->current;
+
+				_navigator->set_position_setpoint_triplet_updated();
+
+				issue_command(_mission_item);
+
+				_takeoff_state = vtol_takeoff_state::TRANSITION;
+
+				break;
+			}
+
+		case vtol_takeoff_state::TRANSITION: {
+				position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+				_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
+				_mission_item.lat = _loiter_location(0);
+				_mission_item.lon = _loiter_location(1);
+
+				// 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;
+				_mission_item.loiter_radius = _navigator->get_loiter_radius();
+				_mission_item.altitude = _navigator->get_home_position()->alt + _param_loiter_alt.get();
+
+				mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+
+				_navigator->set_position_setpoint_triplet_updated();
+
+				_takeoff_state = vtol_takeoff_state::CLIMB;
+
+				break;
+			}
+
+		case vtol_takeoff_state::CLIMB: {
+
+				// the VTOL takeoff is done, proceed loitering and upate the navigation state to LOITER
+				_navigator->get_mission_result()->finished = true;
+				_navigator->set_mission_result_updated();
+
+				break;
+			}
+
+		default: {
+
+				break;
+			}
+		}
+	}
+}
+
+void
+VtolTakeoff::set_takeoff_position()
+{
+	// set current mission item to takeoff
+	set_takeoff_item(&_mission_item, _transition_alt_amsl);
+
+	_mission_item.lat = _navigator->get_home_position()->lat;
+	_mission_item.lon = _navigator->get_home_position()->lon;
+
+	_navigator->get_mission_result()->finished = false;
+	_navigator->set_mission_result_updated();
+
+	// convert mission item to current setpoint
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	mission_apply_limitation(_mission_item);
+	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+
+	pos_sp_triplet->previous.valid = false;
+	pos_sp_triplet->current.yaw_valid = true;
+	pos_sp_triplet->next.valid = false;
+
+	_navigator->set_position_setpoint_triplet_updated();
+}

src/modules/navigator/vtol_takeoff.h

@@ -0,0 +1,81 @@
+/***************************************************************************
+ *
+ *   Copyright (c) 2022 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 vtol_takeoff.h
+ *
+ * Helper class to do a VTOL takeoff and transition into a loiter.
+ *
+ */
+
+#pragma once
+
+#include "navigator_mode.h"
+#include "mission_block.h"
+
+#include <lib/mathlib/mathlib.h>
+
+#include <px4_platform_common/module_params.h>
+class VtolTakeoff : public MissionBlock, public ModuleParams
+{
+public:
+	VtolTakeoff(Navigator *navigator);
+	~VtolTakeoff() = default;
+
+	void on_activation() override;
+	void on_active() override;
+
+	void setTransitionAltitudeAbsolute(const float alt_amsl) {_transition_alt_amsl = alt_amsl; }
+
+	void setLoiterLocation(matrix::Vector2d loiter_location) { _loiter_location = loiter_location; }
+	void setLoiterHeight(const float height_m) { _loiter_height = height_m; }
+
+private:
+
+	enum class vtol_takeoff_state {
+		TAKEOFF_HOVER = 0,
+		ALIGN_HEADING,
+		TRANSITION,
+		CLIMB,
+		ABORT_TAKEOFF_AND_LAND
+	} _takeoff_state;
+
+	float _transition_alt_amsl{0.f};	// absolute altitude at which vehicle will transition to forward flight
+	matrix::Vector2d _loiter_location;
+	float _loiter_height{0};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::VTO_LOITER_ALT>) _param_loiter_alt
+	)
+
+	void set_takeoff_position();
+};

src/modules/navigator/vtol_takeoff_params.c

@@ -0,0 +1,53 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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 VTOLTakeoff_params.c
+ *
+ * Parameters for the VTOL takeoff navigation mode.
+ *
+ */
+
+/**
+ * VTOL Takeoff relative loiter altitude.
+ *
+ * Altitude relative to home at which vehicle will loiter after front transition.
+ *
+ * @unit m
+ * @min 20
+ * @max 300
+ * @decimal 1
+ * @increment 1
+ * @group VTOL Takeoff
+ */
+PARAM_DEFINE_FLOAT(VTO_LOITER_ALT, 80);