I tried the example in this folder : /libraries/AP_InertialSensor/examples/INS_generic.
But it turns out that reading the gyro and acce data from a MPU9250 Sensor is really slow. I got only One Sample for a few seconds.
Then I tracked the code and found that the Scheduler::suspend_timer_procs function actually stalled all the process.
While I thought that it would be fast to just get a mutex. Besides, I don’t think that mutex is locked most of the time, And the processor load is very low all the time.
I wonder why this is happening.