diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index f2b4d3750e..4dcf635f06 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -439,7 +439,7 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec const float stop_distance = math::max(0.f, distance - min_dist_to_keep - delay_distance); const float vel_max_posctrl = xy_p * stop_distance; - const float vel_max_smooth = math::trajectory::computeMaxSpeedFromBrakingDistance(max_jerk, max_accel, stop_distance); + const float vel_max_smooth = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_accel, stop_distance, 0.f); const float projection = bin_direction.dot(setpoint_dir); float vel_max_bin = vel_max; diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index f57918b52f..b52c5dfef1 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -173,12 +173,12 @@ float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float constraint return math::constrain(val, min, max); } -float FlightTaskAutoLineSmoothVel::_constrainAbsPrioritizeMin(float val, float min, float max) +float FlightTaskAutoLineSmoothVel::_constrainAbs(float val, float max) { - return math::sign(val) * math::max(math::min(fabsf(val), fabsf(max)), fabsf(min)); + return math::sign(val) * math::min(fabsf(val), fabsf(max)); } -float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget() const +float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget(float next_target_speed) const { // Compute the maximum allowed speed at the waypoint assuming that we want to // connect the two lines (prev-current and current-next) @@ -190,17 +190,22 @@ float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget() const // 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(); - const bool waypoint_overlap = Vector2f(&(_target - _prev_wp)(0)).length() < _target_acceptance_radius; + const float distance_current_next = (_target - _next_wp).xy().norm(); + const bool waypoint_overlap = (_target - _prev_wp).xy().norm() < _target_acceptance_radius; const bool yaw_align_check_pass = (_param_mpc_yaw_mode.get() != 4) || _yaw_sp_aligned; + if (distance_current_next > 0.001f && !waypoint_overlap && yaw_align_check_pass) { + Vector3f pos_traj; + pos_traj(0) = _trajectory[0].getCurrentPosition(); + pos_traj(1) = _trajectory[1].getCurrentPosition(); + pos_traj(2) = _trajectory[2].getCurrentPosition(); // Max speed between current and next - const float max_speed_current_next = _getMaxSpeedFromDistance(distance_current_next); - const float alpha = acosf(Vector2f(&(_target - _prev_wp)(0)).unit_or_zero() * - Vector2f(&(_target - _next_wp)(0)).unit_or_zero()); + const float max_speed_current_next = _getMaxSpeedFromDistance(distance_current_next, next_target_speed); + const float alpha = acosf(Vector2f((_target - pos_traj).xy()).unit_or_zero().dot( + Vector2f((_target - _next_wp).xy()).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. @@ -214,14 +219,12 @@ float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget() const return speed_at_target; } -float FlightTaskAutoLineSmoothVel::_getMaxSpeedFromDistance(float braking_distance) const +float FlightTaskAutoLineSmoothVel::_getMaxSpeedFromDistance(float braking_distance, float final_speed) const { - float max_speed = math::trajectory::computeMaxSpeedFromBrakingDistance(_param_mpc_jerk_auto.get(), + float max_speed = math::trajectory::computeMaxSpeedFromDistance(_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()); + braking_distance, + final_speed); return max_speed; } @@ -248,34 +251,24 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints() pos_traj(0) = _trajectory[0].getCurrentPosition(); pos_traj(1) = _trajectory[1].getCurrentPosition(); pos_traj(2) = _trajectory[2].getCurrentPosition(); - Vector2f pos_traj_to_dest_xy(_position_setpoint - pos_traj); + Vector2f pos_traj_to_dest_xy = (_position_setpoint - pos_traj).xy(); Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.unit_or_zero()); - // Unconstrained desired velocity vector - Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * _mc_cruise_speed; - - Vector2f vel_max_xy; - vel_max_xy(0) = _getMaxSpeedFromDistance(fabsf(pos_traj_to_dest_xy(0))); - vel_max_xy(1) = _getMaxSpeedFromDistance(fabsf(pos_traj_to_dest_xy(1))); - const bool has_reached_altitude = fabsf(_position_setpoint(2) - pos_traj(2)) < _param_nav_mc_alt_rad.get(); - Vector2f vel_min_xy; - if (has_reached_altitude) { - // Compute the minimum speed in NE frame. This is used - // to force the drone to pass the waypoint with a desired speed - Vector2f u_prev_to_target_xy((_target - _prev_wp).unit_or_zero()); - vel_min_xy = u_prev_to_target_xy * _getSpeedAtTarget(); + // If the drone has to change altitude, stop at the waypoint, otherwise fly through + const float arrival_speed = has_reached_altitude ? _getSpeedAtTarget(0.f) : 0.f; + const Vector2f max_arrival_vel = u_pos_traj_to_dest_xy * arrival_speed; - } else { - // The drone has to change altitude, stop at the waypoint - vel_min_xy.setAll(0.f); - } + Vector2f vel_abs_max_xy(_getMaxSpeedFromDistance(fabsf(pos_traj_to_dest_xy(0)), max_arrival_vel(0)), + _getMaxSpeedFromDistance(fabsf(pos_traj_to_dest_xy(1)), max_arrival_vel(1))); + + + const Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * _mc_cruise_speed; // Constrain the norm of each component using min and max values - Vector2f vel_sp_constrained_xy; - vel_sp_constrained_xy(0) = _constrainAbsPrioritizeMin(vel_sp_xy(0), vel_min_xy(0), vel_max_xy(0)); - vel_sp_constrained_xy(1) = _constrainAbsPrioritizeMin(vel_sp_xy(1), vel_min_xy(1), vel_max_xy(1)); + Vector2f vel_sp_constrained_xy(_constrainAbs(vel_sp_xy(0), vel_abs_max_xy(0)), + _constrainAbs(vel_sp_xy(1), vel_abs_max_xy(1))); for (int i = 0; i < 2; i++) { // If available, constrain the velocity using _velocity_setpoint(.) @@ -286,7 +279,6 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints() _velocity_setpoint(i) = vel_sp_constrained_xy(i); } } - } if (PX4_ISFINITE(_position_setpoint(2))) { diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp index e5a97f7234..1ec21c6d2d 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp @@ -69,19 +69,11 @@ protected: static float _constrainOneSide(float val, float constraint); /**< Constrain val between INF and constraint */ - /** - * Constrain the abs value below max but above min - * Min can be larger than max and has priority over it - * The whole computation is done on the absolute values but the returned - * value has the sign of val - * @param val the value to constrain and boost - * @param min the minimum value that the function should return - * @param max the value by which val is constrained before the boost is applied - */ - static float _constrainAbsPrioritizeMin(float val, float min, float max); + static float _constrainAbs(float val, float max); /** Constrain the value -max <= val <= max */ - float _getSpeedAtTarget() const; - float _getMaxSpeedFromDistance(float braking_distance) const; + /** Give 0 if next is the last target **/ + float _getSpeedAtTarget(float next_target_speed) const; + float _getMaxSpeedFromDistance(float braking_distance, float final_speed) const; void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */ void _updateTrajConstraints(); diff --git a/src/lib/mathlib/math/TrajMath.hpp b/src/lib/mathlib/math/TrajMath.hpp index eecb7f28cc..ad042583ce 100644 --- a/src/lib/mathlib/math/TrajMath.hpp +++ b/src/lib/mathlib/math/TrajMath.hpp @@ -45,23 +45,26 @@ namespace math namespace trajectory { -/* Compute the maximum possible speed on the track given the remaining distance, - * the maximum acceleration and the maximum jerk. +/* Compute the maximum possible speed on the track given the desired speed, + * 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) + * Equation to solve: vel_final^2 = vel_initial^2 - 2*accel*(x - vel_initial*2*accel/jerk) * * @param jerk maximum jerk * @param accel maximum acceleration - * @param braking_distance distance to the desired stopping point + * @param braking_distance distance to the desired point + * @param final_speed the still-remaining speed of the vehicle when it reaches the braking_distance * * @return maximum speed */ -inline float computeMaxSpeedFromBrakingDistance(const float jerk, const float accel, const float braking_distance) +inline float computeMaxSpeedFromDistance(const float jerk, const float accel, const float braking_distance, + const float final_speed) { - float b = 4.0f * accel * accel / jerk; - float c = - 2.0f * accel * braking_distance; - float max_speed = 0.5f * (-b + sqrtf(b * b - 4.0f * c)); + auto sqr = [](float f) {return f * f;}; + float b = 4.0f * sqr(accel) / jerk; + float c = - 2.0f * accel * braking_distance - sqr(final_speed); + float max_speed = 0.5f * (-b + sqrtf(sqr(b) - 4.0f * c)); return max_speed; }