File content as of revision 29:59f4808e2eb6:

#include "mbed.h"
#include "MPU6050.h"
#include "HMC5883L.h"
#include "LPS25H.h"
#include "GMS6_CR6.h"
#include "Vector.h"
#include "Matrix.h"
#include "Vector_Matrix_operator.h"
#include "myConstants.h"
#include "SDFileSystem.h"
#include "BufferedSerial.h"
#include "ConfigFile.h"

/****************************** private define    ******************************/
//#define RULE1
//#define RULE2
//#define RULE3
//#define RULE4
//#define SERVO_DEBUG
#define DIRECTION_DEBUG
//#define SPIRAL_DEBUG

#ifdef DIRECTION_DEBUG
const float TargetDirection = -90.0f;        // 真東に飛ぶ
#endif
const float dt              = 0.01f;        // 割り込み周期(s)
const float ServoMax        = 0.0046f;      // サーボの最大パルス長(s)
const float ServoMin        = 0.0012f;      // サーボの最小パルス長(s)
const float PullMax         = 25;           // 引っ張れる紐の最大量(mm)
const float BorderSpiral    = 40.0f;        // スパイラル検知角度
const short BorderOpt       = 30000;        // 光センサーの閾値
const float BorderGravity   = 0.3f;         // 無重力状態の閾値
const int   BorderParafoil  = 0;            // 物理スイッチのOFF出力
const int   MaxCount        = 3;            // 投下シグナルを何回連続で検知したら投下と判断するか（×0.2[s]）
const int   WaitTime        = 1;            // 投下後、安定するまで何秒滑空するか
const float Alpha           = 30.0f;        // 目標方向と自分の進行方向との差の閾値(deg)（制御則1&2&3の定数
const float Beta            = 60.0f;        // 目標方向と自分の進行方向との間に取るべき角度差(deg)（制御則3の定数
const float BorderDistance  = 5.0f;         // 落下制御に入るための目標値との距離の閾値(m)（制御則2の定数
const float BorderHeight    = 10.0f;        // 制御則3と2を切り替える高度の閾値(m)

enum Direction {LEFT, RIGHT, NEUTRAL};

/****************************** 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
SDFileSystem    sd(PB_5, PB_4, PB_3, PB_10, "sd");  // microSD
FILE *          fp;                                 // ログファイルのポインタ
BufferedSerial  xbee(PA_9, PA_10, PC_1, 256, 16);   // Xbee
ConfigFile      cfg;                                // ConfigFile
PwmOut          servoL(PB_6), servoR(PC_7);         // サーボ用PWM出力
AnalogIn        optSensor(PC_0);                    // 照度センサ用アナログ入力
AnalogIn        servoVcc(PA_1);                     // バッテリー電圧監視用アナログ入力(サーボ用)
AnalogIn        logicVcc(PA_0);                     // バッテリー電圧監視用アナログ入力(ロジック用)
DigitalIn       paraSensor(PB_0);                   // パラフォイルに繋がる（予定）の物理スイッチ
Ticker          ticker;                             // 割り込みタイマー
Timer           timer;                              // 時間計測用タイマー
Direction       dir = NEUTRAL;                      // 旋回方向

int     lps_cnt = 0;            // 気圧センサ読み取りカウント
bool    INT_flag = true;        // 割り込み可否フラグ
/* こちらの変数群はメインループでは参照しない */
Vector  raw_acc(3);             // 加速度(m/s^2)  生
Vector  raw_gyro(3);            // 角速度(deg/s)  生
Vector  raw_geomag(3);          // 地磁気(?)  生
float   raw_press;              // 気圧(hPa)  生
float   raw_temp;               // 温度(℃)　生
/* メインループ内ではこちらを参照する */
Vector  acc(3);                 // 加速度(m/s^2)
Vector  gyro(3);                // 角速度(deg/s)
Vector  geomag(3);              // 地磁気(?)
float   p0;                     // 気圧の初期値
float   press;                  // 気圧(hPa)
float   temp;                   // 温度(℃)
float   height;                 // 高さ(m)

