diff --git a/docs/assets/flight_modes/fw_goto_altitude_foh.png b/docs/assets/flight_modes/fw_goto_altitude_foh.png new file mode 100644 index 00000000000..14f5f91ace9 Binary files /dev/null and b/docs/assets/flight_modes/fw_goto_altitude_foh.png differ diff --git a/docs/assets/flight_modes/fw_waypoint_altitude_foh.png b/docs/assets/flight_modes/fw_waypoint_altitude_foh.png new file mode 100644 index 00000000000..63fee4f50e8 Binary files /dev/null and b/docs/assets/flight_modes/fw_waypoint_altitude_foh.png differ diff --git a/docs/en/flight_modes_fw/hold.md b/docs/en/flight_modes_fw/hold.md index 9265479c87f..bb4c07e327d 100644 --- a/docs/en/flight_modes_fw/hold.md +++ b/docs/en/flight_modes_fw/hold.md @@ -77,6 +77,23 @@ You can test it in [Gazebo](../sim_gazebo_gz/index.md) using a fixed wing frame: make px4_sitl gz_rc_cessna ``` +## Altitude Ramp on Reposition (Go-to) + +When you command the vehicle to a new loiter location at a different altitude — for example a QGroundControl _Go to location_ / [reposition](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_REPOSITION) while in Hold mode — PX4 does not change the altitude setpoint in a single step. +Instead it ramps the altitude setpoint linearly (a first order hold, FOH) from the vehicle's **current altitude** to the new target altitude, reaching the target by the time the vehicle arrives at the loiter circle around the new location. +The result is a smooth diagonal climb or descent along the transit to the new location, rather than an immediate climb/descent followed by level flight. + +![Fixed-wing altitude profile for a climbing go-to in Hold mode](../../assets/flight_modes/fw_goto_altitude_foh.png) + +The ramp is anchored at the altitude the vehicle is at when the new target is received, and its progress is measured by the vehicle's horizontal approach to the target (not by time). + +If the vehicle cannot follow the ramp (for example when the required climb or sink rate exceeds what the aircraft can achieve), the altitude setpoint still reaches the full target altitude at the loiter circle. +Any remaining altitude error is then removed by climbing or sinking once the vehicle reaches the horizontal position of the new location. + +::: info +The ramp is (re)started whenever the target altitude changes; a reposition that keeps the same altitude does not change the vehicle's altitude. +::: + ## Parameters Hold mode behaviour can be configured using the parameters below. diff --git a/docs/en/flight_modes_fw/mission.md b/docs/en/flight_modes_fw/mission.md index 6dfde8ae1e4..fb33f679e5f 100644 --- a/docs/en/flight_modes_fw/mission.md +++ b/docs/en/flight_modes_fw/mission.md @@ -217,6 +217,23 @@ The equation is: $$L_{1_{distance}}=\frac{1}{\pi}L_{1_{damping}}L_{1_{period}}\left \| \vec{v}_{ {xy}_{ground} } \right \|$$ +## Altitude Changes Between Waypoints + +When the target altitude changes from one waypoint to the next, PX4 does not change the altitude setpoint in a single step. +Instead it ramps the altitude setpoint linearly (a first order hold, FOH) from the vehicle's **current altitude** to the new target altitude, reaching the target by the time the vehicle arrives at the acceptance radius of the current waypoint. +The result is a smooth diagonal climb or descent along the leg, rather than an immediate climb/descent followed by level flight. + +![Fixed-wing altitude profile for a climbing mission leg](../../assets/flight_modes/fw_waypoint_altitude_foh.png) + +The ramp is anchored at the altitude the vehicle is at when the new target is received, and its progress is measured by the vehicle's horizontal approach to the waypoint (not by time). + +If the vehicle cannot follow the ramp (for example when the required climb or sink rate exceeds what the aircraft can achieve), the altitude setpoint still reaches the full target altitude at the acceptance radius. +Any remaining altitude error is then removed by climbing or sinking once the vehicle reaches the horizontal position of the waypoint. + +::: info +The ramp is (re)started whenever the target altitude changes; consecutive waypoints at the same altitude are held level. +::: + ## Mission Takeoff Starting flights with mission takeoff (and landing using a mission landing) is the recommended way of operating a plane autonomously. diff --git a/src/modules/fw_mode_manager/CMakeLists.txt b/src/modules/fw_mode_manager/CMakeLists.txt index 0cbec8f3da8..ede6247c6e0 100644 --- a/src/modules/fw_mode_manager/CMakeLists.txt +++ b/src/modules/fw_mode_manager/CMakeLists.txt @@ -61,6 +61,8 @@ px4_add_module( SRCS FixedWingModeManager.cpp FixedWingModeManager.hpp + FirstOrderHoldAltitude.cpp + FirstOrderHoldAltitude.hpp ControllerConfigurationHandler.cpp ControllerConfigurationHandler.hpp MODULE_CONFIG @@ -68,3 +70,5 @@ px4_add_module( DEPENDS ${POSCONTROL_DEPENDENCIES} ) + +px4_add_unit_gtest(SRC FirstOrderHoldAltitudeTest.cpp EXTRA_SRCS FirstOrderHoldAltitude.cpp LINKLIBS geo mathlib) diff --git a/src/modules/fw_mode_manager/FirstOrderHoldAltitude.cpp b/src/modules/fw_mode_manager/FirstOrderHoldAltitude.cpp new file mode 100644 index 00000000000..2e38862430f --- /dev/null +++ b/src/modules/fw_mode_manager/FirstOrderHoldAltitude.cpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * Copyright (c) 2026 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. + * + ****************************************************************************/ + +#include "FirstOrderHoldAltitude.hpp" + +#include + +#include +#include + +float calculateFirstOrderHoldAltitude(const double target_lat, const double target_lon, const float target_altitude, + const double current_lat, const double current_lon, const float current_altitude, + const float acc_rad, FirstOrderHoldAltitudeState &state) +{ + // The target altitude is considered reached within the acceptance radius of the target. + const float completion_radius = acc_rad; + + const float d_curr = get_distance_to_next_waypoint(target_lat, target_lon, current_lat, current_lon); + + // Start a new ramp whenever the target altitude changes (a genuinely new altitude setpoint). Ramp updates + // that keep the same target altitude leave the ongoing ramp untouched so it keeps progressing smoothly. + const bool new_target = !PX4_ISFINITE(state.target_altitude) + || fabsf(target_altitude - state.target_altitude) > FLT_EPSILON; + + if (new_target) { + state.target_altitude = target_altitude; + // Always start the ramp from the current (measured) altitude. + state.ramp_start_altitude = current_altitude; + state.ramp_start_distance = d_curr; + state.min_distance = d_curr; + } + + // Track the closest horizontal approach so the ramp only ever progresses toward the target. + state.min_distance = math::min(state.min_distance, d_curr); + + float position_sp_alt = target_altitude; + + // Only ramp if the target was still outside the completion radius when the ramp started, otherwise there is + // no meaningful distance to interpolate over and we command the target altitude directly. + if (state.ramp_start_distance > completion_radius) { + // The setpoint is interpolated linearly from the ramp start altitude (at the ramp start distance) to the + // target altitude (reached at the completion radius around the target). + const float grad = (target_altitude - state.ramp_start_altitude) / (completion_radius - state.ramp_start_distance); + const float progress_distance = math::constrain(state.min_distance, completion_radius, state.ramp_start_distance); + position_sp_alt = target_altitude + grad * (progress_distance - completion_radius); + } + + return position_sp_alt; +} diff --git a/src/modules/fw_mode_manager/FirstOrderHoldAltitude.hpp b/src/modules/fw_mode_manager/FirstOrderHoldAltitude.hpp new file mode 100644 index 00000000000..2b87eedeb80 --- /dev/null +++ b/src/modules/fw_mode_manager/FirstOrderHoldAltitude.hpp @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2026 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include + + +/** + * @brief State of the altitude first order hold (FOH), persisted across calls. + * + * A ramp is (re)started whenever the target (current waypoint) altitude changes. It always starts from the + * current (measured) altitude at the vehicle's current horizontal distance to the target, hence the ramp + * anchor fields have to survive between calls. + */ +struct FirstOrderHoldAltitudeState { + float target_altitude{NAN}; ///< altitude setpoint currently ramped toward [m AMSL] + float ramp_start_altitude{NAN}; ///< altitude the current ramp started from [m AMSL] + float ramp_start_distance{NAN}; ///< horizontal distance to the target when the ramp started [m] + float min_distance{FLT_MAX}; ///< closest horizontal approach to the target during this ramp [m] +}; + +/** + * @brief Calculate the altitude setpoint using an altitude first order hold (FOH). + * + * Whenever the target (current waypoint) altitude changes, a new ramp is started from the current (measured) + * vehicle altitude at the vehicle's present horizontal distance to the target. The setpoint is then linearly + * interpolated along the remaining distance such that the new target altitude is reached at the acceptance + * radius around the target. While the target altitude stays the same the ramp keeps progressing and is never + * restarted, even if the position setpoint is otherwise updated. + * + * @param target_lat target (current waypoint) latitude [deg] + * @param target_lon target (current waypoint) longitude [deg] + * @param target_altitude target (current waypoint) altitude [m AMSL] + * @param current_lat current vehicle latitude [deg] + * @param current_lon current vehicle longitude [deg] + * @param current_altitude current vehicle altitude [m AMSL] + * @param acc_rad acceptance radius around the current waypoint [m] + * @param state in/out FOH state, persisted across calls + * @return altitude setpoint [m AMSL] + */ +float calculateFirstOrderHoldAltitude(const double target_lat, const double target_lon, const float target_altitude, + const double current_lat, const double current_lon, const float current_altitude, + const float acc_rad, FirstOrderHoldAltitudeState &state); diff --git a/src/modules/fw_mode_manager/FirstOrderHoldAltitudeTest.cpp b/src/modules/fw_mode_manager/FirstOrderHoldAltitudeTest.cpp new file mode 100644 index 00000000000..cf87f1828bb --- /dev/null +++ b/src/modules/fw_mode_manager/FirstOrderHoldAltitudeTest.cpp @@ -0,0 +1,170 @@ +/**************************************************************************** + * + * Copyright (c) 2026 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. + * + ****************************************************************************/ + +#include +#include + +#include + +#include "FirstOrderHoldAltitude.hpp" + +// run with: make tests TESTFILTER=FirstOrderHoldAltitude + +// Target waypoint on the equator, ~1113 m away from the vehicle's start position. +static constexpr double kVehicleLat = 0.0; +static constexpr double kVehicleLon = 0.0; +static constexpr double kCurrLat = 0.0; +static constexpr double kCurrLon = 0.01; + +static constexpr float kStartAlt = 100.0f; // altitude the vehicle is at when the target is received +static constexpr float kCurrAlt = 200.0f; // target altitude + +TEST(FirstOrderHoldAltitudeTest, FirstCallWithoutHistoryRampsFromCurrentAltitude) +{ + // No altitude setpoint has been commanded yet: the ramp starts from the vehicle's current altitude, so at + // the start distance the commanded altitude equals the current altitude. + FirstOrderHoldAltitudeState state{}; + const float alt = calculateFirstOrderHoldAltitude(kCurrLat, kCurrLon, kCurrAlt, kVehicleLat, kVehicleLon, kStartAlt, 0.0f, state); + EXPECT_NEAR(alt, kStartAlt, 1e-2f); +} + +TEST(FirstOrderHoldAltitudeTest, RampAlwaysStartsFromCurrentAltitude) +{ + // The ramp always starts from the current (measured) altitude, regardless of any prior ramp state. + FirstOrderHoldAltitudeState state{}; + const float current_altitude = 90.0f; + const float alt = calculateFirstOrderHoldAltitude(kCurrLat, kCurrLon, kCurrAlt, kVehicleLat, kVehicleLon, current_altitude, 0.0f, + state); + EXPECT_NEAR(alt, current_altitude, 1e-2f); +} + +TEST(FirstOrderHoldAltitudeTest, MidpointReturnsInterpolatedAltitude) +{ + // Start the ramp at the full leg length, then move the vehicle halfway to the target: the commanded + // altitude should be the linear midpoint between the ramp start and target altitude. + FirstOrderHoldAltitudeState state{}; + + // First call captures the ramp start at the full leg length, ramping from kStartAlt. + calculateFirstOrderHoldAltitude(kCurrLat, kCurrLon, kCurrAlt, kVehicleLat, kVehicleLon, kStartAlt, 0.0f, state); + + // Move the vehicle to half the leg length from the target (same target altitude -> ramp keeps progressing). + const double half_lon = kCurrLon / 2.0; + const float alt = calculateFirstOrderHoldAltitude(kCurrLat, kCurrLon, kCurrAlt, kCurrLat, half_lon, kStartAlt, 0.0f, state); + EXPECT_NEAR(alt, 0.5f * (kStartAlt + kCurrAlt), 1e-2f); +} + +TEST(FirstOrderHoldAltitudeTest, ReachesTargetAltitudeAtAcceptanceRadius) +{ + // Within the acceptance radius around the target the full target altitude is commanded. + const float acc_rad = 50.0f; + FirstOrderHoldAltitudeState state{}; + + // Start the ramp far from the target. + calculateFirstOrderHoldAltitude(kCurrLat, kCurrLon, kCurrAlt, kVehicleLat, kVehicleLon, kStartAlt, acc_rad, state); + + // Place the vehicle on the target waypoint (distance 0, i.e. inside the acceptance radius). + const float alt = calculateFirstOrderHoldAltitude(kCurrLat, kCurrLon, kCurrAlt, kCurrLat, kCurrLon, kStartAlt, acc_rad, state); + EXPECT_FLOAT_EQ(alt, kCurrAlt); +} + +TEST(FirstOrderHoldAltitudeTest, MinDistanceOnlyDecreases) +{ + // Progress the vehicle close to the target, then move it back away: the commanded altitude must not fall + // back toward the ramp start because the closest approach is latched. + FirstOrderHoldAltitudeState state{}; + + // Start the ramp at the full leg length. + calculateFirstOrderHoldAltitude(kCurrLat, kCurrLon, kCurrAlt, kVehicleLat, kVehicleLon, kStartAlt, 0.0f, state); + + // Move close to the target (small remaining distance). + const double close_lon = kCurrLon * 0.99; + const float alt_close = calculateFirstOrderHoldAltitude(kCurrLat, kCurrLon, kCurrAlt, kCurrLat, close_lon, kStartAlt, 0.0f, state); + + // Move back to the start: altitude must not decrease again (closest approach is retained). + const float alt_back = calculateFirstOrderHoldAltitude(kCurrLat, kCurrLon, kCurrAlt, kVehicleLat, kVehicleLon, kStartAlt, 0.0f, state); + EXPECT_FLOAT_EQ(alt_back, alt_close); +} + +TEST(FirstOrderHoldAltitudeTest, SameTargetAltitudeDoesNotRestartRamp) +{ + // A position setpoint update that keeps the same target altitude must not restart the ramp: the ramp start + // altitude and distance stay latched from the first call. + FirstOrderHoldAltitudeState state{}; + + calculateFirstOrderHoldAltitude(kCurrLat, kCurrLon, kCurrAlt, kVehicleLat, kVehicleLon, kStartAlt, 0.0f, state); + const float captured_start_alt = state.ramp_start_altitude; + const float captured_start_dist = state.ramp_start_distance; + + // Call again from a different vehicle altitude but with the same target altitude. + calculateFirstOrderHoldAltitude(kCurrLat, kCurrLon, kCurrAlt, kVehicleLat, kVehicleLon, /*current_altitude*/ 300.0f, 0.0f, state); + + EXPECT_FLOAT_EQ(state.ramp_start_altitude, captured_start_alt); + EXPECT_FLOAT_EQ(state.ramp_start_distance, captured_start_dist); +} + +TEST(FirstOrderHoldAltitudeTest, ChangedTargetAltitudeRestartsRamp) +{ + // A new target altitude restarts the ramp from the current altitude at the current distance. + FirstOrderHoldAltitudeState state{}; + + // First target: ramp toward kCurrAlt and drive it to completion. + calculateFirstOrderHoldAltitude(kCurrLat, kCurrLon, kCurrAlt, kVehicleLat, kVehicleLon, kStartAlt, 0.0f, state); + const float alt_reached = calculateFirstOrderHoldAltitude(kCurrLat, kCurrLon, kCurrAlt, kCurrLat, kCurrLon, kStartAlt, 0.0f, state); + ASSERT_FLOAT_EQ(alt_reached, kCurrAlt); + + // New target altitude while far from the target again: the ramp restarts from the current altitude, which + // here equals the previous target altitude (the vehicle reached it). + const float new_target = 260.0f; + const float alt = calculateFirstOrderHoldAltitude(kCurrLat, kCurrLon, new_target, kVehicleLat, kVehicleLon, + /*current_altitude*/ kCurrAlt, 0.0f, state); + EXPECT_NEAR(alt, kCurrAlt, 1e-2f); + EXPECT_FLOAT_EQ(state.target_altitude, new_target); +} + +TEST(FirstOrderHoldAltitudeTest, LoiterRadiusReachesTargetThroughAcceptanceRadius) +{ + // The loiter circle reaches the FOH through the acceptance radius (the caller folds the loiter radius into + // acc_rad). Inside that radius the full target altitude is commanded, even at a nonzero remaining distance. + const float loiter_radius = 600.0f; + const float acc_rad = loiter_radius; + FirstOrderHoldAltitudeState state{}; + + // Start the ramp far from the target. + calculateFirstOrderHoldAltitude(kCurrLat, kCurrLon, kCurrAlt, kVehicleLat, kVehicleLon, kStartAlt, acc_rad, state); + + // Place the vehicle ~556 m from the target, i.e. inside the loiter circle. + const double half_lon = kCurrLon / 2.0; + const float alt = calculateFirstOrderHoldAltitude(kCurrLat, kCurrLon, kCurrAlt, kCurrLat, half_lon, kStartAlt, + acc_rad, state); + EXPECT_FLOAT_EQ(alt, kCurrAlt); +} diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.cpp b/src/modules/fw_mode_manager/FixedWingModeManager.cpp index b69330a5e79..4f70be21777 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.cpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.cpp @@ -807,39 +807,9 @@ FixedWingModeManager::control_auto_position(const float control_interval, const const float target_airspeed = pos_sp_curr.cruising_speed > FLT_EPSILON ? pos_sp_curr.cruising_speed : NAN; // waypoint is a plain navigation waypoint - float position_sp_alt = pos_sp_curr.alt; - - // Altitude first order hold (FOH) - if (_position_setpoint_previous_valid && - ((pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_POSITION) || - (pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) - ) { - const float d_curr_prev = get_distance_to_next_waypoint(pos_sp_curr.lat, pos_sp_curr.lon, pos_sp_prev.lat, - pos_sp_prev.lon); - - // Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one - if (d_curr_prev > math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius))) { - // Calculate distance to current waypoint - const float d_curr = get_distance_to_next_waypoint(pos_sp_curr.lat, pos_sp_curr.lon, _current_latitude, - _current_longitude); - - // Save distance to waypoint if it is the smallest ever achieved, however make sure that - // _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp - _min_current_sp_distance_xy = math::min(d_curr, _min_current_sp_distance_xy, d_curr_prev); - - // if the minimal distance is smaller than the acceptance radius, we should be at waypoint alt - // navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude - if (_min_current_sp_distance_xy > math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius))) { - // The setpoint is set linearly and such that the system reaches the current altitude at the acceptance - // radius around the current waypoint - const float delta_alt = (pos_sp_curr.alt - pos_sp_prev.alt); - const float grad = -delta_alt / (d_curr_prev - math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius))); - const float a = pos_sp_prev.alt - grad * d_curr_prev; - - position_sp_alt = a + grad * _min_current_sp_distance_xy; - } - } - } + _foh_altitude_active = true; + const float position_sp_alt = calculateFirstOrderHoldAltitude(pos_sp_curr.lat, pos_sp_curr.lon, pos_sp_curr.alt, + _current_latitude, _current_longitude, _current_altitude, acc_rad, _foh_altitude_state); const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = { .timestamp = hrt_absolute_time(), @@ -938,6 +908,13 @@ FixedWingModeManager::control_auto_loiter(const float control_interval, const Ve loiter_direction_ccw = _param_nav_loiter_rad.get() < -FLT_EPSILON; } + // Smoothly ramp the altitude setpoint from the current altitude such that the loiter altitude is reached + // by the time the vehicle arrives at the loiter circle. + const float acc_rad = math::max(_directional_guidance.switchDistance(500.0f), loiter_radius); + _foh_altitude_active = true; + const float position_sp_alt = calculateFirstOrderHoldAltitude(pos_sp_curr.lat, pos_sp_curr.lon, pos_sp_curr.alt, + _current_latitude, _current_longitude, _current_altitude, acc_rad, _foh_altitude_state); + Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; Vector2f curr_wp_local{_global_local_proj_ref.project(curr_wp(0), curr_wp(1))}; Vector2f vehicle_to_loiter_center{curr_wp_local - curr_pos_local}; @@ -999,7 +976,7 @@ FixedWingModeManager::control_auto_loiter(const float control_interval, const Ve const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = { .timestamp = hrt_absolute_time(), - .altitude = pos_sp_curr.alt, + .altitude = position_sp_alt, .height_rate = NAN, .equivalent_airspeed = target_airspeed, .pitch_direct = NAN, @@ -2046,6 +2023,10 @@ FixedWingModeManager::Run() // mode is active _ctrl_configuration_handler.resetLastPublishTime(); + // The controller (and thus the altitude FOH) does not run while an external mode is active. Mark the FOH + // as inactive so that returning to an internal auto mode starts a fresh ramp instead of resuming stale state. + _foh_altitude_active = false; + } else if (_local_pos_sub.update(&_local_pos)) { const hrt_abstime now = _local_pos.timestamp; @@ -2202,8 +2183,9 @@ FixedWingModeManager::Run() && PX4_ISFINITE(_pos_sp_triplet.next.lon) && PX4_ISFINITE(_pos_sp_triplet.next.alt); - // reset the altitude foh (first order hold) logic - _min_current_sp_distance_xy = FLT_MAX; + // The altitude first order hold (FOH) restarts its ramp itself whenever the target altitude + // changes, so it must not be reset here: a new triplet that keeps the same target altitude + // (e.g. a lat/lon-only update) has to leave the ongoing ramp progressing. _go_direct_to_destination = false; } @@ -2261,6 +2243,17 @@ FixedWingModeManager::Run() reset_takeoff_state(); } + if (!_foh_altitude_active) { + // The altitude first order hold only runs for AUTO position/loiter waypoints. Whenever it did not run + // on the previous cycle (any other mode, but also AUTO sub-types like velocity/idle/course-hold that + // skip it) its ramp state is stale, so reset it here. (Re-)engaging FOH then starts a fresh ramp from + // the current altitude instead of a last-commanded setpoint that went stale while it was not running. + _foh_altitude_state = FirstOrderHoldAltitudeState{}; + } + + // Cleared here every cycle and set again by the FOH call sites when they actually run this cycle. + _foh_altitude_active = false; + int8_t old_landing_gear_position = _new_landing_gear_position; _new_landing_gear_position = landing_gear_s::GEAR_KEEP; // is overwritten in Takeoff and Land diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.hpp b/src/modules/fw_mode_manager/FixedWingModeManager.hpp index 4862daab031..1f24e47436f 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.hpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.hpp @@ -43,6 +43,7 @@ #include "launchdetection/LaunchDetector.h" #include "runway_takeoff/RunwayTakeoff.h" #include "ControllerConfigurationHandler.hpp" +#include "FirstOrderHoldAltitude.hpp" #include #include @@ -401,7 +402,8 @@ private: float _spoilers_setpoint{0.f}; hrt_abstime _time_in_fixed_bank_loiter{0}; // [us] - float _min_current_sp_distance_xy{FLT_MAX}; + FirstOrderHoldAltitudeState _foh_altitude_state{}; + bool _foh_altitude_active{false}; ///< whether the altitude FOH ran on the current cycle (else its state is reset) #ifdef CONFIG_FIGURE_OF_EIGHT /* Loitering */ @@ -544,7 +546,8 @@ private: * @param pos_sp_next next position setpoint */ void control_auto_loiter(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed, - const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next); + const position_setpoint_s &pos_sp_curr, + const position_setpoint_s &pos_sp_next); /**