Update telemetry.c

Heading corrected GPS output corrected