Telemetry fixed (gather the raw data correctly)
Improved steering using curvature