main.cpp

00001 //==================================================
00002 //Logger (micro SD)
00003 //MPU board: mbed LPC1768
00004 //Accelerometer +Gyro sensor : GY-521
00005 //microSD slot micro sdcard slot(DIP)
00006 //2019/10/11 A.Toda
00007 //==================================================
00008 #include "mbed.h"
00009 #include "MPU6050.h"
00010 #include "SDFileSystem.h"
00011 
00012 //==================================================
00013 #define RAD_TO_DEG          57.2957795f             // 180 / π
00014 #define MAX_MEAN_COUNTER 100
00015 #define ACC_X 1.3//offset of x-axi accelerometer
00016 //==================================================
00017 
00018 //Pointer of sd card
00019 FILE *fp;
00020 
00021 //Timer interrupt
00022 Ticker flipper;
00023 
00024 //Port Setting
00025 MPU6050 mpu(p9, p10);  //Accelerometer + Gyro
00026                         //(SDA,SCLK)
00027 
00028 SDFileSystem sd(p5, p6, p7, p8, "sd");//pins for sd slot 
00029 
00030 Serial pc(USBTX, USBRX);    //UART
00031 
00032 //==================================================
00033 //Accelerometer and gyro data
00034 //==================================================
00035 double acc[3]; //variables for accelerometer
00036 double gyro[3]; //variables for gyro
00037 
00038 double offset_gyro_x=0.0;
00039 double offset_gyro_y=0.0;
00040 
00041 double sum_gyro_x=0.0;
00042 double sum_gyro_y=0.0;
00043 
00044 double threshold_acc,threshold_acc_ini;
00045 
00046 //==================================================
00047 //Atitude data
00048 //==================================================
00049 double roll_and_pitch_acc[2];//atitude from acceleromter
00050 double roll_and_pitch[2];//atitude from gyro and acceleromter
00051 
00052 //==================================================
00053 //Timer valiables
00054 //==================================================
00055 double passed_time=0.0;
00056 
00057 //==================================================
00058 //Gyro and accelerometer functions
00059 //==================================================
00060 //get data
00061 void  aquisition_sensor_values(double *a,double *g){
00062     
00063     float ac[3],gy[3];
00064     
00065     mpu.getAccelero(ac);//get acceleration (Accelerometer)
00066                                 //x_axis acc[0]
00067                                 //y_axis acc[1]
00068                                 //z_axis acc[2]
00069     mpu.getGyro(gy);   //get rate of angle(Gyro)
00070                       //x_axis gyro[0]
00071                       //y_axis gyro[1]
00072                       //z_axis gyro[2]
00073         
00074     //Invertion for direction of Accelerometer axis
00075     ac[0]*=(-1.0);
00076     ac[0]+=ACC_X;
00077     
00078     ac[2]*=(-1.0);
00079         
00080     //Unit convertion of rate of angle(radian to degree)
00081     gy[0]*=RAD_TO_DEG;
00082     gy[0]*=(-1.0);
00083         
00084     gy[1]*=RAD_TO_DEG;        
00085         
00086     gy[2]*=RAD_TO_DEG;
00087     gy[2]*=(-1.0);
00088   
00089     for(int i=0;i<3;i++){
00090         a[i]=double(ac[i]);
00091         g[i]=double(gy[i]);
00092         }
00093     g[0]-=offset_gyro_x;//offset rejection
00094     g[1]-=offset_gyro_y;//offset rejection
00095     
00096     return;
00097     
00098 }
00099 
00100 //calculate offset of gyro
00101 void offset_calculation_for_gyro(){
00102     
00103     //Accelerometer and gyro setting 
00104     mpu.setAcceleroRange(0);//acceleration range is +-2G
00105     mpu.setGyroRange(1);//gyro rate is +-500degree per second(dps)
00106     
00107     //calculate offset of gyro
00108     for(int mean_counter=0; mean_counter<MAX_MEAN_COUNTER ;mean_counter++){
00109         aquisition_sensor_values(acc,gyro);
00110         sum_gyro_x+=gyro[0];
00111         sum_gyro_y+=gyro[1];
00112         wait(0.01);
00113         }
00114     
00115     offset_gyro_x=sum_gyro_x/MAX_MEAN_COUNTER;
00116     offset_gyro_y=sum_gyro_y/MAX_MEAN_COUNTER;
00117     
00118     return;
00119 }
00120 
00121 //atitude calculation from acceleromter
00122 void atitude_estimation_from_accelerometer(double *a,double *roll_and_pitch){
00123     
00124     roll_and_pitch[0] = atan(a[1]/a[2])*RAD_TO_DEG;//roll
00125     roll_and_pitch[1] = atan(a[0]/sqrt( (a[1]*a[1]+a[2]*a[2]) ) )*RAD_TO_DEG;//pitch
00126     
00127     return;
00128 }
00129 
00130 //atitude calculation
00131 void atitude_update(){
00132     
00133     aquisition_sensor_values(acc,gyro);
00134     
00135     roll_and_pitch[0]+=gyro[0]*0.01;
00136     roll_and_pitch[1]+=gyro[1]*0.01;
00137     
00138     threshold_acc=sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
00139     
00140     if((threshold_acc>=0.9*threshold_acc_ini)
00141           &&(threshold_acc<=1.1*threshold_acc_ini)){
00142         
00143         atitude_estimation_from_accelerometer(acc,roll_and_pitch_acc);
00144         roll_and_pitch[0] = 0.98*roll_and_pitch[0] + 0.02*roll_and_pitch_acc[0];
00145         roll_and_pitch[1] = 0.98*roll_and_pitch[1] + 0.02*roll_and_pitch_acc[1];
00146         
00147         }else{}
00148     
00149     pc.printf("roll=%f pitch=%f\r\n",roll_and_pitch[0],roll_and_pitch[1]);
00150     fprintf(fp, "%f,%f,%f\r\n",passed_time,roll_and_pitch[0],roll_and_pitch[1]);
00151     
00152     passed_time+=0.01;
00153     
00154     return;
00155 
00156 }
00157 
00158 //==================================================
00159 //Main
00160 //==================================================
00161 int main() {
00162     
00163     //UART initialization
00164     pc.baud(115200);
00165     
00166     //gyro and accelerometer initialization
00167     offset_calculation_for_gyro();
00168     
00169     //identify initilal atitude
00170     aquisition_sensor_values(acc,gyro);
00171     atitude_estimation_from_accelerometer(acc,roll_and_pitch);
00172     
00173     threshold_acc_ini=sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
00174     
00175     //create folder(sd) in sd card
00176     mkdir("/sd", 0777);
00177     //create "Atitude_angles.csv" in folder(sd)
00178     fp = fopen("/sd/Atitude_angles.csv", "w");
00179         
00180     if(fp == NULL) {
00181         error("Could not open file for write\n");
00182     }
00183     
00184     //Logging starts
00185     pc.printf("Logging starts.");
00186     
00187     //Ticker
00188     flipper.attach(&atitude_update, 0.01);
00189     
00190     //while
00191     while(1) {
00192         if(passed_time>=30.0){
00193             flipper.detach();
00194             fclose(fp);//close "Atitude_angles.csv"
00195             
00196             break;
00197             }//if ends
00198     }//while ends
00199     
00200     pc.printf("Logging finished.");
00201 }