Update imu.c

fix heading calculation (does not mater too much when using mag , but screwed up with acc headfree ... when pitched or rolled)