File content as of revision 18:9dd72e417c60:

#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 "SDFileSystem.h"
#include "BufferedSerial.h"
#include "ConfigFile.h"

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

#define RULE1
//#define RULE2
//#define RULE3

const float dt              = 0.01f;        // 割り込み周期(s)
const float ServoMax        = 0.0023f;      // サーボの最大パルス長(s)
const float ServoMin        = 0.0006f;      // サーボの最小パルス長(s)
const float PullMax         = 25.0f;        // 引っ張れる紐の最大量(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  = 10.0f;        // 落下制御に入るための目標値との距離の閾値(m)（制御則2の定数

/****************************** 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
BufferedSerial  xbee(PA_9, PA_10, PC_1);            // 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          INT_timer;                          // 割り込みタイマー
Timer           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  target_p(2);            // 目標情報（経度、緯度）（rad）
Vector  p(2);                   // 位置情報(経度, 緯度)（rad）
Vector  pre_p(2);               // 過去の位置情報(経度, 緯度)（rad）
int     UTC_t = 0;              // UTC時刻
int     pre_UTC_t = 0;          // 前のUTC時刻

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;               // シーケンス制御のステップ
int     count = 0;              // 時間測定用カウンター
int     dir = 0;                // 旋回方向（0：左　1：右）
char    data[512] = {};         // 送信データ用配列

/** 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 ******************************/
void    LoadConfig();                       // config読み取り
int     Find_last();                        // SDカード初期化用関数
void    KalmanInit();                       // カルマンフィルタ初期化
void    KalmanUpdate();                     // カルマンフィルタ更新
float   Distance(Vector p1, Vector p2);     // 緯度・経度の差から2点間の距離を算出(m)
bool    thrown();                           // 投下されたかどうかを判断する
void    INT_func();                         // 割り込み用関数
void    toString(Matrix& m);                // デバッグ用
void    toString(Vector& v);                // デバッグ用

inline float min(float a, float b)
{
    return ((a > b) ? b : a);
}

/******************************   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初期化

    //Config読み取り
    LoadConfig();

    //SDカード初期化
    FILE *fp;
    char filename[15];
    int n = Find_last();
    if(n < 0) {
        pc.printf("Could not read a SD Card.\n");
        return 0;
    }
    sprintf(filename, "/sd/log%03d.csv", n+1);
    fp = fopen(filename, "w");
    fprintf(fp, "log data\r\n");
    xbee.printf("log data\r\n");

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

    // 目標座標をベクトルに代入
    target_p.SetComp(1, target_x * DEG_TO_RAD);
    target_p.SetComp(2, target_y * DEG_TO_RAD);

    /* ------------------------------ ↓↓↓ ここからメインループ ↓↓↓ ------------------------------ */
    while(1) {
        timer.stop();
        timer.reset();
        timer.start();
        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)成分

#ifdef RULE3
            yaw = atan2(-R_12, R_11) * RAD_TO_DEG + MAG_DECLINATION - Beta;
#else
            yaw = atan2(-R_12, R_11) * RAD_TO_DEG + MAG_DECLINATION;
#endif
            roll = atan2(-R_23, R_33) * RAD_TO_DEG;
            pitch = asin(R_13) * RAD_TO_DEG;

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



            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 {                                        // 更新されていなかったら
                // GPSの補完処理を追加予定
            }

            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;    // ロジック電源電圧

            // データをmicroSDに保存し、XBeeでPCへ送信する
            sprintf(data, "%d, %.1f,%.1f,%.1f, %.3f,%.5f,%.5f",
                    UTC_t, yaw, pitch, roll,
                    press, gms.longitude, gms.latitude);
            fprintf(fp, data);
            fflush(fp);
            xbee.printf(data);
            
            sprintf(data, ", %.3f,%.3f,%.3f, %.3f,%d\r\n",
                    sv, lv, vrt_acc,
                    Distance(target_p, p), optSensor.read_u16());  
                    
            fprintf(fp, data);
            fflush(fp);
            xbee.printf(data);                  

            INT_flag = TRUE;            // 割り込み許可
        }


        /******************************* 制御ルーチン *******************************/
        {

            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;
                            step = 1;
                        }
                    }
                    break;

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

                        /* いずれも地球を完全球体と仮定 */
                        float target_lng = target_x * DEG_TO_RAD;
                        float target_lat = target_y * DEG_TO_RAD;
                        /* 北から西回りで目標方向の角度を出力 */
                        float targetY = cos( target_lat ) * sin( target_lng - p.GetComp(1) );
                        float targetX = cos( p.GetComp(2) ) * sin( target_lat ) - sin( p.GetComp(2) ) * cos( target_lat ) * cos( target_lng - p.GetComp(1) );
                        float theta = -atan2f( targetY, targetX );
                        float delta_theta = 0.0f;

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

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

                            pull_L = 20;
                            pull_R = 0;

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

                            pull_L = 0;
                            pull_R = 20;

                        }
                    }

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

                    break;

#ifdef RULE2
                // 落下シーケンス
                case 2:
                    pull_L = 25;
                    pull_R = 0;

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

            // 指定された引っ張り量だけ紐を引っ張る
            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) * pull_L / PullMax + ServoMin);
            servoR.pulsewidth((ServoMax - ServoMin) * pull_R / PullMax + ServoMin);



        }

        myled = 0; // LED is OFF

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


    }

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

void 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");
    } 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");
        }
        if (cfg.getValue("target_y", &value[0], sizeof(value))) target_y = atof(value);
        else {
            pc.printf("Failed to get value for target_y\n");
        }
    }
}

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);
        // 地磁気ベクトルの大きさに拘束条件を与える
        /*{
            Vector gm(3);
            gm.SetComp(1, post_x1.GetComp(1));
            gm.SetComp(2, post_x1.GetComp(2));
            gm.SetComp(3, post_x1.GetComp(3));

            gm = MAG_MAGNITUDE * gm.Normalize();

            post_x1.SetComp(1, gm.GetComp(1));
            post_x1.SetComp(2, gm.GetComp(2));
            post_x1.SetComp(3, gm.GetComp(3));
        }*/
        // 事後誤差分散行列の更新
        post_P1 = (I1 - K1 * H1) * pri_P1;
    }
}

float Distance(Vector p1, Vector p2)
{
    if(p1.GetDim() != p2.GetDim()) return 0.0f;

    static float mu_y = (p1.GetComp(2) + p2.GetComp(2)) * 0.5f;
    static float s_mu_y = sin(mu_y);
    static float w = sqrt(1 - GPS_SQ_E * s_mu_y * s_mu_y);
    static float m = GPS_A * (1 - GPS_SQ_E) / (w * w * w);
    static float n = GPS_A / w;
    static float d1 = m * (p1.GetComp(2) - p2.GetComp(2));
    static float 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 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;
    }

    if(INT_flag != FALSE) {
        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;

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

}