diff --git a/msg/commander_state.msg b/msg/commander_state.msg index 02f4c7e620..ca51625819 100644 --- a/msg/commander_state.msg +++ b/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 diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 8c9d7e2d1d..6c92ae83fd 100644 --- a/msg/vehicle_status.msg +++ b/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 diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index 931210cb85..8d96384909 100644 --- a/src/lib/events/enums.json +++ b/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]" diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 879fdf27b0..206477b3fc 100644 --- a/src/modules/commander/Commander.cpp +++ b/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; diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 75a7bb0b9a..4b1353e139 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/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 { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e07ec91174..d9f56b2813 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/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; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index ca174cd973..648fd6f620 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/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; diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 7803826920..9075f34926 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/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 diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index ee9b3a8f58..f3f50e1b1e 100644 --- a/src/modules/navigator/navigator.h +++ b/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, 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!) */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9f2585e0e5..409be05f80 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/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 && diff --git a/src/modules/navigator/vtol_takeoff.cpp b/src/modules/navigator/vtol_takeoff.cpp new file mode 100644 index 0000000000..945798130c --- /dev/null +++ b/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(); +} diff --git a/src/modules/navigator/vtol_takeoff.h b/src/modules/navigator/vtol_takeoff.h new file mode 100644 index 0000000000..a353d89ce8 --- /dev/null +++ b/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 + +#include +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) _param_loiter_alt + ) + + void set_takeoff_position(); +}; diff --git a/src/modules/navigator/vtol_takeoff_params.c b/src/modules/navigator/vtol_takeoff_params.c new file mode 100644 index 0000000000..63379f1cf9 --- /dev/null +++ b/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);