BNO055のyaw軸の問題

こちらのブログでBNO055の問題点を知った。

l52secondary.blog.fc2.com

yaw軸の問題については同様に体験していたので、クォータニオンについていろいろ調べてみたが、よく理解できなかったため、とりあえず先のブログのコードを参考にクォータニオンで値を取得し、オイラー角に変換してみた。改善したように思うが、それでもずれるときはずれるみたい。

旧版C-Style Ver.190706で動作確認してみた。 「C:\Daisen\C-Style for TJ3B V190706\Build\Build_V190617\D_I2C.c」の「UINT get_bno(BYTE dno)」を次のように書き換えると試せる。 （注意）以下のコードは無保証です。

UINT get_bno(BYTE dno)
{
    BYTE adrs = BNO_ADRS;
    BYTE n;
    U_UINT d;
    float wf, xf, yf, zf, ysqr, t0, t1, t2, t3, t4;
    int xd, yd, zd;
    // DSR1603:9D-Compass(BNO055)の初期設定
    if (gIsSetDSR1603 == false) set_bno();
    /*
     * BNO055 datasheet
     * https://cdn-shop.adafruit.com/datasheets/BST_BNO055_DS000_12.pdf
     * 3.4 Axis remap 
     * 3.6.5.5 Orientation (Quaternion)
     */
    n = 0;
    gI2C_Buf[n++] = 0x20; // Register 0x20:AZIMUTH(LSB-MSB), 0x22:ROLL(LSB-MSB), 0x24:PITCH(LSB-MSB), 0x26:PITCH(LSB-MSB)
    if (i2c_send(adrs, n) == false) return(gDeg[dno]);
    if (i2c_recv(adrs, 8) == false) return(gDeg[dno]);
    //
    n = 0;
    d.L = gI2C_Buf[n++];
    d.H = gI2C_Buf[n++];
    wf = (int) d.W;
    d.L = gI2C_Buf[n++];
    d.H = gI2C_Buf[n++];
    xf = (int) d.W;
    d.L = gI2C_Buf[n++];
    d.H = gI2C_Buf[n++];
    yf = (int) d.W;
    d.L = gI2C_Buf[n++];
    d.H = gI2C_Buf[n++];
    zf = (int) d.W;
    wf /= 16384; // 2^14
    xf /= 16384;
    yf /= 16384;
    zf /= 16384;
    /**
     * クォータニオン→オイラー角変換は以下を参考にした
     * http://l52secondary.blog.fc2.com/blog-entry-50.html
     * センサモニタ
     * 0(dir(yaw)), 1(pitch), 2(roll)
     * yaw   :左向359←0→1右向
     * pitch :前下がり90←180→前上げ270
     * roll  :右下がり0←90→右上がり180
     */
    ysqr = yf * yf;
    // X-axis rotation
    t0 = 2.0 * (wf * xf + yf * zf);
    t1 = 1.0 - 2.0 * (xf * xf + ysqr);
    xd = (int)(atan2(t0, t1) * 57.2957795131) + 180; // オリジナルとオフセットを揃える
    xd = (xd + 360) % 360;
    gDeg[1] = (UINT) xd;  // pitch
    // Y-axis rotation
    t2 = 2.0 * (wf * yf - zf * xf);
    if (t2 > 1.0) {
        t2 = 1.0;
    } else if (t2 < -1.0) {
        t2 = -1.0;
    }
    yd = (int)(- asin(t2) * 57.2957795131) + 90; // オリジナルと向き・オフセットを揃える
    yd = (yd + 360) % 360;
    gDeg[2] = (UINT) yd;  // roll
    // Z-axis rotation
    t3 = 2.0 * (wf * zf + xf * yf);
    t4 = 1.0 - 2.0 * (ysqr + zf * zf);
    zd = (int)(- atan2(t3, t4) * 57.2957795131); // オリジナルと向きを揃える
    zd = (zd + 360) % 360;
    gDeg[0] = (UINT) zd;  // yaw
    return(gDeg[dno]);
}