Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 10:46:33 +08:00
Code Issues Actions 31 Packages Projects Releases Wiki Activity

TrajectoryConstraints: clarify waypoint indexing

Browse Source
This commit is contained in:
Matthias Grob
2024-11-28 16:19:09 +01:00
parent 06dde4ede8
commit 092e5e8f9d
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/motion_planning/TrajectoryConstraints.hpp
+3 -3
View File
@@ -104,15 +104,15 @@ inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, co
* *
* @return the maximum speed at waypoint[0] which allows it to follow the trajectory while respecting the dynamic limits * @return the maximum speed at waypoint[0] which allows it to follow the trajectory while respecting the dynamic limits
*/ */
template <size_t N> template <int N>
float computeXYSpeedFromWaypoints(const Vector3f waypoints[N], const VehicleDynamicLimits &config) float computeXYSpeedFromWaypoints(const Vector3f waypoints[N], const VehicleDynamicLimits &config)
{ {
static_assert(N >= 2, "Need at least 2 points to compute speed"); static_assert(N >= 2, "Need at least 2 points to compute speed");
float max_speed = 0.f; float max_speed = 0.f;
for (size_t j = 0; j < N - 1; j++) { // go backwards through the waypoints
size_t i = N - 2 - j; for (int i = (N - 2); i >= 0; i--) {
max_speed = computeStartXYSpeedFromWaypoints(waypoints[i], max_speed = computeStartXYSpeedFromWaypoints(waypoints[i],
waypoints[i + 1], waypoints[i + 1],
waypoints[min(i + 2, N - 1)], waypoints[min(i + 2, N - 1)],