File content as of revision 0:98c7c6fe50cc:

#include "mbed.h"
#include "MS5607I2C.h"
#include "SDFileSystem.h"
#include "MPU6050.h"
#include <stdio.h>

/***************************/
/* 打上前に値を確認！！！！！ */
/***************************/
#define L_HOKUI 35.546257       // 10進法 KBIC 3本の樹
#define L_TOUKEI 139.671875
#define LNCH_ACC 2              // 発射判定閾値[G]
#define TRANS_TIME 50           // 打上から制御開始までの時間[sec/10]
/***************************/

#define DEG_TO_RAD(x) ( x * 0.01745329 )
#define RAD_TO_DEG(x) ( x * 57.29578 )

#define HIGH    1
#define LOW     0

#define STDBY_MODE 0   // 待機
#define TRANS_MODE 1   // 輸送
#define CTRL_MODE  2   // 制御

#define GPS_RATE  10 // GPSの取得レート
#define SAVE_RATE 50 // SDへの保存の間隔[sec/10]

#define ACC_SSF  2048   //Sensitivity Scale Factor
#define GYRO_SSF 16.4
#define PRESS_INIT 10   // 基準気圧の平均を取る数

#define KPL 2.0     // ラダーゲイン
#define KPE 2.0     // エレベータゲイン

#define GOAL_R 50   // 目標地点にこれだけ近づいたら制御方法変える
#define DELTA_E_FULLUP      2000
#define DELTA_E_NTRL        1500
#define DELTA_E_FULLDOWN    1000
#define DELTA_L_FULLR       2000
#define DELTA_L_NTRL        1500
#define DELTA_L_FULLL       1000

#define PI 3.14159265358979         // π
#define A_RADIUS 6378137.000        // 地球長半径 [m]
#define B_RADIUS 6356752.314245     // 地球短半径 (WGS84)[m]
#define ECC2 0.006694379990          // 離心率の二乗

/* ペリフェラル宣言 */
Serial pc(USBTX,USBRX);
Serial gps(p9, p10);
Serial twe(p13, p14);
PwmOut eServo(p21);                     // ラダー用サーボ
PwmOut lServo(p22);                     // エレベータ用サーボ
DigitalOut camSw(p20);                  // カメラスイッチ
SDFileSystem sd(p5, p6, p7, p8, "sd");
MPU6050 mpu;
MS5607I2C ms5607(p28, p27, true);
BusOut leds(LED1, LED2, LED3, LED4);

/* グローバル変数宣言 */
int mode = STDBY_MODE;
int cnt = 0;

int gps_i;   // 文字数，モード（センテンス終わりか？判定）
char gps_data[256];
float beta, alpha;
float set_point_l, set_point_e;
float delta_l=1500, delta_e=1500;

FILE *fp;
char all_data[256];

/* プロトタイプ宣言 */
bool Init();
void getData();
float getAlt(float press, float press0, float temp);
//void filter_x(double dt, double C, double x_est[], double x_gps[], double vg_gps[]);
void filter_alt(double dt, double C, double &alt_est, double alt_s, double vz);
void filter_vz(double dt, double C, double &vz_est, double vz);

bool Init()
{
    gps.baud(9600);                     // ボーレート
    pc.baud(115200);
    twe.baud(115200);
    eServo.period_us(20000);          // servo requires a 20ms period
    lServo.period_us(20000);
    lServo.pulsewidth_us(0);
    eServo.pulsewidth_us(0);

    gps.printf("$PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29\r\n");    // GPRMCだけにする
    gps.printf("$PMTK300,100,0,0,0,0*2C\r\n");                              // position fixを100msecに（レート10spsにする）

    mpu.initialize();
    ms5607.printCoefficients();

    pc.puts("Init finish!\n");
    return true;
}

/******************/
// GPSの取得周期（10SPS）に沿って，他のデータ取得等の処理を行う．
/******************/

