39 #include <mathlib/mathlib.h> 40 #include <px4_platform_common/defines.h> 48 position_setpoint = _end;
49 velocity_setpoint.
setNaN();
53 Vector3f start_to_end = _end - _start;
54 float distance_start_to_end = start_to_end.
norm();
57 Vector3f vehicle_to_end = _end - _position;
58 float distance_vehicle_to_end = vehicle_to_end.
norm();
59 float distance_from_start =
Vector3f(_start - _position).norm();
60 float progress = distance_from_start / (distance_from_start + distance_vehicle_to_end);
62 float distance_from_boundary = 0.f;
65 if (progress < 0.5
f) {
66 distance_from_boundary = (2 * progress) * distance_start_to_end;
69 distance_from_boundary = (2 * (1 - progress)) * distance_start_to_end;
73 float velocity = 0.5f + (distance_from_boundary / 4.f);
75 velocity_setpoint = vehicle_to_end.
unit_or_zero() * velocity;
78 if (start_to_end * vehicle_to_end < 0) {
Type norm_squared() const
void setLineFromTo(const matrix::Vector3f &start, const matrix::Vector3f &end)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
constexpr _Tp min(_Tp a, _Tp b)
Vector3< float > Vector3f
lib to return setpoints on a straight line
Vector unit_or_zero(const Type eps=Type(1e-5)) const
void generateSetpoints(matrix::Vector3f &position_setpoint, matrix::Vector3f &velocity_setpoint)
Generate setpoints on a straight line according to parameters.