File content as of revision 0:e647f6de3d26:

//==================================================
//Auto pilot(prototype2)
//
//MPU board: mbed LPC1768
//Multiplexer TC74HC157AP
//Accelerometer +Gyro sensor : GY-521
//2019/11/17 A.Toda
//==================================================
#include "mbed.h"
#include "MPU6050.h"
#include "SDFileSystem.h"

#include "Vector.h"
#include "Matrix.h"
#include "Vector_Matrix_operator.h"

#include "math.h"

//==================================================
#define RAD_TO_DEG          57.2957795f             // 180 / π
#define MAX_MEAN_COUNTER 500

#define ACC_X -1.205178682//offset of x-axi accelerometer
#define ACC_Y -0.141728488//offset of y-axi accelerometer
#define ACC_Z -0.339272785//offset of z-axi accelerometer

#define ACC_GAIN_X 1.0
#define ACC_GAIN_Y 0.995906146
#define ACC_GAIN_Z 1.017766039

#define THRESHOLD_PWM 0.0015
#define M_A_PWM 0.0017
#define SERVO_PERIOD  0.020

#define PITCH_TARGET 0.0

/*
8/2/2020でのゲイン
#define P_P_GAIN 3.0
#define P_I_GAIN 0.5
#define P_D_GAIN 1.0
*/

#define OFFSET_PWM_ELE 0.0015
#define MAX_PWM_ELE 0.00175
#define MIN_PWM_ELE 0.00125

#define LPF_ELE_K 0.110//この値が小さいとローパスフィルタが強くなる 0.050は小さすぎる
                        //0.200は動きが機敏過ぎる。
//==================================================

//Port Setting
SDFileSystem sd(p5, p6, p7, p8, "sd");//pins for sd slot 
MPU6050 mpu(p9, p10);  //Accelerometer + Gyro
                        //(SDA,SCLK)
DigitalIn logging_terminater(p16);
InterruptIn reading_port(p18); 
DigitalOut mux_switch(p19);
PwmOut ELE(p21);

Serial pc(USBTX, USBRX);    //UART

//Pointer of sd card
FILE *fp;

//==================================================
//Accelerometer and gyro data
//==================================================
double acc[3]; //variables for accelerometer
double gyro[3]; //variables for gyro

double offset_gyro_x=0.0;
double offset_gyro_y=0.0;

double sum_gyro_x=0.0;
double sum_gyro_y=0.0;

double threshold_acc,threshold_acc_ini;

double dev  =0.0;
double i_dev=0.0;
double d_dev=0.0;

double old_i_dev=0.0;
//==================================================
//Atitude data
//==================================================
double roll_and_pitch_acc[2];//atitude from acceleromter
double roll_and_pitch[2];//atitude from gyro and acceleromter

/*--------------------------行列、ベクトル-----------------------------*/
Matrix rate_angle_matrix(4,4);//角速度行列 クォータニオンの更新に使う
Vector quaternion(4),pre_quaternion(4),dump_1(4);;//クォータニオン
/*-------------------------------------------------------------------*/

//==================================================
//Timer valiables
//==================================================
Timer ch_time;//timer for calculate pulse width
Timer passed_time;//timer for calculate atitude

double measured_pre_pulse=0.0;
double measured_pulse=0.0;

double time_new;
double time_old;

double pulse_width_ele,deflection_ele,old_deflection_ele;

//=================================================
//PID Gain
//==================================================
float P_P_GAIN,P_I_GAIN,P_D_GAIN;
int P_GAIN,I_GAIN,D_GAIN;

//=================================================
//エレベータの舵角
//=================================================
double pitch_command;

//=================================================
//手動か自動かのインジケータ0なら手動、1なら自動
//=================================================
int m_a_indicater;
//=================================================
//Functions for rising and falind edge interrution
//=================================================
//rise edge
void rising_edge(){
    ch_time.reset();//reset timer counter
    measured_pre_pulse=ch_time.read();
    
}

//falling edge
void falling_edge(){
    
    measured_pre_pulse=(ch_time.read()-measured_pre_pulse);
    //pc.printf("The pulse width=%f\r\n",measured_pre_pulse);
    if(measured_pre_pulse>M_A_PWM){
        mux_switch=1;
        m_a_indicater=1;//set indicater as auto
        }else{
            mux_switch=0;
            m_a_indicater=0;//set indicater as manual
            }
}

