Output 400 Hz PWM.
Don't use math.h because sqrt makes ChibiOS freeze.
Add missing semicolons.

Oops.
Increase CH_FREQUENCY to 100000.
Set control loop frequency to 1 kHz.
Make IMU work.
Debugging IMU.