File content as of revision 9:6d4578dcc1ed:

#include "mbed.h"
#include "MPU6050.h"
#include "HMC5883L.h"
#include "LPS25H.h"
#include "GMS6_CR6.h"
#include "ErrorLogger.h"
#include "Vector.h"
#include "Matrix.h"
#include "Vector_Matrix_operator.h"
#include "myConstants.h"
#include "Log.h"

/********** private define    **********/
#define TRUE    1
#define FALSE   0

#define x       1
#define y       2
#define z       3

const float dt = 0.1f;              // 割り込み周期(s)
const float ServoMax = 0.0023f;     // サーボの最大パルス長(s)
const float ServoMin = 0.0006f;     // サーボの最小パルス長(s)
const float PullMax  = 25.0f;       // 引っ張れる紐の最大量(mm)
/********** private macro     **********/

/********** private typedef   **********/

/********** private variables **********/
DigitalOut myled(LED1);                 // デバッグ用LEDのためのデジタル出力
I2C         i2c(PB_9, PB_8);            // I2Cポート
MPU6050     mpu(&i2c);                  // 加速度・角速度センサ
HMC5883L    hmc(&i2c);                  // 地磁気センサ
LPS25H      lps(&i2c);                  // 気圧センサ
Serial      gps(PA_11, PA_12);          // GPS通信用シリアルポート
Serial      pc(SERIAL_TX, SERIAL_RX);   // PC通信用シリアルポート
GMS6_CR6    gms(&gps, &pc);             // GPS
Log         logger(PB_5, PB_4, PB_3, PB_10, PA_9, PA_10, PC_1);    // ロガー（microSD、XBee）
PwmOut      servoL(PB_6), servoR(PC_7); // サーボ用PWM出力
AnalogIn    rf(PC_0);                   // 照度センサ用アナログ入力
AnalogIn    servoVcc(PA_0);             // バッテリー電圧監視用アナログ入力(サーボ用)
AnalogIn    logicVcc(PA_1);             // バッテリー電圧監視用アナログ入力(ロジック用)
Ticker      INT_timer;                  // 割り込みタイマー

int lps_cnt = 0;                // 気圧センサ読み取りカウント
uint8_t INT_flag = TRUE;        // 割り込み可否フラグ
Vector raw_acc(3);              // 加速度(m/s^2)  生
Vector raw_gyro(3);             // 角速度(deg/s)  生
Vector raw_geomag(3);           // 地磁気(?)  生
float raw_press;                // 気圧(hPa)  生
Vector acc(3);                  // 加速度(m/s^2)
Vector gyro(3);                 // 角速度(deg/s)
Vector geomag(3);               // 地磁気(?)
float press;                    // 気圧(hPa)

Vector raw_g(3);                // 重力ベクトル  生
Vector g(3);                    // 重力ベクトル
Vector p(2);                    // 位置情報(経度, 緯度)
Vector pre_p(2);                // 過去の位置情報(経度, 緯度)
int UTC_t = 0;                  // UTC時刻
int pre_UTC_t = 0;              // 前のUTC時刻
//Vector n(3);                  // 地磁気ベクトル

Vector b_f(3);                  // 機体座標に固定された、機体前方向きのベクトル（x軸）
Vector b_u(3);                  // 機体座標に固定された、機体上方向きのベクトル（z軸）
Vector b_l(3);                  // 機体座標に固定された、機体左方向きのベクトル（y軸）

Vector r_f(3);                  // 参照座標に固定された、北向きのベクトル（X軸）
Vector r_u(3);                  // 参照座標に固定された、上向きのベクトル（Z軸）
Vector r_l(3);                  // 参照座標に固定された、西向きのベクトル（Y軸）

int pull_L = 0;                 // 左サーボの引っ張り量(mm：0~PullMax)
int pull_R = 0;                 // 右サーボの引っ張り量(mm：0~PullMax)

float yaw = 0.0f;               // 本体のヨー角（deg）
float pitch = 0.0f;             // 本体のピッチ角（deg）
float roll = 0.0f;              // 本体のロール角（deg）

/* ----- Kalman Filter ----- */
Vector pri_x(6);
Matrix pri_P(6, 6);
Vector post_x(6);
Matrix post_P(6, 6);
Matrix F(6, 6), H(3, 6);
Matrix R(6, 6), Q(3, 3);
Matrix I(6, 6);
Matrix K(6, 3);
Matrix S(3, 3), inv(3, 3);
/* ----- ------------- ----- */

Timer timer;

char data[512] = {};

