Revision 0:bdd9c193b6f1, committed 2019-12-30

Comitter:

Joeatsumi

Date:

Mon Dec 30 09:01:52 2019 +0000

Commit message:

20191230

Changed in this revision

diff -r 000000000000 -r bdd9c193b6f1 MPU6050.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050.lib	Mon Dec 30 09:01:52 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Joeatsumi/code/MPU6050_alter/#44c458576810

diff -r 000000000000 -r bdd9c193b6f1 SDFileSystem.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SDFileSystem.lib	Mon Dec 30 09:01:52 2019 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/SDFileSystem/#8db0d3b02cec

diff -r 000000000000 -r bdd9c193b6f1 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Dec 30 09:01:52 2019 +0000
@@ -0,0 +1,201 @@
+//==================================================
+//Logger (micro SD)
+//MPU board: mbed LPC1768
+//Accelerometer +Gyro sensor : GY-521
+//microSD slot micro sdcard slot(DIP)
+//2019/10/11 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
+//==================================================
+
+//Pointer of sd card
+FILE *fp;
+
+//Timer interrupt
+Ticker flipper;
+
+//Port Setting
+MPU6050 mpu(p9, p10);  //Accelerometer + Gyro
+                        //(SDA,SCLK)
+
+SDFileSystem sd(p5, p6, p7, p8, "sd");//pins for sd slot 
+
+Serial pc(USBTX, USBRX);    //UART
+
+//==================================================
+//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
+//==================================================
+double passed_time=0.0;
+
+//==================================================
+//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]*0.01;
+    roll_and_pitch[1]+=gyro[1]*0.01;
+    
+    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",passed_time,roll_and_pitch[0],roll_and_pitch[1]);
+    
+    passed_time+=0.01;
+    
+    return;
+
+}
+
+//==================================================
+//Main
+//==================================================
+int main() {
+    
+    //UART initialization
+    pc.baud(115200);
+    
+    //gyro and accelerometer initialization
+    offset_calculation_for_gyro();
+    
+    //identify 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.");
+    
+    //Ticker
+    flipper.attach(&atitude_update, 0.01);
+    
+    //while
+    while(1) {
+        if(passed_time>=30.0){
+            flipper.detach();
+            fclose(fp);//close "Atitude_angles.csv"
+            
+            break;
+            }//if ends
+    }//while ends
+    
+    pc.printf("Logging finished.");
+}

diff -r 000000000000 -r bdd9c193b6f1 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Dec 30 09:01:52 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/e1686b8d5b90
\ No newline at end of file