So actually since you said the attitude didn't work properly with this change, did you try reverting these changes and keeping the changes to sensor.c