From 85366a80dbb176c67e7dc742b6492a6621b29990 Mon Sep 17 00:00:00 2001 From: oravla5 Date: Wed, 4 Jun 2025 18:08:43 +0200 Subject: [PATCH] flight_task: added dead-reckon rtl flight task implementation --- .../flight_mode_manager/CMakeLists.txt | 1 + .../flight_mode_manager/FlightModeManager.cpp | 8 +- .../tasks/ReturnDeadReckoning/CMakeLists.txt | 39 +++ .../FlightTaskReturnDeadReckoning.cpp | 289 ++++++++++++++++++ .../FlightTaskReturnDeadReckoning.hpp | 144 +++++++++ 5 files changed, 479 insertions(+), 2 deletions(-) create mode 100644 src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/CMakeLists.txt create mode 100644 src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.cpp create mode 100644 src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.hpp diff --git a/src/modules/flight_mode_manager/CMakeLists.txt b/src/modules/flight_mode_manager/CMakeLists.txt index 84ccba6ac0..d2885b154f 100644 --- a/src/modules/flight_mode_manager/CMakeLists.txt +++ b/src/modules/flight_mode_manager/CMakeLists.txt @@ -48,6 +48,7 @@ list(APPEND flight_tasks_all ManualAltitudeSmoothVel ManualPosition Transition + ReturnDeadReckoning ) if(NOT px4_constrained_flash_build) diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 686c59e88f..13f1a666cc 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -255,8 +255,12 @@ void FlightModeManager::start_flight_task() // Dead-Reckoning Return if (nav_state_rtl_dr) { found_some_task = true; - PX4_INFO("Going into Dead-Reckoning Return mode"); - task_failure = false; + + FlightTaskError error = switchTask(FlightTaskIndex::ReturnDeadReckoning); + + task_failure = (error != FlightTaskError::NoError); + + matching_task_running = matching_task_running && !task_failure; } // Emergency descend diff --git a/src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/CMakeLists.txt new file mode 100644 index 0000000000..a346656fa6 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2024 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. +# +############################################################################ + +px4_add_library(FlightTaskReturnDeadReckoning + FlightTaskReturnDeadReckoning.cpp +) + +target_link_libraries(FlightTaskReturnDeadReckoning PUBLIC FlightTask) +target_include_directories(FlightTaskReturnDeadReckoning PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.cpp b/src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.cpp new file mode 100644 index 0000000000..ac6a2026d6 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.cpp @@ -0,0 +1,289 @@ + +/**************************************************************************** + * + * Copyright (c) 2019-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 FlightTaskReturnDeadReckoning.cpp + */ + +#include "FlightTaskReturnDeadReckoning.hpp" + +bool FlightTaskReturnDeadReckoning::activate(const trajectory_setpoint_s &last_setpoint) +{ + PX4_INFO("FlightTaskReturnDeadReckoning::activate"); + if (!FlightTask::activate(last_setpoint)) { + PX4_ERR("Failed to activate task"); + return false; + } + + _updateSubscriptions(); + _state = State::INIT; + + if (!_updateBearingToHome()) { + PX4_ERR("Failed to compute bearing to home"); + return false; + } + + if (!_computeReturnParameters()) { + PX4_ERR("Failed to compute return parameters"); + return false; + } + + if (!_initializeSmoothers()) { + PX4_ERR("Failed to initialize smoothers"); + return false; + } + + return true; +} + +bool FlightTaskReturnDeadReckoning::update() +{ + if (!FlightTask::update()) { + return false; + } + + _updateSubscriptions(); + + if (_isGlobalPositionValid()) { + // Update the bearing to home + if (!_updateBearingToHome()) { + PX4_ERR("Failed to compute bearing to home"); + return false; + } + } + + _updateState(); + _updateSetpoints(); + + return true; +} + +void FlightTaskReturnDeadReckoning::_updateState() +{ + switch (_state) { + case State::INIT: + if (_isAboveReturnAltitude()) { + _state = State::RETURN; + PX4_INFO("Returning to home position at %.2fm (MSL) with bearing %.2f deg", + (double) _rtl_alt, (double) math::degrees(_bearing_to_home)); + } else { + _state = State::ASCENT; + PX4_INFO("Ascending to return altitude %.2fm (MSL) with bearing %.2f deg", + (double) _rtl_alt, (double) math::degrees(_bearing_to_home)); + } + break; + + case State::ASCENT: + if (_isAboveReturnAltitude()) { + _state = State::RETURN; + PX4_INFO("Returning to home position at %.2fm (MSL) with bearing %.2f deg", + (double) _rtl_alt, (double) math::degrees(_bearing_to_home)); + } + break; + + case State::RETURN: + if (_isWithinHomePositionRadius()) { + _state = State::HOLD; + PX4_INFO("Holding altitude at %.2fm (MSL) over home position", (double) _rtl_alt); + } + break; + + case State::HOLD: + break; + + default: + PX4_ERR("Unknown state"); + return; + } +} + +void FlightTaskReturnDeadReckoning::_updateSetpoints() +{ + switch (_state) { + case State::INIT: + _slew_rate_acceleration_x.update(0.0f, _deltatime); + _slew_rate_acceleration_y.update(0.0f, _deltatime); + _slew_rate_velocity_z.update(0.0f, _deltatime); + + // Hold current altitude + _velocity_setpoint(2) = _slew_rate_velocity_z.getState(); + break; + + case State::ASCENT: + _slew_rate_acceleration_x.update(0.0f, _deltatime); + _slew_rate_acceleration_y.update(0.0f, _deltatime); + _slew_rate_velocity_z.update(-_param_mpc_z_v_auto_up.get(), _deltatime); + + // Ascent until reaching the return altitude + _velocity_setpoint(2) = _slew_rate_velocity_z.getState(); + break; + + case State::RETURN: + _slew_rate_acceleration_x.update(_rtl_acc*cosf(_bearing_to_home), _deltatime); + _slew_rate_acceleration_y.update(_rtl_acc*sinf(_bearing_to_home), _deltatime); + _slew_rate_velocity_z.update(0.0f, _deltatime); + + // Stay at the return altitude + _position_setpoint(2) = -(_rtl_alt - (float) _home_position(2)); + break; + + case State::HOLD: + _slew_rate_acceleration_x.update(0.0f, _deltatime); + _slew_rate_acceleration_y.update(0.0f, _deltatime); + _slew_rate_velocity_z.update(0.0f, _deltatime); + + // Stay at the return altitude + _position_setpoint(2) = -(_rtl_alt - (float) _home_position(2)); + break; + + default: + PX4_ERR("Unknown state"); + return; + + }; + + // Heading setpoint + _heading_smoothing.update(_bearing_to_home, _deltatime); + _yaw_setpoint = _heading_smoothing.getSmoothedHeading(); + _yawspeed_setpoint = _heading_smoothing.getSmoothedHeadingRate(); + + // Acceleration setpoint + _acceleration_setpoint.xy() = matrix::Vector2f( + _slew_rate_acceleration_x.getState(), + _slew_rate_acceleration_y.getState() + ); + + return; +} + +void FlightTaskReturnDeadReckoning::_updateSubscriptions() +{ + _sub_vehicle_global_position.update(); +} + +bool FlightTaskReturnDeadReckoning::_readHomePosition(matrix::Vector3d &home_position) +{ + home_position.setNaN(); + + if (_sub_home_position.get().valid_hpos && _sub_home_position.get().valid_alt) { + home_position(0) = _sub_home_position.get().lat; + home_position(1) = _sub_home_position.get().lon; + home_position(2) = (double) _sub_home_position.get().alt; + + return true; + } + + return false; +} + +bool FlightTaskReturnDeadReckoning::_readGlobalPosition(matrix::Vector3d &global_position) +{ + + global_position.setNaN(); + + global_position(0) = _sub_vehicle_global_position.get().lat; + global_position(1) = _sub_vehicle_global_position.get().lon; + global_position(2) = (double) _sub_vehicle_global_position.get().alt; + + return true; +} + +bool FlightTaskReturnDeadReckoning::_updateBearingToHome() +{ + if (_readGlobalPosition(_start_vehicle_global_position) && _readHomePosition(_home_position)) { + _bearing_to_home = _computeBearing(_start_vehicle_global_position, _home_position); + } + + return !isnanf(_bearing_to_home); +} + +float FlightTaskReturnDeadReckoning::_computeBearing(const matrix::Vector3d &_global_position_start, const matrix::Vector3d &_global_position_end) +{ + float bearing = NAN; + if (_global_position_start.isAllFinite() && _global_position_end.isAllFinite()) { + bearing = get_bearing_to_next_waypoint(_global_position_start(0), _global_position_start(1), + _global_position_end(0), _global_position_end(1)); + } + + return bearing; +} + +bool FlightTaskReturnDeadReckoning::_computeReturnParameters() +{ + _rtl_alt = _param_rtl_return_alt.get(); + _rtl_alt = math::max((float) _start_vehicle_global_position(2), (float) _home_position(2) + _rtl_alt); + + _rtl_acc = _param_mpc_acc_hor_max.get(); + + return true; +} + +bool FlightTaskReturnDeadReckoning::_initializeSmoothers() +{ + // Initialize the heading smoother + _heading_smoothing.setMaxHeadingRate(_param_mpc_yawrauto_max.get()); + _heading_smoothing.setMaxHeadingAccel(_param_mpc_yawrauto_acc.get()); + _heading_smoothing.reset(_sub_vehicle_local_position.get().heading, 0.0f); + + // Initialize the position smoother + _slew_rate_velocity_z.setSlewRate(_param_mpc_acc_up_max.get()); + _slew_rate_velocity_z.setForcedValue((float) _sub_vehicle_local_position.get().vz); + + // Initialize the acceleration smoothers + _slew_rate_acceleration_x.setSlewRate(_param_mpc_jerk_auto.get()); + _slew_rate_acceleration_x.setForcedValue(_sub_vehicle_local_position.get().ax); + _slew_rate_acceleration_y.setSlewRate(_param_mpc_jerk_auto.get()); + _slew_rate_acceleration_y.setForcedValue(_sub_vehicle_local_position.get().ay); + + return true; +} + +bool FlightTaskReturnDeadReckoning::_isAboveReturnAltitude() const +{ + float current_alt =_sub_vehicle_global_position.get().alt; + float target_alt = _rtl_alt - _param_nav_mc_alt_rad.get(); + return current_alt > target_alt; +} + +bool FlightTaskReturnDeadReckoning::_isWithinHomePositionRadius() +{ + if (_isGlobalPositionValid()) { + _readGlobalPosition(_start_vehicle_global_position); + return get_distance_to_next_waypoint( + _start_vehicle_global_position(0), _start_vehicle_global_position(1), + _home_position(0), _home_position(1)) < _param_nav_acc_rad.get(); + } + return false; +} + + diff --git a/src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.hpp b/src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.hpp new file mode 100644 index 0000000000..37619a7fad --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.hpp @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * Copyright (c) 2019-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 FlightTaskReturnDeadReckoning.hpp + */ + +#pragma once + +#include "FlightTask.hpp" +#include +#include +#include +#include +#include + +class FlightTaskReturnDeadReckoning : public FlightTask +{ +public: + FlightTaskReturnDeadReckoning() = default; + virtual ~FlightTaskReturnDeadReckoning() = default; + + bool update() override; + bool activate(const trajectory_setpoint_s &last_setpoint) override; + +private: + uORB::SubscriptionData _sub_vehicle_global_position{ORB_ID(vehicle_global_position)}; + + void _updateState(); + + void _updateSetpoints(); + + + /** + * Initialize home position + * @return true on success, false on error + */ + bool _readHomePosition(matrix::Vector3d &home_position); + + /** + * Reads current GNSS position + * @return true on success, false on error + */ + bool _readGlobalPosition(matrix::Vector3d &global_position); + + + bool _updateBearingToHome(); + + /** + * Compute bearing to home position + * @return true on success, false on error + */ + float _computeBearing(const matrix::Vector3d &_global_position_start, + const matrix::Vector3d &_global_position_end); + + /** + * Computes return altitude velocity + * @return true on success, false on error + */ + bool _computeReturnParameters(); + + /** + * Initialize the trajectory setpoint smoothers + * @return true on success, false on error + */ + bool _initializeSmoothers(); + + void _updateSubscriptions(); + + bool _isGlobalPositionValid() const + { + return _sub_vehicle_global_position.get().lat_lon_valid && _sub_vehicle_global_position.get().alt_valid; + } + + bool _isAboveReturnAltitude() const; + + bool _isWithinHomePositionRadius(); + + + // Variable storing the home position + matrix::Vector3d _home_position; /**< Stores home position */ + matrix::Vector3d _start_vehicle_global_position; /**< Stores vehicle last known GNSS position */ + float _bearing_to_home{0.0f}; /**< Stores bearing between home and last GNSS position */ + + HeadingSmoothing _heading_smoothing; /**< Smoother for heading */ + SlewRate _slew_rate_acceleration_x{0.0f}; /**< Slew rate for x-acceleration setpoint */ + SlewRate _slew_rate_acceleration_y{0.0f}; /**< Slew rate for y-acceleration setpoint */ + SlewRate _slew_rate_velocity_z{0.0f}; /**< Smoother for vertical velocity */ + + + float _rtl_alt{0.0f}; /**< Return altitude */ + float _rtl_acc{0.0f}; /**< Return acceleration */ + + enum class State { + UNKNOWN = 0, + INIT, + ASCENT, + RETURN, + HOLD + }; + State _state{State::UNKNOWN}; /**< State machine for flight task */ + + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, + (ParamFloat) _param_mpc_acc_hor_max, + (ParamFloat) _param_rtl_return_alt, //< RTL altitude + (ParamFloat) _param_mpc_yawrauto_max, //< max yaw rate in auto modes + (ParamFloat) _param_mpc_yawrauto_acc, //< max yaw acceleration in auto modes + (ParamFloat) _param_mpc_jerk_auto, //< maximum jerk in auto modes + (ParamFloat) _param_mpc_z_v_auto_up, //< max vertical velocity up + (ParamFloat) _param_mpc_acc_up_max, //< max vertical acceleration up + (ParamFloat) _param_nav_mc_alt_rad, //< max vertical error + (ParamFloat) _param_nav_acc_rad //< max horizontal error + ); +};