PQ_Hybrid_Electrical_Equipment_Team
9軸センサのmpu9250を用いて，姿勢を計算するプログラムです． センサフュージョンにはMadgwick Filterというものを用いており，カルマンフィルターよりも数学的に難しくなく（でも難解），計算量も少なそうです．強い重力下や無重力の影響がどのように出るのかが今のところ不明です(余りひどすぎるということは無いと思いますが).
Dependencies: MadgwickFilter Quaternion mbed mpu9250_i2c
Diff: Hybrid_AttitudeEstimation.cpp
- Revision:
- 0:6ed89fd48f04
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Hybrid_AttitudeEstimation.cpp Sat Jan 28 19:56:35 2017 +0000
@@ -0,0 +1,72 @@
+#include "mbed.h"
+#include "mpu9250_i2c.h"
+#define NINE_SDA p9
+#define NINE_SCL p10
+
+#include "Quaternion.hpp"
+#include "MadgwickFilter.hpp"
+
+I2C i2cBus(NINE_SDA, NINE_SCL);
+mpu9250 nine(i2cBus, AD0_HIGH);
+RawSerial pc(USBTX, USBRX, 115200);
+
+const double PI = 3.1415926535897932384626433832795f;//4.0*atan(1.0);
+const double DEG_TO_RAD = 0.01745329251994329576923690768489f;//PI / 180.0;
+
+volatile bool sendFlag = false;
+
+
+const double ACC_LPF_COEF = 0.9;
+const double GYRO_LPF_COEF = 0.8;
+const double MAG_LPF_COEF = 0.9;
+
+int main() {
+
+ double imu[2][6] = {0};
+ double mag[2][3] = {0};
+ double accLPF[3] = {0};
+ double gyroLPF[3] = {0};
+ double magLPF[3] = {0};
+
+
+ pc.baud(115200);
+
+ //初期設定
+ nine.setAccLPF(NO_USE);
+ nine.setGyro(_1000DPS);
+ nine.setAcc(_16G);
+ //オフセット設定
+ nine.setOffset(0.097143593f, 3.202854768f, 0.055246519f,
+ 0.0052546f, -0.009152758f, 0.142725298f,
+ 17.925f, -27.45f, 17.025f);
+
+ //madgwick filter timer スタート
+ MadgwickFilter attitude(0.05);
+
+ while(1) {
+ //角速度と加速度，磁束密度データの取得
+ nine.getGyroAcc(imu[1]);
+ nine.getMag(mag[1]);
+ for(int i = 0;i < 3;i++){
+ gyroLPF[i] = GYRO_LPF_COEF * imu[0][i] + (1 - GYRO_LPF_COEF) * imu[1][i];
+ accLPF[i] = ACC_LPF_COEF*imu[0][i+3] + (1 - ACC_LPF_COEF) * imu[1][i+3];
+ magLPF[i] = MAG_LPF_COEF*mag[0][i] + (1 - MAG_LPF_COEF) * mag[1][i];
+
+ imu[0][i] = imu[1][i];
+ imu[0][i+3] = imu[1][i+3];
+ mag[0][i] = mag[1][i];
+ }
+
+ //更新
+ 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]);
+ sendFlag = false;
+
+ //姿勢取得 with Quaternion
+ Quaternion myQ;
+ attitude.getAttitude(&myQ);
+
+ //描画 for unity
+ pc.printf("Attitude:%f,%f,%f,%f\r\n", myQ.x, myQ.y, myQ.z, myQ.w); //for unity
+ //pc.printf("Attitude:%f,%f,%f,%f\r\n", attitude.q1, attitude.q2, attitude.q3,attitude.q0); //for unity
+ }
+}