File content as of revision 0:8483298c086d:

#include "mbed.h"
#include "MPU6050.h"
#include "MS5607I2C.h"
#include "SDFileSystem.h"
#define MAX_RDATA       256         //最大受信コマンド
#define MAX_TOTAL_DATA  256         //最大データフレーム
#define DATA_BUNDLE     10          //SDへ1度に保存するデータフレーム
#define DEPLOY_TIME_1   18000        //発射からの1回目のサーボ駆動(ミリ秒)  18秒
#define DEPLOY_TIME_2   65500        //2回目のサーボ駆動(ミリ秒)           83.5-18秒    
#define DEPLOY_ALT2     300         //パラ2開放高度(m)
#define DUTY_BASE       0.0975        //デューティー比   ベース     136*0.001/2 + 0.03
#define DUTY_1          0.084        //              開放1     108*0.001/2 + 0.03
#define DUTY_2          0.069        //              開放2     78*0.001/2 + 0.03

Serial sci(p9, p10);                   //通信線
//Serial sci(USBTX, USBRX);                   
BusOut myleds(LED1, LED2, LED3, LED4);  //LED
//Serial pc(USBTX, USBRX);              //USB
SDFileSystem sd(p5, p6, p7, p8, "sd");  //MicroSDCard
Serial uar(p13,p14);                    //GPS
I2C i2c(p28, p27);                      //音声合成
const int tlk_addr = 0x2E<<1;           //音声合成LSIのアドレス
MPU6050 mpu;                            //MPU6050(加速度ジャイロ)
MS5607I2C ms5607(p28, p27, false);      //MS5607(気圧)
Ticker ticker;                          //タイマー割り込み
extern "C" void mbed_reset();           //リセット関数の宣言 
Timer timer;                            //タイマ
FILE *fp;                               //SDカード
PwmOut servo(p21);                      //サーボモータ
DigitalOut f_pin(p20);                   //フライトピン
Timer xtimer;                           //打上時間通達用のタイマー
Timer t;                                //デバッグ用

unsigned int pre_cnt=0;                 //スタンバイモードのカウント
unsigned int cnt=0;                     //カウント(ループ回数)
int32_t press, temp;                    //気圧, 気温
float altitude;                         //高度
int16_t ax, ay, az;                     //3軸加速度
int16_t acc;                            //合成加速度
int16_t gx, gy, gz;                     //3軸ジャイロ
unsigned int ms_time;                   //タイマー時間
//unsigned int voice_state;             //喋っている状況
unsigned int mode=0;                    //モード 0:スタンバイモード 1:フライトモード 2:発射 3or4:開放 5:着地

//RTC
struct tm rtc;
time_t seconds;
int time_h=0,time_m=0,time_s=0;
int xtime_h=0,xtime_m=0,xtime_s=0;
char time_buf[32];

//gps
int gps_i, rlock, stn;
char gps_data[256];
char ns,ew;
float gps_time,hokui,tokei;
unsigned int g_hokui, g_tokei;
unsigned int htime, mtime, stime;

const float R = 287.052; // specific gas constant R*/M0
const float g = 9.80665; // standard gravity 
const float t_grad = 0.0065; // gradient of temperature
const float t0 = 273.15 + 15; // temperature at 0 altitude
const float p0 = 101325; // pressure at 0 altitude

unsigned int servo_duty;                //サーボデューティー比
unsigned int launch_time;               //発射した時間
unsigned int deploy_time_1;             //開放1した時間
unsigned int deploy_time_2;             //開放2した時間
float gnd_alt, gnd_temp;                //地上の高度
float vertex=0.0;                       //最高高度
int pre_acc, acc_sw;                    //前の合成加速度
int dep_sw;                             //開放タイミング
int silent=0;                           //サイレントモード
int servo_sw=0;                         //サーボ保持やめるタイミング
int servo_sw2=0;                        //開放2後一定間隔でサーボを動かすタイミング

char total_data[256];                   //データフレーム
char data_buffer[DATA_BUNDLE][256];     //データバッファ(SDへ一度に保存する)
char Rdata[MAX_RDATA];                  //受信データfrom通信線

