File content as of revision 1:8a25883c423c:

#include "mbed.h"
#include "xprintf.h"
#include "MPL3115A2.h"
#include "HMC5883L.h"
#include "MPU6050.h"
#include "SDFileSystem.h"
#include "EthernetPowerControl.h"
#define M_PI 3.141592

//hmc calibration mode
#define STOP 0  //initial
#define CAL 1   //calibration
#define RUN 2   //run

//Sequence phase
#define Preparing 1
#define Rising 2
#define Descending 3 
#define Landing_Fusing 4 
#define Moving 5

//Moving step
#define get_goaldirection 0
#define attitude_determination 1
#define calculate_direction 2
#define direction_control 3
#define jump 4

DigitalOut myled1(LED1);
DigitalOut myled2(LED2);
DigitalOut myled3(LED3);
DigitalOut myled4(LED4);

//ピンアサイン確認
Serial gps(p13, p14);
Serial Mu2(p28, p27);
I2C i2c(p9, p10);       // sda, scl
Serial mp(USBTX, USBRX);
SDFileSystem sd(p5, p6, p7, p8, "sd"); 
LocalFileSystem local("local");
DigitalOut controlmotor1(p22),controlmotor2(p23);//方向制御モーター
PwmOut pin21(p21);
DigitalOut jumpmotor(p24);

//溶断
DigitalOut para(p18); 
DigitalOut marker(p19);
DigitalOut jumparm(p20);
//
DigitalOut Flightpin(p26);

Timer timer;
int val;
int val_total;
unsigned char timer_set;
unsigned char phase;
unsigned char step;
unsigned char pitch;

//気圧///////
MPL3115A2 APsensor(&i2c, &mp);
int alt;
int ini_alt;
int min_alt;
int min_alt5;
int max_alt;
int rela_max;
int rela_ini;
int flag1;
int flag2;
int flag3;
unsigned char alttimes;
void getAltitude(){
    //wait_ms(300); 
    Altitude a;
    Temperature t;
    Pressure p;    
    APsensor.readAltitude(&a);
    APsensor.readTemperature(&t);        
    APsensor.setModeStandby();
    APsensor.setModeBarometer();
    APsensor.setModeActive();
    APsensor.readPressure(&p);
    alt = a;
    //気球試験用
    if(phase == Preparing){
        rela_ini = alt - ini_alt;
        if(rela_ini > 50)flag1 = flag1+1;
    }
    
    if(phase == Rising){
        if(max_alt < alt){
            max_alt = alt;
        }
        else{
            rela_max = max_alt -alt;
            if(rela_max > 150)flag2 = flag2+1;
        }
    }    
    //着地判定
    if(phase == Descending){
        if (min_alt > alt){
            min_alt = alt;
            min_alt5 = min_alt + 5;
            flag3 -= 1;
        }
        else {
            if( alt < min_alt5){
                flag3 +=2;
            }
        }
    }
    mp.printf("alt:%d,max:%d,ini:%d,phase:%d\r\n",alt,max_alt,ini_alt,phase);
    mp.printf("rela_ini:%d,rela_max:%d\r\n",rela_ini,rela_max);
    mp.printf("flag1:%d,flag2:%d,flag3:%d\r\n",flag1,flag2,flag3);
    //mp.printf("OFF_H: 0x%X, OFF_T: 0x%X, OFF_P: 0x%X\r\n", APsensor.offsetAltitude(), APsensor.offsetTemperature(), APsensor.offsetPressure()); 
    APsensor.setModeStandby();
    APsensor.setModeAltimeter();
    APsensor.setModeActive();
}
/////

//GPS
int i,rlock,mode;
char gps_data[256];
char ns,ew;
float w_time,hokui,tokei;
float g_hokui,g_tokei;
float d_hokui,m_hokui,d_tokei,m_tokei;
unsigned char c;
//目標経度，緯度
float goal_tokei,goal_hokui;
float goal_direction;
float direction_y;
float direction_x;
float angular_difference;
 
