PQ_Hybrid_Electrical_Equipment_Team / Mbed 2 deprecated Hybrid_AttitudeEstimation

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

Dependencies:   MadgwickFilter Quaternion mbed mpu9250_i2c

Hybrid_AttitudeEstimation.cpp

Committer:
Gaku0606
Date:
2017-01-28
Revision:
1:5d0e86cdc791
Parent:
0:6ed89fd48f04

File content as of revision 1:5d0e86cdc791:

#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
    }
}