From 624473ff18be6f2475b2b0554e7e67b4e32dd76f Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 14 Oct 2019 20:35:45 +0200 Subject: [PATCH 1/3] mc_pos_control: execute failsafe with invalid setpoints --- .../mc_pos_control/mc_pos_control_main.cpp | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) 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 ec79aa542d84..6757f7e9f14c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -579,9 +579,9 @@ MulticopterPositionControl::Run() // check if any task is active if (_flight_tasks.isAnyTaskActive()) { - - // setpoints from flighttask - vehicle_local_position_setpoint_s setpoint; + // setpoints and constraints for the position controller from flighttask or failsafe + vehicle_local_position_setpoint_s setpoint = FlightTask::empty_setpoint; + vehicle_constraints_s constraints = FlightTask::empty_constraints; _flight_tasks.setYawHandler(_wv_controller); @@ -593,6 +593,8 @@ MulticopterPositionControl::Run() } else { setpoint = _flight_tasks.getPositionSetpoint(); + constraints = _flight_tasks.getConstraints(); + _failsafe_land_hysteresis.set_state_and_update(false, time_stamp_current); // Check if position, velocity or thrust pairs are valid -> trigger failsaife if no pair is valid @@ -612,7 +614,6 @@ MulticopterPositionControl::Run() // publish trajectory setpoint _traj_sp_pub.publish(setpoint); - vehicle_constraints_s constraints = _flight_tasks.getConstraints(); landing_gear_s gear = _flight_tasks.getGear(); // check if all local states are valid and map accordingly @@ -642,7 +643,6 @@ MulticopterPositionControl::Run() constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get()); } - // limit altitude only if local position is valid if (PX4_ISFINITE(_states.position(2))) { limit_altitude(setpoint); @@ -655,6 +655,9 @@ MulticopterPositionControl::Run() // update position controller setpoints if (!_control.updateSetpoint(setpoint)) { warn_rate_limited("Position-Control Setpoint-Update failed"); + failsafe(setpoint, _states, true, !was_in_failsafe); + _control.updateSetpoint(setpoint); + constraints = FlightTask::empty_constraints; } // Generate desired thrust and yaw. @@ -977,7 +980,7 @@ MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint setpoint.vz = _param_mpc_land_speed.get(); if (warn) { - PX4_WARN("Failsafe: blind hover"); + PX4_WARN("Failsafe: blind land"); } } @@ -991,6 +994,10 @@ MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint // emergency descend with a bit below hover thrust setpoint.vz = NAN; setpoint.thrust[2] = _param_mpc_thr_hover.get() * .8f; + + if (warn) { + PX4_WARN("Failsafe: blind descend"); + } } _in_failsafe = true; From 6ac28012ff6b708d86732a64d20ef285502555be Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 9 Oct 2019 14:58:00 +0200 Subject: [PATCH 2/3] 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: From 935897a7abeb56ad197478475788cb43a5316cff Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 15 Oct 2019 13:53:17 +0200 Subject: [PATCH 3/3] FlightTaskDescend: set no vertical thrust when commanding velocity --- .../FlightTasks/tasks/Descend/FlightTaskDescend.cpp | 13 +++++++------ .../FlightTasks/tasks/Descend/FlightTaskDescend.hpp | 8 +++----- 2 files changed, 10 insertions(+), 11 deletions(-) diff --git a/src/lib/FlightTasks/tasks/Descend/FlightTaskDescend.cpp b/src/lib/FlightTasks/tasks/Descend/FlightTaskDescend.cpp index cce5497f2715..8e948cc3394f 100644 --- a/src/lib/FlightTasks/tasks/Descend/FlightTaskDescend.cpp +++ b/src/lib/FlightTasks/tasks/Descend/FlightTaskDescend.cpp @@ -39,11 +39,10 @@ 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); + // stay level to minimize horizontal drift + _thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, NAN); + // keep heading _yaw_setpoint = _yaw; - _yawspeed_setpoint = 0.0f; return ret; } @@ -52,11 +51,13 @@ bool FlightTaskDescend::update() if (PX4_ISFINITE(_velocity(2))) { // land with landspeed _velocity_setpoint(2) = _param_mpc_land_speed.get(); + _thrust_setpoint(2) = NAN; } else { - return false; + // descend with constant thrust (crash landing) + _velocity_setpoint(2) = NAN; + _thrust_setpoint(2) = -_param_mpc_thr_hover.get() * 0.7f; } return true; - } diff --git a/src/lib/FlightTasks/tasks/Descend/FlightTaskDescend.hpp b/src/lib/FlightTasks/tasks/Descend/FlightTaskDescend.hpp index ba9c9015c08c..7e6035d9ab62 100644 --- a/src/lib/FlightTasks/tasks/Descend/FlightTaskDescend.hpp +++ b/src/lib/FlightTasks/tasks/Descend/FlightTaskDescend.hpp @@ -33,7 +33,6 @@ /** * @file FlightTaskDescend.hpp - * */ #pragma once @@ -44,15 +43,14 @@ 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 */ + (ParamFloat) _param_mpc_thr_hover, ///< thrust at hover equilibrium + (ParamFloat) _param_mpc_land_speed ///< velocity for controlled descend ) };