简体中文
English
简体中文
日本語

Arduino 上手教程

2. 设备开发 & 案例程序

5. 扩展模块

6. 应用案例

StickC-Plus IMU 姿态传感器

案例程序

编译要求

  • M5Stack 板管理版本 >= 3.2.5
  • 开发板选项 = M5StickCPlus
  • M5Unified 库版本 >= 0.2.18
  • M5GFX 库版本 >= 0.2.24
cpp
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83
#include <M5Unified.h>
#include <math.h>

float accX = 0.0F;
float accY = 0.0F;
float accZ = 0.0F;

float pitch = 0.0F;
float roll  = 0.0F;
float yaw   = 0.0F;

uint32_t lastUpdate = 0;
constexpr float RAD_TO_DEGREE = 57.2957795F;
constexpr float ALPHA = 0.98F;

void printRow(int16_t y, const char* axisLabel, float axisValue,
              const char* angleLabel, float angleValue) {
    M5.Display.setCursor(5, y);
    M5.Display.print(axisLabel);

    M5.Display.setCursor(30, y);
    M5.Display.printf("%6.2f", axisValue);

    M5.Display.setCursor(95, y);
    M5.Display.print(angleLabel);

    M5.Display.setCursor(155, y);
    M5.Display.printf("%6.2f", angleValue);
}

void setup() {
    auto cfg = M5.config();
    M5.begin(cfg);

    M5.Display.setRotation(1);
    M5.Display.fillScreen(TFT_BLACK);
    M5.Display.setTextFont(4);
    M5.Display.setTextSize(1);
    M5.Display.setTextColor(TFT_WHITE, TFT_BLACK);

    M5.Display.setCursor(100, 2);
    M5.Display.print("IMU");

    lastUpdate = millis();
}

void loop() {
    if (!M5.Imu.update()) {
        delay(10);
        return;
    }

    auto data = M5.Imu.getImuData();

    accX = data.accel.x;
    accY = data.accel.y;
    accZ = data.accel.z;

    uint32_t now = millis();
    float deltaTime = (now - lastUpdate) * 0.001F;
    lastUpdate = now;
    deltaTime = constrain(deltaTime, 0.0F, 0.5F);

    const float accelPitch =
        atan2f(-accX, sqrtf(accY * accY + accZ * accZ)) * RAD_TO_DEGREE;
    const float accelRoll =
        atan2f(accY, accZ) * RAD_TO_DEGREE;

    pitch = ALPHA * (pitch + data.gyro.y * deltaTime)
          + (1.0F - ALPHA) * accelPitch;
    roll = ALPHA * (roll + data.gyro.x * deltaTime)
         + (1.0F - ALPHA) * accelRoll;
    yaw += data.gyro.z * deltaTime;

    if (yaw > 180.0F) yaw -= 360.0F;
    if (yaw < -180.0F) yaw += 360.0F;

    printRow(36, "X:", accX, "Pitch:", pitch);
    printRow(69, "Y:", accY, "Roll:", roll);
    printRow(102, "Z:", accZ, "Yaw:", yaw);

    delay(100);
}

该程序在屏幕上以大字体分行显示 X/Y/Z 三轴加速度,以及通过加速度和陀螺仪数据计算的 Pitch、Roll、Yaw 姿态角,约每 100 ms 更新一次。

IMU 三轴方向示意图

Page Tools
PDF
On This Page