File content as of revision 4:21a356ae0747:

/*
This system is vertion 2.0. Thsi program enable us to read signals from receiver ,measure balance of circit, and actuate servos. 
*/

#include "mbed.h"
#include "string.h"
#include "MPU6050.h"

#include <stdio.h>
#include <errno.h>

#include "SDBlockDevice.h"
#include "FATFileSystem.h"

#include "myConstants.h"

#define READBUFFERSIZE  70
#define DELIMITER   ","
#define M_PI 3.141592
#define PI2  (2*M_PI)

/*ジャイロセンサの補正値（引く）*/
#define GYRO_FIX_X 290.5498
#define GYRO_FIX_Y 99.29363
#define GYRO_FIX_Z 61.41011

/*---------ピン指定-------------------*/
DigitalOut myled(LED1);

DigitalIn switch_off(p30);//sdカードへの書き込みを終了させる

InterruptIn button(p5);//GPSモジュールからの1pps信号を読み取る（立ち下がり割り込み）

InterruptIn input_from_Aileron_1(p11);
InterruptIn input_from_elevator(p12);//
InterruptIn input_from_throttle(p13);//
InterruptIn input_from_rudder(p14);//
InterruptIn input_from_gear(p15);//
InterruptIn input_from_Aileron_2(p16);//
InterruptIn input_from_pitch(p17);//
InterruptIn input_from_AUX5(p18);//


MPU6050 mpu(p28, p27);//sda scl

Serial pc(USBTX, USBRX);    // tx, rx
Serial gps(p9,p10);// gpsとの接続 tx, rx 
SDBlockDevice   blockDevice(p5, p6, p7, p8);  // mosi, miso, sck, cs

//declare PWM pins here
PwmOut ESC (p21);
PwmOut Elevator_servo(p22);
PwmOut Rudder_servo(p23);
PwmOut Aileron_R_servo(p25);
PwmOut Aileron_L_servo(p26);

/*-----------------------------------*/

/*-----ファイルポインタ-----*/
// File system declaration
FATFileSystem   fileSystem("fs");
/*-----------------------*/

/*------------------------GPSの変数------------------------*/
int GPS_flag_update=0;
int GPS_interruptIn=0;

float longtude_gps,latitude_gps,azimuth_gps,speed_gps;
    
//char NMEA_sentense[READBUFFERSIZE]="$GPRMC,222219.000,A,3605.4010,N,14006.8240,E,0.11,109.92,191016,,,A";
char NMEA_sentense[READBUFFERSIZE];

/*--------------------------------------------------------*/


/*--------------------------角度とセンサ--------------------------*/
float sensor_array[6];
float Acc_Pitch_Roll[2];
float Gyro_Pitch_Roll[2];//pitch and roll
float omega_z;
/*--------------------------------------------------------------*/

/*----------PID コントロール-----------------*/
#define Gain_Kp 0.8377
#define Gain_Ki  0.4450
#define Gain_Kd  0.3890
 
double Prop_p,Int_p,Dif_p;
double Pre_Prop_p;
/*-----------------------------------------*/

/*-----------サーボコントロール----------------*/
//define period of servo here
#define SERVO_PERIOD 0.020 // servo requires a 20ms period
#define NUTRAL  0.0015
#define UPPER_DUTY  0.0020
#define LOWER_DUTY  0.0010

#define THRESHOLD 0.0018

double pitch_duty,roll_duty,yaw_duty;

double first_period,second_period,third_period,fourth_period;
double fifth_period,sixth_period,seventh_period,eighth_period;

double first_period_old=0.0,second_period_old=0.0,third_period_old=0.0,fourth_period_old=0.0;
double fifth_period_old=0.0,sixth_period_old=0.0,seventh_period_old=0.0,eighth_period_old=0.0;

double throttle_duty,elevator_duty,rudder_duty,switch_duty,aileron_duty;

/*------------------------------------------*/

/*-----タイマー---------*/
Timer passed_time;//経過時間の計算
Timer ch_time;//

float Time_new,Time_old=0.0;
float Servo_command=0.002;
/*---------------------*/
/*---------PWM割り込みフラグ-------*/
int PWM_flag_update=0;
/*-------------------------------*/

