From de90543d6ff4cfb00cf518d2493a7dad068f32d6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 9 Oct 2019 14:58:00 +0200 Subject: [PATCH] FlightTasks: add Descend task to land without GPS This adds a flight task to catch the case where we want to do an emergency descent without GPS but only a baro. Previously, this would lead to the navigator land class being called without position estimates which then made the flight tasks fail and react with a flight task failsafe. This however meant that landed was never detected and a couple of confusing error messages. This applies if NAV_RCL_ACT is set to 3 "land". --- src/lib/FlightTasks/CMakeLists.txt | 1 + .../FlightTasks/tasks/Descend/CMakeLists.txt | 39 ++++++++++++ .../tasks/Descend/FlightTaskDescend.cpp | 62 +++++++++++++++++++ .../tasks/Descend/FlightTaskDescend.hpp | 58 +++++++++++++++++ src/modules/commander/Commander.cpp | 3 +- .../mc_pos_control/mc_pos_control_main.cpp | 22 +++++++ src/modules/navigator/navigator_main.cpp | 6 +- 7 files changed, 184 insertions(+), 7 deletions(-) create mode 100644 src/lib/FlightTasks/tasks/Descend/CMakeLists.txt create mode 100644 src/lib/FlightTasks/tasks/Descend/FlightTaskDescend.cpp create mode 100644 src/lib/FlightTasks/tasks/Descend/FlightTaskDescend.hpp diff --git a/src/lib/FlightTasks/CMakeLists.txt b/src/lib/FlightTasks/CMakeLists.txt index fd64349513d4..6143dae51c65 100644 --- a/src/lib/FlightTasks/CMakeLists.txt +++ b/src/lib/FlightTasks/CMakeLists.txt @@ -63,6 +63,7 @@ list(APPEND flight_tasks_all AutoFollowMe Offboard Failsafe + Descend Transition ${flight_tasks_to_add} ) diff --git a/src/lib/FlightTasks/tasks/Descend/CMakeLists.txt b/src/lib/FlightTasks/tasks/Descend/CMakeLists.txt new file mode 100644 index 000000000000..5ef5777a7162 --- /dev/null +++ b/src/lib/FlightTasks/tasks/Descend/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2019 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(FlightTaskDescend + FlightTaskDescend.cpp +) + +target_link_libraries(FlightTaskDescend PUBLIC FlightTask) +target_include_directories(FlightTaskDescend PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/lib/FlightTasks/tasks/Descend/FlightTaskDescend.cpp b/src/lib/FlightTasks/tasks/Descend/FlightTaskDescend.cpp new file mode 100644 index 000000000000..cce5497f2715 --- /dev/null +++ b/src/lib/FlightTasks/tasks/Descend/FlightTaskDescend.cpp @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 FlightTaskDescend.cpp + */ + +#include "FlightTaskDescend.hpp" + +bool FlightTaskDescend::activate(vehicle_local_position_setpoint_s last_setpoint) +{ + bool ret = FlightTask::activate(last_setpoint); + _position_setpoint = {NAN, NAN, NAN}; + _velocity_setpoint = {NAN, NAN, NAN}; + _thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -_param_mpc_thr_hover.get() * 0.6f); + _yaw_setpoint = _yaw; + _yawspeed_setpoint = 0.0f; + return ret; +} + +bool FlightTaskDescend::update() +{ + if (PX4_ISFINITE(_velocity(2))) { + // land with landspeed + _velocity_setpoint(2) = _param_mpc_land_speed.get(); + + } else { + return false; + } + + return true; + +} diff --git a/src/lib/FlightTasks/tasks/Descend/FlightTaskDescend.hpp b/src/lib/FlightTasks/tasks/Descend/FlightTaskDescend.hpp new file mode 100644 index 000000000000..ba9c9015c08c --- /dev/null +++ b/src/lib/FlightTasks/tasks/Descend/FlightTaskDescend.hpp @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 FlightTaskDescend.hpp + * + */ + +#pragma once + +#include "FlightTask.hpp" + +class FlightTaskDescend : public FlightTask +{ +public: + FlightTaskDescend() = default; + + virtual ~FlightTaskDescend() = default; + bool update() override; + bool activate(vehicle_local_position_setpoint_s last_setpoint) override; + +private: + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, + (ParamFloat) _param_mpc_land_speed, + (ParamFloat) + _param_mpc_thr_hover /**< throttle value at which vehicle is at hover equilibrium */ + ) +}; diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index ab7d45610117..cb48bdd80d53 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3197,8 +3197,7 @@ Commander::update_control_mode() break; case vehicle_status_s::NAVIGATION_STATE_DESCEND: - /* TODO: check if this makes sense */ - control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 6757f7e9f14c..c36978eb0dee 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -845,6 +845,28 @@ MulticopterPositionControl::start_flight_task() // we want to be in this mode, reset the failure count _task_failure_count = 0; } + + } else if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND) { + + // Emergency descend + should_disable_task = false; + FlightTaskError error = FlightTaskError::NoError; + + error = _flight_tasks.switchTask(FlightTaskIndex::Descend); + + if (error != FlightTaskError::NoError) { + if (prev_failure_count == 0) { + PX4_WARN("Descend activation failed with error: %s", _flight_tasks.errorToString(error)); + } + + task_failure = true; + _task_failure_count++; + + } else { + // we want to be in this mode, reset the failure count + _task_failure_count = 0; + } + } // manual position control diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 32c096bfb924..a30a813fb721 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -591,11 +591,6 @@ Navigator::run() _precland.set_mode(PrecLandMode::Required); break; - case vehicle_status_s::NAVIGATION_STATE_DESCEND: - _pos_sp_triplet_published_invalid_once = false; - navigation_mode_new = &_land; - break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_dataLinkLoss; @@ -620,6 +615,7 @@ Navigator::run() case vehicle_status_s::NAVIGATION_STATE_ACRO: case vehicle_status_s::NAVIGATION_STATE_ALTCTL: case vehicle_status_s::NAVIGATION_STATE_POSCTL: + case vehicle_status_s::NAVIGATION_STATE_DESCEND: case vehicle_status_s::NAVIGATION_STATE_TERMINATION: case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: case vehicle_status_s::NAVIGATION_STATE_STAB: