function

Planning.Bezier.prepareWaypointPlan

Validate and convert the fixed-capacity payload of one waypoint message

Public declarations

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

Role	Name	Type	Dimensions / attributes / default	Description
input	`waypoint`	`Real`	`[:, 3]`	Local ENU metres or global latitude/longitude/altitude rows
input	`velocityEnu`	`Real`	`[size(waypoint, 1), 3]`	World-frame velocity at each waypoint [m/s]
input	`yaw`	`Real`	`[size(waypoint, 1)](each unit = "rad")`
input	`waypointCount`	`Integer`	`(min = 2, max = size(waypoint, 1))`
input	`globalFrame`	`Boolean`
input	`originGeodetic`	`Real`	`[3]`	Local origin [latitude_deg, longitude_deg, altitude_m]
input	`nominalSpeed`	`Real`	`(unit = "m/s")`
input	`minSegmentDuration`	`Real`	`(unit = "s")`
output	`localWaypoint`	`Real`	`[size(waypoint, 1), 3]`	Fixed-capacity local East-North-Up rows [m]
output	`acceptedVelocity`	`Real`	`[size(waypoint, 1), 3]`	Fixed-capacity local velocity rows [m/s]
output	`acceptedYaw`	`Real`	`[size(waypoint, 1)](each unit = "rad")`
output	`segmentDuration`	`Real`	`[size(waypoint, 1) - 1](each unit = "s")`
output	`totalDuration`	`Real`	`(unit = "s")`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function prepareWaypointPlan
  "Validate and convert the fixed-capacity payload of one waypoint message"
  input Real waypoint[:, 3]
    "Local ENU metres or global latitude/longitude/altitude rows";
  input Real velocityEnu[size(waypoint, 1), 3]
    "World-frame velocity at each waypoint [m/s]";
  input Real yaw[size(waypoint, 1)](each unit = "rad");
  input Integer waypointCount(min = 2, max = size(waypoint, 1));
  input Boolean globalFrame;
  input Real originGeodetic[3]
    "Local origin [latitude_deg, longitude_deg, altitude_m]";
  input Real nominalSpeed(unit = "m/s");
  input Real minSegmentDuration(unit = "s");
  output Real localWaypoint[size(waypoint, 1), 3]
    "Fixed-capacity local East-North-Up rows [m]";
  output Real acceptedVelocity[size(waypoint, 1), 3]
    "Fixed-capacity local velocity rows [m/s]";
  output Real acceptedYaw[size(waypoint, 1)](each unit = "rad");
  output Real segmentDuration[size(waypoint, 1) - 1](each unit = "s");
  output Real totalDuration(unit = "s");
algorithm
  assert(waypointCount >= 2 and waypointCount <= size(waypoint, 1),
    "Waypoint count must fit the fixed-capacity plan");
  assert(nominalSpeed > 0.0, "Nominal speed must be positive");
  assert(minSegmentDuration > 0.0, "Minimum segment duration must be positive");

  for waypointIndex in 1:size(waypoint, 1) loop
    if waypointIndex <= waypointCount then
      if globalFrame then
        localWaypoint[waypointIndex, :] := Geodesy.geodeticToLocalEnu(
          Geodesy.GeodeticOrigin(
            originGeodetic[1], originGeodetic[2], originGeodetic[3]),
          waypoint[waypointIndex, 1],
          waypoint[waypointIndex, 2],
          waypoint[waypointIndex, 3]);
      else
        localWaypoint[waypointIndex, :] := waypoint[waypointIndex, :];
      end if;
      acceptedVelocity[waypointIndex, :] := velocityEnu[waypointIndex, :];
      acceptedYaw[waypointIndex] := yaw[waypointIndex];
    else
      localWaypoint[waypointIndex, :] := zeros(3);
      acceptedVelocity[waypointIndex, :] := zeros(3);
      acceptedYaw[waypointIndex] := 0.0;
    end if;
  end for;

  for segmentIndex in 1:(size(waypoint, 1) - 1) loop
    if segmentIndex < waypointCount then
      segmentDuration[segmentIndex] := max(
        minSegmentDuration,
        sqrt(sum(
          (localWaypoint[segmentIndex + 1, :]
            - localWaypoint[segmentIndex, :]) .^ 2)) / nominalSpeed);
    else
      segmentDuration[segmentIndex] := 0.0;
    end if;
  end for;
  totalDuration := sum(segmentDuration);
end prepareWaypointPlan;