void getGPS(){
  //目的地 ブラックロック修正
    goal_tokei = 119.1218;
    goal_hokui = 40.8797;
    c = gps.getc();
  if( c=='$' || i == 256){
    mode = 0;
    i = 0;
    for(int j=0; j<256; j++){
        gps_data[j]=NULL;
    }
  }
  if(mode==0){
    if((gps_data[i]=c) != '\r'){
      i++;
    }else{
      gps_data[i]='\0';
      
      if( sscanf(gps_data, "$GPGGA,%f,%f,%c,%f,%c,%d",&w_time,&hokui,&ns,&tokei,&ew,&rlock) >= 1){
          if(rlock==1){
          //mp.printf("Status:Lock(%d)\n\r",rlock);
          //logitude
          d_tokei= int(tokei/100);
          m_tokei= (tokei-d_tokei*100)/60;
          g_tokei= d_tokei+m_tokei;
          //Latitude
          d_hokui=int(hokui/100);
          m_hokui=(hokui-d_hokui*100)/60;
          g_hokui=d_hokui+m_hokui;
          mp.printf("Lon:%.6f, Lat:%.6f\n\r",g_tokei, g_hokui);
          Mu2.printf("@DT14%f,%f\r\n",g_tokei, g_hokui);
          float longitudinalDifference =-(goal_tokei - g_tokei);
          float latitudinalDifference = goal_hokui - g_hokui;  
          
          //球面三角法
          direction_y = cos(goal_hokui*M_PI/180)*sin(longitudinalDifference*M_PI/180);
          direction_x = cos(g_hokui*M_PI/180)*sin(goal_hokui*M_PI/180) - sin(g_hokui*M_PI/180)*cos(goal_hokui*M_PI/180)*cos(longitudinalDifference*M_PI/180);
          goal_direction = atan2f(direction_y,direction_x);
          if(goal_direction < 0)goal_direction += 2*M_PI;
           
          //磁北に対する角度に調節 ネバダ東に14度
          goal_direction -= 0.244346;
          if (goal_direction < 0) goal_direction += 2*M_PI;
          mp.printf("goal:%f\n\r",goal_direction*180/M_PI);
          //mp.printf("longitudinalDifference:%f",longitudinalDifference);
          //mp.printf("latitudinalDifference:%f",latitudinalDifference);
          if(phase == Descending)getAltitude();
          if(phase == Moving)step = attitude_determination;
        }
        else{
          mp.printf("\n\rStatus:unLock(%d)\n\r",rlock);
          mp.printf("%s",gps_data);
          Mu2.printf("@DT02No\r\n");
          if(phase == Descending)getAltitude();
          if(phase == Moving)step = jump;
        }
        sprintf(gps_data, "");
      }
    }
  }
}

void MU2initialize(){
    Mu2.baud(19200);
    Mu2.printf("@EI34\r\n");
    wait(1);
    Mu2.printf("@DI25\r\n");
    wait(1);
    Mu2.printf("@CH0F\r\n");
    wait(1);
    Mu2.printf("@GI34\r\n");
    wait(1);
 }
 
/////////////////////
//地磁気
HMC5883L compass(p9, p10);
Ticker interrupt;
double heading0 = 0.0;
double heading1 = 0.0;
double heading2 = 0.0;
double heading3 = 0.0;
double headingLPF = 0.0;
double initHeading;
double tgtHeading;
double preHeading = 0.0;
unsigned char hmc_mode;
 
int maxX, minX, maxY, minY;
int ofsX = 0;                   //calibration x
int ofsY = 0;                   //calibration y
int counter = 0;
int ofsXset = 1122;
int ofsYset = -466;
int ofsZset = -522;