Vector  raw_g(3);               // 重力ベクトル  生
Vector  g(3);                   // 重力ベクトル
Vector  target_p(2);            // 目標情報（経度、緯度）（rad）
Vector  p(2);                   // 現在の位置情報(補完含む)(経度, 緯度)（rad）
Vector  new_p(2);               // 最新の位置情報(経度, 緯度)（rad）
Vector  pre_p(2);               // 過去の位置情報(経度, 緯度)（rad）
int     UTC_t = 0;              // UTC時刻
int     pre_UTC_t = 0;          // 前のUTC時刻
int     ss = 0;              // 時刻の秒数の小数部分

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）z軸周り
float   pitch = 0.0f;           // 本体のピッチ角（deg）y軸周り
float   roll = 0.0f;            // 本体のロール角（deg）x軸周り

float   vrt_acc = 0.0f;         // 鉛直方向の加速度成分（落下検知に使用）

int     step = 0;               // シーケンス制御のステップ
char    data[512] = {};         // 送信データ用配
int     loopTime = 0;           // 1ループに掛かる時間（デバッグ用
float   sv = 0.0f;              // サーボ電源電圧
float   lv = 0.0f;              // ロジック電源電圧

/** config.txt **
* #から始めるのはコメント行
* #イコールの前後に空白を入れない
* target_x=111.222
* target_y=33.444
*/
float target_x, target_y;

/* ---------- Kalman Filter ---------- */
// 地磁気ベクトル用
// ジャイロのz軸周りのバイアスも推定
Vector pri_x1(7);
Matrix pri_P1(7, 7);
Vector post_x1(7);
Matrix post_P1(7, 7);
Matrix F1(7, 7), H1(3, 7);
Matrix R1(7, 7), Q1(3, 3);
Matrix I1(7, 7);
Matrix K1(7, 3);
Matrix S1(3, 3), S_inv1(3, 3);

// 重力ベクトル用
// ジャイロのx軸、y軸周りのバイアスも同時に推定
Vector pri_x2(5);
Matrix pri_P2(5, 5);
Vector post_x2(5);
Matrix post_P2(5, 5);
Matrix F2(5, 5), H2(3, 5);
Matrix R2(5, 5), Q2(3, 3);
Matrix I2(5, 5);
Matrix K2(5, 3);
Matrix S2(3, 3), S_inv2(3, 3);
/* ---------- ------------- ---------- */


/****************************** private functions ******************************/
bool    SD_Init();                          // SDカード初期化
void    SensorsInit();                      // 各種センサーの初期化
void    KalmanInit();                       // カルマンフィルタ初期化
bool    LoadConfig();                       // config読み取り
int     Find_last();                        // SDカード初期化用関数
void    DataProcessing();                   // データ処理関数
void    ControlRoutine();                   // 制御ルーチン関数
void    KalmanUpdate();                     // カルマンフィルタ更新
float   Distance(Vector p1, Vector p2);     // 緯度・経度の差から2点間の距離を算出(m)
bool    Thrown();                           // 投下されたかどうかを判断する
void    DataUpdate();                       // データ取得&更新関数
void    toString(Matrix& m);                // デバッグ用
void    toString(Vector& v);                // デバッグ用

/** 小さい値を返す関数
 * @param   a: 1つ目の入力値
 * @param   b: 2つ目の入力値
 * 
 * @return  a,bのうち、小さい方の値
 */
inline float min(float a, float b)
{
    return ((a > b) ? b : a);
}

/** 気圧の変化から高度を計算する関数
 * @param   press: 現在の気圧
 * 
 * @return  height: 現在の基準点からの高度
 */
inline float Height(float press) {
    return (153.8f * (pow((p0/press),0.1902f)-1.0f)*(25.0f+273.15f));
}

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