char spk_line[][256] = {
    "suta-toa,pu.\r",
    "konnnichiwa.watashiwa,ti'-mu,;ba-te,ku'suno,ro/ke',toni,tousaisareteiru,jinnkouti'nou,ri',tu/de_su.;yorosiku.\r",
    "ze'nnkaino,ri/se',to/kara <NUMK VAL=8 COUNTER=funn> <NUMK VAL=10 COUNTER=byo-> keikasimasi'ta.\r",
    ";uchiageyotei,ji'kokumade,a'to <NUMK VAL=8 COUNTER=funn> <NUMK VAL=59 COUNTER=byo-> de_su.\r",
    "rise,to.\r",
    "esudhi-ka-/do,rise,to.\r",
    "tijoukou'do/wo,sokuteityuude_su,sibara+ku,omachikudasa'i.\r",
    "tijoukou'dono,so/kuteiga,kanryousimasi'ta.\r",
    ";huraitomo-'do,;o'nn.\r",
    ";de'-tano/kirokuto,keisokuga,kaisisarema'sita.\r",
    ";rihuto,;o'hu rokextuxtu'toga,uchiagerarema'sita.\r",
    "parashu-'to,ichi,hou+shutu.\r",
    "parashu-'to,ni',hou+shutu.\r",
    "sa-bomo-'ta-,kudo-.\r",
    "rokextuxtu'towa,rakkatyuude_su.\r",
    "rokextuxtu'tono,tyakutiwo,kakuninnsima'sita.\r",
    ";uchiageyotei,ji'koku/wo,setteisima'sita.\r",
    ";genzaiji'koku/wo,setteisima'sita.\r",
    "huraito'pin,go'-.\r",
    "huraito'pin,no-go'-.setuzokuwo/kakuninnsitekudasa'i.\r",
    "sairento/mo'-do,o'hu.\r",
    "ge'nzai,kaihousagyoutyuude'_su.\r",
    "sintyokudo'ude_suka?\r",
    "#J",
    "#K",
    };

//プロトタイプ宣言
void tick();                            //データ取得割り込み
void sci_rx();                          //通信線コマンド割り込み
void gps_rx();                          //GPS通信線コマンド割り込み
void deployment();                      //開放アルゴリズム
void talk_line(int num);                //指定したセリフを喋る
void random_talk();                     //ランダムで喋る

