The basic SW design is shown here:
The loop statistics looks like this:
The timing is shown here:
Right after the new data available interrupt (yellow), the MPU9250 (first accelerometer, the gyro and the magnetometer; the pressure sensor xyz will be read (blue) every xxx time (because of his data update rate of …..). The double calculations (scaling, Madgwick quaternions and PID controller) need only about 360 μs (hardware floating point unit on the STM32F4 chip; very nice!). After that the 4 CAN messages are send to the BLDC motor controllers (magenta). Since the CAN mailbox only takes three messages at once the main loop will be blocked until the first message has been send.