植松電機スタッフもやってみた　　～SPC機体制作その⑤　IMUユニット動作確認～

こんにちは。星野です。

今日は、IMUユニットの動作確認を行っていこうと思います。

用意するもの

①前回制作した機体

②Atom Motion キット

③IMUユニット

・パソコン

IMUユニットの動作確認

加速度、ジャイロの値を表示させる

①IMUユニットのコネクタをATOM Motion キットのPORT.Bに接続します。

②以下のプログラムを書き込みます。

#include "M5Atom.h"

float accX = 0, accY = 0, accZ = 0;
float gyroX = 0, gyroY = 0, gyroZ = 0;


void setup(){
    M5.begin(true, false, true);
    Serial.begin(115200);
    M5.IMU.Init();
    M5.IMU.SetGyroFsr(M5.IMU.GFS_2000DPS); //ジャイロのレンジを設定    250,500,1000,2000DPSから選択可能
    M5.IMU.SetAccelFsr(M5.IMU.AFS_2G); //加速度のレンジを設定    2,4,8,16Gから選択可能
}

void loop(){

    M5.IMU.getGyroData(&gyroX, &gyroY, &gyroZ);// ジャイロの値を取得
    M5.IMU.getAccelData(&accX, &accY, &accZ);// 加速度の値を取得
     
    Serial.printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n", accX, accY, accZ, gyroX, gyroY, gyroZ);
    delay(100);
}

③動かしてみる

IMUユニットに軸の向きを書いたシールが貼ってあるので、それを元に各軸の値を見てみます。

Z軸を上向きにし、シリアルモニタで確認したところ、加速度X、Yの値が０付近で、Z軸は1G付近の値を取得できました。

姿勢の計算

カメラ撮影でブレを少なくするために、機体の姿勢を知る必要があります。

IMUユニットから姿勢データを取得してみましょう。

①カメラマウントにIMUユニットを載せます。

Y軸が機体の正面にくるようにIMUユニットを載せ、両面テープで固定します。

②下記のプログラムを書き込みます。

#include "M5Atom.h"

float roll, pitch, yaw;

void setup() {
  M5.begin(true, true, true);
  M5.IMU.Init();
  M5.IMU.SetGyroFsr(M5.IMU.GFS_2000DPS); //ジャイロのレンジを設定    250,500,1000,2000DPSから選択可能
  M5.IMU.SetAccelFsr(M5.IMU.AFS_2G); //加速度のレンジを設定    2,4,8,16Gから選択可能
}

void loop() {
  M5.IMU.getAhrsData(&pitch,&roll,&yaw);  //ロール、ピッチ、ヨーの値を取得
  Serial.printf("%.2f,%.2f,%.2f\n", roll, pitch, yaw); //ロール、ピッチ、ヨーの値を表示

  delay(5);
}

このプログラムでは、「getAhrsData」という関数で加速度、ジャイロからロール、ピッチ、ヨーの値を取得しています。

「getAhrsData」関数の中では、「Madgwick Filter」を組み込みフィルターをかけているみたいです。

Madgwick Filterの詳しい内容を知りたい方は、こちらが参考になるかと思います。

https://qiita.com/fumiya_sato/items/df28dff5ccc653183235

③カメラマウントを手で動かしてみる