Reference generator which generates a reference based on position triplets.

Collaboration diagram for Waypoint reference generator:

Author: Mats Nåvik Hval

Date: 12.08.2011 MNH: Initial version.

Gives a desired heading, position and speed for a path of straight legs between waypoints, with a circle arc at each inner waypoint. The vessel sails to the target waypoint (it starts with waypoint 1; the leg is from the previous waypoint to the target). The target point is on the line from the previous to the target waypoint, a lookahead distance ahead of the vessel measured along that line, but not beyond the target waypoint (the lookahead distance is three times the parameter Lookahead). The desired heading is the direction from the vessel to the target point. The object moves on to the next waypoint, after an accepted step, when the horizontal distance to the target waypoint is below the radius of that waypoint. The last waypoint is never left. The cross-track error is the distance from the vessel to the leg, with a sign. All waypoint arrays have the same size N (at least 2). The waypoints are in the NED frame.

Input ports

Name	Width	Description
PositionNED	3	Position of the vessel in the NED frame
VelocityNED	3	Velocity of the vessel in the NED frame. Registered, not used

Output ports

Name	Width	Description
DesiredHeadingNED	1	Heading from the vessel to the target point in radians
DesiredHeadingRateNED	1	Always 0
DesiredQuaternion	4	Quaternion of a rotation of the desired heading about the z axis: cos(h/2), 0, 0, sin(h/2)
DesiredPositionNED	3	The target point
DesiredSurgeSpeed	1	The value of WaypointLeg_Speed for the leg being sailed
CrossTrackError	1	Signed distance from the vessel to the leg. Its sign is the side of the leg the vessel is on. 0 on the track, and the distance with a positive sign abeam the target waypoint

Configuration parameters

Name	Width	Description
Waypoint_N	N	North coordinates of the waypoints. Required
Waypoint_E	N	East coordinates of the waypoints. Required
Waypoint_D	N	Down coordinates of the waypoints. Default 0 for all
Waypoint_r	N	Radius of acceptance of each waypoint, also the distance before an inner waypoint where the turn starts. Required. Its size sets N
WaypointLeg_Speed	N-1	Desired speed on each leg: element i is the leg from waypoint i to waypoint i+1. Required
Lookahead	1	One third of the lookahead distance along the leg. Default 100
Sideslip-compensation	1	String. Default n. Read but not used by the code