上一篇文章我们简单运用了M5 ATOM上的六轴芯片MPU6886，这一次我们进一步开发，让电脑实时显示芯片的空间位置。
什么是欧拉角
知乎有大佬已经写的非常详细了，想了解的可以点击：如何通俗地解释欧拉角？之后为何要引入四元数？
里面有几个飞机真是太形象了，我这里就白嫖过来了。
如何得到欧拉角
我们有MPU6886六轴芯片，可以很简单的获取到每个轴的加速度值和陀螺仪的值。通过这些值经过四元数最后计算出欧拉角，具体可以直接看代码。
void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az,float *pitch,float *roll,float *yaw) {
float recipNorm;
float halfvx, halfvy, halfvz;
float halfex, halfey, halfez;
float qa, qb, qc;
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
halfvx = q1 * q3 - q0 * q2;
halfvy = q0 * q1 + q2 * q3;
halfvz = q0 * q0 - 0.5f + q3 * q3;
halfex = (ay * halfvz - az * halfvy);
halfey = (az * halfvx - ax * halfvz);
halfez = (ax * halfvy - ay * halfvx);
if(twoKi > 0.0f) {
integralFBx += twoKi * halfex * (1.0f / sampleFreq);
integralFBy += twoKi * halfey * (1.0f / sampleFreq);
integralFBz += twoKi * halfez * (1.0f / sampleFreq);
gx += integralFBx;
gy += integralFBy;
gz += integralFBz;
}
else {
integralFBx = 0.0f;
integralFBy = 0.0f;
integralFBz = 0.0f;
}
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
}
gx *= (0.5f * (1.0f / sampleFreq));
gy *= (0.5f * (1.0f / sampleFreq));
gz *= (0.5f * (1.0f / sampleFreq));
qa = q0;
qb = q1;
qc = q2;
q0 += (-qb * gx - qc * gy - q3 * gz);
q1 += (qa * gx + qc * gz - q3 * gy);
q2 += (qa * gy - qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy - qc * gx);
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
*pitch = asin(-2 * q1 * q3 + 2 * q0* q2); // pitch
*roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1); // roll
*yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3); //yaw
*pitch *= RAD_TO_DEG;
*yaw *= RAD_TO_DEG;
*yaw -= 8.5;
*roll *= RAD_TO_DEG;
}如何上传数据
我们在ESP32上或者其他MCU中通过json格式的数据上传，可以是串口也可以是socket等等，代码如下：
Serial.printf("{\"functionName\": \"sendEuler\", \"pitch\":%f, \"roll\":%f, \"yaw\":%f}\r\n" ,pitch,roll,yaw);然后上位机通过解析json数据：
def readData(self):
self.readBuf = self.ser.readline().decode("utf-8")
try:
jsonData=json.loads(self.readBuf)
return jsonData["pitch"],jsonData["yaw"],jsonData["roll"]
except:
pass最后通过pyopengl与pygame在屏幕上渲染出来。