function

Planning.Bezier.waypointDurations

Per-segment durations that hold a nominal average speed between waypoints

Assigns each segment a duration equal to its chord length divided by a nominal average speed, floored at minDuration. For a rest-to-rest septic segment the peak speed is roughly 2.2 times this average. Use it to size a waypoint trajectory from a single cruise parameter.

Public declarations

Explicit source declarations; dimensions, modifications and defaults are shown as written, without evaluation.

Role	Name	Type	Dimensions / attributes / default	Description
input	`waypoints`	`Real`	`[:, 3]`	World-frame waypoints, one [x, y, z] per row
input	`nominalSpeed`	`Real`	`(unit = "m/s")`	Average speed along each segment
input	`minDuration`	`Real`	`(unit = "s") = 1.0`	Lower bound on any segment
output	`durations`	`Real`	`[size(waypoints, 1) - 1]`	Segment durations [s]

Modelica declarations and implementation

Show parameters, interfaces and source

Parameters, inputs, outputs, defaults and equations from the source file.

function waypointDurations
    "Per-segment durations that hold a nominal average speed between waypoints"
    input Real waypoints[:, 3] "World-frame waypoints, one [x, y, z] per row";
    input Real nominalSpeed(unit = "m/s") "Average speed along each segment";
    input Real minDuration(unit = "s") = 1.0 "Lower bound on any segment";
    output Real durations[size(waypoints, 1) - 1] "Segment durations [s]";
  algorithm
    assert(nominalSpeed > 0.0, "Nominal speed must be positive");
    assert(minDuration > 0.0, "Minimum duration must be positive");
    // Inline the chord so the loop's only target is the output array, which the
    // eFMI lowering proves totally defined element by element.
    for i in 1:(size(waypoints, 1) - 1) loop
      durations[i] := max(minDuration, sqrt(
        (waypoints[i + 1, 1] - waypoints[i, 1])^2
        + (waypoints[i + 1, 2] - waypoints[i, 2])^2
        + (waypoints[i + 1, 3] - waypoints[i, 3])^2) / nominalSpeed);
    end for;
    annotation(Documentation(info="<html>
      <p>Assigns each segment a duration equal to its chord length divided by a
      nominal average speed, floored at <code>minDuration</code>. For a
      rest-to-rest septic segment the peak speed is roughly 2.2 times this
      average. Use it to size a waypoint trajectory from a single cruise
      parameter.</p>
    </html>"));
  end waypointDurations;