////////////
//メイン関数//
////////////
int main() {
    talk_line(0);
    //Real Time Clock
    rtc.tm_sec = 00;                        // 0-59
    rtc.tm_min = 00;                        // 0-59
    rtc.tm_hour = 00;                       // 0-23
    rtc.tm_year = 115;                      // year since 1900
    seconds = mktime(&rtc);                 //時刻生成
    set_time(seconds);                      //時刻設定

    servo.period_ms(20);                    //サーボ波長
    servo = DUTY_BASE;                      //サーボをベース状態
    wait(1); servo.pulsewidth_us(0);        //保持やめる
    mpu.initialize();                       //MPU6050(加速度ジャイロ)初期
    sci.attach(sci_rx, Serial::RxIrq);      //シリアル受信割り込み
    uar.baud(4800);                         //GPS
    uar.attach(gps_rx, Serial::RxIrq);      //GPS割り込み
    timer.start();                          //タイマ開始
    timer.reset();                          //タイマ初期化
    xtimer.start();                          //タイマ開始
    xtimer.reset();                          //タイマ初期化
    for(int i=0;i<4;i++){myleds=myleds|1<<i;wait(0.4);} myleds=0x06;
    sci.baud(9600);
    sci.printf("\n\n******************************\n");
    sci.printf("**********START UP!!**********\n");
    sci.printf("******************************\n");
    talk_line(1);
    
    while(1){
        //RTC
        seconds = time(NULL);
        strftime(time_buf,sizeof(time_buf),"%H,%M,%S",localtime(&seconds));
        sscanf(time_buf,"%d,%d,%d",&time_h,&time_m,&time_s);
        if(mode==0){
            pre_cnt++;
            random_talk();
            myleds=myleds^0xff; wait(0.1);
        }
        else
        if(mode){
            
            //SDへの記録
            
            if(cnt%DATA_BUNDLE==DATA_BUNDLE-1){
                fp = fopen("/sd/data.csv", "a");
                if(fp==NULL){
                    sci.printf("<!>Error<!> ---> Could not open file\r\n");
                }else{
                    for(int i=0; i<DATA_BUNDLE; i++){       //DATA_BUDLE分のフレームを一度に保存
                        fprintf(fp, "%s", data_buffer[i]);
                        sprintf(data_buffer[i], "");
                    }
                    fclose(fp); 
                }
            }
            }
    }
}
//////////////////
//データ取得割り込み//
//////////////////
void tick(){
    t.start();
    t.reset();
    ms_time = timer.read_ms();                                      //タイマー(ミリ秒)
    
    press = ms5607.getPressure();                                       //気圧取得
    temp = ms5607.getTemperature()*100;                                 //気温取得
    altitude = t0/t_grad*(1-exp((t_grad*R/g)*log(press/p0)))-gnd_alt;   //高度取得
    if(altitude<2000 && altitude>vertex) vertex=altitude;
    if(vertex-altitude>2){dep_sw++;}else{dep_sw=0;}
    deployment();
    
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);                   //加速度ジャイロ取

    ax=float(ax); ay=float(ay); az=float(az);
    gx=float(gx);   gy=float(gy);   gz=float(gz);
    ax=float(ax)/2048*10000; ay=float(ay)/2048*10000; az=float(az)/2048*10000;
    gx=float(gx)/65.5*100;   gy=float(gy)/65.5*100;   gz=float(gz)/65.5*100;
    pre_acc = acc;
    acc = int(sqrt(double(ax*ax+ay*ay+az*az)));                       //合成加速度
    
    //データフレームへ格納
    sprintf(total_data,"%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%2d%2d%2d,%1d,%d,%2d%2d%2d,%d\r\n",
        press,temp,ax,ay,az,gx,gy,gz,
        g_hokui, g_tokei, htime, mtime, stime,
        mode, ms_time, time_h, time_m, time_s, cnt
        );
    sprintf(data_buffer[cnt%DATA_BUNDLE],"%s",total_data);   //データバッファへ格納
    //sci.printf("%s",total_data);                  //通信線へ
    //pc.printf("%s",total_data);                   //PCへ
    //LED Flip-flops
    if(cnt%6<4) myleds = 1 << cnt%6;
    else myleds = 4 >> (cnt%6-4);
    cnt++;          //カウント
    
    //sci.printf("%f\r\n",t.read());
}
////////////////////
//通信コマンド割り込み//
////////////////////
unsigned short sci_i=0;     //コマンドN文字目
char cmd[3];                //コマンド命令
char val[MAX_RDATA-3];      //コマンド引数
void sci_rx () {
    //受信データが大きすぎる場合
    if(sci_i>MAX_RDATA-1){
        sci_i = MAX_RDATA-1;
        Rdata[sci_i]='\n';
    }
    Rdata[sci_i]=sci.getc();    //受信データの格納
    if(Rdata[sci_i]=='\n'){     //受信データ区切り→コマンドごとの処理
        Rdata[sci_i-1]='\0';
        sci_i=0;
        
        sci.printf("\nReceved data: \"%s\" \r\n", Rdata);
        sscanf(Rdata,"%3s %s", cmd, val);   //コマンドを命令と引数に分割
        //リセット・コマンド
        if(strcmp(cmd, "RST")==0){
            sci.printf("---> Mbed reset \r\n"); talk_line(4);
            wait(0.1); mbed_reset();
        }else
        //フライトモード・コマンド
        if(strcmp(cmd, "FLT")==0){ 
            sci.printf("---> Flight-Mode ON! \r\n"); talk_line(8);
            mode=1;                                 //フライトモードオン!!
            ticker.attach(&tick, 0.1);              //タイマー割り込み
            timer.start();                          //タイマ開始
            timer.reset();                          //タイマ初期化
        }else
        //SDリセット・コマンド
        if(strcmp(cmd, "SDR")==0){
            fp = fopen("/sd/data.csv", "w");
            if(fp==NULL){ sci.printf("<!>Error<!> ---> Could not open file\r\n");}
            else{ fprintf(fp, ""); fclose(fp); sci.printf("---> SDcard Reset \r\n"); talk_line(5);}
            wait(0.1);
        }else
        //地上高度測定コマンド
        if(strcmp(cmd, "ALT")==0){
            sci.printf("---> Measureing state of rest\r\n"); talk_line(6);
            for(int i=0;i<100;i++){
                gnd_alt += t0/t_grad*(1-exp((t_grad*R/g)*log(ms5607.getPressure()/p0)));
                if(i%10==9){ sci.printf("@"); myleds=myleds^0xff; }
                wait(0.1);
            }
            gnd_alt/=100;
            sci.printf("\n---> Measurement Complete\r\n"); talk_line(7);
        }else
        //現在時刻設定
        if(strcmp(cmd, "SCT")==0){
            sci.printf("---> Set Current Time\r\n"); talk_line(17);
            rtc.tm_hour = int(atof(val)/10000);   // 0-23
            rtc.tm_min = int((atof(val)-rtc.tm_hour*10000)/100);    // 0-59
            rtc.tm_sec = int(atof(val)-rtc.tm_hour*10000-rtc.tm_min*100);    // 0-59
            seconds = mktime(&rtc);
            set_time(seconds);
        }else    
        //打上時刻設定設定
        if(strcmp(cmd, "SXT")==0){
            sci.printf("---> Set Launch Time\r\n"); talk_line(16);
            xtime_h = int(atof(val)/10000);   // 0-23
            xtime_m = int((atof(val)-xtime_h*10000)/100);    // 0-59
            xtime_s = int(atof(val)-xtime_h*10000-xtime_m*100);    // 0-59
        }else
        //パルス設定プログラム
        if(strcmp(cmd, "PWM")==0){
            servo = atof(val)*0.001/2 + 0.03;
            wait(1); servo.pulsewidth_us(0);
            sci.printf("\n---> Set Pulse Width\r\n"); talk_line(13);
        }else
        //フライトピンチェック
        if(strcmp(cmd, "FPC")==0){
            if(f_pin){sci.printf("---> Flight Pin Go !\r\n");talk_line(18);}
            else{sci.printf("<!><!><!> Flight Pin No Go <!><!><!>\r\n");talk_line(19);}
        }else
        //サイレントモード
        if(strcmp(cmd, "SLT")==0){
            if(silent==0){sci.printf("---> Silent Mode On !\r\n");silent=1;}
            else{sci.printf("---> Silent Mode OFF \r\n");silent=0;talk_line(20);}
        }else
        //スピーク・コマンド
        if(strcmp(cmd, "SPK")==0){
            int line_num = atoi(val);
            sci.printf("---> Now talking \"%s\" \r\n", spk_line[line_num]);
            talk_line(line_num);
        }else
        //トーク・コマンド
        if(strcmp(cmd, "TLK")==0){
            if(strlen(val)+4 > MAX_RDATA-3){
                sci.printf("<!>Error<!> ---> Exceeds size limit\r\n");
            }else{
                sci.printf("---> Now talking \"%s\" \r\n", val);
                strcat(val, ".\r");
                i2c.write(tlk_addr, val, strlen(val));
            }
        }else{
        //エラー
            sci.printf("<!>Error<!> ---> Unknown command \r\n");
        }
        strcpy(cmd, ""); strcpy(val, "");
    }else{                      //受信データ区切りでないなら格納続ける
        sci_i++;
    }
}

