Heading difference

A problem that pops up always. When needing to calculate heading difference between two compass courses?


float calculateHeadError(float heading, float target)
{
#define PI2 PI*2.0
while (heading<0) heading += PI2; while (target<0) target += PI2; while (heading>PI2) heading -= PI2;
while (target>PI2) target -= PI2;

float res = heading-target;

if ((headingPI))
{
res =(heading+PI2)-target;
}

if ((heading>PI) && (target