MPU-6050 Digital Motion Processor (DMP) Quaternion Spatial Tracking

Configure the hardware Digital Motion Processor (DMP) inside the MPU-6050 to compute spatial orientation quaternions on-chip, freeing 90% of MCU CPU cycles.

[Internal Gyro + Accel Sensors] --> [MPU-6050 DMP Coprocessor Firmware]
                                              |
                                   [1024-Byte Hardware FIFO]
                                              |
                                    (Interrupt Pin Pulse @ 200Hz)
                                              v
                                [Microcontroller I2C Master]
                                              |
                                  [Quaternion to Euler / 3D Pose]

// C++: Quaternion Normalization & Pitch/Roll Decomposition
#include <cmath>

struct Quaternion {
    float w, x, y, z;

    void normalize() {
        float mag = sqrtf(w * w + x * x + y * y + z * z);
        if (mag > 0.0f) {
            w /= mag; x /= mag; y /= mag; z /= mag;
        }
    }
};

struct EulerAngles {
    float roll, pitch, yaw; // Radians
};

EulerAngles quaternionToEuler(const Quaternion &q) {
    EulerAngles angles;

    // Roll (x-axis rotation)
    float sinr_cosp = 2.0f * (q.w * q.x + q.y * q.z);
    float cosr_cosp = 1.0f - 2.0f * (q.x * q.x + q.y * q.y);
    angles.roll = atan2f(sinr_cosp, cosr_cosp);

    // Pitch (y-axis rotation)
    float sinp = 2.0f * (q.w * q.y - q.z * q.x);
    if (fabsf(sinp) >= 1.0f) {
        angles.pitch = copysignf(M_PI / 2.0f, sinp); // Use 90 degrees if out of range
    } else {
        angles.pitch = asinf(sinp);
    }

    // Yaw (z-axis rotation)
    float siny_cosp = 2.0f * (q.w * q.z + q.x * q.y);
    float cosy_cosp = 1.0f - 2.0f * (q.y * q.y + q.z * q.z);
    angles.yaw = atan2f(siny_cosp, cosy_cosp);

    return angles;
}