/*----------プロトタイプ宣言----------*/
void gps_rx();
void gps_read();

double Degree_to_PWM_duty(double degree);
double PID_duty_pitch(double target,double vol,float dt);
double PID_duty_rudder(double target,double vol,float dt,float yaw_rate);
double PID_duty_roll(double target,double vol,float dt);

void Acc_to_Pitch_Roll(float x_acc,float y_acc,float z_acc,float qua[2]);
void scan_update(float box_sensor[6]);
void sensor_update();

//gpsの1pps信号の割り込み
void flip();


void Initialize_ESC();
void Activate_ESC();

void Input_Aileron_1();
void Input_elevator();
void Input_throttle();
void Input_rudder();
void Input_gear();
void Input_Aileron_2();
void Input_pitch();
void Input_AUX5();

void Input_Aileron_1_fall();
void Input_elevator_fall();
void Input_throttle_fall();
void Input_rudder_fall();
void Input_gear_fall();
void Input_Aileron_2_fall();
void Input_pitch_fall();
void Input_AUX5_fall();
/*---------------------------------*/


int main() {
    
    wait(3);
    
    
    /*sdカードにファイルを構成する そのファイルを書き込み状態にする*/
    
    
    
       pc.printf("--- Mbed OS filesystem example ---\n");

    // Try to mount the filesystem
    pc.printf("Mounting the filesystem... ");
    fflush(stdout);

    int err = fileSystem.mount(&blockDevice);
    pc.printf("%s\n", (err ? "Fail :(" : "OK"));
    if (err) {
        // Reformat if we can't mount the filesystem
        // this should only happen on the first boot
        pc.printf("No filesystem found, formatting... ");
        fflush(stdout);
        err = fileSystem.reformat(&blockDevice);
        pc.printf("%s\n", (err ? "Fail :(" : "OK"));
        if (err) {
            error("error: %s (%d)\n", strerror(-err), err);
        }
    }

    // Open the numbers file
    pc.printf("Opening \"/fs/log.csv\"... ");
    fflush(stdout);

    FILE*   f = fopen("/fs/log.csv", "r+");
    FILE*   fp = fopen("/fs/log.txt", "r+");
    
    pc.printf("%s\n", (!f ? "Fail :(" : "OK"));
    
    if (!f) {
        // Create the numbers file if it doesn't exist
        pc.printf("No file found, creating a new file... ");
        fflush(stdout);
        f = fopen("/fs/log.csv", "w+");
        fp = fopen("/fs/log.txt", "w+");
        
        pc.printf("%s\n", (!f ? "Fail :(" : "OK"));
            }
            
    
    
    
    /*define period of serve and ESC*/
    ESC.period(SERVO_PERIOD);    
    Elevator_servo.period(SERVO_PERIOD);
    Rudder_servo.period(SERVO_PERIOD);
    Aileron_R_servo.period(SERVO_PERIOD);
    Aileron_L_servo.period(SERVO_PERIOD);
    /*------------------------------*/
    Elevator_servo.pulsewidth(NUTRAL); // servo position determined by a pulsewidth between 1-2ms
    Rudder_servo.pulsewidth(NUTRAL);
    Aileron_R_servo.pulsewidth(NUTRAL);
    Aileron_L_servo.pulsewidth(NUTRAL);
    
    Activate_ESC();//Activate ESC
    
    /*--------------ピン変化割り込みに対応した関数の宣言--------------*/
    button.fall(&flip);//GPSモジュールからの1pps信号を立ち下がり割り込みで読み取る。

    input_from_throttle.rise(&Input_throttle);//

    input_from_Aileron_1.fall(&Input_Aileron_1_fall);
    input_from_elevator.fall(&Input_elevator_fall);//
    input_from_throttle.fall(&Input_throttle_fall);//
    input_from_rudder.fall(&Input_rudder_fall);//
    input_from_gear.fall(&Input_gear_fall);
    input_from_Aileron_2.fall(&Input_Aileron_2_fall);//
    input_from_pitch.fall(&Input_pitch_fall);//
    input_from_AUX5.fall(&Input_AUX5_fall);//    
    
    ch_time.start();//時間計測スタート
    /*----------------------------------------------------------*/
    
    pc.baud(115200);
    gps.baud(115200);
    
    //gps.attach(gps_rx, Serial::RxIrq);//シリアル割り込み
    //pc.attach(gps_rx, Serial::RxIrq);//シリアル割り込み
    
    passed_time.start();//タイマー開始
    Time_old=float(passed_time.read());
    
    /*初期姿勢角の算出*/
    scan_update(sensor_array);//角速度、加速度を求める
    Acc_to_Pitch_Roll(sensor_array[3],sensor_array[4],sensor_array[5],Acc_Pitch_Roll);
    Gyro_Pitch_Roll[0]=Acc_Pitch_Roll[0];//初期姿勢角の算出 Pitch
    Gyro_Pitch_Roll[1]=Acc_Pitch_Roll[1];//初期姿勢角の算出 Roll
    
    pc.printf("Start!!\r\n");
    
    while(1) {
       
        switch (GPS_interruptIn) {
        case 1:
           
           gps_rx();
           gps_read();    
           GPS_interruptIn--;
            break;
            
        default:
            
           sensor_update();       
            break;
        }//switch end
        
        /*
        ここでプロポの信号のパルス幅を読み取って、手動or 自動の切り替えを行う必要がある。
        以下のpitch control等は自動操縦とみなしてよい。
        */
        
        
        if(switch_duty>THRESHOLD){//オートパイロット
            
            //Pitch control
            //pitch_duty=NUTRAL+PID_duty_pitch(0.0,Gyro_Pitch_Roll[0],(Time_new-Time_old));
            elevator_duty=NUTRAL+PID_duty_pitch(0.0,Gyro_Pitch_Roll[0],(Time_new-Time_old));
            Elevator_servo.pulsewidth(elevator_duty); 
            
            //roll control
            //roll_duty=NUTRAL+PID_duty_roll(0.0,Gyro_Pitch_Roll[1],(Time_new-Time_old))
            aileron_duty=NUTRAL+PID_duty_roll(0.0,Gyro_Pitch_Roll[1],(Time_new-Time_old));
            
            //角速度だけフィードバック    
            rudder_duty=NUTRAL+PID_duty_rudder(0.0,0.0,(Time_new-Time_old),omega_z);
            ///pc.printf("%f\r\n",rudder_duty);
            
            
            //スロットルはマニュアル
            //Servo_command
            if( (float(passed_time.read())-Servo_command)>0.003 ){
                
                ESC.pulsewidth(throttle_duty);            
                Elevator_servo.pulsewidth(elevator_duty);
                Rudder_servo.pulsewidth(rudder_duty); 
                Aileron_R_servo.pulsewidth(aileron_duty);
                Aileron_L_servo.pulsewidth(aileron_duty);
                
                Servo_command=float(passed_time.read());//サーボを動かしたタイミングの更新
                }else{}
            
            }else{//マニュアルモード
            
            if( (float(passed_time.read())-Servo_command)>0.003 ){
                            
                            ESC.pulsewidth(throttle_duty);
                            Elevator_servo.pulsewidth(elevator_duty); 
                            Rudder_servo.pulsewidth(rudder_duty);
                            Aileron_R_servo.pulsewidth(aileron_duty);
                            Aileron_L_servo.pulsewidth(aileron_duty);
                            
                            Servo_command=float(passed_time.read());//サーボを動かしたタイミングの更新
                            
                            }else{}
                
                }
        
        pc.printf("%f,%f,%f,%f,%f,%f\r\n",Time_new,Gyro_Pitch_Roll[0],Gyro_Pitch_Roll[1],azimuth_gps,latitude_gps,longtude_gps);
        err = fprintf(f,"%f,%f,%f,%f\r\n",Time_new,Gyro_Pitch_Roll[0],Gyro_Pitch_Roll[1],azimuth_gps);
        err = fprintf(fp,"%f,%f\r\n",latitude_gps,longtude_gps);
                    
        Time_old=Time_new;//時間更新
        
        if(switch_off==1){
            //fclose(fp);
            //flipper.detach();
            
            err = fclose(f);
            err = fclose(fp);
            
            pc.printf("Writing finish!");
            myled=0;
            break;
                    
        }
                    
        //wait(0.002);//この値は10倍されると考える
        }//while ends
    
}//main ends

