In MahonyAHRS.cpp specify update frequency to 25Hz but the example use delay of 1000 ms. So the algorith is wrong and this code is useless.