File content as of revision 13:df1e8a650185:

#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

const float dt = 0.01f;              // 割り込み周期(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
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_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）z軸周り
float pitch = 0.0f;             // 本体のピッチ角（deg）y軸周り
float roll = 0.0f;              // 本体のロール角（deg）x軸周り

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

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

/* ----- Kalman Filter ----- */
// 地磁気ベクトル用
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);

// 重力ベクトル用
// ジャイロのバイアスも同時に推定する
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);
/* ----- ------------- ----- */

Timer timer;

char data[512] = {};

/********** private functions **********/
void LoadConfig();                      // config読み取り
int find_last();                        // SDカード初期化用関数
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初期化
    /*
    //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);
    
    /* ---------- ↓↓↓ ここからメインループ ↓↓↓ ---------- */
    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_12, R_11) * RAD_TO_DEG - MAG_DECLINATION;
            roll = atan2(-R_23, R_33) * RAD_TO_DEG;
            pitch = asin(R_13) * RAD_TO_DEG;
            
            pc.printf("%.3f, %.3f, %.3f\r\n", 
                    //geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3));
                    //yaw, pitch, roll);
                    post_x2.GetComp(4), 
                    post_x2.GetComp(5), 
                    post_x1.GetComp(7));
            
            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;
            
            // データをmicroSDに保存し、XBeeでPCへ送信する
            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, optSensor.read_u16());
            //fprintf(fp, data);
            //fflush(fp);
            //xbee.printf(data);
            
            INT_flag = TRUE;            // 割り込み許可
        }
        
        
        // 制御ルーチン
        {
            
            if(fabs(roll) > 40.0f) {
                // スパイラル回避行動
                if(roll > 0) {
                    pull_L = PullMax;
                    pull_R = 0;
                } else {
                    pull_L = 0;
                    pull_R = PullMax;
                }
            } else {
                
            }
                
            //pull_L = (pull_L+5)%PullMax;
            //pull_R = (pull_R+5)%PullMax;
            //pull_L = 0;
            //pull_R = 30;
            
            
            
            // 指定された引っ張り量だけ紐を引っ張る
            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
        /*pc.printf("%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n", 
                post_x.GetComp(4), post_x.GetComp(5), post_x.GetComp(6), 
                yaw, pitch, roll, 
                geomag.GetNorm());*/
        
        // ループはきっかり0.2秒ごと
        while(timer.read_ms() < 200);
        
        
    }
    
    /* ---------- ↑↑↑ ここまでメインループ ↑↑↑ ---------- */
    //fclose(fp);
}

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

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

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