diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp index eae6375b3e..7749eb0d84 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp @@ -44,7 +44,6 @@ #include "FlightTaskManualAltitudeSmoothVel.hpp" #include #include -#include "StraightLine.hpp" #include #include #include diff --git a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt index daa0a62bf4..280123ce1e 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt @@ -32,7 +32,6 @@ ############################################################################ px4_add_library(FlightTaskUtility - StraightLine.cpp Sticks.cpp StickAccelerationXY.cpp StickYaw.cpp diff --git a/src/modules/flight_mode_manager/tasks/Utility/StraightLine.cpp b/src/modules/flight_mode_manager/tasks/Utility/StraightLine.cpp deleted file mode 100644 index eb49726878..0000000000 --- a/src/modules/flight_mode_manager/tasks/Utility/StraightLine.cpp +++ /dev/null @@ -1,90 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018-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 StraightLine.cpp - */ - -#include "StraightLine.hpp" -#include -#include - -using namespace matrix; - -void StraightLine::generateSetpoints(matrix::Vector3f &position_setpoint, matrix::Vector3f &velocity_setpoint) -{ - if (isEndReached()) { - // Vehicle has reached target, lock position - position_setpoint = _end; - velocity_setpoint.setNaN(); - return; - } - - Vector3f start_to_end = _end - _start; - float distance_start_to_end = start_to_end.norm(); - - // capture progress as ratio between 0 and 1 of entire distance - Vector3f vehicle_to_end = _end - _position; - float distance_vehicle_to_end = vehicle_to_end.norm(); - float distance_from_start = Vector3f(_start - _position).norm(); - float progress = distance_from_start / (distance_from_start + distance_vehicle_to_end); - - float distance_from_boundary = 0.f; - - // calculate the distance to the closer boundary - if (progress < 0.5f) { - distance_from_boundary = (2 * progress) * distance_start_to_end; - - } else { - distance_from_boundary = (2 * (1 - progress)) * distance_start_to_end; - } - - // ramp velocity based on the distance to the boundary - float velocity = 0.5f + (distance_from_boundary / 4.f); - velocity = math::min(velocity, _speed); - velocity_setpoint = vehicle_to_end.unit_or_zero() * velocity; - - // check if we plan to go against the line direction which indicates we reached the goal - if (start_to_end * vehicle_to_end < 0) { - _end_reached = true; - } -} - -void StraightLine::setLineFromTo(const Vector3f &start, const Vector3f &end) -{ - if (PX4_ISFINITE(start.norm_squared()) && PX4_ISFINITE(end.norm_squared())) { - _start = start; - _end = end; - _end_reached = false; - } -} diff --git a/src/modules/flight_mode_manager/tasks/Utility/StraightLine.hpp b/src/modules/flight_mode_manager/tasks/Utility/StraightLine.hpp deleted file mode 100644 index cdd776da06..0000000000 --- a/src/modules/flight_mode_manager/tasks/Utility/StraightLine.hpp +++ /dev/null @@ -1,81 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018-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 StraightLine.hpp - * - * lib to return setpoints on a straight line - * - * @author Christoph Tobler - */ - -#pragma once - -#include - -class StraightLine -{ -public: - StraightLine(const matrix::Vector3f &pos) : _position(pos) {}; - ~StraightLine() = default; - - // setter functions - void setLineFromTo(const matrix::Vector3f &start, const matrix::Vector3f &end); - void setSpeed(const float &speed) { _speed = speed; }; - - /** - * Generate setpoints on a straight line according to parameters - * - * @param position_setpoint: reference to the 3D vector with the position setpoint to update - * @param velocity_setpoint: reference to the 3D vector with the velocity setpoint to update - */ - void generateSetpoints(matrix::Vector3f &position_setpoint, matrix::Vector3f &velocity_setpoint); - - /** - * Check if the end was reached - * - * @return false when on the way from start to end, true when end was reached - */ - bool isEndReached() const { return _end_reached; } - - void reset() { _end_reached = true; } - -private: - const matrix::Vector3f &_position; /**< vehicle position (dependency injection) */ - - matrix::Vector3f _start; /**< Start point of the straight line */ - matrix::Vector3f _end; /**< End point of the straight line */ - float _speed = 1.f; /**< desired speed between accelerating and decelerating */ - - bool _end_reached = true; /**< Flag to lock further movement when end is reached */ -};