void gps_rx(){
       
      // __disable_irq(); // 割り込み禁止
       
    int i=0;
          
      while(gps.getc()!='$'){
      }
      while( (NMEA_sentense[i]=gps.getc()) != '\r'){
      //while( (NMEA_sentense[i]=pc.getc()) != '\r'){
        //pc.putc(NMEA_sentense[i]);
        i++;
        if(i==READBUFFERSIZE){  
           i=(READBUFFERSIZE-1);
           break;
         }
      }
      NMEA_sentense[i]='\0';
      //pc.printf("\r\n");
      
      if(GPS_flag_update==0){
              GPS_flag_update++;
            }
            
    //__enable_irq(); // 割り込み許可
    
}//void gps_rx

void gps_read(){
    /*------gps のNMEAフォーマットの処理プログラム----------------*/
        float temp=0.0, deg=0.0, min=0.0;
            
        char *pszTime;//UTC時刻
        char* pszLat;////緯度
        char* pszLong;//経度
        char* pszSp;//速度
        char* pszSpd;//速度方向
        char* pszDate;//日付
        
        //strtokで処理した文字列は内容がNullになるので、テストでは、毎回NMEA_sentense[128]を初期化しないといけない
        //char NMEA_sentense[128]="$GPRMC,222219.000,A,3605.4010,N,14006.8240,E,0.11,109.92,191016,,,A";
        
        //Time_old=float(passed_time.read());
       
       
        
        if((GPS_flag_update==1)&&(0==strncmp( "GPRMC", NMEA_sentense, 5 ))){//GPS_flag_updateも判定に加えた方がいい
        //if(0==strncmp( "$GPRMC", NMEA_sentense, 6 )){//GPS_flag_updateも判定に加えた方がいい
        
                   strtok( NMEA_sentense, DELIMITER );  // $GPRMC
                   pszTime = strtok( NULL, DELIMITER );  // UTC時刻
                   strtok( NULL, DELIMITER );  // ステータス
                   pszLat = strtok( NULL, DELIMITER ); // 緯度(dddmm.mmmm)
                   strtok( NULL, DELIMITER );  // 北緯か南緯か
                   pszLong = strtok( NULL, DELIMITER );  // 経度(dddmm.mmmm)
                   strtok( NULL, DELIMITER );  // 東経か西経か
                   
                   pszSp = strtok( NULL, DELIMITER );  // 速度（vvv.v）
                   pszSpd = strtok( NULL, DELIMITER );  // 速度方向（ddd.d）
                   pszDate = strtok( NULL, DELIMITER );  // 日付
                   
                    if( NULL != pszLong ){//pszLongがnullでないのならば
                          temp = atof(pszLat);
                          deg = (int)(temp/100);
                          min = temp - deg * 100;
                          latitude_gps = deg + min / 60;//latitudeの算出
                          
                          temp = atof(pszLong);
                          deg = (int)(temp/100);
                          min = temp - deg * 100;
                          longtude_gps = deg + min / 60;//longtudeの算出
                          
                          speed_gps=atof(pszSp)*KNOT_TO_METER_PER_SEC;
                          
                          azimuth_gps=atof(pszSpd);
                          
                          GPS_flag_update--;
                          
                          //pc.printf("%f,%f\r\n",longtude_gps,latitude_gps);
                          
                      }else{}//end of ( NULL != pszLong ) 
                      
         }else{}//end of  if(0==strncmp( "$GPRMC", NMEA_sentense, 6 )){
    }


