Added IMU failsafe and 20 average