//地磁気キャリブレーション用
void intrpt() {
    int16_t raw[3];     
    compass.getXYZ(raw);
    //double heading = atan2(static_cast<double>(raw[2]-ofsY), static_cast<double>(raw[0]-ofsX)); //y=raw[2]
    double heading = atan2(static_cast<double>(raw[2]-ofsYset), static_cast<double>(raw[0]-ofsXset));
    if(heading < 0)heading += 2*M_PI;
    if(heading > 2*M_PI)heading -= 2*M_PI;
    heading3 = heading2;
    heading2 = heading1;
    heading1 = heading0;
    heading0 = heading;
    headingLPF = (heading0 + heading1 + heading2 + heading3)/4;//low pass filter
    
    switch(hmc_mode) {
    case STOP:
        if(counter == 100) {
            initHeading = headingLPF;
            hmc_mode = CAL;
            maxX = raw[0];
            minX = raw[0];
            maxY = raw[2];
            minY = raw[2];
            counter = 0;
            //mp.printf("STOP end\n\r");
        }
        break;
    case CAL:
        if(raw[0] > maxX)maxX = raw[0];
        if(raw[0] < minX)minX = raw[0];
        if(raw[2] > maxY)maxY = raw[2];
        if(raw[2] < minY)minY = raw[2];
        if((counter > 100) 
            && (headingLPF > (initHeading-0.01)) 
            && (headingLPF < (initHeading+0.01))) {
            ofsX = (maxX + minX)/2;
            ofsY = (maxY + minY)/2;
            //hmc_mode = RUN;
            counter = 0;
            //mp.printf("ofs=%d,%d\n\r",ofsX,ofsY);
        }
        break;
    case RUN:
        mp.printf("heading=%f,x=%d,y=%d\n\r",headingLPF*180/M_PI,raw[0]-ofsXset,raw[2]-ofsYset);
        //mp.printf("%d,%d,%d\r\n",raw[0],raw[2],raw[1]);
        //step = calculate_direction;
        break;
    
    }
    counter++;
}

//加速度
MPU6050 mpu;
int16_t ax, ay, az;
int16_t gx, gy, gz;
int32_t axa, aya, aza;
int32_t gxa, gya, gza;

//オフセット値
double axoffset = 586.711;
double ayoffset = -174.847;
double azoffset = -2195.34375;

//センサの出力あたりの加速度[m/s^2]
double axrate = 0.000601615;
double ayrate = 0.000598334;
double azrate = 0.0005845;

//3軸キャリブレーション
double phi;
double theta;
double heading3axis;

double bfy;
double bfx;


//3軸からheading算出
void getheading3axis() {
    //加速度算出
    mpu.getAcceleration(&ax, &ay, &az);
    double axcal = -axrate * (static_cast<double>(ax) - axoffset);
    double aycal = -ayrate * (static_cast<double>(ay) - ayoffset);
    double azcal = -azrate * (static_cast<double>(az) - azoffset);
    
    phi = atan2(aycal,azcal);
    //phi += M_PI;
    theta = atan2(-axcal,aycal*sin(phi)+azcal*cos(phi));
    if(theta > 0)pitch = 1;
    if(theta < 0)pitch = 0;
    //theta += M_PI;
    
    //地磁気算出
    int16_t raw[3];     
    compass.getXYZ(raw);
    bfy = -(static_cast<double>(raw[1]-ofsZset))*sin(phi) + (static_cast<double>(raw[2]-ofsYset))*cos(phi);
    bfx = (static_cast<double>(raw[0]-ofsXset))*cos(theta) 
        + (static_cast<double>(raw[2]-ofsYset))*sin(theta)*sin(phi)
        + (static_cast<double>(raw[1]-ofsZset))*sin(theta)*cos(phi);
    double heading = atan2(-bfy, bfx);
    if(heading < 0)heading += 2*M_PI;
    if(heading > 2*M_PI)heading -= 2*M_PI;
    heading3 = heading2;
    heading2 = heading1;
    heading1 = heading0;
    heading0 = heading;
    headingLPF = (heading0 + heading1 + heading2 + heading3)/4;//low pass filter
    //headingLPF += 0.349066;
    if(headingLPF > 2*M_PI) headingLPF -= 2*M_PI;
    //mp.printf("ax:%f,ay:%f,az:%f\r\n",axcal,aycal,azcal);
    mp.printf("heading:%f,phi:%f,theta:%f\r\n",headingLPF*180/M_PI,phi*180/M_PI,theta*180/M_PI);        
 }
 
//ADXL375
const int addr = 0xA6;
char adxl[6];
short int axout = 0;
short int ayout = 0;
short int azout = 0;

void adxl_init(){
    char fifo[2] = {0x38,0x80};
    i2c.write(addr,fifo,2);     //FIFO_Mode -> Stream
    char power[2] = {0x2D,0x08};
    i2c.write(addr,power,2);    //measurebit -> 1
}

void adxl_get(){
    adxl[0] = 0x32;
    i2c.write(addr,adxl,1);
    i2c.read(0xA7,adxl,6);        
    axout = (((int16_t )adxl[1]) << 8) | adxl[0];
    ayout = (((int16_t )adxl[3]) << 8) | adxl[2];
    azout = (((int16_t )adxl[5]) << 8) | adxl[4];
}


//モーター駆動方向制御
void motor_drive(){
        controlmotor1.write(1);
        controlmotor2.write(0);
        pin21.write(1.0);
        //if(pin1)mp.printf("Yes!\r\n");
}

//溶断プログラム
//最終チェックポイント////////////////////////////////
void burning(){
    wait(5);
    myled1 = 1;
    //para = 1;
    wait(1);    
    myled1 = 0;
    //para = 0;
    wait(5);
    myled2 = 1;
    //marker = 1;
    wait(1);
    myled2 = 0;
    //marker = 0;
    wait(5);
    myled3 = 1;
    //jumparm = 1;
    wait(1);
    myled3 = 0;
    //jumparm = 0;    
}
//跳躍モーター駆動まいまいかわいい
void jumping(){
    timer.start();
    //モーター駆動時間調整
    while(val < 4){
        val = timer.read();
        jumpmotor = 1;
        myled4 != myled4;
    }
    timer.reset();
    timer.stop();
    jumpmotor = 0;
    val = 0;
}

///test
//GPSなし方向制御，跳躍テスト
float test_tokei,test_hokui;
void test_get_direction(){
    //目的地 大岡山駅
    goal_tokei = 139.686234;
    goal_hokui = 35.614330;
    
    //テスト場所石川台一号館
    test_tokei = 139.681529;
    test_hokui = 35.605263;
    
    float longitudinalDifference = goal_tokei - test_tokei;
    float latitudinalDifference = goal_hokui - test_hokui;
    //球面三角法
    direction_y = cos(goal_hokui*M_PI/180)*sin(longitudinalDifference*M_PI/180);
    direction_x = cos(test_hokui*M_PI/180)*sin(goal_hokui*M_PI/180) - sin(test_hokui*M_PI/180)*cos(goal_hokui*M_PI/180)*cos(longitudinalDifference*M_PI/180);
    goal_direction = atan2f(direction_y,direction_x);
    if(goal_direction < 0)goal_direction += 2*M_PI;
           
    //磁北に対する角度に調節 東京7度
    goal_direction += 0.122173;
    //goal_direction2 += 0.122173;
    if (goal_direction < 0) goal_direction += 2*M_PI;
} 