void getData()
{
    char c = gps.getc();
    if( c=='$' || gps_i == 256) {
        gps_i = 0;
    }
    if((gps_data[gps_i]=c) != '\r') {
        gps_i++;
    } else {

        gps_data[gps_i]='\0';

        float w_time, hokui, tokei;   // 時刻，北緯，東経
        float vb, beta2;              // 速度，真方位
        char ns, ew, status;          // NS，EW，その他
        if( sscanf(gps_data, "$GPRMC,%f,%c,%f,%c,%f,%c,%f,%f,",&w_time,&status,&hokui,&ns,&tokei,&ew,&vb,&beta2) >= 1) {
            //Longitude
            double d_tokei= int(tokei/100);
            double m_tokei= (tokei-d_tokei*100)/60;
            double g_tokei= d_tokei+m_tokei;         // 10進法に換算
            //Latitude
            double d_hokui= int(hokui/100);
            double m_hokui= (hokui-d_hokui*100)/60;
            double g_hokui= d_hokui+m_hokui;
            /* ヒュベニの公式 */
            double dy = (L_HOKUI-g_hokui)/180*PI;        //ラジアン
            double dx = (L_TOUKEI-g_tokei)/180*PI;
            double yAve = (g_hokui+L_HOKUI)/2/180*PI;
            double w = sqrt(1-ECC2*sin(yAve)*sin(yAve));
            double m = A_RADIUS*(1-ECC2)/(w*w*w);
            double n = A_RADIUS/w;
            double distance = sqrt((dy*m)*(dy*m)+(dx*n*cos(yAve))*(dx*n*cos(yAve)));
            double xDist = dx*n*cos(yAve);
            double yDist = dy*m;

            /* 高度 */
            static float alt, alt0;             //前の高度
            static float press_arr[PRESS_INIT]; // 基準高度はこれの平均をとる
            static float press0 = 0;
            float temp = ms5607.getTemperature();   // 気圧
            float press = ms5607.getPressure();     // 温度
            
            /* 基準電圧の取得 */
            if(mode == STDBY_MODE) {
                for(int i=0; i<PRESS_INIT-1; i++) press_arr[i] = press_arr[i+1]; // 気圧データをシフト
                press_arr[PRESS_INIT-1] = press;
            }
            if(mode == TRANS_MODE) { //発射直後，基準電圧を計算
                if(cnt==0 && press0==0) {
                    for(int i=0; i<PRESS_INIT; i++) press0 += press_arr[i];
                    press0 /= PRESS_INIT;
                }
            }
            
            double vz_calc = (alt - alt0) / 0.1;
            alt0 = alt;
            alt = getAlt(press, press0, temp);          //高度の算出
            double alt_est = 0.0, vz_est = 0.0;
            float dt = 1/GPS_RATE;
            filter_alt(dt, 0.1, alt_est, alt, vz_est);
            filter_vz(dt, 0.01, vz_est, vz_calc);

            /* 加速度 */
            int16_t ax, ay, az;  // mpu生データ
            int16_t gx, gy, gz;
            mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
            float fax = float(ax)/ACC_SSF; // mpu物理量に換算
            float fay = float(ay)/ACC_SSF;
            float faz = float(az)/ACC_SSF;
            //float fgx = float(gx)/GYRO_SSF;
            //float fgy =float(gy)/GYRO_SSF;
            //float fgz = float(gz)/GYRO_SSF;
            float resAcc = sqrt(fax*fax + fay*fay + faz*faz); //合成加速度

            /* アングルオフ */
            if(beta2 > 180) beta2 -= 360;
            double beta3 = RAD_TO_DEG(atan2(yDist, xDist));
            beta = beta2 - beta3;
            if(beta > 180) beta -= 360;
            if(beta <-180) beta += 360;

            /* 降下角 */
            alpha = -atan(vz_est/vb);

            /* サーボ操作 P制御 */
            if(distance <= GOAL_R){
                // ラダー操舵
                set_point_l = DELTA_L_NTRL + 500*(beta * KPL / 180);
                if(set_point_l > DELTA_L_FULLR) set_point_l = DELTA_L_FULLR;
                if(set_point_l < DELTA_L_FULLL) set_point_l = DELTA_L_FULLL;
                if((set_point_l-delta_l)>=0){
                    delta_l += 100;
                    if((set_point_l-delta_l)<0) delta_l = set_point_l;
                }else{
                    delta_l -= 100;
                    if((set_point_l-delta_l)>0) delta_l = set_point_l+1;
                }
                
                // エレベータ制御
                set_point_e = DELTA_E_NTRL + 500*(alpha * KPE / 180);
                if(set_point_e > DELTA_E_FULLUP) set_point_e = DELTA_E_FULLUP;
                if(set_point_e < DELTA_E_FULLDOWN) set_point_e = DELTA_E_FULLDOWN;
                if((set_point_e-delta_e)>=0){
                    delta_e += 100;
                    if((set_point_e-delta_e)<0) delta_e = set_point_e;
                }else{
                    delta_e -= 100;
                    if((set_point_e-delta_e)>0) delta_e = set_point_e+1;
                }
            }else{
                if(cnt%10) delta_e = DELTA_E_NTRL;
                else delta_e = DELTA_E_FULLUP;
                delta_l = DELTA_L_FULLR;            // 要検討
            }
            lServo.pulsewidth_us(delta_l);
            eServo.pulsewidth_us(delta_e);

            /* 取得データの整理と出力 */
            sprintf(all_data, "%6lf,%.2f,%.2f,%.2f,%d,%d\r\n",
                    distance, vb, beta, alt, cnt, mode);
            pc.printf("%s", all_data);
            if(!(cnt%10)) twe.printf("%.6f,%.6f,%.2f,%d\r\n", g_tokei, g_hokui, alt, cnt);
            //pc.printf("%s\r\n",gps_data);
            //pc.printf("%f,%f\r\n", delta_l, set_point_l);
            
            /* カメラスイッチ操作 */
            if(mode==TRANS_MODE){
                if(cnt<=20)             camSw = HIGH;   // 2秒間長押しで電源オンの後，一回短押しで撮影開始
                if(cnt>20 && cnt<=25)   camSw = LOW;
                if(cnt>25 && cnt<=30)   camSw = HIGH;
                if(cnt>30)              camSw = LOW;
            }

            /* SDカードに保存 */
            if((cnt%SAVE_RATE)==0) fp = fopen("/sd/myfile.txt", "a");
            fprintf(fp, "%s", all_data);
            if((cnt%SAVE_RATE)==SAVE_RATE-1) fclose(fp);
            sprintf(gps_data, "");
            
            /* カウント */
            cnt++;
            if(cnt>=600) cnt = 0;
            
            /* モード遷移判定 */
            if(mode == STDBY_MODE) {       // 待機⇒制御
                if(resAcc > LNCH_ACC) {
                    mode = TRANS_MODE;
                    cnt = 0;
                }
                leds = 1 << cnt%4;
            }else
            if(mode == TRANS_MODE) {
                if(cnt > TRANS_TIME) {
                    mode = CTRL_MODE;
                    cnt = 0;
                }
                leds = 3 << cnt%4;
            }else
            if(mode == CTRL_MODE) {
                leds = 15*(cnt%2);
            }
        }
    }
}

