Fixed headphone detect in FM test.
Improved code.
Base for the final production version.