void sensor_update(){
           scan_update(sensor_array);//角速度、加速度を求める
       
       Time_new=float(passed_time.read());//時間の更新
       
       Gyro_Pitch_Roll[0]+=sensor_array[1]*(Time_new-Time_old);//初期姿勢角の算出 Pitch
       Gyro_Pitch_Roll[1]+=sensor_array[0]*(Time_new-Time_old);//初期姿勢角の算出 Roll
       
       
       /*--------------------加速センサでジャイロの値を補正する --------------------*/
            
          
            
       if(9.8065*1.05 > sqrt((sensor_array[3]*sensor_array[3]+sensor_array[4]*sensor_array[4]+sensor_array[5]*sensor_array[5])))
                  {
                            Acc_to_Pitch_Roll(sensor_array[3],sensor_array[4],sensor_array[5],Acc_Pitch_Roll);
                            Gyro_Pitch_Roll[0]*=0.95;//
                            Gyro_Pitch_Roll[1]*=0.95;//                         
                            Gyro_Pitch_Roll[0]+=(0.05*Acc_Pitch_Roll[0]);//
                            Gyro_Pitch_Roll[1]+=(0.05*Acc_Pitch_Roll[1]);//
                            
                            
                    }else{}
       
      //pc.printf("%f,%f,%f\r\n",Time_new,Acc_Pitch_Roll[0]*RAD_TO_DEG,Acc_Pitch_Roll[1]*RAD_TO_DEG);
    //pc.printf("%f,%f,%f\r\n",Time_new,Gyro_Pitch_Roll[0],Gyro_Pitch_Roll[1]);

       //Time_old=Time_new;
       
    }
