Added test code to determine if the autopilot millis() is working correctly and it isn't. It only updates the milliseconds every .4 seconds.