Vehicles.Cubs2.OuterLoopComponents.CrossTrackGuidance

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

block CrossTrackGuidance
  "Straight-segment guidance with nonlinear cross-track correction"
  extends RouteGuidanceInterface;

protected
  discrete Integer activeWaypoint(min=1, max=6, start=1);
  discrete Integer segmentEndIndex(min=2, max=7, start=2);
  discrete Real segmentStart[3];
  discrete Real segmentEnd[3];
  discrete Real segmentVector[3];
  discrete Real horizontalSegmentVector[2];
  discrete Real horizontalSegmentLength;
  discrete Real segmentHeading;
  discrete Real segmentUnit[2];
  discrete Real crossTrackUnit[2];
  discrete Real positionFromSegmentStart[2];
  discrete Real alongTrackDistance;
  discrete Real remainingAlongTrackDistance;
  discrete Real pathProgress;
  discrete Real pathAltitude;
  discrete Real altitudeError;
  discrete Real crossTrackError;
  discrete Real steeringCorrection;

algorithm
  when sample(0.0, dt) then
    activeWaypoint := pre(currentWaypoint);
    segmentEndIndex := pre(currentWaypoint) + 1;
    if pre(currentWaypoint) == 1 then
      segmentStart := route.waypoints[1, :];
      segmentEnd := route.waypoints[2, :];
    elseif pre(currentWaypoint) == 2 then
      segmentStart := route.waypoints[2, :];
      segmentEnd := route.waypoints[3, :];
    elseif pre(currentWaypoint) == 3 then
      segmentStart := route.waypoints[3, :];
      segmentEnd := route.waypoints[4, :];
    elseif pre(currentWaypoint) == 4 then
      segmentStart := route.waypoints[4, :];
      segmentEnd := route.waypoints[5, :];
    elseif pre(currentWaypoint) == 5 then
      segmentStart := route.waypoints[5, :];
      segmentEnd := route.waypoints[6, :];
    else
      segmentStart := route.waypoints[6, :];
      segmentEnd := route.waypoints[7, :];
    end if;

    segmentVector := segmentEnd - segmentStart;
    horizontalSegmentVector := horizontalPart(segmentVector);
    horizontalSegmentLength := max(MathUtilities.norm2(horizontalSegmentVector), 1e-6);
    segmentHeading := atan2(horizontalSegmentVector[2], horizontalSegmentVector[1]);
    segmentUnit := horizontalSegmentVector / horizontalSegmentLength;
    crossTrackUnit := {-segmentUnit[2], segmentUnit[1]};
    positionFromSegmentStart :=
      horizontalDisplacement(estimate.position_m, segmentStart);
    alongTrackDistance := positionFromSegmentStart * segmentUnit;
    remainingAlongTrackDistance :=
      max(0.0,
          horizontalSegmentLength
          - MathUtilities.clip(alongTrackDistance, 0.0, horizontalSegmentLength));
    pathProgress :=
      MathUtilities.clip(alongTrackDistance / horizontalSegmentLength, 0.0, 1.0);
    pathAltitude :=
      segmentStart[3] + pathProgress * (segmentEnd[3] - segmentStart[3]);
    altitudeError := pathAltitude - estimate.position_m[3];
    crossTrackError := positionFromSegmentStart * crossTrackUnit;
    steeringCorrection :=
      atan2(-crossTrackError, max(route.crossTrackSteeringDistance, 1e-6));

    setpoints.speed := route.cruiseSpeed;
    setpoints.flightPathAngle :=
      MathUtilities.clip(atan2(route.altitudeToFlightPathGain * altitudeError,
                 max(route.altitudeLookaheadDistance, 1e-6)),
           -route.flightPathAngleLimit,
           route.flightPathAngleLimit);
    setpoints.heading := MathUtilities.wrapAngle(segmentHeading + steeringCorrection);
    setpoints.acceleration :=
      route.speedToAccelerationGain * (setpoints.speed - estimate.speed);

    if remainingAlongTrackDistance < route.waypointSwitchingDistance then
      if pre(currentWaypoint) >= route.nSegments then
        currentWaypoint := 1;
      else
        currentWaypoint := pre(currentWaypoint) + 1;
      end if;
    else
      currentWaypoint := pre(currentWaypoint);
    end if;
  end when;
end CrossTrackGuidance;