File content as of revision 1:491e67888f28:

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

//#define TEST
#define FLIGHT
/***************************/
/* 打上前に値を確認！！！！！ */
/***************************/
#ifdef TEST
#define L_HOKUI 34.74102777       // 10進法 urasabaku
#define L_TOUKEI 139.42511111
#define LNCH_ACC 2              // 発射判定閾値[G]
#define TRANS_TIME 50           // 打上から制御開始までの時間[sec/10]
#define OPEN_TIME 50            // 150mに達してから開放されるまで（含ロケットアビオとのラグ）
#endif

#ifdef FLIGHT
#define L_HOKUI 34.74102777       // 10進法 urasabaku
#define L_TOUKEI 139.42511111
#define LNCH_ACC 4              // 発射判定閾値[G]
#define TRANS_TIME 100           // 打上から制御開始までの時間[sec/10]
#define OPEN_TIME 50            // 150mに達してから開放されるまで（含ロケットアビオとのラグ）
#endif
/***************************/

#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      1000
#define DELTA_E_NTRL        1500
#define DELTA_E_FULLDOWN    2000
#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=DELTA_L_NTRL, set_point_e=DELTA_E_NTRL;
float delta_l=0, delta_e=0;

FILE *fp;
char all_data[256];

/* プロトタイプ宣言 */
bool Init();
void getData();
float getAlt(float press, float press0, float temp);
//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()
{
    /*①*/leds = leds|1;
    pc.printf("*** Start up! ***\r\n");
    gps.baud(9600);                     // ボーレート
    pc.baud(115200);
    twe.baud(115200);
    eServo.period_us(20000);          // servo requires a 20ms period
    lServo.period_us(20000);

    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();
    
    /*②*/leds = leds | (1<<1);
    pc.printf("Test start!\r\n");
    lServo.pulsewidth_us(DELTA_L_NTRL);
    eServo.pulsewidth_us(DELTA_E_NTRL);
    wait(1);
    lServo.pulsewidth_us(DELTA_L_FULLR);
    eServo.pulsewidth_us(DELTA_E_FULLUP);
    wait(1);
    lServo.pulsewidth_us(DELTA_L_FULLL);
    eServo.pulsewidth_us(DELTA_E_FULLDOWN);
    wait(1);
    lServo.pulsewidth_us(DELTA_L_NTRL);
    eServo.pulsewidth_us(DELTA_E_NTRL);
    wait(1);
    
    /*③*/leds = leds | (1<<2);
    float temp, press;
    float fax, fay, faz;
    int16_t ax, ay, az;
    int16_t gx, gy, gz;
    for(int i=0; i<10; i++){
        temp = ms5607.getTemperature();   // 気圧
        press = ms5607.getPressure();     // 温度
        mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
        fax = float(ax)/ACC_SSF; // mpu物理量に換算
        fay = float(ay)/ACC_SSF;
        faz = float(az)/ACC_SSF;
        twe.printf("%f,%f,%f,%f,%f,%f,%f,%f", temp, press, fax, fay, faz);
        wait(0.1);
    }
    
    /*④*/leds = leds | (1<<3);
    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 vz;                      // 高度，沈下速度
            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;
                }
            }
            
            /* 沈下速度計算 */
            static float dt = 1/GPS_RATE;
            alt0 = alt;
            alt = getAlt(press, press0, temp);
            vz = (alt0 - alt)*dt;
            
            /* だきゅんフィルタ */
            /*
            double vz_calc, alt_est, vz_est;
            double dt = 1/GPS_RATE;
            if(mode >= TRANS_MODE){
                static double alt0 = getAlt(press, press0, temp);          //高度の算出
                vz_calc = (alt - alt0) / dt;
                alt0 = alt;
                alt = getAlt(press, press0, temp);          //高度の算出
                alt_est = 0.0, vz_est = 0.0;
                filter_alt(dt, 1, alt_est, alt, vz_est);
                filter_vz(dt, 1, 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/vb)*180/PI;

            /* サーボ操作 P制御 */
            if(mode!=CTRL_MODE) {
                delta_l = 0; delta_l = 0;
            }else 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;
                }
                
                // エレベータ制御
                if(alpha>0){
                    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;            // 要検討
            }
            if(g_tokei == 0){
                delta_e = DELTA_E_NTRL;
                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);*/
            sprintf(all_data, "%.6f,%.6f,%.2f,%.2f,%.2f,%d,%d\r\n",
                    g_tokei, g_hokui, alt, 
                    vb, beta2, cnt, mode); 
            pc.printf("%s", all_data);
            if(!(cnt%10)) twe.printf("%.6f,%.6f,%.2f,%d,%d\r\n", g_tokei, g_hokui, alt, cnt, mode);
            //pc.printf("%s\r\n",gps_data);
            //pc.printf("%f,%f\r\n", delta_l, set_point_l);

            /* SDカードに保存 */
            if(mode >= TRANS_MODE){ // 輸送モード以降で保存を開始
                if((cnt%SAVE_RATE)==0) fp = fopen("/sd/data.txt", "a");
                fprintf(fp, "%s", all_data);
                if((cnt%SAVE_RATE)==SAVE_RATE-1) fclose(fp);
                sprintf(gps_data, "");
            }
            
            /* カメラスイッチ操作 */
            if(mode==TRANS_MODE){
                if(cnt<=20)             camSw = HIGH;   // 2秒間長押しで電源オンの後，一回短押しで撮影開始
                if(cnt>20 && cnt<=50)   camSw = LOW;
                if(cnt>50 && cnt<=55)   camSw = HIGH;
                if(cnt>55)              camSw = LOW;
            }else{
                camSw = LOW;
            }
            
            /* カウント */
            cnt++;
            if(cnt>=600) cnt = 0;
            
            /* モード遷移判定 */
            if(mode == STDBY_MODE) {       // 待機⇒制御
                if(resAcc > LNCH_ACC && cnt>10) {
                    mode = TRANS_MODE;
                    cnt = 0;
                }
                leds = 1 << cnt%4;
            }else
            if(mode == TRANS_MODE) {
                if(cnt > TRANS_TIME && alt < 150) {
                    static int ctrl_start_cnt = 0;
                    ctrl_start_cnt++;
                    if(ctrl_start_cnt > OPEN_TIME){
                        delta_l = DELTA_L_NTRL; // 制御開始時に通常のデューティ比にする
                        delta_e = DELTA_E_NTRL;
                        mode = CTRL_MODE;
                        cnt = 0;
                    }
                }
                leds = 3 << cnt%4;
            }else
            if(mode == CTRL_MODE) {
                leds = 15*(cnt%2);
            }
        }
    }
}

int main()
{
    Init();
    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;
}