double Degree_to_PWM_duty(double degree){
    
    double duty=0.0;
    
    duty=(0.2/1000)/20.0*degree;
    
    if((duty+NUTRAL)>=UPPER_DUTY){
        duty=UPPER_DUTY-NUTRAL;
    }else if((duty+NUTRAL)<=LOWER_DUTY){
        duty=NUTRAL-LOWER_DUTY;
    }else{}
    
    return duty;
    
    }
    
double PID_duty_pitch(double target,double vol,float dt){
            //ここで角度の単位で偏差は入力される
            //出力はPWMで
            double duty=0.0;
            double add_duty=0.0;
            
            Prop_p=target-vol;
            
            //pc.printf("%f/r/n",Prop_p);
            
            Int_p+=Prop_p*double(dt);
            Dif_p=(Prop_p - Pre_Prop_p) / double(dt);
            
            Pre_Prop_p=Prop_p;
            
            //add_duty=Gain_Kp*Prop_p+Gain_Ki*Int_p+Gain_Kd*Dif_p;
            
            add_duty=1.0*Prop_p;
            
            duty+=add_duty;
            duty=Degree_to_PWM_duty(duty);
            
            return duty;
            
}

double PID_duty_rudder(double target,double vol,float dt,float yaw_rate){
            //ここで角度の単位で偏差は入力される
            //出力はPWMで
            double duty=0.0;
            double add_duty=0.0;
            
            Prop_p=target-vol;
            
            //pc.printf("%f/r/n",Prop_p);
            
            Int_p+=Prop_p*double(dt);
            Dif_p=(Prop_p - Pre_Prop_p) / double(dt);
            
            Pre_Prop_p=Prop_p;
            
            //add_duty=Gain_Kp*Prop_p+Gain_Ki*Int_p+Gain_Kd*Dif_p;
            
            //add_duty=1.0*Prop_p;
            //今回は角速度に反応させてラダーを動かす
            add_duty=double(yaw_rate)*0.1;
            
            duty+=add_duty;
            duty=Degree_to_PWM_duty(duty);
            
            return duty;
            
}

double PID_duty_roll(double target,double vol,float dt){
            //ここで角度の単位で偏差は入力される
            //出力はPWMで
            double duty=0.0;
            double add_duty=0.0;
            
            Prop_p=target-vol;
            
            //pc.printf("%f/r/n",Prop_p);
            
            Int_p+=Prop_p*double(dt);
            Dif_p=(Prop_p - Pre_Prop_p) / double(dt);
            
            Pre_Prop_p=Prop_p;
            
            //add_duty=Gain_Kp*Prop_p+Gain_Ki*Int_p+Gain_Kd*Dif_p;
            
            add_duty=1.0*Prop_p;
            
            duty+=add_duty;
            duty=Degree_to_PWM_duty(duty);
            
            return duty;
            
}
void Initialize_ESC(){
    
    ESC.pulsewidth(0.002);
    wait(3);
    
    ESC.pulsewidth(0.001);
    wait(5);
    
    }
    
    
void Activate_ESC(){
    
    ESC.pulsewidth(0.001);
    wait(5);    
    
    }

