From 56dc86c589b808037facfa88204ddde2297a172c Mon Sep 17 00:00:00 2001 From: Silvan Date: Thu, 26 Jun 2025 14:14:29 +0200 Subject: [PATCH] Navigator: Takeoff mode: add state 2-state takeoff for fixed-wing Signed-off-by: Silvan --- src/modules/navigator/navigator_main.cpp | 4 + src/modules/navigator/takeoff.cpp | 155 +++++++++++++++++++---- src/modules/navigator/takeoff.h | 22 +++- src/modules/navigator/takeoff_params.c | 57 +++++++++ 4 files changed, 208 insertions(+), 30 deletions(-) create mode 100644 src/modules/navigator/takeoff_params.c diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index dc53c6928d..c9926ed06f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/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 diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index 19ab9e8732..a918bc4279 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -43,7 +43,8 @@ #include 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; + } +} diff --git a/src/modules/navigator/takeoff.h b/src/modules/navigator/takeoff.h index 6fb5d8ea19..91303edbd4 100644 --- a/src/modules/navigator/takeoff.h +++ b/src/modules/navigator/takeoff.h @@ -42,8 +42,10 @@ #include "navigator_mode.h" #include "mission_block.h" +#include +#include -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(NAN), static_cast(NAN)}; + float _loiter_height_z{NAN}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_tko_clmb_out_alt + ) }; diff --git a/src/modules/navigator/takeoff_params.c b/src/modules/navigator/takeoff_params.c new file mode 100644 index 0000000000..0fbb3e5469 --- /dev/null +++ b/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);