From 453b6a39e45105a2f006b83f934d7d17cf94e885 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Wed, 18 Sep 2019 13:54:46 +0200 Subject: [PATCH] TrajMath: move from FlightTasks/Utility into mathlib library because the function is also used by other libraries --- .../CollisionPrevention/CollisionPrevention.cpp | 4 +--- .../FlightTaskAutoLineSmoothVel.cpp | 10 +++++----- .../tasks/Utility => mathlib/math}/TrajMath.hpp | 14 +++++++++----- src/lib/mathlib/mathlib.h | 1 + 4 files changed, 16 insertions(+), 13 deletions(-) rename src/lib/{FlightTasks/tasks/Utility => mathlib/math}/TrajMath.hpp (89%) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index a3d2aa8918..b413b25b69 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -39,8 +39,6 @@ #include -#include - using namespace matrix; using namespace time_literals; namespace @@ -294,7 +292,7 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, float delay_distance = curr_vel_parallel * (col_prev_dly + data_age * 1e-6f); float stop_distance = math::max(0.f, distance - min_dist_to_keep - delay_distance); float vel_max_posctrl = xy_p * stop_distance; - float vel_max_smooth = trajmath::computeMaxSpeedFromBrakingDistance(max_jerk, max_accel, stop_distance); + float vel_max_smooth = math::trajectory::computeMaxSpeedFromBrakingDistance(max_jerk, max_accel, stop_distance); Vector2f vel_max_vec = bin_direction * math::min(vel_max_posctrl, vel_max_smooth); float vel_max_bin = vel_max_vec.dot(setpoint_dir); diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index af7f18cc00..adf8cae8cb 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -38,7 +38,6 @@ #include "FlightTaskAutoLineSmoothVel.hpp" #include #include -#include "TrajMath.hpp" using namespace matrix; @@ -212,9 +211,10 @@ float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget() // 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 = trajmath::computeMaxSpeedInWaypoint(alpha, - _param_mpc_xy_traj_p.get() * _param_mpc_acc_hor.get(), - _target_acceptance_radius); + float accel_tmp = _param_mpc_xy_traj_p.get() * _param_mpc_acc_hor.get(); + float max_speed_in_turn = math::trajectory::computeMaxSpeedInWaypoint(alpha, + accel_tmp, + _target_acceptance_radius); speed_at_target = math::min(math::min(max_speed_in_turn, max_speed_current_next), _mc_cruise_speed); } @@ -223,7 +223,7 @@ float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget() float FlightTaskAutoLineSmoothVel::_getMaxSpeedFromDistance(float braking_distance) { - float max_speed = trajmath::computeMaxSpeedFromBrakingDistance(_param_mpc_jerk_auto.get(), + float max_speed = math::trajectory::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 diff --git a/src/lib/FlightTasks/tasks/Utility/TrajMath.hpp b/src/lib/mathlib/math/TrajMath.hpp similarity index 89% rename from src/lib/FlightTasks/tasks/Utility/TrajMath.hpp rename to src/lib/mathlib/math/TrajMath.hpp index e18e98a9c7..eecb7f28cc 100644 --- a/src/lib/FlightTasks/tasks/Utility/TrajMath.hpp +++ b/src/lib/mathlib/math/TrajMath.hpp @@ -34,12 +34,15 @@ /** * @file TrajMath.hpp * - * collection of functions used in trajectory generators + * collection of functions used for trajectory generation */ #pragma once -namespace trajmath +namespace math +{ + +namespace trajectory { /* Compute the maximum possible speed on the track given the remaining distance, @@ -54,7 +57,7 @@ namespace trajmath * * @return maximum speed */ -float computeMaxSpeedFromBrakingDistance(const float jerk, const float accel, const float braking_distance) +inline float computeMaxSpeedFromBrakingDistance(const float jerk, const float accel, const float braking_distance) { float b = 4.0f * accel * accel / jerk; float c = - 2.0f * accel * braking_distance; @@ -77,11 +80,12 @@ float computeMaxSpeedFromBrakingDistance(const float jerk, const float accel, co * * @return maximum tangential speed */ -float computeMaxSpeedInWaypoint(const float alpha, const float accel, const float d) +inline float computeMaxSpeedInWaypoint(const float alpha, const float accel, const float d) { float tan_alpha = tanf(alpha / 2.0f); float max_speed_in_turn = sqrtf(accel * d * tan_alpha); return max_speed_in_turn; } -} /* namespace trajmath */ +} /* namespace traj */ +} /* namespace math */ diff --git a/src/lib/mathlib/mathlib.h b/src/lib/mathlib/mathlib.h index 5815e77849..2c2572821b 100644 --- a/src/lib/mathlib/mathlib.h +++ b/src/lib/mathlib/mathlib.h @@ -45,5 +45,6 @@ #include "math/Functions.hpp" #include "math/matrix_alg.h" #include "math/SearchMin.hpp" +#include "math/TrajMath.hpp" #endif