int main()
{
    Init();
    pc.printf("*** Start up! ***\r\n");
    gps.attach(getData,Serial::RxIrq);
    while(1) {}
}
// 高度計算
float getAlt(float press, float press0, float temp)
{
    return (pow((press0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
}

// 位置（速度）フィルタ
void filter_x(double dt, double C, double x_est[], double x_gps[], double vg_gps[])
{
    static double x0[] = { x_gps[0], x_gps[1], x_gps[2] };
    static int called = 0;
    if (!called) {
        for (int i = 0; i < 2; i++)
            x_est[i] = x0[i];
        called++;
        return;
    }
    double xt[2];
    for (int i = 0; i < 2; i++) {
        xt[i] = x0[i] + vg_gps[i] * dt;
        x_est[i] = C*x_gps[i] + (1 - C)*xt[i];
        x0[i] = x_est[i];
    }
}
// 高度フィルタ
void filter_alt(double dt, double C, double &alt_est, double alt_s, double vz)
{
    static double alt0 = alt_s;
    static int called = 0;
    if (!called) {
        alt_est = alt0;
        called++;
        return;
    }
    double altt = alt0 + vz * dt;
    {
        alt_est = C*alt_s + (1 - C)*altt;
        alt0 = alt_est;
    }
}
// 垂直速度フィルタ
void filter_vz(double dt, double C, double &vz_est, double vz)
{
    static double vz0 = 0.0;
    static int called = 0;
    if (!called) {
        vz_est = vz0;
        called++;
        return;
    }
    vz_est = C*vz + (1 - C)*vz0;
    vz0 = vz_est;
}