File content as of revision 0:ff469cc9ac07:

//==================================================
//Auto pilot(prototype)
//
//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"

//==================================================
#define RAD_TO_DEG          57.2957795f             // 180 / π
#define MAX_MEAN_COUNTER 100
#define ACC_X 1.3//offset of x-axi accelerometer

#define THRESHOLD_PWM 0.0015
#define SERVO_PERIOD  0.020

//==================================================

//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;

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

//==================================================
//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;

//=================================================
//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>THRESHOLD_PWM){
        mux_switch=1;
        }else{
            mux_switch=0;
            }
}

//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[0]+=ACC_X;
    
    ac[2]*=(-1.0);
        
    //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;
}
    
//atitude calculation
void atitude_update(){
    
    aquisition_sensor_values(acc,gyro);
    
    roll_and_pitch[0]+=gyro[0]*(time_new-time_old);
    roll_and_pitch[1]+=gyro[1]*(time_new-time_old);
    
    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{}
    
    //pc.printf("roll=%f pitch=%f\r\n",roll_and_pitch[0],roll_and_pitch[1]);
    fprintf(fp, "%f,%f,%f\r\n",time_new,roll_and_pitch[0],roll_and_pitch[1]);
    
    return;

}

double deflection_of_ele(double pitch){
    
    double add_deflection=(pitch*6.0/1000.0)/1000.0;
    
    return add_deflection;
    }
    
//==================================================
//Main
//==================================================
int main() {

    //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
    
    //gyro and accelerometer initialization
    offset_calculation_for_gyro();
    
    //determine initilal atitude
    aquisition_sensor_values(acc,gyro);
    atitude_estimation_from_accelerometer(acc,roll_and_pitch);
    
    threshold_acc_ini=sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
    
    //create folder(sd) in sd card
    mkdir("/sd", 0777);
    //create "Atitude_angles.csv" in folder(sd)
    fp = fopen("/sd/Atitude_angles.csv", "w");
        
    if(fp == NULL) {
        error("Could not open file for write\n");
    }
    
    //Logging starts
    pc.printf("Logging starts.");

    
    //while
    while(1){
         
         if(logging_terminater==1){
             end_of_log();
             }else{}
         
         time_new=passed_time.read();
         
         atitude_update();
        
         time_old=time_new;
         
         //determine deflectionangle of elevator
         /*
         ここから先でサーボの操舵角を姿勢角に応じて変化させる。関数deflection_of_ele
         はpitch角に応じてサーボのパルス幅を返す関数である。
         もし制御則を自ら実装できるのであれば、姿勢角や角速度を引数としてサーボパルス幅を
         戻り値とする関数を作成し、機体を理論的に制御する事が可能である。
         */
         
         deflection_ele=deflection_of_ele(roll_and_pitch[1]);
         
         ELE.pulsewidth(THRESHOLD_PWM+deflection_ele);
         pc.printf("%f,%f\r\n",THRESHOLD_PWM+deflection_ele,roll_and_pitch[1]);
         
         wait(0.002);
        
    }//while ends
    
}//main ends