//terminate logging
void end_of_log(){
     //flipper.detach();
     fclose(fp);//close "Atitude_angles.csv"
     pc.printf("Logging was terminated.");
     
    }
//==================================================
//Gyro and accelerometer functions
//==================================================
//get data
void  aquisition_sensor_values(double *a,double *g){
    
    float ac[3],gy[3];
    
    mpu.getAccelero(ac);//get acceleration (Accelerometer)
                                //x_axis acc[0]
                                //y_axis acc[1]
                                //z_axis acc[2]
    mpu.getGyro(gy);   //get rate of angle(Gyro)
                      //x_axis gyro[0]
                      //y_axis gyro[1]
                      //z_axis gyro[2]
                      
    //Invertion for direction of Accelerometer axis
    ac[0]*=(-1.0);
    ac[2]*=(-1.0);
    
    ac[0]=(ac[0]-ACC_X)/ACC_GAIN_X;
    ac[1]=(ac[1]-ACC_Y)/ACC_GAIN_Y;
    ac[2]=(ac[2]-ACC_Z)/ACC_GAIN_Z;
            
    //Unit convertion of rate of angle(radian to degree)
    gy[0]*=RAD_TO_DEG;
    gy[0]*=(-1.0);
        
    gy[1]*=RAD_TO_DEG;        
    gy[2]*=RAD_TO_DEG;
    gy[2]*=(-1.0);
  
    for(int i=0;i<3;i++){
        a[i]=double(ac[i]);
        g[i]=double(gy[i]);
        }
    g[0]-=offset_gyro_x;//offset rejection
    g[1]-=offset_gyro_y;//offset rejection
    
    return;
    
}

//calculate offset of gyro
void offset_calculation_for_gyro(){
    
    //Accelerometer and gyro setting 
    mpu.setAcceleroRange(0);//acceleration range is +-2G
    mpu.setGyroRange(1);//gyro rate is +-500degree per second(dps)
    
    //calculate offset of gyro
    for(int mean_counter=0; mean_counter<MAX_MEAN_COUNTER ;mean_counter++){
        aquisition_sensor_values(acc,gyro);
        sum_gyro_x+=gyro[0];
        sum_gyro_y+=gyro[1];
        wait(0.01);
        }
    
    offset_gyro_x=sum_gyro_x/MAX_MEAN_COUNTER;
    offset_gyro_y=sum_gyro_y/MAX_MEAN_COUNTER;
    
    return;
}

//atitude calculation from acceleromter
void atitude_estimation_from_accelerometer(double *a,double *roll_and_pitch){
    
    roll_and_pitch[0] = atan(a[1]/a[2])*RAD_TO_DEG;//roll
    roll_and_pitch[1] = atan(a[0]/sqrt( (a[1]*a[1]+a[2]*a[2]) ) )*RAD_TO_DEG;//pitch
    
    return;
}

//quaternion to euler
void quaternion_to_euler(Vector& qua,double *roll_and_pitch ){
    
    double q0=double (qua.GetComp(1));
    double q1=double (qua.GetComp(2));
    double q2=double (qua.GetComp(3));
    double q3=double (qua.GetComp(4));
    
    roll_and_pitch[0]=atan((q2*q3+q0*q1)/(q0*q0-q1*q1-q2*q2+q3*q3)) ;//roll
    roll_and_pitch[1]=-asin(2*(q1*q3-q0*q2));
    
    return;
    }

//quaternion to euler
void euler_to_quaternion(Vector& qua,double *roll_and_pitch ){
    
    double roll_rad_2=(roll_and_pitch[0]/57.3)/2.0;
    double pitch_rad_2=(roll_and_pitch[1]/57.3)/2.0;
    double yaw_rad_2=0.0;
    
    float q0= cos(roll_rad_2)*cos(pitch_rad_2)*cos(yaw_rad_2)
                +sin(roll_rad_2)*sin(pitch_rad_2)*sin(yaw_rad_2);
                
    float q1= sin(roll_rad_2)*cos(pitch_rad_2)*cos(yaw_rad_2)
                -cos(roll_rad_2)*sin(pitch_rad_2)*sin(yaw_rad_2);
                
    float q2= cos(roll_rad_2)*sin(pitch_rad_2)*cos(yaw_rad_2)
                +sin(roll_rad_2)*cos(pitch_rad_2)*sin(yaw_rad_2);

    float q3= cos(roll_rad_2)*cos(pitch_rad_2)*sin(yaw_rad_2)
                -sin(roll_rad_2)*sin(pitch_rad_2)*cos(yaw_rad_2);

    //クォータニオン行列の作成（クォータニオンの演算に用いる）
    float quaternion_elements[4]={q0,q1,q2,q3}; 
    qua.SetComps(quaternion_elements);
    
    
    return;
    }