//番号を指定してセリフを喋るよ
void talk_line(int num){
    if(silent==0) i2c.write(tlk_addr, spk_line[num], strlen(spk_line[num]));
}

void gps_rx () {//I2C Interrupt RX
    if( (gps_data[gps_i]=uar.getc()) != '$'){
        gps_i++;
    }else{
        gps_data[gps_i]='\0';
        if ( sscanf(gps_data, "GPGGA,%f,%f,%c,%f,%c,%d,%d",&gps_time,&hokui,&ns,&tokei,&ew,&rlock,&stn) >= 1) {
            g_hokui=(int(hokui/100)+(hokui-int(hokui/100))/60)*1000000;
            g_tokei=(int(tokei/100)+(tokei-int(tokei/100))/60)*1000000;

            //time set
            htime=int(gps_time/10000); mtime=int((gps_time-htime*10000)/100); stime=int(gps_time-htime*10000-mtime*100);
            htime=htime+9;//UTC =>JST
        }
        gps_i=0;
        sprintf(gps_data,"");
    }
}

void random_talk(){
    int rnd = timer.read_us()%6;
    char msg[128];
    if(silent==0 && pre_cnt%600==0){
        if(rnd>=0 && rnd<=1){
            float nishitime=((xtime_h*3600+xtime_m*60+xtime_s)-(time_h*3600+time_m*60+time_s));
            int ato_m = int(nishitime/60); int ato_s = int(nishitime-ato_m*60);
            sprintf(msg,";uchiageyotei,ji'kokumade,a'to <NUMK VAL=%d COUNTER=funn> <NUMK VAL=%d COUNTER=byo-> de_su.\r",
            ato_m, ato_s);
        }else
        if(rnd>=2 && rnd<=3){
            sprintf(msg,";uchiageyotei,ji'kokuwa <NUMK VAL=%d COUNTER=ji> <NUMK VAL=%d COUNTER=funn> <NUMK VAL=%d COUNTER=byo-> de_su.\r",
            xtime_h, xtime_m, xtime_s);
        }else
        if(rnd==4){
            sprintf(msg,"sintyokuwa,;do'ude_suka?ganbattekudasai\r");
        }else
        if(rnd==5){
            sprintf(msg,"watasini'wa,joukuude,hutatu'no,parashu-'towo,houshutusuru'toiu,sekininngaarima_su.mina'sann,o-enn,yorosikuonegaisima_su.\r");
        }
        i2c.write(tlk_addr, msg, strlen(msg));
    }
}