/********** private functions **********/
void KalmanInit();                      // カルマンフィルタ初期化
void KalmanUpdate();                    // カルマンフィルタ更新
float distance(Vector p1, Vector p2);   // 緯度・経度の差から2点間の距離を算出(m)
void INT_func();                        // 割り込み用関数
void toString(Matrix& m);
void toString(Vector& v);

/********** main function     **********/

int main() {
    
    i2c.frequency(400000);          // I2Cの通信速度を400kHzに設定
    
    if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!");        // mpu6050初期化
    if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!");       // hmc5883l初期化
    
    char* title = "log data\r\n";                                       // ログのタイトル行
    logger.initialize_sdlog(title);                                     // ログファイル初期設定
    
    servoL.period(0.020f);                                              // サーボの信号の周期は20ms
    servoR.period(0.020f);
    
    KalmanInit();
    
    INT_timer.attach(&INT_func, dt);  // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
    
    //重力ベクトルの初期化
    raw_g.SetComp(1, 0.0f);
    raw_g.SetComp(2, 0.0f);
    raw_g.SetComp(3, 1.0f);
    
    // 機体に固定されたベクトルの初期化
    b_f.SetComp(1, 0.0f);
    b_f.SetComp(2, 0.0f);
    b_f.SetComp(3, -1.0f);
    b_u.SetComp(1, 1.0f);
    b_u.SetComp(2, 0.0f);
    b_u.SetComp(3, 0.0f);
    b_l = Cross(b_u, b_f);
    
    /* ---------- ↓↓↓ ここからメインループ ↓↓↓ ---------- */
    while(1) {
        timer.stop();
        timer.reset();
        timer.start();
        // 0.1秒おきにセンサーの出力をpcへ出力
        myled = 1; // LED is ON
        
        INT_flag = FALSE;           // 割り込みによる変数書き換えを阻止
        
        // データ処理
        {
            gms.read();                                     // GPSデータ取得
            UTC_t = (int)gms.time;
            
            // 参照座標系の基底を求める
            r_u = g;
            r_f = geomag.GetPerpCompTo(g).Normalize();
            r_l = Cross(r_u, r_f);
            
            // 回転行列を経由してオイラー角を求める
            // オイラー角はヨー・ピッチ・ロールの順に回転したものとする
            // 各オイラー角を求めるのに回転行列の全成分は要らないので、一部だけ計算する。
            
            float R_11 = r_f * b_f;             // 回転行列の(1,1)成分
            float R_12 = r_f * b_l;             // 回転行列の(1,2)成分
            float R_13 = r_f * b_u;             // 回転行列の(1,3)成分
            float R_23 = r_l * b_u;             // 回転行列の(2,3)成分
            float R_33 = r_u * b_u;             // 回転行列の(3,3)成分
            
            yaw = atan2(R_11, -R_12) * RAD_TO_DEG;
            roll = atan2(R_33, -R_23) * RAD_TO_DEG;
            pitch = atan2(R_11/cos(yaw), R_13) * RAD_TO_DEG;
            
            if(UTC_t - pre_UTC_t >= 1) {                    // GPSデータが更新されていたら
                p.SetComp(1, gms.longitude * DEG_TO_RAD);
                p.SetComp(2, gms.latitude * DEG_TO_RAD);  
            } else {                                        // 更新されていなかったら
                
            }
            
            pre_p = p;
            pre_UTC_t = UTC_t;
        }
        
        float sv = (float)servoVcc.read_u16() * ADC_LSB_TO_V * 2.0f;
        float lv = (float)logicVcc.read_u16() * ADC_LSB_TO_V * 2.0f;
        
        sprintf(data, "%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%d\r\n", 
                g.GetComp(1), g.GetComp(2), g.GetComp(3), 
                geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3), 
                press, gms.longitude, gms.latitude, 
                sv, lv, rf.read_u16());
        logger.write(data);
        
        INT_flag = TRUE;            // 割り込み許可
        
        // 制御ルーチン
        {
            //pull_L = (pull_L+5)%30;
            //pull_R = (pull_R+5)%30;
            pull_L = 0;
            pull_R = 30;
            servoL.pulsewidth((ServoMax - ServoMin) * pull_L / PullMax + ServoMin);
            servoR.pulsewidth((ServoMax - ServoMin) * pull_R / PullMax + ServoMin);
            
        }
        
        myled = 0; // LED is OFF
        
        // ループはきっかり0.2秒ごと
        while(timer.read_ms() < 200);
        
        pc.printf("time: %.3f[s]\r\n", (float)timer.read_ms()/1000.0f);
        
    }
    
    /* ---------- ↑↑↑ ここまでメインループ ↑↑↑ ---------- */
}

