Would make more sense to put this in main_chibios.c right after the imu_periodic call.
Then it would be more clear and not specific to this imu.