void deployment(){
    //サーボ保持解除
    if(servo_sw){
        servo_sw++;
        if(servo_sw>10) {
            servo.pulsewidth_us(0);
            servo_sw=0;
        }
    }
    //発射判定
    if(mode==1){
         if(acc > 30000){
             mode=2; talk_line(10); sci.printf("Lift off!! (accel)\r\n"); timer.reset();
             launch_time = timer.read_ms();
         }else
         if(f_pin==0){
             mode=3; talk_line(10); sci.printf("Lift off!! (flight pin)\r\n"); timer.reset();
             launch_time = timer.read_ms();
         }
    }else
    //開放判定1
    if(mode==2 || mode==3){
        if(ms_time-launch_time > DEPLOY_TIME_1){  //タイマー開放
            mode=4; servo = DUTY_1; servo_sw++;
            sci.printf("Parachute1 deployed (timer)\r\n");
            deploy_time_1 = timer.read_ms(); talk_line(11);
        }else
        if(ms_time-launch_time>8000 && dep_sw==2){       //高度開放
            mode=5; servo = DUTY_1; servo_sw++;
            sci.printf("Parachute1 deployed (altitude)\r\n");
            deploy_time_1 = timer.read_ms(); talk_line(11);
        }
    }else
    //開放判定2
    if(mode==4 || mode==5){
        if(ms_time-deploy_time_1 > DEPLOY_TIME_2){  //タイマー開放
            mode=6; servo = DUTY_2; servo_sw++;
            sci.printf("Parachute2 deployed (timer)\r\n");
            deploy_time_2 = timer.read_ms(); talk_line(12);
        }else
        if(ms_time-deploy_time_1>8000 && altitude<DEPLOY_ALT2){
            mode=7; servo = DUTY_2; servo_sw++;
            sci.printf("Parachute2 deployed (altitude)\r\n");
            deploy_time_2 = timer.read_ms(); talk_line(12);
        }
    }else
    //着地判定
    if(mode==6 || mode==7){
        servo_sw2++;
        if(servo_sw2%10==0) { servo = DUTY_2; servo_sw++; }
        if(abs(acc-pre_acc)<1000){
            acc_sw++;
        }else{acc_sw=0;}
        if(acc_sw==10){
            sci.printf("Rocket Landing! \r\n"); talk_line(15); mode=8;
        }
    }
}