In attitude mode you might need to cover the modulus if it wraps around ...

Also this shouldn't even be updated if not in attitude mode now I think about it.