From fc2ed7b484e4891c743074a26b2c225f2139c0cd Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 7 Nov 2023 16:48:41 +0100 Subject: [PATCH] PositionSmoothing: fix corner altitude bug During a round corner the L1 distance calculation was only done in 2D and the z-axis coordinate was directly coming from the next waypoint. This lead to an unpredictable altitude profile between two waypoints. Sometimes almost all altitude difference was already covered during the turn instead of going diagonally. --- src/lib/motion_planning/PositionSmoothing.cpp | 23 ++++++++----------- src/lib/motion_planning/PositionSmoothing.hpp | 2 +- 2 files changed, 11 insertions(+), 14 deletions(-) diff --git a/src/lib/motion_planning/PositionSmoothing.cpp b/src/lib/motion_planning/PositionSmoothing.cpp index a5c68f04d173..4b3a427a217b 100644 --- a/src/lib/motion_planning/PositionSmoothing.cpp +++ b/src/lib/motion_planning/PositionSmoothing.cpp @@ -134,21 +134,20 @@ const Vector3f PositionSmoothing::_getCrossingPoint(const Vector3f &position, co } // Get the crossing point using L1-style guidance - auto l1_point = _getL1Point(position, waypoints); - return {l1_point(0), l1_point(1), target(2)}; + return _getL1Point(position, waypoints); } -const Vector2f PositionSmoothing::_getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const +const Vector3f PositionSmoothing::_getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const { - const Vector2f pos_traj(_trajectory[0].getCurrentPosition(), - _trajectory[1].getCurrentPosition()); - const Vector2f u_prev_to_target = Vector2f(waypoints[1] - waypoints[0]).unit_or_zero(); - const Vector2f prev_to_pos(pos_traj - Vector2f(waypoints[0])); - const Vector2f prev_to_closest(u_prev_to_target * (prev_to_pos * u_prev_to_target)); - const Vector2f closest_pt = Vector2f(waypoints[0]) + prev_to_closest; + const Vector3f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition(), + _trajectory[2].getCurrentPosition()); + const Vector3f u_prev_to_target = (waypoints[1] - waypoints[0]).unit_or_zero(); + const Vector3f prev_to_pos(pos_traj - waypoints[0]); + const Vector3f prev_to_closest(u_prev_to_target * (prev_to_pos * u_prev_to_target)); + const Vector3f closest_pt = waypoints[0] + prev_to_closest; // Compute along-track error using L1 distance and cross-track error - const float crosstrack_error = Vector2f(closest_pt - pos_traj).length(); + const float crosstrack_error = (closest_pt - pos_traj).length(); const float l1 = math::max(_target_acceptance_radius, 5.f); float alongtrack_error = 0.f; @@ -159,9 +158,7 @@ const Vector2f PositionSmoothing::_getL1Point(const Vector3f &position, const Ve } // Position of the point on the line where L1 intersect the line between the two waypoints - const Vector2f l1_point = closest_pt + alongtrack_error * u_prev_to_target; - - return l1_point; + return closest_pt + alongtrack_error * u_prev_to_target; } const Vector3f PositionSmoothing::_generateVelocitySetpoint(const Vector3f &position, const Vector3f(&waypoints)[3], diff --git a/src/lib/motion_planning/PositionSmoothing.hpp b/src/lib/motion_planning/PositionSmoothing.hpp index 73886d0124ed..61070d9dabd4 100644 --- a/src/lib/motion_planning/PositionSmoothing.hpp +++ b/src/lib/motion_planning/PositionSmoothing.hpp @@ -438,7 +438,7 @@ class PositionSmoothing const Vector3f _generateVelocitySetpoint(const Vector3f &position, const Vector3f(&waypoints)[3], bool is_single_waypoint, const Vector3f &feedforward_velocity_setpoint); - const Vector2f _getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const; + const Vector3f _getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const; const Vector3f _getCrossingPoint(const Vector3f &position, const Vector3f(&waypoints)[3]) const; float _getMaxXYSpeed(const Vector3f(&waypoints)[3]) const; float _getMaxZSpeed(const Vector3f(&waypoints)[3]) const;