dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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.
This commit is contained in:
Matthias Grob 2023-11-07 16:48:41 +01:00
parent 19b681ca1f
commit 4485c7aa11
2 changed files with 11 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/lib/motion_planning/PositionSmoothing.cpp
View File

@ -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],

2
src/lib/motion_planning/PositionSmoothing.hpp
View File

@ -438,7 +438,7 @@ private:
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;