diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index 6556a53373..835e00c99e 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -38,6 +38,7 @@ #include "FlightTaskAutoLineSmoothVel.hpp" #include #include +#include "TrajMath.hpp" using namespace matrix; @@ -182,12 +183,14 @@ void FlightTaskAutoLineSmoothVel::_checkEkfResetCounters() float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget() { - // Compute the maximum allowed speed at the waypoint assuming that we want to connect the two lines (prev-current and current-next) + // Compute the maximum allowed speed at the waypoint assuming that we want to + // connect the two lines (prev-current and current-next) // with a tangent circle with constant speed and desired centripetal acceleration: a_centripetal = speed^2 / radius - // The circle should in theory start and end at the intersection of the lines and the waypoint's acceptance radius. This is not - // exactly true in reality since Navigator switches the waypoint so we have to take in account that the real acceptance radius is smaller. - // It can be that the next waypoint is the last one or that the drone will have to stop for some other reason so we have to make sure - // that the speed at the current waypoint allows to stop at the next waypoint. + // The circle should in theory start and end at the intersection of the lines and the waypoint's acceptance radius. + // This is not exactly true in reality since Navigator switches the waypoint so we have to take in account that + // the real acceptance radius is smaller. + // It can be that the next waypoint is the last one or that the drone will have to stop for some other reason + // so we have to make sure that the speed at the current waypoint allows to stop at the next waypoint. float speed_at_target = 0.0f; const float distance_current_next = Vector2f(&(_target - _next_wp)(0)).length(); @@ -200,12 +203,13 @@ float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget() // Max speed between current and next const float max_speed_current_next = _getMaxSpeedFromDistance(distance_current_next); const float alpha = acos(Vector2f(&(_target - _prev_wp)(0)).unit_or_zero() * - Vector2f(&(_target - _next_wp)(0)).unit_or_zero()) / 2.f; - const float tan_alpha = tan(alpha); - // We choose a maximum centripetal acceleration of MPC_ACC_HOR * MPC_XY_TRAJ_P to take in account that there is a jerk limit (a direct transition from line to circle is not possible) + Vector2f(&(_target - _next_wp)(0)).unit_or_zero()); + // We choose a maximum centripetal acceleration of MPC_ACC_HOR * MPC_XY_TRAJ_P to take in account + // that there is a jerk limit (a direct transition from line to circle is not possible) // MPC_XY_TRAJ_P should be between 0 and 1. - const float max_speed_in_turn = sqrtf(_param_mpc_xy_traj_p.get() * _param_mpc_acc_hor.get() * _target_acceptance_radius - * tan_alpha); + const float max_speed_in_turn = trajmath::computeMaxSpeedInWaypoint(alpha, + _param_mpc_xy_traj_p.get() * _param_mpc_acc_hor.get(), + _target_acceptance_radius); speed_at_target = math::min(math::min(max_speed_in_turn, max_speed_current_next), _mc_cruise_speed); } @@ -214,13 +218,11 @@ float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget() float FlightTaskAutoLineSmoothVel::_getMaxSpeedFromDistance(float braking_distance) { - // Compute the maximum possible velocity on the track given the remaining distance, the maximum acceleration and the maximum jerk. - // We assume a constant acceleration profile with a delay of 2*accel/jerk (time to reach the desired acceleration from opposite max acceleration) - // Equation to solve: 0 = vel^2 - 2*acc*(x - vel*2*acc/jerk) - // To avoid high gain at low distance due to the sqrt, we take the minimum of this velocity and a slope of "traj_p" m/s per meter - float b = 4.f * _param_mpc_acc_hor.get() * _param_mpc_acc_hor.get() / _param_mpc_jerk_auto.get(); - float c = - 2.f * _param_mpc_acc_hor.get() * braking_distance; - float max_speed = 0.5f * (-b + sqrtf(b * b - 4.f * c)); + float max_speed = trajmath::computeMaxSpeedFromBrakingDistance(_param_mpc_jerk_auto.get(), + _param_mpc_acc_hor.get(), + braking_distance); + // To avoid high gain at low distance due to the sqrt, we take the minimum + // of this velocity and a slope of "traj_p" m/s per meter max_speed = math::min(max_speed, braking_distance * _param_mpc_xy_traj_p.get()); return max_speed; diff --git a/src/lib/FlightTasks/tasks/Utility/TrajMath.hpp b/src/lib/FlightTasks/tasks/Utility/TrajMath.hpp new file mode 100644 index 0000000000..4bed8c4fa8 --- /dev/null +++ b/src/lib/FlightTasks/tasks/Utility/TrajMath.hpp @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * 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 TrajMath.hpp + * + * collection of functions used in trajectory generators + */ + +#pragma once + +namespace trajmath +{ + +/* Compute the maximum possible speed on the track given the remaining distance, + * the maximum acceleration and the maximum jerk. + * We assume a constant acceleration profile with a delay of 2*accel/jerk + * (time to reach the desired acceleration from opposite max acceleration) + * Equation to solve: 0 = vel^2 - 2*accel*(x - vel*2*accel/jerk) + * + * @param jerk maximum jerk + * @param accel maximum acceleration + * @param braking_distance distance to the desired stopping point + * + * @return maximum speed + */ +template +const T computeMaxSpeedFromBrakingDistance(T jerk, T accel, T braking_distance) +{ + T b = (T) 4 * accel * accel / jerk; + T c = - (T) 2 * accel * braking_distance; + T max_speed = (T) 0.5 * (-b + sqrtf(b * b - (T) 4 * c)); + + return max_speed; +} + +/* Compute the maximum tangential speed in a circle defined by two line segments of length "d" + * forming a V shape, opened by an angle "alpha". The circle is tangent to the end of the + * two segments as shown below: + * \\ + * | \ d + * / \ + * __='___a\ + * d + * @param alpha angle between the two line segments + * @param accel maximum lateral acceleration + * @param d length of the two line segments + * + * @return maximum tangential speed + */ +template +const T computeMaxSpeedInWaypoint(T alpha, T accel, T d) +{ + T tan_alpha = tan(alpha / (T) 2); + T max_speed_in_turn = sqrtf(accel * d * tan_alpha); + + return max_speed_in_turn; +} +} /* namespace trajmath */