Zeroing the yaw incorrectly has an impact on the Fused Heading

If zeroing the yaw, the fused heading is no longer accurate.  This appears to be related to the change in [this commit](https://github.com/kauailabs/navxmxp/commit/7b8810a1dcb03600ca3b544a1ea4f96fad5f1c19#diff-af9ada5e19848b9b8e11321f500d2014).