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の3軸加速度と、加速度・ジャイロデータから算出したPitch、Roll、Yaw姿勢角を大きなフォントで行ごとに画面表示し、約100 msごとに更新します。

IMU 3軸方向の模式図

Page Tools
PDF
On This Page