MENU

M5STACK ATOM 用python实时显示欧拉角

• April 28, 2020 • Read: 240 • 折腾

上一篇文章我们简单运用了M5 ATOM上的六轴芯片MPU6886,这一次我们进一步开发,让电脑实时显示芯片的空间位置。

什么是欧拉角

知乎有大佬已经写的非常详细了,想了解的可以点击:如何通俗地解释欧拉角?之后为何要引入四元数?
里面有几个飞机真是太形象了,我这里就白嫖过来了。

Pitch

pitch.gif

Yaw

yaw.gif

Roll

roll.gif

如何得到欧拉角

我们有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在屏幕上渲染出来。

效果视频