void KalmanInit() {
    
    // 誤差共分散行列の値を決める（対角成分のみ）
    float alpha_R = 60.0f;
    float alpha_Q = 100.0f;
    R *= alpha_R;
    Q *= alpha_Q;
    
    // 状態方程式のヤコビアンの初期値を代入（時間変化あり）
    float f[36] = {
        1.0f, raw_gyro.GetComp(3)*dt, -raw_gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f, 
        -raw_gyro.GetComp(3)*dt, 1.0f, raw_gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f, 
        raw_gyro.GetComp(2)*dt, -raw_gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f, 
        0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 
        0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,  
        0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
    };
    
    F.SetComps(f);
    
    // 観測方程式のヤコビアンの値を設定（時間変化無し）
    float h[18] = {
        1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,  
        0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f,  
        0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f
    };
    
    H.SetComps(h);
}

void KalmanUpdate() {
    // ヤコビアンの更新
    float f[36] = {
        1.0f, raw_gyro.GetComp(3)*dt, -raw_gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f, 
        -raw_gyro.GetComp(3)*dt, 1.0f, raw_gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f, 
        raw_gyro.GetComp(2)*dt, -raw_gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f, 
        0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 
        0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,  
        0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
    };
    
    F.SetComps(f);
    
    // 事前推定値の更新
    pri_x = F * post_x;
    // 事前誤差分散行列の更新
    pri_P = F * post_P * F.Transpose() + R;
    
    // カルマンゲインの計算
    S = Q + H * pri_P * H.Transpose();
    float det;
    if((det = S.Inverse(inv)) >= 0.0f) {
        pc.printf("E:%.3f\r\n", det);
        return;     // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了
    }
    K = pri_P * H.Transpose() * inv;    
    
    // 事後推定値の更新
    post_x = pri_x + K * (raw_geomag - H * pri_x);
    // 事後誤差分散行列の更新
    post_P = (I - K * H) * pri_P;
}

float distance(Vector p1, Vector p2) {
    if(p1.GetDim() != p2.GetDim()) return 0.0f;
    
    float mu_y = (p1.GetComp(2) + p2.GetComp(2)) * 0.5f;
    float s_mu_y = sin(mu_y);
    float w = sqrt(1 - GPS_SQ_E * s_mu_y * s_mu_y);
    float m = GPS_A * (1 - GPS_SQ_E) / (w * w * w);
    float n = GPS_A / w;
    float d1 = m * (p1.GetComp(2) - p2.GetComp(2));
    float d2 = n * cos(mu_y) * (p1.GetComp(1) - p2.GetComp(1));
    
    return sqrt(d1 * d1 + d2 * d2);
}

/* -------------------- 割り込み関数 -------------------- */

void INT_func() {
    // センサーの値を更新
    mpu.read();
    hmc.read();
    
    for(int i=0; i<3; i++) {
        raw_acc.SetComp(i+1, (float)mpu.data.value.acc[i] * ACC_LSB_TO_G);
        raw_gyro.SetComp(i+1, (float)mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD);
        raw_geomag.SetComp(i+1, (float)hmc.data.value[i] * MAG_LSB_TO_GAUSS);
    }
        
    Vector delta_g = Cross(raw_gyro, raw_g);                            // Δg = ω × g
    raw_g = 0.9f * (raw_g - delta_g * dt) + 0.1f * raw_acc.Normalize();   // 相補フィルタ
    raw_g = raw_g.Normalize();
       
    KalmanUpdate();
        
    // LPS25Hによる気圧の取得は10Hz
    lps_cnt = (lps_cnt+1)%10;
    if(lps_cnt == 0) {
        raw_press = (float)lps.pressure() * PRES_LSB_TO_HPA;
    }
    //pc.printf("%d(us)\r\n", timer.read_us());
    
    if(INT_flag != FALSE) {
        g = raw_g;
        for(int i=0; i<3; i++) {
            geomag.SetComp(i+1, post_x.GetComp(i+1));
        }
        acc = raw_acc;
        gyro = raw_gyro;
        press = raw_press;
        
    }
}

/* -------------------- デバッグ用関数 -------------------- */

void toString(Matrix& m) {
    
    for(int i=0; i<m.GetRow(); i++) {
        for(int j=0; j<m.GetCol(); j++) {
            pc.printf("%.6f\t", m.GetComp(i+1, j+1));
        }
        pc.printf("\r\n");
    }
    
}

void toString(Vector& v) {
    
    for(int i=0; i<v.GetDim(); i++) {
        pc.printf("%.6f\t", v.GetComp(i+1));
    }
    pc.printf("\r\n");
    
}