/////////////////////////main////////////////////////////////////
int main(){
    PHY_PowerDown();//省電力
    wait(2);
    //センサ初期化
    Mu2.baud(19200);
    Mu2.printf("@EI34\r\n");
    wait(1);
    Mu2.printf("@DI25\r\n");
    wait(1);
    Mu2.printf("@CH0F\r\n");
    wait(1);
    Mu2.printf("@GI34\r\n");
    wait(1);
    APsensor.init();
    wait(0.2);
    hmc_mode = RUN;
    mpu.initialize();
    wait(0.2);
    compass.init();
    wait(0.2);
    adxl_init();
    wait(0.2);
    APsensor.setOffsetAltitude(100);
    APsensor.setOffsetTemperature(10);
    APsensor.setOffsetPressure(-32);
    mkdir("/sd/mydir", 0777);
    min_alt = 5000.0;
    Flightpin = 1;
    //高度初期値
    for(int h = 0; h < 10; h++){
        getAltitude();
        ini_alt = ini_alt + alt;
        wait(0.5);
    }
    ini_alt = ini_alt/10;
    mp.printf("ini_alt:%d\r\n",ini_alt);
    int jump_count;
    
    //最終チェックポイント/////////////////////////////////////////
    //phase = Preparing;
    //wait(1500);//開始25分待機
    //phase = Descending;
    //phase = Moving;
    phase = Landing_Fusing;
    /////////////////////////////単機能////////
    
    //wait(5);
   
    //while(1){
        //myled1 = 1;
        //Flightpin = 1;
        //GPS_MU2
        //getGPS(); 
        //方向制御用モーター
        /*
        controlmotor1.write(1);
        controlmotor2.write(0);
        pin21.write(0.8);
        wait(3);
        controlmotor1.write(0);
        controlmotor2.write(0);
        pin21.write(0.0);
        */
        //跳躍用モーター
        //jumpmotor = 1;
        //wait(5);
        //jumpmotor = 0;
        //溶断
        //burning();
        //気圧
        //getAltitude();
        //mpu
        /*
        mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
        double axcal = -axrate * (static_cast<double>(ax) - axoffset);
        double aycal = -ayrate * (static_cast<double>(ay) - ayoffset);
        double azcal = -azrate * (static_cast<double>(az) - azoffset);
        mp.printf("x:%f,y:%f,z:%f",axcal,aycal,azcal);
        */
        //hmc
        /*
        hmc_mode = RUN;
        intrpt();
        wait(0.5);
        */
        //hmc_mpu
        //getheading3axis() ;
        //wait(0.3);   
        //adxl_SDカード 跳躍高さ測定
        /*
        mkdir("/sd/mydir", 0777);
        wait(1);
        FILE *fp = fopen("/sd/mydir/height1.txt", "a");
        wait(1);
        if(fp == NULL) {
            error("Could not open file for write\n");
        }
        timer.reset();
        timer.start();
        val = 0;
        while(val < 5000){
            jumpmotor = 1;
            val = timer.read_ms();
            adxl[0] = 0x32;
            i2c.write(addr,adxl,1);
            i2c.read(0xA7,adxl,6);
            axout = (((int16_t )adxl[1]) << 8) | adxl[0];
            ayout = (((int16_t )adxl[3]) << 8) | adxl[2];
            azout = (((int16_t )adxl[5]) << 8) | adxl[4];
            fprintf(fp, "%d,%d,%d,%d\r\n",val,axout,ayout,azout); 
            wait_ms(2);
            myled1 !=myled1;
        }
        myled1 = 0;
        jumpmotor = 0;
        wait(1);
        fclose(fp);
        */
        //
                
    //}
    /////////////////////////    
       
    //着地検知用timer
    timer.reset();
    timer.start();
    val = 0;  
    while(1){
        switch(phase){
        case Preparing:
            myled1 = 1;
            getAltitude();
            wait(0.5);
            val = timer.read();
            if(flag1 > 20) phase = Rising;
            if( val > 600) phase = Rising; //計35分
            break; 
        case Rising:
            myled1 = 0;
            myled2 = 1;
            getAltitude();
            wait(0.5);
            val = timer.read();
            if(flag2 > 40) phase =  Descending;
            if(val > 1800) phase =  Descending; //計55分
            break;    
        case Descending:
            val = timer.read();  
            myled2 = 0;
            myled3 = 1;
            if(flag3 < 50){
                getGPS();
            }
            else{
                getAltitude();
            }                        
            if ( val >1500){
                timer.reset();
                val = 0;
                timer_set = 1;
            }
            if (timer_set == 1){
                val_total = 1500 + val;
            }
            //最終チェックポイント//////////////////////////////////////////    
            if ( flag3 >= 200){
                if( val > 900){
                phase = Landing_Fusing;
                }
            }
            if( val_total >2400 ){
                wait(600);
                phase = Landing_Fusing;//電源onから75分
            }               
            break;
        case Landing_Fusing:
            timer.reset();
            timer.stop();
            val = 0;
            myled3 = 0;
            myled4 = 1;
            //wait(30);
            burning();
            wait(5);
            Mu2.printf("@DT04FIRE\r\n");
            for ( int gp; gp <= 10; gp++){
                getGPS();
            }

            wait(2);
            jumpmotor = 1;
            wait(10);
            jumpmotor = 0;
            wait(2);
            pin21.write(1.0);
            controlmotor1.write(0);
            controlmotor2.write(1);
            wait(4);
            pin21.write(0.0);
            controlmotor1.write(0);
            controlmotor2.write(0);
            wait(3);
            jumpmotor = 1;
            wait(10);
            jumpmotor = 0;
            phase = Moving;           
            break;
        case Moving:
            myled2 = 0;
            myled1 = 0;
            myled3 = 0;
            myled4 = 0;
            wait(1);
            switch(step){
            case get_goaldirection:
                myled1 = 1;
                getGPS();
                step = attitude_determination;
                wait(1);
                break;
            case attitude_determination:
                myled1 = 0;
                myled2 = 1;
                timer.reset();
                timer.start();
                val = 0;
                while(val < 3){
                    val = timer.read();
                    getheading3axis();
                    wait(0.5);
                }
                timer.reset();
                timer.stop();
                val = 0;
                getheading3axis();
                step = calculate_direction;
                wait(1);
                break;
            case calculate_direction:
                myled2 = 0;
                myled3 = 1;
                if(headingLPF < M_PI){
                    if(goal_direction < headingLPF+M_PI){
                        angular_difference = headingLPF - goal_direction;
                    }
                    else{
                        angular_difference = 2*M_PI + (headingLPF - goal_direction);
                    }
                }
                else{
                    if(goal_direction < headingLPF - M_PI){
                        angular_difference = headingLPF - goal_direction - 2*M_PI;
                    }
                    else{
                        angular_difference = headingLPF - goal_direction;
                    }   
                }
                mp.printf("heading:%f,goal_direction%f\n\r",headingLPF*180/M_PI,goal_direction*180/M_PI);
                //mp.printf("rangular_difference = %f\n\r" , angular_difference*180/M_PI);
                //40度以下でジャンプ
                if ((angular_difference < 0.698132) && (angular_difference > -0.698132)){
                    step = jump;
                }
                else {
                    step = direction_control;
                }
                if(pitch == 1) step = direction_control;
                wait(1);
                break;
                
            case direction_control:
                myled1 = 0;
                myled3 = 0;
                myled4 = 1;
                //int drive_time;
                timer.reset();
                timer.stop();
                timer.start();
                val = 0;
                pin21.write(1.0);
                if(pitch == 1){
                    controlmotor1.write(0);
                    controlmotor2.write(1);
                }else{
                    controlmotor1.write(1);
                    controlmotor2.write(0);
                }
                wait(3);            
                controlmotor1.write(0);
                controlmotor2.write(0);
                pin21.write(0.0);
                timer.reset();
                timer.stop();
                val = 0;
                //step = jump;
                if(jump_count == 2){
                    step = jump;
                }
                else{
                    step = attitude_determination;
                }
                jump_count = jump_count+1;
                break;
                
            case jump:
                //ログファイル
                myled1 = 1;
                myled2 = 1;
                wait(2);
                FILE *fp = fopen("/sd/mydir/height.txt", "a");
                wait(1);
                timer.reset();
                timer.start();
                val = 0;
                jumpmotor = 1;
                Mu2.printf("@DT04JUMP\r\n");
                while(val < 4000){
                    val = timer.read_ms();
                    if(fp == NULL) {
                        jumpmotor = 1;
                    }
                    else{
                        adxl[0] = 0x32;
                        i2c.write(addr,adxl,1);
                        i2c.read(0xA7,adxl,6);
                        axout = (((int16_t )adxl[1]) << 8) | adxl[0];
                        ayout = (((int16_t )adxl[3]) << 8) | adxl[2];
                        azout = (((int16_t )adxl[5]) << 8) | adxl[4];
                        fprintf(fp, "%d,%d,%d,%d\r\n",val,axout,ayout,azout); 
                        wait_ms(2);
                    }
                }
                jumpmotor = 0;
                wait(1);
                if(fp != NULL)fclose(fp);
                myled3 = 1;
                val = 0;
                jump_count = 0;
                wait(3);
                step = get_goaldirection;
                break;
            }
        }                       
    }
}