Fixing time functions in ahrs.cpp. Not complete, does not build.