Added gyro thresholding for when magnetometer is corrupt
Also added ability to system to declare imu's corrupt