學會面向對象編程封裝IMU驅動

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

所以本節(jié)我們將通過面向對象的方式將IMU驅動進行封裝，并為其添加坐標系轉換以及四元數(shù)轉換函數(shù)。

本教程所使用硬件平臺為MicroROS學習板V1.0.0，可點擊閱讀原文購買及查看詳情

一、理論介紹

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

歐拉角轉四元數(shù)的公式我們在第六章入門篇第三節(jié)有介紹，這里回顧一下

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

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 坐標系校準

我們采用右手坐標系，接著我們依次來校準角度數(shù)據(jù)的方向。

打開終端，點擊RST,查看IMU數(shù)據(jù)。

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

可以看到此時X軸為正值，符合右手坐標系法則。

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

Y軸為負值，不符合右手坐標系法則，所以Y的值應該取一次負，使其為正。

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

值為正，表示符合右手坐標系法則。

你可能會問小魚怎么確認怎樣旋轉是正，怎樣旋轉是負，首先要確認軸向，我們開發(fā)板的Z軸朝上，X軸朝前，此時Y軸應該朝左。接著攤開右手手掌，用大拇指朝向軸的方向，比如朝向X軸，然后握起手掌，那么你握的方向就是正方向。

二、開始寫代碼

新建工程example07_mpu6050_oop

接著為其添加依賴

修改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ù)結構體

typedef struct
{
    float x;
    float y;
    float z;
} vector_3d_t; // 通用3D點結構體

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ù)結構體

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

public:
    /**
     * @brief 同u哦NPU6050構造一個新的IMU對象
     *
     * @param mpu
     */
    IMU(MPU6050 &mpu);
    
    ~IMU() = default;
    /**
     * @brief 初始化函數(shù)
     *
     * @param sda 引腳編號
     * @param scl 引腳編號
     * @return true
     * @return false
     */
    bool begin(int sda, int scl);
    /**
     * @brief 歐拉角轉四元數(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對象
IMU imu(mpu);      // 初始化IMU對象

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ù)結構體
    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();
  }
}

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

編譯下載后，你將看到

三、總結

本節(jié)我們通過對MPU6050驅動的封裝，學習了如何在嵌入式上使用面向對象編程的方法，下一節(jié)我們繼續(xù)嘗試使用開源庫來驅動OLED模塊，讓我們的顯示器亮起來。