int main()
{

    i2c.frequency(400000);                  // I2Cの通信速度を400kHzに設定
    
    //Config読み取り
    xbee.printf("load...");
    while(!LoadConfig()) {
        wait(0.1f);
        xbee.printf(".");
    }
    xbee.printf("complete\r\n");
    xbee.printf("target(%.5f, %.5f)\r\n", target_x, target_y);
    
    // SDカード初期化
    xbee.printf("SD Init...");
    while(!SD_Init()) {
        wait(0.1f);
        xbee.printf(".");
    }
    xbee.printf("complete\r\n");
    
    // センサー関連の初期化
    xbee.printf("Sensors Init...");
    SensorsInit();
    xbee.printf("complete\r\n");

    //カルマンフィルタ初期化
    KalmanInit();

    NVIC_SetPriority(TIM5_IRQn, 5);
    ticker.attach(&DataUpdate, dt);         // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
    
    servoL.period(0.020f);                  // サーボの信号の周期は20ms
    servoR.period(0.020f);


    /* ------------------------------ ↓↓↓ ここからメインループ ↓↓↓ ------------------------------ */
    while(1) {
        timer.stop();
        timer.reset();
        timer.start();
        myled = 1;                                          // LED is ON
        
        INT_flag = false;                               // 割り込みによる変数書き換えを阻止
        /******************************* データ処理 *******************************/
        DataProcessing();

        /******************************* 制御ルーチン *******************************/
#ifdef SERVO_DEBUG
        if(xbee.readable()) {
            char cmd = xbee.getc();
            if(cmd == 'w') {
                pull_R += 5;
            } else if(cmd == 's'){
                pull_R -= 5;
            } else if(cmd == 'a'){
                pull_L += 5;
            } else if(cmd == 'd'){
                pull_L -= 5;
            } 
        }
#else
        ControlRoutine();
#endif
            
        sv = (float)servoVcc.read_u16() * ADC_LSB_TO_V * 2.0f;    // サーボ電源電圧
        lv = (float)logicVcc.read_u16() * ADC_LSB_TO_V * 2.0f;    // ロジック電源電圧
        
        // 指定された引っ張り量だけ紐を引っ張る
        if(pull_L < 0) pull_L = 0;
        else if(pull_L > PullMax) pull_L = PullMax;
        if(pull_R < 0) pull_R = 0;
        else if(pull_R > PullMax) pull_R = PullMax;
    
        servoL.pulsewidth((ServoMax - ServoMin) * (PullMax - pull_L) / (float)PullMax + ServoMin);
        servoR.pulsewidth((ServoMax - ServoMin) * pull_R / (float)PullMax + ServoMin);
        
        // データをmicroSDに保存し、XBeeでPCへ送信する
        sprintf(data, "%d.%d, %.1f,%.1f,%.1f, %.3f,%.6f,%.6f, %.3f,%.3f,%.1f, %d, %d,%d\r\n",
                UTC_t, ss, yaw, pitch, roll,
                press, gms.longitude, gms.latitude,
                vrt_acc, height, Distance(target_p, p), 
                optSensor.read_u16(), pull_R, pull_L);
        
        INT_flag = true;            // 割り込み許可
        
        fprintf(fp, data);
        fflush(fp);
        xbee.printf(data);       
        

        myled = 0; // LED is OFF

        loopTime = timer.read_ms();

        // ループはきっかり0.2秒ごと
        while(timer.read_ms() < 200);

    }

    /* ------------------------------ ↑↑↑ ここまでメインループ ↑↑↑ ------------------------------ */
}

/** データ処理関数
 *
 */
void DataProcessing()
{
    static float R_11;                              // 回転行列(1,1)成分
    static float R_12;                              // 回転行列(1,2)成分
    static float r_cos;                             // ロール角のcos値
    static float r_sin;                             // ロール角のsin値
    static float p_cos;                             // ピッチ角のcos値
    static float p_sin;                             // ピッチ角のsin値
    
    gms.read();                                     // GPSデータ取得
    UTC_t = (int)gms.time + 90000;

    // 参照座標系の基底を求める
    r_u = g;
    r_f = geomag.GetPerpCompTo(g).Normalize();
    r_l = Cross(r_u, r_f);

    // 回転行列を経由してオイラー角を求める
    // オイラー角はヨー・ピッチ・ロールの順に回転したものとする
    // 各オイラー角を求めるのに回転行列の全成分は要らないので、一部だけ計算する。

    R_11 = r_f * b_f;                         // 回転行列の(1,1)成分
    R_12 = r_f * b_l;                         // 回転行列の(1,2)成分
    yaw = atan2(-R_12, R_11) * RAD_TO_DEG + MAG_DECLINATION;
    r_cos = g.GetPerpCompTo(b_f).Normalize() * b_u;
    r_sin = Cross(g.GetPerpCompTo(b_f).Normalize(), b_u) * b_f;
    if(r_sin > 0.0f) {
        roll = acos(r_cos) * RAD_TO_DEG;
    } else {
        roll = -acos(r_cos) * RAD_TO_DEG;
    }
    
    p_cos = g.GetPerpCompTo(b_l).Normalize() * b_u;
    p_sin = Cross(g.GetPerpCompTo(b_l).Normalize(), b_u) * b_l;
    if(p_sin > 0.0f) {
        pitch = acos(p_cos) * RAD_TO_DEG;
    } else {
        pitch = -acos(p_cos) * RAD_TO_DEG;
    }

    if(yaw > 180.0f) yaw -= 360.0f;                 // ヨー角を[-π, π]に補正
    else if(yaw < -180.0f) yaw += 360.0f;                // ヨー角を[-π, π]に補正

    if(UTC_t - pre_UTC_t >= 1) {                    // GPSデータが更新されていたら
        pre_p = new_p;
        new_p.SetComp(1, gms.longitude * DEG_TO_RAD);
        new_p.SetComp(2, gms.latitude * DEG_TO_RAD);
        p = new_p;
        pre_UTC_t = UTC_t;
        ss = 0;
    } else {                                        // 更新されていなかったら
        p += 0.2f * (new_p - pre_p);
        ss += 2;
    }

    height = Height(press);



}

