File content as of revision 36:94dc027e05cd:

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

const float dt              = 0.01f;        // 割り込み周期(s)

enum Direction {LEFT, RIGHT, NEUTRAL};

/****************************** private macro     ******************************/
/****************************** private typedef   ******************************/
/****************************** private variables ******************************/
DigitalOut      myled(LED1);                        // デバッグ用LEDのためのデジタル出力
I2C             i2c(p9, p8);                    // I2Cポート
MPU6050         mpu(&i2c);                          // 加速度・角速度センサ
HMC5883L        hmc(&i2c);                          // 地磁気センサ
LPS25H          lps(&i2c);                          // 気圧センサ
Serial          pc(USBTX,USBRX);           // PC通信用シリアルポート
FILE *          fp;                                 // ログファイルのポインタ
ConfigFile      cfg;                                // ConfigFile
Ticker          ticker;                             // 割り込みタイマー
Timer           timer;                              // 時間計測用タイマー
Direction       dir = NEUTRAL;                      // 旋回方向

volatile int     lps_cnt = 0;                       // 気圧センサ読み取りカウント
volatile 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軸）

float     pull_L = 10.0f;             // 左サーボの引っ張り量（目標値）(mm：0~PullMax)
float     pull_R = 10.0f;             // 右サーボの引っ張り量（目標値）(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;         // 鉛直方向の加速度成分（落下検知に使用）

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

#ifdef RULE3_1
float beta = 0.0f;              // β変数 （制御則3_1で使用
#endif
/** 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    SensorsInit();                      // 各種センサーの初期化
void    KalmanInit();                       // カルマンフィルタ初期化
void    DataProcessing();                   // データ処理関数
void    KalmanUpdate();                     // カルマンフィルタ更新
void    DataUpdate();                       // データ取得&更新関数

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

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

int main()
{

    i2c.frequency(400000);                  // I2Cの通信速度を400kHzに設定
    //カルマンフィルタ初期化
    KalmanInit();

    ticker.attach(&DataUpdate, dt);         // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）



    /* ------------------------------ ↓↓↓ ここからメインループ ↓↓↓ ------------------------------ */
    while(1) {
        timer.stop();
        timer.reset();
        timer.start();
        myled = 1;                                          // LED is ON

        INT_flag = false;                               // 割り込みによる変数書き換えを阻止
        /******************************* データ処理 *******************************/
        DataProcessing();
        INT_flag = true;
    }
    /* ------------------------------ ↑↑↑ ここまでメインループ ↑↑↑ ------------------------------ */
}

/** データ処理関数
 *
 */
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値


    // 参照座標系の基底を求める
    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;           // ヨー角を[-π, π]に補正
}


/** 各種センサーの初期化を行う関数
 *
 */
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);
}

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

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

/** データ取得＆更新関数
 *
 */
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();

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