使用openGL可视化MPU6050

Question

在从MPU6050获得加速度计/陀螺仪后，我使用以下函数：

void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az,float mx, float my,float mz,double * quad) {
float norm;
float hx, hy, hz, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;
double* tempQuat = quad;

// auxiliary variables to reduce number of repeated operations
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q0q3 = q0*q3;
float q1q1 = q1*q1;
float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;   
float q2q3 = q2*q3;
float q3q3 = q3*q3;          

// normalise the measurements
norm = sqrt(ax*ax + ay*ay + az*az);       
ax = ax / norm;
ay = ay / norm;
az = az / norm;
norm = sqrt(mx*mx + my*my + mz*mz);          
mx = mx / norm;
my = my / norm;
mz = mz / norm;         

// compute reference direction of flux
hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2)
;         
bx = sqrt((hx*hx) + (hy*hy));
bz = hz;        

// estimated direction of gravity and flux (v and w)
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);  

// error is sum of cross product between reference direction of fields and direction measured by sensors
ex = (ay*vz - az*vy) + (my*wz - mz*wy);
ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
ez = (ax*vy - ay*vx) + (mx*wy - my*wx);

// integral error scaled integral gain
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;

// adjusted gyroscope measurements
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;

// integrate quaternion rate and normalise
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  

// normalise quaternion
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;

*tempQuat++ = q0;
*tempQuat++ = q1;
*tempQuat++ = q2;
*tempQuat++ = q3;
}

若要获取四元数值q0、q1、q3，请执行以下操作。然后我使用这个函数：

void MPU6050_getYawPitchRoll(double * ypr,double *qu)
{
double gx, gy, gz;
double *tempQ = qu;

double q0,q1,q2,q3;
float sqw = q0*q0;
  float sqx = q1*q1;
  float sqy = q2*q2;
  float sqz = q3*q3;

q0= *tempQ++;
q1= *tempQ++;
q2= *tempQ++;
q3= *tempQ++;

gx = 2 * (q1 * q3 - q0 * q2);
gy = 2 * (q0 * q1 + q2 * q3);   
gz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3; 
yaw = atan2(2* (q1 * q2 - q0 * q3), 2 * (q0 * q0 + q1 * q1) - 1) * 180 /   3.1416;  // YAW
pitch = atan(gx / sqrt(gy * gy + gz * gz)) * 180 / 3.1416;  // PITCH    
roll = atan(gy / sqrt(gx * gx + gz * gz)) * 180 / 3.1416;   // ROLL  

psi = atan2(2 * q1 * q2 - 2 * q0 * q3, 2 * q0*q0 + 2 * q1* q1 - 1)* 180 / 3.1416; // psi
theta = -asin(2 * q1 * q3 + 2 * q0 * q2)* 180 / 3.1416; // theta
phi = atan2(2 * q2 * q3 - 2 * q0 * q1, 2 * q0 * q0 + 2 * q3 * q3 - 1)* 180 / 3.1416; // phi
}

以获得偏航、俯仰、滚动、psi、theta和phi。

在PC机上，我使用openGL绘制三维物体。如何使用偏航、俯仰、滚动、psi、θ和phi来控制3D对象(旋转、平移)？是那么回事吗？

glRotatef(-Yaw, 0.0f, 1.0f, 0.0f);
glRotatef(-Pitch, 0.0f, 0.0f, 1.0f);
glRotatef(-Roll, 1.0f, 0.0f, 0.0f);

Answer 1

你一直在用老方法，为什么不用GLM呢？我在我的cinder项目中是这样做的，我阅读了来自MPU和arduino的_quat…

vec4 axis = toAxisAngle(_quat);
// ...
rotate(axis[0], -axis[1], axis[3], axis[2]); // Note the orders and signs

和toAxisAngle：

/**
 * Converts the quaternion into a float array consisting of: rotation angle
 * in radians, rotation axis x,y,z
 *
 * @return 4-element float array
 */
static const float EPS = 1.1920928955078125E-7f;
vec4  toAxisAngle(const quat& q) {
    vec4 res;
    float sa = (float) sqrt(1.0f - q.w * q.w);
    if (sa < EPS) {
        sa = 1.0f;
    } else {
        sa = 1.0f / sa;
    }
    res[0] = (float) acos(q.w) * 2.0f;
    res[1] = q.x * sa;
    res[2] = q.y * sa;
    res[3] = q.z * sa;
    return res;
}