![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ICM20948+madgwickフィルター 地磁気は不使用で6軸のみ使用 最初5s静置してジャイロのオフセットを求めキャリブレーション 地磁気を測らないためヨー軸ドリフトがあるが,緩やかに動かせば2,3分程度はヨー軸も正しい値を測定できた
Dependencies: mbed MadgwickFilter
Diff: main.cpp
- Revision:
- 0:ce9707156696
diff -r 000000000000 -r ce9707156696 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Oct 18 13:41:49 2021 +0000 @@ -0,0 +1,34 @@ +#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); + } +}