Make sure the timing remains valid if the accelerometer/gyroscope fails
Add a function to reset PIDs
Improved quadcopter sketch, work in progress