Ken Hira / Mbed 2 deprecated ICM20948

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

Dependencies:   mbed MadgwickFilter

main.cpp@0:ce9707156696, 2021-10-18 (annotated)

Committer:
hiramitsu
Date:
Mon Oct 18 13:41:49 2021 +0000
Revision:
0:ce9707156696
ICM20948 (use acce and gyro, omit mag)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hiramitsu 0:ce9707156696 1 #include "mbed.h"
hiramitsu 0:ce9707156696 2 #include "ICM20948.hpp"
hiramitsu 0:ce9707156696 3 #include "MadgwickFilter.hpp"
hiramitsu 0:ce9707156696 4
hiramitsu 0:ce9707156696 5 Serial pc(USBTX, USBRX);
hiramitsu 0:ce9707156696 6
hiramitsu 0:ce9707156696 7 int main()
hiramitsu 0:ce9707156696 8 {
hiramitsu 0:ce9707156696 9 pc.baud(115200);
hiramitsu 0:ce9707156696 10 ICM20948 imu;
hiramitsu 0:ce9707156696 11 MadgwickFilter madgwick = MadgwickFilter();
hiramitsu 0:ce9707156696 12
hiramitsu 0:ce9707156696 13 imu.whoAmI();
hiramitsu 0:ce9707156696 14 imu.init();
hiramitsu 0:ce9707156696 15 float acc[3];
hiramitsu 0:ce9707156696 16 float gyro[3];
hiramitsu 0:ce9707156696 17 float eulerVals[3];
hiramitsu 0:ce9707156696 18 int count = 0;
hiramitsu 0:ce9707156696 19
hiramitsu 0:ce9707156696 20 while(1) {
hiramitsu 0:ce9707156696 21 //imu.getAccGyro(acc, gyro);
hiramitsu 0:ce9707156696 22 //pc.printf("Acc: %f %f %f, Gyro: %f %f %f\n\r", acc[0], acc[1], acc[2], gyro[0], gyro[1], gyro[2]);
hiramitsu 0:ce9707156696 23 //wait_ms(500);
hiramitsu 0:ce9707156696 24 imu.getAccGyro(acc, gyro);
hiramitsu 0:ce9707156696 25 madgwick.MadgwickAHRSupdateIMU(gyro[0], gyro[1], gyro[2], acc[0], acc[1], acc[2]);
hiramitsu 0:ce9707156696 26 count++;
hiramitsu 0:ce9707156696 27 if(count == 100){
hiramitsu 0:ce9707156696 28 madgwick.getEulerAngle(eulerVals);
hiramitsu 0:ce9707156696 29 pc.printf("r: %f, p: %f, y: %f\n\r", eulerVals[0], eulerVals[1], eulerVals[2]);
hiramitsu 0:ce9707156696 30 count = 0;
hiramitsu 0:ce9707156696 31 }
hiramitsu 0:ce9707156696 32 wait_ms(10);
hiramitsu 0:ce9707156696 33 }
hiramitsu 0:ce9707156696 34 }