//atitude calculation
void atitude_update(){
    
    //慣性センサの計測
    aquisition_sensor_values(acc,gyro);
    //角速度行列の作成（クォータニオンの演算に用いる）
    float rate_angles[16]={0.0,-float(gyro[0]),-float(gyro[1]),-float(gyro[2])
                          ,float(gyro[0]),0.0,float(gyro[2]),-float(gyro[1])
                          ,float(gyro[1]),-float(gyro[2]),0.0,float(gyro[0])
                          ,float(gyro[0]),float(gyro[1]),-float(gyro[2]),0.0}; 
                          
    rate_angle_matrix.SetComps(rate_angles);
    //クォータニオンの演算
    //pc.printf("クォータニオンの演算\r\n");
    float coefficents=0.5*(time_new-time_old);
    float coefficents_elements[16]={coefficents,0.0,0.0,0.0
                                      ,0.0,coefficents,0.0,0.0
                                      ,0.0,0.0,coefficents,0.0
                                      ,0.0,0.0,0.0,coefficents};
    Vector coefficents_vector(4);
    coefficents_vector.SetComps(coefficents_elements);
    
    dump_1=rate_angle_matrix*coefficents_vector;
    quaternion=dump_1+pre_quaternion;
    
    //正規化
    //pc.printf("正規化\r\n");
    quaternion=quaternion.Normalize();
    //クォータニオンからオイラー角への変換
    quaternion_to_euler(quaternion,roll_and_pitch_acc );
    
    threshold_acc=sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
    
    if((threshold_acc>=0.9*threshold_acc_ini)
          &&(threshold_acc<=1.1*threshold_acc_ini)){
        
        atitude_estimation_from_accelerometer(acc,roll_and_pitch_acc);
        roll_and_pitch[0] = 0.98*roll_and_pitch[0] + 0.02*roll_and_pitch_acc[0];
        roll_and_pitch[1] = 0.98*roll_and_pitch[1] + 0.02*roll_and_pitch_acc[1];
        
        }else{}
    //補正したオイラー角をクォータニオンへ変換
    euler_to_quaternion(pre_quaternion,roll_and_pitch_acc );
    
    //microSDに記録する
    //経過時間,ロール角,ピッチ角,操縦方式,慣性センサの値
    fprintf(fp, "%f,%f,%f,%d,%f,%f,%f,%f,%f,%f\r\n"
        ,time_new,roll_and_pitch[0],roll_and_pitch[1],m_a_indicater,gyro[0],gyro[1],acc[0],acc[1],acc[2],pitch_command);
    
    return;

}

//elevation commnad to PWM
double elevation_to_PWM(double elevation_command){
    
    /*
    PWM信号0.25msが舵角の16.45度に相当する.
    */
    
    double PWM_pitch = (elevation_command*1.519)/100000+ OFFSET_PWM_ELE;
    //double PWM_pitch = ( ((elevation_command)*6.0/1000.0)/1000.0  )+ OFFSET_PWM_ELE;
    
    /*PWMコマンドの上限と下限の設定*/
    if(PWM_pitch>MAX_PWM_ELE){
        PWM_pitch=MAX_PWM_ELE;
        
        }else if(PWM_pitch<MIN_PWM_ELE){
            PWM_pitch=MIN_PWM_ELE;
            }
    
    return PWM_pitch;
    
    }

double deflection_of_ele(double pitch){
    
    double add_deflection=((pitch-PITCH_TARGET)*6.0/1000.0)/1000.0;
    
    return add_deflection;
    }