void Acc_to_Pitch_Roll(float x_acc,float y_acc,float z_acc,float qua[2]){
    
    float Roll_before = (y_acc)/(z_acc);
    float Roll = atan(Roll_before);
             
    float Pitch_before = (x_acc/sqrt((y_acc*y_acc+z_acc*z_acc)) );
    float Pitch = -atan(Pitch_before);
    
    qua[0]=Pitch*RAD_TO_DEG;
    qua[1]=Roll*RAD_TO_DEG;
    //float Acc_Pitch_Roll[2];
    
    }
    
void scan_update(float box_sensor[6]){
    
    float a[3];//加速度の値
    float g[3];//ジャイロの値[rad/s]
    
    //int16_t raw_compass[3];//地磁気センサの値
    
    mpu.setAcceleroRange(0);//acceleration range is +-4G
    mpu.setGyroRange(0);//gyro rate is +-degree per second(dps)
    
    mpu.getAccelero(a);//   加速度を格納する配列[m/s2]  a[0] is x axis,a[1] is y axis,a[2] is z axis;
    mpu.getGyro(g);//   角速度を格納する配列[rad/s]
    
    //pc.printf("%f,%f,%f\r\n",g[0],g[1],g[2]);
    
    //compass.getXYZ(raw_compass);//地磁気の値を格納する配列
    
    box_sensor[0]=((-g[0])-(GYRO_FIX_X/7505.7))*RAD_TO_DEG;
    box_sensor[1]=((g[1])-(GYRO_FIX_Y/7505.7))*RAD_TO_DEG;
    box_sensor[2]=((-g[2])-(GYRO_FIX_Z/7505.7))*RAD_TO_DEG;
    
    omega_z=box_sensor[2];
    
    box_sensor[3]=a[0];
    box_sensor[4]=-a[1];
    box_sensor[5]=a[2];
    
    //pc.printf("%f,%f,%f\r\n",box_sensor[0],box_sensor[1],box_sensor[2]);
}

void flip(){
    
    GPS_interruptIn++;
    
};


//ch1
void Input_Aileron_1(){
    
    first_period_old=ch_time.read();
    //pc.printf("ch1=%f",second_period);    
    };

void Input_Aileron_1_fall(){
    first_period=ch_time.read();
    aileron_duty=first_period-first_period_old;
};
    

//ch2
void Input_elevator(){
    second_period_old=ch_time.read();
};

void Input_elevator_fall(){
    second_period=ch_time.read();    
    elevator_duty=second_period-second_period_old;
    };
        
    
//ch3
void Input_throttle(){
    
    first_period_old=ch_time.read();
    second_period_old=ch_time.read();
    third_period_old=ch_time.read();
    fourth_period_old=ch_time.read();  
    fifth_period_old=ch_time.read();
    sixth_period_old=ch_time.read();
    
    
}

void Input_throttle_fall(){
    third_period=ch_time.read();
    
    /*デコード*/
    throttle_duty=third_period-third_period_old;
    //rudder_duty=fourth_period-fourth_period_old;
    //elevator_duty=second_period-second_period_old;
    //aileron_duty=first_period-first_period_old;
    
    //pc.printf("throttle_duty=%f:rudder_duty=%f:elevator_duty=%f\r\n",throttle_duty,rudder_duty,elevator_duty);
}

//ch4
void Input_rudder(){
    fourth_period_old=ch_time.read();  
}

void Input_rudder_fall(){
    fourth_period=ch_time.read();
    rudder_duty=fourth_period-fourth_period_old;  
}

//ch5
void Input_gear(){
    fifth_period_old=ch_time.read();

}

void Input_gear_fall(){
    fifth_period=ch_time.read();
    switch_duty=fifth_period-fifth_period_old;
}

//ch6
void Input_Aileron_2(){
    sixth_period_old=ch_time.read();
    }
    
void Input_Aileron_2_fall(){
    sixth_period=ch_time.read();
    };
    
//ch7
void Input_pitch(){
    seventh_period_old=ch_time.read();
}

void Input_pitch_fall(){
    seventh_period=ch_time.read();
    }
        
//ch8
void Input_AUX5(){
    eighth_period_old=ch_time.read();
}

void Input_AUX5_fall(){
    eighth_period=ch_time.read();
}