PQ_Hybrid_Electrical_Equipment_Team / Mbed 2 deprecated Hybrid_AttitudeEstimation

9軸センサのmpu9250を用いて，姿勢を計算するプログラムです． センサフュージョンにはMadgwick Filterというものを用いており，カルマンフィルターよりも数学的に難しくなく（でも難解），計算量も少なそうです．強い重力下や無重力の影響がどのように出るのかが今のところ不明です(余りひどすぎるということは無いと思いますが).

Dependencies:   MadgwickFilter Quaternion mbed mpu9250_i2c

Hybrid_AttitudeEstimation.cpp@1:5d0e86cdc791, 2017-01-28 (annotated)

Committer:
Gaku0606
Date:
Sat Jan 28 21:28:44 2017 +0000
Revision:
1:5d0e86cdc791
Parent:
0:6ed89fd48f04 
doxygen??????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Gaku0606 0:6ed89fd48f04 1 #include "mbed.h"
Gaku0606 0:6ed89fd48f04 2 #include "mpu9250_i2c.h"
Gaku0606 0:6ed89fd48f04 3 #define NINE_SDA p9
Gaku0606 0:6ed89fd48f04 4 #define NINE_SCL p10
Gaku0606 0:6ed89fd48f04 5
Gaku0606 0:6ed89fd48f04 6 #include "Quaternion.hpp"
Gaku0606 0:6ed89fd48f04 7 #include "MadgwickFilter.hpp"
Gaku0606 0:6ed89fd48f04 8
Gaku0606 0:6ed89fd48f04 9 I2C i2cBus(NINE_SDA, NINE_SCL);
Gaku0606 0:6ed89fd48f04 10 mpu9250 nine(i2cBus, AD0_HIGH);
Gaku0606 0:6ed89fd48f04 11 RawSerial pc(USBTX, USBRX, 115200);
Gaku0606 0:6ed89fd48f04 12
Gaku0606 0:6ed89fd48f04 13 const double PI = 3.1415926535897932384626433832795f;//4.0*atan(1.0);
Gaku0606 0:6ed89fd48f04 14 const double DEG_TO_RAD = 0.01745329251994329576923690768489f;//PI / 180.0;
Gaku0606 0:6ed89fd48f04 15
Gaku0606 0:6ed89fd48f04 16 volatile bool sendFlag = false;
Gaku0606 0:6ed89fd48f04 17
Gaku0606 0:6ed89fd48f04 18
Gaku0606 0:6ed89fd48f04 19 const double ACC_LPF_COEF = 0.9;
Gaku0606 0:6ed89fd48f04 20 const double GYRO_LPF_COEF = 0.8;
Gaku0606 0:6ed89fd48f04 21 const double MAG_LPF_COEF = 0.9;
Gaku0606 0:6ed89fd48f04 22
Gaku0606 0:6ed89fd48f04 23 int main() {
Gaku0606 0:6ed89fd48f04 24
Gaku0606 0:6ed89fd48f04 25 double imu[2][6] = {0};
Gaku0606 0:6ed89fd48f04 26 double mag[2][3] = {0};
Gaku0606 0:6ed89fd48f04 27 double accLPF[3] = {0};
Gaku0606 0:6ed89fd48f04 28 double gyroLPF[3] = {0};
Gaku0606 0:6ed89fd48f04 29 double magLPF[3] = {0};
Gaku0606 0:6ed89fd48f04 30
Gaku0606 0:6ed89fd48f04 31
Gaku0606 0:6ed89fd48f04 32 pc.baud(115200);
Gaku0606 0:6ed89fd48f04 33
Gaku0606 0:6ed89fd48f04 34 //初期設定
Gaku0606 0:6ed89fd48f04 35 nine.setAccLPF(NO_USE);
Gaku0606 0:6ed89fd48f04 36 nine.setGyro(_1000DPS);
Gaku0606 0:6ed89fd48f04 37 nine.setAcc(_16G);
Gaku0606 0:6ed89fd48f04 38 //オフセット設定
Gaku0606 0:6ed89fd48f04 39 nine.setOffset(0.097143593f, 3.202854768f, 0.055246519f,
Gaku0606 0:6ed89fd48f04 40 0.0052546f, -0.009152758f, 0.142725298f,
Gaku0606 0:6ed89fd48f04 41 17.925f, -27.45f, 17.025f);
Gaku0606 0:6ed89fd48f04 42
Gaku0606 0:6ed89fd48f04 43 //madgwick filter timer スタート
Gaku0606 0:6ed89fd48f04 44 MadgwickFilter attitude(0.05);
Gaku0606 0:6ed89fd48f04 45
Gaku0606 0:6ed89fd48f04 46 while(1) {
Gaku0606 0:6ed89fd48f04 47 //角速度と加速度，磁束密度データの取得
Gaku0606 0:6ed89fd48f04 48 nine.getGyroAcc(imu[1]);
Gaku0606 0:6ed89fd48f04 49 nine.getMag(mag[1]);
Gaku0606 0:6ed89fd48f04 50 for(int i = 0;i < 3;i++){
Gaku0606 0:6ed89fd48f04 51 gyroLPF[i] = GYRO_LPF_COEF * imu[0][i] + (1 - GYRO_LPF_COEF) * imu[1][i];
Gaku0606 0:6ed89fd48f04 52 accLPF[i] = ACC_LPF_COEF*imu[0][i+3] + (1 - ACC_LPF_COEF) * imu[1][i+3];
Gaku0606 0:6ed89fd48f04 53 magLPF[i] = MAG_LPF_COEF*mag[0][i] + (1 - MAG_LPF_COEF) * mag[1][i];
Gaku0606 0:6ed89fd48f04 54
Gaku0606 0:6ed89fd48f04 55 imu[0][i] = imu[1][i];
Gaku0606 0:6ed89fd48f04 56 imu[0][i+3] = imu[1][i+3];
Gaku0606 0:6ed89fd48f04 57 mag[0][i] = mag[1][i];
Gaku0606 0:6ed89fd48f04 58 }
Gaku0606 0:6ed89fd48f04 59
Gaku0606 0:6ed89fd48f04 60 //更新
Gaku0606 0:6ed89fd48f04 61 attitude.MadgwickAHRSupdate(gyroLPF[0]*DEG_TO_RAD, gyroLPF[1]*DEG_TO_RAD, gyroLPF[2]*DEG_TO_RAD, accLPF[0], accLPF[1], accLPF[2], magLPF[0], magLPF[1], magLPF[2]);
Gaku0606 0:6ed89fd48f04 62 sendFlag = false;
Gaku0606 0:6ed89fd48f04 63
Gaku0606 0:6ed89fd48f04 64 //姿勢取得 with Quaternion
Gaku0606 0:6ed89fd48f04 65 Quaternion myQ;
Gaku0606 0:6ed89fd48f04 66 attitude.getAttitude(&myQ);
Gaku0606 0:6ed89fd48f04 67
Gaku0606 0:6ed89fd48f04 68 //描画 for unity
Gaku0606 0:6ed89fd48f04 69 pc.printf("Attitude:%f,%f,%f,%f\r\n", myQ.x, myQ.y, myQ.z, myQ.w); //for unity
Gaku0606 0:6ed89fd48f04 70 //pc.printf("Attitude:%f,%f,%f,%f\r\n", attitude.q1, attitude.q2, attitude.q3,attitude.q0); //for unity
Gaku0606 0:6ed89fd48f04 71 }
Gaku0606 0:6ed89fd48f04 72 }