Atsumi Toda / Mbed 2 deprecated Auto_pilot_sd_logging

ドローン用計測制御基板の作り方 vol.1 ハードウェア編 p.21掲載 sdカードへの書き込み

Dependencies:   mbed MPU6050_alter SDFileSystem

main.cpp@0:bdd9c193b6f1, 2019-12-30 (annotated)

Committer:
Joeatsumi
Date:
Mon Dec 30 09:01:52 2019 +0000
Revision:
0:bdd9c193b6f1
20191230

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joeatsumi 0:bdd9c193b6f1 1 //==================================================
Joeatsumi 0:bdd9c193b6f1 2 //Logger (micro SD)
Joeatsumi 0:bdd9c193b6f1 3 //MPU board: mbed LPC1768
Joeatsumi 0:bdd9c193b6f1 4 //Accelerometer +Gyro sensor : GY-521
Joeatsumi 0:bdd9c193b6f1 5 //microSD slot micro sdcard slot(DIP)
Joeatsumi 0:bdd9c193b6f1 6 //2019/10/11 A.Toda
Joeatsumi 0:bdd9c193b6f1 7 //==================================================
Joeatsumi 0:bdd9c193b6f1 8 #include "mbed.h"
Joeatsumi 0:bdd9c193b6f1 9 #include "MPU6050.h"
Joeatsumi 0:bdd9c193b6f1 10 #include "SDFileSystem.h"
Joeatsumi 0:bdd9c193b6f1 11
Joeatsumi 0:bdd9c193b6f1 12 //==================================================
Joeatsumi 0:bdd9c193b6f1 13 #define RAD_TO_DEG 57.2957795f // 180 / π
Joeatsumi 0:bdd9c193b6f1 14 #define MAX_MEAN_COUNTER 100
Joeatsumi 0:bdd9c193b6f1 15 #define ACC_X 1.3//offset of x-axi accelerometer
Joeatsumi 0:bdd9c193b6f1 16 //==================================================
Joeatsumi 0:bdd9c193b6f1 17
Joeatsumi 0:bdd9c193b6f1 18 //Pointer of sd card
Joeatsumi 0:bdd9c193b6f1 19 FILE *fp;
Joeatsumi 0:bdd9c193b6f1 20
Joeatsumi 0:bdd9c193b6f1 21 //Timer interrupt
Joeatsumi 0:bdd9c193b6f1 22 Ticker flipper;
Joeatsumi 0:bdd9c193b6f1 23
Joeatsumi 0:bdd9c193b6f1 24 //Port Setting
Joeatsumi 0:bdd9c193b6f1 25 MPU6050 mpu(p9, p10); //Accelerometer + Gyro
Joeatsumi 0:bdd9c193b6f1 26 //(SDA,SCLK)
Joeatsumi 0:bdd9c193b6f1 27
Joeatsumi 0:bdd9c193b6f1 28 SDFileSystem sd(p5, p6, p7, p8, "sd");//pins for sd slot
Joeatsumi 0:bdd9c193b6f1 29
Joeatsumi 0:bdd9c193b6f1 30 Serial pc(USBTX, USBRX); //UART
Joeatsumi 0:bdd9c193b6f1 31
Joeatsumi 0:bdd9c193b6f1 32 //==================================================
Joeatsumi 0:bdd9c193b6f1 33 //Accelerometer and gyro data
Joeatsumi 0:bdd9c193b6f1 34 //==================================================
Joeatsumi 0:bdd9c193b6f1 35 double acc[3]; //variables for accelerometer
Joeatsumi 0:bdd9c193b6f1 36 double gyro[3]; //variables for gyro
Joeatsumi 0:bdd9c193b6f1 37
Joeatsumi 0:bdd9c193b6f1 38 double offset_gyro_x=0.0;
Joeatsumi 0:bdd9c193b6f1 39 double offset_gyro_y=0.0;
Joeatsumi 0:bdd9c193b6f1 40
Joeatsumi 0:bdd9c193b6f1 41 double sum_gyro_x=0.0;
Joeatsumi 0:bdd9c193b6f1 42 double sum_gyro_y=0.0;
Joeatsumi 0:bdd9c193b6f1 43
Joeatsumi 0:bdd9c193b6f1 44 double threshold_acc,threshold_acc_ini;
Joeatsumi 0:bdd9c193b6f1 45
Joeatsumi 0:bdd9c193b6f1 46 //==================================================
Joeatsumi 0:bdd9c193b6f1 47 //Atitude data
Joeatsumi 0:bdd9c193b6f1 48 //==================================================
Joeatsumi 0:bdd9c193b6f1 49 double roll_and_pitch_acc[2];//atitude from acceleromter
Joeatsumi 0:bdd9c193b6f1 50 double roll_and_pitch[2];//atitude from gyro and acceleromter
Joeatsumi 0:bdd9c193b6f1 51
Joeatsumi 0:bdd9c193b6f1 52 //==================================================
Joeatsumi 0:bdd9c193b6f1 53 //Timer valiables
Joeatsumi 0:bdd9c193b6f1 54 //==================================================
Joeatsumi 0:bdd9c193b6f1 55 double passed_time=0.0;
Joeatsumi 0:bdd9c193b6f1 56
Joeatsumi 0:bdd9c193b6f1 57 //==================================================
Joeatsumi 0:bdd9c193b6f1 58 //Gyro and accelerometer functions
Joeatsumi 0:bdd9c193b6f1 59 //==================================================
Joeatsumi 0:bdd9c193b6f1 60 //get data
Joeatsumi 0:bdd9c193b6f1 61 void aquisition_sensor_values(double *a,double *g){
Joeatsumi 0:bdd9c193b6f1 62
Joeatsumi 0:bdd9c193b6f1 63 float ac[3],gy[3];
Joeatsumi 0:bdd9c193b6f1 64
Joeatsumi 0:bdd9c193b6f1 65 mpu.getAccelero(ac);//get acceleration (Accelerometer)
Joeatsumi 0:bdd9c193b6f1 66 //x_axis acc[0]
Joeatsumi 0:bdd9c193b6f1 67 //y_axis acc[1]
Joeatsumi 0:bdd9c193b6f1 68 //z_axis acc[2]
Joeatsumi 0:bdd9c193b6f1 69 mpu.getGyro(gy); //get rate of angle(Gyro)
Joeatsumi 0:bdd9c193b6f1 70 //x_axis gyro[0]
Joeatsumi 0:bdd9c193b6f1 71 //y_axis gyro[1]
Joeatsumi 0:bdd9c193b6f1 72 //z_axis gyro[2]
Joeatsumi 0:bdd9c193b6f1 73
Joeatsumi 0:bdd9c193b6f1 74 //Invertion for direction of Accelerometer axis
Joeatsumi 0:bdd9c193b6f1 75 ac[0]*=(-1.0);
Joeatsumi 0:bdd9c193b6f1 76 ac[0]+=ACC_X;
Joeatsumi 0:bdd9c193b6f1 77
Joeatsumi 0:bdd9c193b6f1 78 ac[2]*=(-1.0);
Joeatsumi 0:bdd9c193b6f1 79
Joeatsumi 0:bdd9c193b6f1 80 //Unit convertion of rate of angle(radian to degree)
Joeatsumi 0:bdd9c193b6f1 81 gy[0]*=RAD_TO_DEG;
Joeatsumi 0:bdd9c193b6f1 82 gy[0]*=(-1.0);
Joeatsumi 0:bdd9c193b6f1 83
Joeatsumi 0:bdd9c193b6f1 84 gy[1]*=RAD_TO_DEG;
Joeatsumi 0:bdd9c193b6f1 85
Joeatsumi 0:bdd9c193b6f1 86 gy[2]*=RAD_TO_DEG;
Joeatsumi 0:bdd9c193b6f1 87 gy[2]*=(-1.0);
Joeatsumi 0:bdd9c193b6f1 88
Joeatsumi 0:bdd9c193b6f1 89 for(int i=0;i<3;i++){
Joeatsumi 0:bdd9c193b6f1 90 a[i]=double(ac[i]);
Joeatsumi 0:bdd9c193b6f1 91 g[i]=double(gy[i]);
Joeatsumi 0:bdd9c193b6f1 92 }
Joeatsumi 0:bdd9c193b6f1 93 g[0]-=offset_gyro_x;//offset rejection
Joeatsumi 0:bdd9c193b6f1 94 g[1]-=offset_gyro_y;//offset rejection
Joeatsumi 0:bdd9c193b6f1 95
Joeatsumi 0:bdd9c193b6f1 96 return;
Joeatsumi 0:bdd9c193b6f1 97
Joeatsumi 0:bdd9c193b6f1 98 }
Joeatsumi 0:bdd9c193b6f1 99
Joeatsumi 0:bdd9c193b6f1 100 //calculate offset of gyro
Joeatsumi 0:bdd9c193b6f1 101 void offset_calculation_for_gyro(){
Joeatsumi 0:bdd9c193b6f1 102
Joeatsumi 0:bdd9c193b6f1 103 //Accelerometer and gyro setting
Joeatsumi 0:bdd9c193b6f1 104 mpu.setAcceleroRange(0);//acceleration range is +-2G
Joeatsumi 0:bdd9c193b6f1 105 mpu.setGyroRange(1);//gyro rate is +-500degree per second(dps)
Joeatsumi 0:bdd9c193b6f1 106
Joeatsumi 0:bdd9c193b6f1 107 //calculate offset of gyro
Joeatsumi 0:bdd9c193b6f1 108 for(int mean_counter=0; mean_counter<MAX_MEAN_COUNTER ;mean_counter++){
Joeatsumi 0:bdd9c193b6f1 109 aquisition_sensor_values(acc,gyro);
Joeatsumi 0:bdd9c193b6f1 110 sum_gyro_x+=gyro[0];
Joeatsumi 0:bdd9c193b6f1 111 sum_gyro_y+=gyro[1];
Joeatsumi 0:bdd9c193b6f1 112 wait(0.01);
Joeatsumi 0:bdd9c193b6f1 113 }
Joeatsumi 0:bdd9c193b6f1 114
Joeatsumi 0:bdd9c193b6f1 115 offset_gyro_x=sum_gyro_x/MAX_MEAN_COUNTER;
Joeatsumi 0:bdd9c193b6f1 116 offset_gyro_y=sum_gyro_y/MAX_MEAN_COUNTER;
Joeatsumi 0:bdd9c193b6f1 117
Joeatsumi 0:bdd9c193b6f1 118 return;
Joeatsumi 0:bdd9c193b6f1 119 }
Joeatsumi 0:bdd9c193b6f1 120
Joeatsumi 0:bdd9c193b6f1 121 //atitude calculation from acceleromter
Joeatsumi 0:bdd9c193b6f1 122 void atitude_estimation_from_accelerometer(double *a,double *roll_and_pitch){
Joeatsumi 0:bdd9c193b6f1 123
Joeatsumi 0:bdd9c193b6f1 124 roll_and_pitch[0] = atan(a[1]/a[2])*RAD_TO_DEG;//roll
Joeatsumi 0:bdd9c193b6f1 125 roll_and_pitch[1] = atan(a[0]/sqrt( (a[1]*a[1]+a[2]*a[2]) ) )*RAD_TO_DEG;//pitch
Joeatsumi 0:bdd9c193b6f1 126
Joeatsumi 0:bdd9c193b6f1 127 return;
Joeatsumi 0:bdd9c193b6f1 128 }
Joeatsumi 0:bdd9c193b6f1 129
Joeatsumi 0:bdd9c193b6f1 130 //atitude calculation
Joeatsumi 0:bdd9c193b6f1 131 void atitude_update(){
Joeatsumi 0:bdd9c193b6f1 132
Joeatsumi 0:bdd9c193b6f1 133 aquisition_sensor_values(acc,gyro);
Joeatsumi 0:bdd9c193b6f1 134
Joeatsumi 0:bdd9c193b6f1 135 roll_and_pitch[0]+=gyro[0]*0.01;
Joeatsumi 0:bdd9c193b6f1 136 roll_and_pitch[1]+=gyro[1]*0.01;
Joeatsumi 0:bdd9c193b6f1 137
Joeatsumi 0:bdd9c193b6f1 138 threshold_acc=sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
Joeatsumi 0:bdd9c193b6f1 139
Joeatsumi 0:bdd9c193b6f1 140 if((threshold_acc>=0.9*threshold_acc_ini)
Joeatsumi 0:bdd9c193b6f1 141 &&(threshold_acc<=1.1*threshold_acc_ini)){
Joeatsumi 0:bdd9c193b6f1 142
Joeatsumi 0:bdd9c193b6f1 143 atitude_estimation_from_accelerometer(acc,roll_and_pitch_acc);
Joeatsumi 0:bdd9c193b6f1 144 roll_and_pitch[0] = 0.98*roll_and_pitch[0] + 0.02*roll_and_pitch_acc[0];
Joeatsumi 0:bdd9c193b6f1 145 roll_and_pitch[1] = 0.98*roll_and_pitch[1] + 0.02*roll_and_pitch_acc[1];
Joeatsumi 0:bdd9c193b6f1 146
Joeatsumi 0:bdd9c193b6f1 147 }else{}
Joeatsumi 0:bdd9c193b6f1 148
Joeatsumi 0:bdd9c193b6f1 149 pc.printf("roll=%f pitch=%f\r\n",roll_and_pitch[0],roll_and_pitch[1]);
Joeatsumi 0:bdd9c193b6f1 150 fprintf(fp, "%f,%f,%f\r\n",passed_time,roll_and_pitch[0],roll_and_pitch[1]);
Joeatsumi 0:bdd9c193b6f1 151
Joeatsumi 0:bdd9c193b6f1 152 passed_time+=0.01;
Joeatsumi 0:bdd9c193b6f1 153
Joeatsumi 0:bdd9c193b6f1 154 return;
Joeatsumi 0:bdd9c193b6f1 155
Joeatsumi 0:bdd9c193b6f1 156 }
Joeatsumi 0:bdd9c193b6f1 157
Joeatsumi 0:bdd9c193b6f1 158 //==================================================
Joeatsumi 0:bdd9c193b6f1 159 //Main
Joeatsumi 0:bdd9c193b6f1 160 //==================================================
Joeatsumi 0:bdd9c193b6f1 161 int main() {
Joeatsumi 0:bdd9c193b6f1 162
Joeatsumi 0:bdd9c193b6f1 163 //UART initialization
Joeatsumi 0:bdd9c193b6f1 164 pc.baud(115200);
Joeatsumi 0:bdd9c193b6f1 165
Joeatsumi 0:bdd9c193b6f1 166 //gyro and accelerometer initialization
Joeatsumi 0:bdd9c193b6f1 167 offset_calculation_for_gyro();
Joeatsumi 0:bdd9c193b6f1 168
Joeatsumi 0:bdd9c193b6f1 169 //identify initilal atitude
Joeatsumi 0:bdd9c193b6f1 170 aquisition_sensor_values(acc,gyro);
Joeatsumi 0:bdd9c193b6f1 171 atitude_estimation_from_accelerometer(acc,roll_and_pitch);
Joeatsumi 0:bdd9c193b6f1 172
Joeatsumi 0:bdd9c193b6f1 173 threshold_acc_ini=sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
Joeatsumi 0:bdd9c193b6f1 174
Joeatsumi 0:bdd9c193b6f1 175 //create folder(sd) in sd card
Joeatsumi 0:bdd9c193b6f1 176 mkdir("/sd", 0777);
Joeatsumi 0:bdd9c193b6f1 177 //create "Atitude_angles.csv" in folder(sd)
Joeatsumi 0:bdd9c193b6f1 178 fp = fopen("/sd/Atitude_angles.csv", "w");
Joeatsumi 0:bdd9c193b6f1 179
Joeatsumi 0:bdd9c193b6f1 180 if(fp == NULL) {
Joeatsumi 0:bdd9c193b6f1 181 error("Could not open file for write\n");
Joeatsumi 0:bdd9c193b6f1 182 }
Joeatsumi 0:bdd9c193b6f1 183
Joeatsumi 0:bdd9c193b6f1 184 //Logging starts
Joeatsumi 0:bdd9c193b6f1 185 pc.printf("Logging starts.");
Joeatsumi 0:bdd9c193b6f1 186
Joeatsumi 0:bdd9c193b6f1 187 //Ticker
Joeatsumi 0:bdd9c193b6f1 188 flipper.attach(&atitude_update, 0.01);
Joeatsumi 0:bdd9c193b6f1 189
Joeatsumi 0:bdd9c193b6f1 190 //while
Joeatsumi 0:bdd9c193b6f1 191 while(1) {
Joeatsumi 0:bdd9c193b6f1 192 if(passed_time>=30.0){
Joeatsumi 0:bdd9c193b6f1 193 flipper.detach();
Joeatsumi 0:bdd9c193b6f1 194 fclose(fp);//close "Atitude_angles.csv"
Joeatsumi 0:bdd9c193b6f1 195
Joeatsumi 0:bdd9c193b6f1 196 break;
Joeatsumi 0:bdd9c193b6f1 197 }//if ends
Joeatsumi 0:bdd9c193b6f1 198 }//while ends
Joeatsumi 0:bdd9c193b6f1 199
Joeatsumi 0:bdd9c193b6f1 200 pc.printf("Logging finished.");
Joeatsumi 0:bdd9c193b6f1 201 }