Heading PID controller.

Collaboration diagram for Heading PID controller:

Author: Mats Nåvik Hval

Date: 20.06.2011 MNH: Initial version.

A PID controller on a heading angle. The proportional term uses the heading difference wrapped to (-pi, pi], so the controller turns the short way round. The derivative term uses the difference of the desired and the current yaw rate. The output is

\[ \tau = K_p \Delta\psi + K_d (\dot\psi_d - \dot\psi) + K_i \int \Delta\psi\, dt \]

with \( \Delta\psi = \mathrm{atan2}(\sin(\psi_d - \psi), \cos(\psi_d - \psi)) \). Angles are in radians (the code takes sin and cos). The unit of the output is not stated in the code.

Input ports

Name	Width	Description
CurrentHeading	1	Current heading
DesiredHeading	1	Desired heading
CurrentYawRate	1	Current yaw rate
DesiredYawRate	1	Desired yaw rate

Output ports

Name	Width	Description
Torque	1	The control output \( \tau \)
AngleDiff	1	The wrapped heading difference \( \Delta\psi \)

Configuration parameters

The names are in lower case here, unlike Ship::WpHeadingController.

Name	Width	Description
kp	1	Proportional gain. Default 0
ki	1	Integral gain. Default 0
kd	1	Derivative gain. Default 0

States

Name	Width	Description
Error	1	Integral of the heading difference. Initial value 0