Fixed Magnetometer negative.
Merged ComPAS tested code with magnetometer fix.