Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[1.14] PositionSmoothing: fix corner altitude bug #22342

Merged
merged 1 commit into from
Nov 10, 2023
Merged

[1.14] PositionSmoothing: fix corner altitude bug #22342

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 10 additions & 13 deletions src/lib/motion_planning/PositionSmoothing.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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],
Expand Down
2 changes: 1 addition & 1 deletion src/lib/motion_planning/PositionSmoothing.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
Loading