/* Normalizes the angle ( -PI < angle <= PI )  */
void IMU_normalizeAngle(float *a)
/* Normalizes the angle ( -PI < angle <= PI )  */
void IMU_normalizeAngle(float *a)
{
   while( *a >= IMU_PI ) {
      *a -= 2 * IMU_PI;
   }
   while( *a < -IMU_PI ) {
      *a += 2 * IMU_PI;
   }
   return;
}