diff --git a/src/lib/FlightTasks/CMakeLists.txt b/src/lib/FlightTasks/CMakeLists.txt index 91748c0c0a..0ce1043ead 100644 --- a/src/lib/FlightTasks/CMakeLists.txt +++ b/src/lib/FlightTasks/CMakeLists.txt @@ -60,6 +60,7 @@ list(APPEND flight_tasks_all AutoLine AutoFollowMe Offboard + Failsafe ${flight_tasks_to_add} ) diff --git a/src/lib/FlightTasks/tasks/Failsafe/CMakeLists.txt b/src/lib/FlightTasks/tasks/Failsafe/CMakeLists.txt new file mode 100644 index 0000000000..cdef516a19 --- /dev/null +++ b/src/lib/FlightTasks/tasks/Failsafe/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2018 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(FlightTaskFailsafe + FlightTaskFailsafe.cpp +) + +target_link_libraries(FlightTaskFailsafe PUBLIC FlightTask) +target_include_directories(FlightTaskFailsafe PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.cpp b/src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.cpp new file mode 100644 index 0000000000..011ce0d4c8 --- /dev/null +++ b/src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.cpp @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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 FlightTaskFailsafe.cpp + */ + +#include "FlightTaskFailsafe.hpp" + +bool FlightTaskFailsafe::activate() +{ + bool ret = FlightTask::activate(); + _position_setpoint = _position; + _velocity_setpoint *= 0.0f; + _thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -MPC_THR_HOVER.get() * 0.5f); + _yaw_setpoint = _yaw; + _yawspeed_setpoint = 0.0f; + return ret; +} + +bool FlightTaskFailsafe::update() +{ + if (PX4_ISFINITE(_position(0)) && PX4_ISFINITE(_position(1))) { + _velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f; + _thrust_setpoint(0) = _thrust_setpoint(1) = 0.0f; + + } else if (PX4_ISFINITE(_velocity(0)) && PX4_ISFINITE(_velocity(1))) { + _position_setpoint(0) = _position_setpoint(1) = NAN; + _thrust_setpoint(0) = _thrust_setpoint(1) = NAN; + } + + if (PX4_ISFINITE(_position(2))) { + _velocity_setpoint(2) = 0.0f; + _thrust_setpoint(2) = NAN; + + } else if (PX4_ISFINITE(_velocity(2))) { + _position_setpoint(2) = NAN; + _thrust_setpoint(2) = NAN; + } + + return true; + +} \ No newline at end of file diff --git a/src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.hpp b/src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.hpp new file mode 100644 index 0000000000..acd6eb598c --- /dev/null +++ b/src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.hpp @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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 FlightTaskFailsafe.hpp + * + */ + +#pragma once + +#include "FlightTask.hpp" + +class FlightTaskFailsafe : public FlightTask +{ +public: + FlightTaskFailsafe() = default; + + virtual ~FlightTaskFailsafe() = default; + bool update() override; + bool activate() override; + +private: + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, + (ParamFloat) MPC_LAND_SPEED, + (ParamFloat) MPC_THR_HOVER /**< throttle value at which vehicle is at hover equilibrium */ + ) +};