fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-31 20:21:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Create TrajMath library and move waypoint speed calculations in it

Browse Source
This commit is contained in:
bresch
2019-08-09 12:04:48 +02:00
committed by Mathieu Bresciani
parent b1698b78bc
commit 4eb9c7d812
2 changed files with 108 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+19 -17
View File
@@ -38,6 +38,7 @@
#include "FlightTaskAutoLineSmoothVel.hpp"
#include <mathlib/mathlib.h>
#include <float.h>
#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;