上一節(jié)我們成功讀取到了IMU的數(shù)據(jù)，其中角度用歐拉角的方式表示的，在我們機(jī)器人世界里姿態(tài)的表示往往使用四元數(shù)表示（如果不清楚他們之間的關(guān)系可以回看第六章機(jī)器人學(xué)篇），所以我們需要將歐拉角轉(zhuǎn)換成四元數(shù)。除此之外我們還需要將其坐標(biāo)系矯正到右手坐標(biāo)系。

所以本節(jié)我們將通過(guò)面向?qū)ο蟮姆绞綄MU驅(qū)動(dòng)進(jìn)行封裝，并為其添加坐標(biāo)系轉(zhuǎn)換以及四元數(shù)轉(zhuǎn)換函數(shù)。

本教程所使用硬件平臺(tái)為MicroROS學(xué)習(xí)板V1.0.0，可點(diǎn)擊閱讀原文購(gòu)買(mǎi)及查看詳情

一、理論介紹

1.1.歐拉角轉(zhuǎn)四元數(shù)

歐拉角轉(zhuǎn)四元數(shù)的公式我們?cè)诘诹氯腴T(mén)篇第三節(jié)有介紹，這里回顧一下

根據(jù)公式我們可以寫(xiě)出代碼

typedef struct
{
    float w;
    float x;
    float y;
    float z;
} quaternion_t;

Euler2Quaternion(float roll, float pitch, float yaw, quaternion_t &q)
{
    double cr = cos(roll * 0.5);
    double sr = sin(roll * 0.5);
    double cy = cos(yaw * 0.5);
    double sy = sin(yaw * 0.5);
    double cp = cos(pitch * 0.5);
    double sp = sin(pitch * 0.5);
    q.w = cy * cp * cr + sy * sp * sr;
    q.x = cy * cp * sr - sy * sp * cr;
    q.y = sy * cp * sr + cy * sp * cr;
    q.z = sy * cp * cr - cy * sp * sr;
}

1.2 坐標(biāo)系校準(zhǔn)

我們采用右手坐標(biāo)系，接著我們依次來(lái)校準(zhǔn)角度數(shù)據(jù)的方向。

打開(kāi)終端，點(diǎn)擊RST,查看IMU數(shù)據(jù)。

首先是X軸，我們讓開(kāi)發(fā)板上愛(ài)神丘比特的劍頭指向自己，然后從右側(cè)往左側(cè)傾斜。

可以看到此時(shí)X軸為正值，符合右手坐標(biāo)系法則。

接著是Y軸，平放，將箭頭朝向自己的胸口，接著抬高板子，讓箭頭指向自己的頭部，觀察Y軸的變化。

Y軸為負(fù)值，不符合右手坐標(biāo)系法則，所以Y的值應(yīng)該取一次負(fù)，使其為正。

接著是Z軸，平放，將箭頭朝向自己的胸口，然后逆時(shí)針旋轉(zhuǎn)板子，觀察數(shù)值變化。

值為正，表示符合右手坐標(biāo)系法則。

你可能會(huì)問(wèn)小魚(yú)怎么確認(rèn)怎樣旋轉(zhuǎn)是正，怎樣旋轉(zhuǎn)是負(fù)，首先要確認(rèn)軸向，我們開(kāi)發(fā)板的Z軸朝上，X軸朝前，此時(shí)Y軸應(yīng)該朝左。接著攤開(kāi)右手手掌，用大拇指朝向軸的方向，比如朝向X軸，然后握起手掌，那么你握的方向就是正方向。

二、開(kāi)始寫(xiě)代碼

新建工程example07_mpu6050_oop

接著為其添加依賴(lài)

修改platformio.ini

[env:featheresp32]
platform = espressif32
board = featheresp32
framework = arduino
lib_deps = 
    https://ghproxy.com/https://github.com/rfetick/MPU6050_light.git

接著在lib下新建IMU文件夾，并在文件夾下新建IMU.h和IMU.cpp

IMU.h

#ifndef __IMU_H__
#define __IMU_H__
#include "Wire.h"
#include "MPU6050_light.h"

typedef struct
{
    float w;
    float x;
    float y;
    float z;
} quaternion_t; // 四元數(shù)結(jié)構(gòu)體

typedef struct
{
    float x;
    float y;
    float z;
} vector_3d_t; // 通用3D點(diǎn)結(jié)構(gòu)體

typedef struct
{
    quaternion_t orientation;
    vector_3d_t angle_euler;
    vector_3d_t angular_velocity;
    vector_3d_t linear_acceleration;
} imu_t; // IMU數(shù)據(jù)結(jié)構(gòu)體

class IMU
{
private:
    MPU6050 *mpu_; // mpu6050指針

public:
    /**
     * @brief 同u哦NPU6050構(gòu)造一個(gè)新的IMU對(duì)象
     *
     * @param mpu
     */
    IMU(MPU6050 &mpu);
    