//PID controller
double pitch_PID_controller(double pitch,double target,double gyro_pitch){
    
    dev=target-pitch;
    
    //アンチワインドアップ
    i_dev=old_i_dev+dev*(time_new-time_old);
    if(i_dev>=25.0){
        i_dev=25.0;
    }else if(i_dev<=-25.0){
        i_dev=-25.0;
    }
    //アンチワインドアップ終わり
            
    old_i_dev=i_dev;
    
    d_dev=-gyro_pitch;
    
    pitch_command = double(P_P_GAIN*dev+P_I_GAIN*i_dev+P_D_GAIN*d_dev);
    
    double pwm_command = elevation_to_PWM(pitch_command);//pwm信号に変換
    
    return pwm_command;

    }

//LPF

double LPF_pitch(double c_com,double old_com){
    
    double lpf_output=(1-LPF_ELE_K)*old_com+LPF_ELE_K*c_com;
    
    return lpf_output;
    }

//==================================================
//Main
//==================================================
int main() {

    wait(5.0);
    
    //UART initialization
    pc.baud(115200);
    
    //define servo period
    ELE.period(SERVO_PERIOD);  // servo requires a 20ms period
    pulse_width_ele=0.0015;
    
    //timer starts
    ch_time.start();
    passed_time.start();
    
    time_old=0.0;
    
    //declare interrupitons
    reading_port.rise(rising_edge);
    reading_port.fall(falling_edge);
    
    /*
    mux_switch=0;//set circit as manual mode
    m_a_indicater=0;//set indicater as manual
    */
    
    //gyro and accelerometer initialization
    offset_calculation_for_gyro();
    
    //determine initilal atitude
    aquisition_sensor_values(acc,gyro);
    atitude_estimation_from_accelerometer(acc,roll_and_pitch);
    euler_to_quaternion(pre_quaternion,roll_and_pitch);
    
    threshold_acc_ini=sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
    
    /*
    PIDゲインをsdカードのテキストファイルから読み取る
    int型しか読みとれないので、希望する値の10倍をテキストファイルに書き込んでいる。
    Pゲインを3.0,Iゲインを0.5,Dゲインを1.0とする場合
    
    30,5,10
    
    のようにテキストデータをsdカードに用意する。  
    */
    //open PID gain file in sd card 
    FILE*ga = fopen("/sd/Gain_data.txt", "r");
    if(ga == NULL) {
        error("Could not open file for write\n");
    }
    
    while (!feof(ga)) {
        int n = fscanf(ga, "%d,%d,%d",&P_GAIN, &I_GAIN, &D_GAIN);
        
        if(n!= 3){
            error("Could not read 3 elements");
            }
    }
    
    P_P_GAIN=P_GAIN/10.0;
    P_I_GAIN=I_GAIN/10.0;
    P_D_GAIN=D_GAIN/10.0;
    
    fclose(ga);
    
    //create folder(sd) in sd card
    mkdir("/sd", 0777);
    //create "Atitude_angles.csv" in folder(sd)
    fp = fopen("/sd/Atitude_angles.csv", "a");//ファイルがある場合は追加で書き込み
        
    if(fp == NULL) {
        error("Could not open file for write\n");
    }
    
    //Logging starts
    pc.printf("Logging starts.");
    
    //write PID gain on sd card 
    fprintf(fp,"%f,%f,%f\r\n",P_P_GAIN,P_I_GAIN,P_D_GAIN);
    
    //while
    while(1){
         
         if(logging_terminater==1){
             end_of_log();
             }else{}
         
         time_new=passed_time.read();
         
         atitude_update();
        
         time_old=time_new;
         
         /*
         ここから先でサーボの操舵角を姿勢角に応じて変化させる。関数pitch_PID_controller
         はpitch角とpitch角速度に応じてサーボのパルス幅を返す関数である。
         */
         
         deflection_ele = pitch_PID_controller(roll_and_pitch[1],PITCH_TARGET,gyro[1]);
         
         //LPF
         deflection_ele = LPF_pitch(deflection_ele,old_deflection_ele);
         old_deflection_ele = deflection_ele;
         
         //servo output
         ELE.pulsewidth(deflection_ele);
         
         //PCにつないでデバッグを行う際に表示する
         pc.printf("%f,%f,%f,%f,%d\r\n",time_new,deflection_ele,roll_and_pitch[0],roll_and_pitch[1],m_a_indicater);
         
         wait(0.002);
        
    }//while ends
    
}//main ends