###### tags: `東京威力 TEL`
{%hackmd BJrTq20hE %}

# IV. IMU -- BNO055

## <font color = "orange">03. 實際應用</font>

### <font color ="pink">3-1. 四元數轉歐拉角 </font>

void CAR_IMU::get_euler(double x, double y, double z, double w){
    double len = sqrt(w*w + x*x + y*y + z*z);
    w /= len;
    x /= len;
    y /= len;
    z /= len;

    // roll (x-axis rotation)
    double q32 = 2 * (y*z + w*x);
    double q33 = 1 - 2 * (x*x + y*y);
    roll = atan(q32/q33) * 180/PI;

    // pitch (y-axis rotation)
    double q31 = 2 * (x*z - w*y);
    pitch = asin(-q31) * 180/PI;

    // yaw (z-axis rotation)
    double q21 = 2 * (x*y - w*z);
    double q11 = 1 - 2 * (y*y + z*z);
    yaw = atan(q21/q11) * 180/PI;

    imu_pub.x = roll;
    imu_pub.y = pitch;
    imu_pub.z = yaw;
    imu_publisher.publish(imu_pub); 
}

:::warning
比賽時間不夠就沒多研究了，最後也沒用上。
:::