/** 制御ルーチン関数
 *
 */
void ControlRoutine()
{
    static int   count = 0;
    static float target_lng;
    static float target_lat;
    static float target_X;
    static float target_Y;
    static float theta;
    static float delta_theta;
    
    switch(step) {

            // 投下&安定滑空シーケンス
        case 0:
            if(Thrown() || count != 0) {
                count++;
                // 投下直後に紐を引く場合はコメントアウトをはずす
                 //pull_L = 15;
                 //pull_R = 15;
                if(count >= WaitTime*5) {
                    pull_L = 0;
                    pull_R = 0;
#ifdef SPIRAL_DEBUG
                    step = 2;
#else
                    step = 1;
#endif
                }
            }
            break;

            // 制御シーケンス
        case 1:
            if(fabs(roll) > BorderSpiral) {
                // スパイラル回避行動
                if(roll > 0) {
                    pull_L = PullMax;
                    pull_R = 0;
                } else {
                    pull_L = 0;
                    pull_R = PullMax;
                }
            } else {

                /* いずれも地球を完全球体と仮定 */
                target_lng = target_x * DEG_TO_RAD;
                target_lat = target_y * DEG_TO_RAD;
                /* 北から西回りで目標方向の角度を出力 */
                target_Y = cos( target_lat ) * sin( target_lng - p.GetComp(1) );
                target_X = cos( p.GetComp(2) ) * sin( target_lat ) - sin( p.GetComp(2) ) * cos( target_lat ) * cos( target_lng - p.GetComp(1) );
                
#ifdef RULE4
                if(height > BorderHeight) {
                    theta = -atan2f( target_Y, target_X ) * RAD_TO_DEG + Beta;
                    if(theta > 180.0f) theta -= 360.0f;
                } else {
                    theta = -atan2f( target_Y, target_X ) * RAD_TO_DEG;
                }
#elif RULE3
                theta = -atan2f( target_Y, target_X ) * RAD_TO_DEG + Beta;
                
                if(theta > 180.0f) theta -= 360.0f;
                else if(theta < -180.0f) theta += 360.0f;
#else
                theta = -atan2f( target_Y, target_X ) * RAD_TO_DEG;
#endif

#ifdef DIRECTION_DEBUG
                theta = TargetDirection;
#endif

                if(yaw >= 0.0f) {                                       // ヨー角が正
                    if(theta >= 0.0f) {                                 // 目標角も正ならば、
                        if(theta - yaw > Alpha) dir = LEFT;             // 単純に差を取って閾値αと比べる
                        else if(theta - yaw < -Alpha) dir = RIGHT;
                        else dir = NEUTRAL;
                    } else {                                            // 目標角が負であれば
                        theta += 360.0f;                                // 360°足して正の値に変換してから
                        delta_theta = theta - yaw;                      // 差を取る（yawから左回りに見たthetaとの差分）
                        if(delta_theta < 180.0f) {                      // 差が180°より小さければ左旋回
                            if(delta_theta > Alpha) dir = LEFT;
                            else dir = NEUTRAL;
                        } else {                                        // 180°より大きければ右旋回
                            if(360.0f - delta_theta > Alpha) dir = RIGHT;
                            else dir = NEUTRAL;
                        }
                    }
                } else {                                                // ヨー角が負
                    if(theta <= 0.0f) {                                 // 目標角も負ならば、
                        if(theta - yaw > Alpha) dir = LEFT;                // 単純に差を取って閾値αと比べる
                        else if(theta - yaw < -Alpha) dir = RIGHT;
                        else dir = NEUTRAL;
                    } else {                                            // 目標角が正であれば、
                        delta_theta = theta - yaw;                      // 差を取る（yawから左回りに見たthetaとの差分）
                        if(delta_theta < 180.0f) {                      // 差が180°より小さければ左旋回
                            if(delta_theta > Alpha) dir = LEFT;
                            else dir = NEUTRAL;
                        } else {                                        // 180°より大きければ右旋回
                            if(360.0f - delta_theta > Alpha) dir = RIGHT;
                            else dir = NEUTRAL;
                        }
                    }
                }

                if(dir == LEFT) {           //目標は左方向

                    pull_L = 20;
                    pull_R = 0;

                } else if (dir == RIGHT) {   //目標は右方向

                    pull_L = 0;
                    pull_R = 20;

                } else if (dir == NEUTRAL) {
                    pull_L = 0;
                    pull_R = 0;
                }
            }
#ifdef RULE4
            if(height <= BorderHeight) {
                // 目標地点との距離が閾値以下だった場合、落下シーケンスへと移行する
                if(Distance(target_p, p) < BorderDistance) step = 2;
            }
#else
#ifdef RULE2
            // 目標地点との距離が閾値以下だった場合、落下シーケンスへと移行する
            if(Distance(target_p, p) < BorderDistance) step = 2;
#endif
#endif

            break;

#if defined(RULE2) || defined(RULE4) || defined(SPIRAL_DEBUG)
            // 落下シーケンス
        case 2:
            pull_L = 0;
            pull_R = PullMax;

#ifndef SPIRAL_DEBUG
            // もし落下中に目標値から離れてしまった場合は、体勢を立て直して再び滑空
            // 境界で制御が不安定にならないよう閾値にマージンを取る
            if(Distance(target_p, p) > BorderDistance+3.0f) step = 1;
#endif
            break;
#endif
    }
}