    ~IMU() = default;
    /**
     * @brief 初始化函數(shù)
     *
     * @param sda 引腳編號(hào)
     * @param scl 引腳編號(hào)
     * @return true
     * @return false
     */
    bool begin(int sda, int scl);
    /**
     * @brief 歐拉角轉(zhuǎn)四元數(shù)
     *
     * @param roll 輸入X
     * @param pitch 輸入y
     * @param yaw 輸入Z
     * @param q  返回的四元數(shù)引用
     */
    static void Euler2Quaternion(float roll, float pitch, float yaw, quaternion_t &q);
    /**
     * @brief 獲取IMU數(shù)據(jù)函數(shù)
     *
     * @param imu
     */
    void getImuData(imu_t &imu);
    /**
     * @brief 更新IMU數(shù)據(jù)，同上一節(jié)中的mou.update
     *
     */
    void update();
};

#endif // __IMU_H__

IMU.cpp

#include "IMU.h"

IMU::IMU(MPU6050 &mpu)
{
    mpu_ = &mpu;
};

bool IMU::begin(int sda, int scl)
{
    Wire.begin(sda, scl);
    byte status = mpu_- >begin();
    Serial.print(F("MPU6050 status: "));
    Serial.println(status);
    if (status != 0)
    {
        return false;
    } // stop everything if could not connect to MPU6050

    Serial.println(F("Calculating offsets, do not move MPU6050"));
    delay(1000);
    // mpu.upsideDownMounting = true; // uncomment this line if the MPU6050 is mounted upside-down
    mpu_- >calcOffsets(); // gyro and accelero
    Serial.println("Done!n");
    return true;
}

void IMU::Euler2Quaternion(float roll, float pitch, float yaw, quaternion_t &q)
{
    double cr = cos(roll * 0.5);
    double sr = sin(roll * 0.5);
    double cy = cos(yaw * 0.5);
    double sy = sin(yaw * 0.5);
    double cp = cos(pitch * 0.5);
    double sp = sin(pitch * 0.5);
    q.w = cy * cp * cr + sy * sp * sr;
    q.x = cy * cp * sr - sy * sp * cr;
    q.y = sy * cp * sr + cy * sp * cr;
    q.z = sy * cp * cr - cy * sp * sr;
}

void IMU::getImuData(imu_t &imu)
{
    imu.angle_euler.x = mpu_- >getAngleX();
    imu.angle_euler.y = -mpu_- >getAngleY();
    imu.angle_euler.z = mpu_- >getAngleZ();

    imu.angular_velocity.x = mpu_- >getAccAngleX();
    imu.angular_velocity.y = -mpu_- >getAccAngleY();
    imu.angular_velocity.z = mpu_- >getGyroZ();

    imu.linear_acceleration.x = mpu_- >getAccX();
    imu.linear_acceleration.y = mpu_- >getAccY();
    imu.linear_acceleration.z = mpu_- >getAccZ();

    IMU::Euler2Quaternion(imu.angle_euler.x, imu.angle_euler.y, imu.angle_euler.z,
                          imu.orientation);
}

void IMU::update()
{
    mpu_- >update();
}

main.cpp

#include < Arduino.h >
#include "IMU.h"

MPU6050 mpu(Wire); // 初始化MPU6050對(duì)象
IMU imu(mpu);      // 初始化IMU對(duì)象

imu_t imu_data;
unsigned long timer = 0;

void setup()
{
  Serial.begin(115200);
  imu.begin(18, 19); // 初始化IMU,使用18，19引腳
}

void loop()
{
  imu.update();
  if ((millis() - timer) > 100)
  {
    imu.getImuData(imu_data); // 獲取IMU數(shù)據(jù)結(jié)構(gòu)體
    Serial.printf("imu:teuler(%f,%f,%f)n",
                  imu_data.angle_euler.x, imu_data.angle_euler.y, imu_data.angle_euler.z);
    Serial.printf("imu:torientation(%f,%f,%f,%f)n",
                  imu_data.orientation.w, imu_data.orientation.x, imu_data.orientation.y, imu_data.orientation.z);
    timer = millis();
  }
}

對(duì)于代碼的解釋已經(jīng)放到了注釋之中。

編譯下載后，你將看到

三、總結(jié)

本節(jié)我們通過(guò)對(duì)MPU6050驅(qū)動(dòng)的封裝，學(xué)習(xí)了如何在嵌入式上使用面向?qū)ο?a href="http://www.brongaenegriffin.com/v/tag/1315/" target="_blank">編程的方法，下一節(jié)我們繼續(xù)嘗試使用開(kāi)源庫(kù)來(lái)驅(qū)動(dòng)OLED模塊，讓我們的顯示器亮起來(lái)。