Ken Hira / Mbed 2 deprecated ICM20948

ICM20948+madgwickフィルター 地磁気は不使用で6軸のみ使用 最初5s静置してジャイロのオフセットを求めキャリブレーション 地磁気を測らないためヨー軸ドリフトがあるが，緩やかに動かせば2,3分程度はヨー軸も正しい値を測定できた

Dependencies:   mbed MadgwickFilter

main.cpp

Committer:
hiramitsu
Date:
2021-10-18
Revision:
0:ce9707156696

File content as of revision 0:ce9707156696:

#include "mbed.h"
#include "ICM20948.hpp"
#include "MadgwickFilter.hpp"

Serial pc(USBTX, USBRX);

int main()
{
    pc.baud(115200);
    ICM20948 imu;
    MadgwickFilter madgwick = MadgwickFilter();
    
    imu.whoAmI();
    imu.init();
    float acc[3];
    float gyro[3];
    float eulerVals[3];
    int count = 0;

    while(1) {
        //imu.getAccGyro(acc, gyro);
        //pc.printf("Acc: %f %f %f, Gyro: %f %f %f\n\r", acc[0], acc[1], acc[2], gyro[0], gyro[1], gyro[2]);
        //wait_ms(500);
        imu.getAccGyro(acc, gyro);
        madgwick.MadgwickAHRSupdateIMU(gyro[0], gyro[1], gyro[2], acc[0], acc[1], acc[2]);
        count++;
        if(count == 100){
            madgwick.getEulerAngle(eulerVals);
            pc.printf("r: %f, p: %f, y: %f\n\r", eulerVals[0], eulerVals[1], eulerVals[2]);
            count = 0;
        }
        wait_ms(10);
    }
}