/** 各種センサーの初期化を行う関数
 *
 */
void SensorsInit()
{
    
    if(!mpu.init()) error("mpu6050 Initialize Error !!");        // mpu6050初期化
    if(!hmc.init()) error("hmc5883l Initialize Error !!");       // hmc5883l初期化
    
    //重力ベクトルの初期化
    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);

    // 目標座標をベクトルに代入
    target_p.SetComp(1, target_x * DEG_TO_RAD);
    target_p.SetComp(2, target_y * DEG_TO_RAD);
    
    // 地表の気圧を取得
    p0 = 0;
    for(int i=0; i<10; i++) {
        p0 += (float)lps.pressure() * PRES_LSB_TO_HPA;
        wait(0.1f);
    }
    p0 /= 10.0f;
    
    xbee.printf(".");
}

/** マイクロSDカードの初期化を行う関数
 *
 */
bool SD_Init()
{
    //SDカード初期化
    char filename[15];
    int n = Find_last();
    if(n < 0) {
        pc.printf("Could not read a SD Card.\n");
        return false;
    }
    sprintf(filename, "/sd/log%03d.csv", n+1);
    fp = fopen(filename, "w");
    fprintf(fp, "log data\r\n");
    xbee.printf("log data\r\n");
    
    return true;
}

/** コンフィグファイルを読み込む関数
 *
 */
bool LoadConfig()
{
    char value[20];
    //Read a configuration file from a mbed.
    if (!cfg.read("/sd/config.txt")) {
        pc.printf("Config file does not exist\n");
        return false;
    } else {
        //Get values
        if (cfg.getValue("target_x", &value[0], sizeof(value))) target_x = atof(value);
        else {
            pc.printf("Failed to get value for target_x\n");
            return false;
        }
        if (cfg.getValue("target_y", &value[0], sizeof(value))) target_y = atof(value);
        else {
            pc.printf("Failed to get value for target_y\n");
            return false;
        }
    }
    return true;
}

/** ログファイルの番号の最大値を得る関数
 *
 * @return マイクロSD内に存在するログファイル番号の最大値
 */
int Find_last()
{
    int i, n = 0;
    char c;
    DIR *dp;
    struct dirent *dirst;
    dp = opendir("/sd/");
    if (!dp) {
        pc.printf("Could not open directry.\n");
        return -1;
    }
    while((dirst = readdir(dp)) != NULL) {
        if(sscanf(dirst->d_name, "log%03d.csv%c", &i, &c) == 1 && i>n) {
            n = i;
        }
    }
    closedir(dp);
    return n;
}

