先日のM5atomの内臓IMUの精度がいまいちだったので、精度を追求してみた。
そもそも、MPU6886は内臓ゆえにもともと利用できるスケッチがあるが、どうもその全体像がはっきりしない。ということで、MP6886データシートを張ります。だいたい、MPU6050と同じような仕様です。
【参考】
・MPU6886データシート
やったこと
・スケッチ例と関数をみる
・精度を見る
・結果
・スケッチ例と関数をみる
参考を見れば、m5atomのスケッチ例は以下のとおりです。
【参考】
①M5Atom/examples/Basics/MPU6886/MPU6886.ino
主要な利用法を抜き出すと、以下の通りです。
#include "M5Atom.h"
...
void setup()
{
M5.begin(true, true, true);
M5.IMU.Init();
}
void loop()
{
delay(50);
M5.IMU.getAttitude(&pitch, &roll);
...
Serial.printf("%.2f,%.2f,%.2f,%.2f\n", pitch, roll, arc, val);
...
}
実は、このM5.IMU.getAttitude(&pitch, &roll);
の精度がいまいちでした。
というか、たぶん素で出力しているようです。
次に、参考になるスケッチは以前のようにm5stickcのコードです。
m5stickcでは該当する部分は、以下のように実装しています。
【参考】
②【倒立振り子入門】ArduinoNanoとm5stickcをそれぞれ利用して、PID制御で倒立振り子を立てる♪
#include <M5StickC.h>
void setup() {
M5.begin();
...
M5.MPU6886.Init();
}
void loop() {
M5.MPU6886.getAhrsData(&pitch, &roll, &yaw);
...
}
同様な関数は無いかm5atom.hを眺めると以下のとおりありました。
まず、m5stickcではM5.MPU6886としていましたが、atomではm5atom.hで以下のようにMPU6886 IMU;でIMUというインスタンスで呼べるようになっています。
※その他の関数も以下で定義されているので、冗長ですが示しておきます
【参考】
③M5Atom/src/M5Atom.h
class M5Atom
{
private:
bool _isInited = false;
/* data */
public:
M5Atom(/* args */);
~M5Atom();
MPU6886 IMU;
LED_DisPlay dis;
Button Btn = Button(39, true, 10);
void begin(bool SerialEnable = true, bool I2CEnable = true, bool DisplayEnable = false);
void update();
};
extern M5Atom M5;
そして、MPU6886.hを見ると使える関数が分かります。get関数に混じって初期化関数setもあります。
getAttitude(double *pitch, double *roll);
に加えてgetAhrsData(float *pitch,float *roll,float *yaw);
も使えそうです。
【参考】
④M5Atom/src/utility/MPU6886.h
...
class MPU6886 {
public:
MPU6886();
int Init(void);
void getAccelAdc(int16_t* ax, int16_t* ay, int16_t* az);
void getGyroAdc(int16_t* gx, int16_t* gy, int16_t* gz);
void getTempAdc(int16_t *t);
void getAccelData(float* ax, float* ay, float* az);
void getGyroData(float* gx, float* gy, float* gz);
void getTempData(float *t);
void SetGyroFsr(Gscale scale);
void SetAccelFsr(Ascale scale);
void getAhrsData(float *pitch,float *roll,float *yaw);
void getAttitude(double *pitch, double *roll);
これらの違いを見ておきましょう。
【参考】
⑤M5Atom/src/utility/MPU6886.cpp
参考⑤の当該関数の部分を見ると以下のように実装されています。
※m5stackというかArduinoではこういう構造で実装するんだなというのが分かります
中身を見ると、getAhrsData(float *pitch,float *roll,float *yaw)はMahonyAHRSupdateIMUを利用して計算しているのが分かります。
この関数は以下のコード見るとmadgwickを組み込んでフィルターした結果を出力しているようです。
【参考】
⑥M5Atom/src/utility/MahonyAHRS.cpp
⑦M5Atom/src/utility/MahonyAHRS.h
void MPU6886::getAhrsData(float *pitch,float *roll,float *yaw){
float accX = 0;
float accY = 0;
float accZ = 0;
float gyroX = 0;
float gyroY = 0;
float gyroZ = 0;
getGyroData(&gyroX,&gyroY,&gyroZ);
getAccelData(&accX,&accY,&accZ);
MahonyAHRSupdateIMU(gyroX * DEG_TO_RAD, gyroY * DEG_TO_RAD, gyroZ * DEG_TO_RAD, accX, accY, accZ,pitch,roll,yaw);
}
一方、getAttitudeは加速度からarcsinとarctanを利用して計算しています。
これだと、一定のドリフトやノイズが乗るのはやむをえないという状況です。
因みに、57.295=180/πです。
void MPU6886::getAttitude(double *pitch, double *roll)
{
float accX = 0;
float accY = 0;
float accZ = 0;
float gyroX = 0;
float gyroY = 0;
float gyroZ = 0;
getGyroData(&gyroX, &gyroY, &gyroZ);
getAccelData(&accX, &accY, &accZ);
if ((accX < 1) && (accX > -1))
{
*pitch = asin(-accX) * 57.295;
}
if (accZ != 0)
{
*roll = atan(accY / accZ) * 57.295;
}
( *pitch ) = _alpha * ( *pitch ) + (1 - _alpha) * _last_theta;
( *roll ) = _alpha * ( *roll ) + (1 - _alpha) * _last_phi;
}
これ以外に、カルマンフィルターで処理するのもよさそうですが、ここでは取り上げません。exampleとしてMPU6050のコードがあるので、同様に動くと思います。
【参考】
⑧1軸 姿勢制御モジュール の倒立に向けた準備 ーリアクションホイールへの道6ー
⑨TKJElectronics/KalmanFilter
初期化メソッドを利用
参考⑧では、以下の初期化メソッドを利用しています。
M5.IMU.SetGyroFsr(M5.IMU.GFS_500DPS);
M5.IMU.SetAccelFsr(M5.IMU.AFS_4G);
パラメーターは、初期値等は以下のとおり定義されています。それを上記のように設定変更しています。
class MPU6886 {
public:
enum Ascale {
AFS_2G = 0,
AFS_4G,
AFS_8G,
AFS_16G
};
enum Gscale {
GFS_250DPS = 0,
GFS_500DPS,
GFS_1000DPS,
GFS_2000DPS
};
Gscale Gyscale = GFS_2000DPS;
Ascale Acscale = AFS_8G;
・精度を見る
ということで、検証するコードは以下のように書けます。
一応、検証としてmadgwickfilterもコメントアウトして残している。
また、yawについては、時系列に比例するドリフトが残っており、それを改善できないかと思い、線形の寄与分を差し引くファクターをfpとしてボタン押下で算出するようにしたが、残念ながらボタンが押しにくく上手く機能していない。、
以下の元コードを見るとLED表示はおまけと考えると上記の関数を利用することにより、必要なコードは極めて短くなった。
元のコード
#include "M5Atom.h"
//#include <MadgwickAHRS.h> // 角度と角速度にMadgwickフィルタをかけるライブラリ
//Madgwick MadgwickFilter; // Madgwickフィルタのオブジェクトを設定
float roll, pitch, yaw,yaw0;
//float accX=0,accY=0, accZ = 0;
//float gyroX=0 ,gyroY=0,gyroZ = 0;
//float temp = 0;
//float r_rand = 180 / PI;
//float dt=0, st=0, fp =0.3;
void setup() {
M5.begin(true, true, true);
M5.IMU.Init();
M5.IMU.SetGyroFsr(M5.IMU.GFS_250DPS); //250,,500,1000,2000
M5.IMU.SetAccelFsr(M5.IMU.AFS_2G); //2,4,8,16
//MadgwickFilter.begin(100); // Madgwickフィルタの周波数を100Hzに設定。
//st= micros();
//update_fp();
}
/*
void update_fp(){
M5.IMU.getAhrsData(&pitch,&roll,&yaw0);
delay(10);
M5.IMU.getAhrsData(&pitch,&roll,&yaw);
dt = (micros()-st)/1000000;
fp = (yaw-yaw0)/dt;
}
*/
void loop() {
/*
if (M5.Btn.wasPressed())
{
update_fp();
}
*/
//dt = (micros()-st)/1000000;
//M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
//M5.IMU.getAccelData(&accX,&accY,&accZ);
//MadgwickFilter.updateIMU(gyroX, gyroY, gyroZ, accX-accX0, accY-accY0, accZ-accZ0);
//float pitch = MadgwickFilter.getPitch();
//float roll = MadgwickFilter.getRoll();
//float yaw = MadgwickFilter.getYaw();
M5.IMU.getAhrsData(&pitch,&roll,&yaw);
Serial.printf("%.2f,%.2f,%.2f,\n", pitch, roll, yaw); //-fp*dt-yaw0);
//LED表示
int line1 = map(pitch, -90, 90, 4, 0);
int line2 = map(roll, -90, 90, 4, 0);
int line3 = map(yaw, -180, 180, 0xf00000, 0);
if(line1 >= 0 && line1 < 5){
M5.dis.clear();
if(line2 >= 0 && line2 < 5){
M5.dis.drawpix(line1 * 5 + line2, line3); // 0xf00000);
}
}
delay(5);
M5.update();
}
必要なコード
#include "M5Atom.h"
float roll, pitch, yaw;
void setup() {
M5.begin(true, true, true);
M5.IMU.Init();
M5.IMU.SetGyroFsr(M5.IMU.GFS_250DPS); //250,,500,1000,2000
M5.IMU.SetAccelFsr(M5.IMU.AFS_2G); //2,4,8,16
}
void loop() {
M5.IMU.getAhrsData(&pitch,&roll,&yaw);
Serial.printf("%.2f,%.2f,%.2f,\n", pitch, roll, yaw);
delay(5);
M5.update();
}
・結果
結果以下のように測定できた。
上でも書いたが、pitch, rollはノイズもほとんどなく、綺麗なデータが並びかつ机を振動させると反応よく振動を表している。
一方、上でも書いたがyawはドリフトが少し入ってこれは取り除けなかった。
裏返しが正しい位置(笑) pic.twitter.com/nIb43PHyKL
— ウワン (@MuAuan) June 18, 2021
まとめ
・m5atomの内臓IMUを精度良く利用できるようになった
・getAhrsData関数は、内部でmadgwickフィルターを利用していた
・初期化も関数で簡単に出来る
・atomの関数群の構築の仕方が理解できた
・これを使って精度のよい倒立振り子を作ろう