/** カルマンフィルタの初期化を行う関数
 *
 */
void KalmanInit()
{
    // 重力
    {
        // 誤差共分散行列の値を決める（対角成分のみ）
        float alpha_R2 = 0.002f;
        float alpha_Q2 = 0.5f;
        R2 *= alpha_R2;
        Q2 *= alpha_Q2;

        // 観測方程式のヤコビアンの値を設定（時間変化無し）
        float h2[15] = {
            1.0f, 0.0f, 0.0f, 0.0f, 0.0f,
            0.0f, 1.0f, 0.0f, 0.0f, 0.0f,
            0.0f, 0.0f, 1.0f, 0.0f, 0.0f
        };

        H2.SetComps(h2);
    }

    // 地磁気
    {
        // 誤差共分散行列の値を決める（対角成分のみ）
        float alpha_R1 = 0.002f;
        float alpha_Q1 = 0.5f;
        R1 *= alpha_R1;
        Q1 *= alpha_Q1;

        // 観測方程式のヤコビアンの値を設定（時間変化無し）
        float h1[21] = {
            1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f,
            0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
            0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f
        };

        H1.SetComps(h1);
    }
}

/** カルマンフィルタの更新を行う関数
 *
 */
void KalmanUpdate()
{
    // 重力

    {
        // ヤコビアンの更新
        float f2[25] = {
            1.0f, (raw_gyro.GetComp(3) - post_x1.GetComp(7))*dt, -(raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, 0.0f, post_x2.GetComp(3)*dt,
            -(raw_gyro.GetComp(3) - post_x1.GetComp(7))*dt, 1.0f, (raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, -post_x2.GetComp(3)*dt, 0.0f,
            (raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, -(raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, 1.0f, post_x2.GetComp(2)*dt, -post_x2.GetComp(1)*dt,
            0.0f, 0.0f, 0.0f, 1.0f, 0.0f,
            0.0f, 0.0f, 0.0f, 0.0f, 1.0f
        };

        F2.SetComps(f2);

        // 事前推定値の更新
        //pri_x2 = F2 * post_x2;

        float pri_x2_vals[5] = {
            post_x2.GetComp(1) + post_x2.GetComp(2) * (raw_gyro.GetComp(3) - post_x1.GetComp(7)) * dt - post_x2.GetComp(3) * (raw_gyro.GetComp(2) - post_x2.GetComp(5)) * dt,
            post_x2.GetComp(2) + post_x2.GetComp(3) * (raw_gyro.GetComp(1) - post_x2.GetComp(4)) * dt - post_x2.GetComp(1) * (raw_gyro.GetComp(3) - post_x1.GetComp(7)) * dt,
            post_x2.GetComp(3) + post_x2.GetComp(1) * (raw_gyro.GetComp(2) - post_x2.GetComp(5)) * dt - post_x2.GetComp(2) * (raw_gyro.GetComp(1) - post_x2.GetComp(4)) * dt,
            post_x2.GetComp(4),
            post_x2.GetComp(5)
        };

        pri_x2.SetComps(pri_x2_vals);

        // 事前誤差分散行列の更新
        pri_P2 = F2 * post_P2 * F2.Transpose() + R2;

        // カルマンゲインの計算
        S2 = Q2 + H2 * pri_P2 * H2.Transpose();
        static float det;
        if((det = S2.Inverse(S_inv2)) >= 0.0f) {
            pc.printf("E:%.3f\r\n", det);
            return;     // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了
        }
        K2 = pri_P2 * H2.Transpose() * S_inv2;

        // 事後推定値の更新
        post_x2 = pri_x2 + K2 * (raw_acc - H2 * pri_x2);
        // 事後誤差分散行列の更新
        post_P2 = (I2 - K2 * H2) * pri_P2;
    }


    // 地磁気
    {
        // ヤコビアンの更新
        float f1[49] = {
            1.0f, (raw_gyro.GetComp(3) - post_x1.GetComp(7))*dt, -(raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, 0.0f, 0.0f, 0.0f, -post_x1.GetComp(2) * dt,
            -(raw_gyro.GetComp(3) - post_x1.GetComp(7))*dt, 1.0f, (raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, 0.0f, 0.0f, 0.0f, post_x1.GetComp(1) * dt,
            (raw_gyro.GetComp(2) - post_x2.GetComp(5))*dt, -(raw_gyro.GetComp(1) - post_x2.GetComp(4))*dt, 1.0f, 0.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, 0.0f, 1.0f, 0.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, 0.0f, 1.0f
        };

        F1.SetComps(f1);

        // 事前推定値の更新
        //pri_x1 = F1 * post_x1;
        float pri_x1_vals[7] = {
            post_x1.GetComp(1) + post_x1.GetComp(2) * (raw_gyro.GetComp(3) - post_x1.GetComp(7)) * dt - post_x1.GetComp(3) * (raw_gyro.GetComp(2) - post_x2.GetComp(5)) * dt,
            post_x1.GetComp(2) + post_x1.GetComp(3) * (raw_gyro.GetComp(1) - post_x2.GetComp(4)) * dt - post_x1.GetComp(1) * (raw_gyro.GetComp(3) - post_x1.GetComp(7)) * dt,
            post_x1.GetComp(3) + post_x1.GetComp(1) * (raw_gyro.GetComp(2) - post_x2.GetComp(5)) * dt - post_x1.GetComp(2) * (raw_gyro.GetComp(1) - post_x2.GetComp(4)) * dt,
            post_x1.GetComp(4),
            post_x1.GetComp(5),
            post_x1.GetComp(6),
            post_x1.GetComp(7)
        };

        pri_x1.SetComps(pri_x1_vals);

        // 事前誤差分散行列の更新
        pri_P1 = F1 * post_P1 * F1.Transpose() + R1;

        // カルマンゲインの計算
        S1 = Q1 + H1 * pri_P1 * H1.Transpose();
        static float det;
        if((det = S1.Inverse(S_inv1)) >= 0.0f) {
            pc.printf("E:%.3f\r\n", det);
            return;     // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了
        }
        K1 = pri_P1 * H1.Transpose() * S_inv1;

        // 事後推定値の更新
        post_x1 = pri_x1 + K1 * (raw_geomag - H1 * pri_x1);
        // 事後誤差分散行列の更新
        post_P1 = (I1 - K1 * H1) * pri_P1;
    }
}

/** GPS座標から距離を算出
 * @param 座標1（経度、緯度）（rad）
 * @param 座標2（経度、緯度）（rad）
 *
 * @return 2点間の距離（m）
 */
float Distance(Vector p1, Vector p2)
{
    static float mu_y;
    static float s_mu_y;
    static float w;
    static float m;
    static float n;
    static float d1;
    static float d2;
    
    if(p1.GetDim() != p2.GetDim()) return 0.0f;

    mu_y = (p1.GetComp(2) + p2.GetComp(2)) * 0.5f;
    s_mu_y = sin(mu_y);
    w = sqrt(1 - GPS_SQ_E * s_mu_y * s_mu_y);
    m = GPS_A * (1 - GPS_SQ_E) / (w * w * w);
    n = GPS_A / w;
    d1 = m * (p1.GetComp(2) - p2.GetComp(2));
    d2 = n * cos(mu_y) * (p1.GetComp(1) - p2.GetComp(1));

    return sqrt(d1 * d1 + d2 * d2);
}

/** 投下を検知する関数
 *
 * @return 投下されたかどうか（true=投下　false=未投下
 *
 */
bool Thrown()
{
    static int opt_count = 0;
    static int g_count = 0;
    static int para_count = 0;

    if(optSensor.read_u16() > BorderOpt) opt_count++;
    else opt_count = 0;
    if(vrt_acc < BorderGravity) g_count++;
    else g_count = 0;
    if((int)paraSensor == BorderParafoil) para_count++;
    else para_count = 0;

    if(opt_count >= MaxCount || g_count >= MaxCount || para_count >= MaxCount) {
        return true;
    }

    return false;

}

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

/** データ取得＆更新関数
 *
 */
void DataUpdate()
{
    // センサーの値を更新
    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;
        raw_temp = TempLsbToDeg(lps.temperature());
    }

    if(INT_flag) {
        g = raw_g;
        for(int i=0; i<3; i++) {
            geomag.SetComp(i+1, post_x1.GetComp(i+1));
        }
        acc = raw_acc;
        gyro = raw_gyro;
        press = raw_press;
        temp = raw_temp;

        vrt_acc = raw_acc * raw_g;

    }
}

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

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");

}