Atsumi Toda / Mbed 2 deprecated Auto_pilot_prototype_1

ドローン用計測制御基板の作り方 vol.1 ハードウェア編 p.32掲載 オートパイロットの基本機能

Dependencies:   mbed MPU6050_alter SDFileSystem

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

Committer:
Joeatsumi
Date:
Mon Dec 30 09:24:54 2019 +0000
Revision:
0:ff469cc9ac07
20191230

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joeatsumi 0:ff469cc9ac07 1 //==================================================
Joeatsumi 0:ff469cc9ac07 2 //Auto pilot(prototype)
Joeatsumi 0:ff469cc9ac07 3 //
Joeatsumi 0:ff469cc9ac07 4 //MPU board: mbed LPC1768
Joeatsumi 0:ff469cc9ac07 5 //Multiplexer TC74HC157AP
Joeatsumi 0:ff469cc9ac07 6 //Accelerometer +Gyro sensor : GY-521
Joeatsumi 0:ff469cc9ac07 7 //2019/11/17 A.Toda
Joeatsumi 0:ff469cc9ac07 8 //==================================================
Joeatsumi 0:ff469cc9ac07 9 #include "mbed.h"
Joeatsumi 0:ff469cc9ac07 10 #include "MPU6050.h"
Joeatsumi 0:ff469cc9ac07 11 #include "SDFileSystem.h"
Joeatsumi 0:ff469cc9ac07 12
Joeatsumi 0:ff469cc9ac07 13 //==================================================
Joeatsumi 0:ff469cc9ac07 14 #define RAD_TO_DEG 57.2957795f // 180 / π
Joeatsumi 0:ff469cc9ac07 15 #define MAX_MEAN_COUNTER 100
Joeatsumi 0:ff469cc9ac07 16 #define ACC_X 1.3//offset of x-axi accelerometer
Joeatsumi 0:ff469cc9ac07 17
Joeatsumi 0:ff469cc9ac07 18 #define THRESHOLD_PWM 0.0015
Joeatsumi 0:ff469cc9ac07 19 #define SERVO_PERIOD 0.020
Joeatsumi 0:ff469cc9ac07 20
Joeatsumi 0:ff469cc9ac07 21 //==================================================
Joeatsumi 0:ff469cc9ac07 22
Joeatsumi 0:ff469cc9ac07 23 //Port Setting
Joeatsumi 0:ff469cc9ac07 24 SDFileSystem sd(p5, p6, p7, p8, "sd");//pins for sd slot
Joeatsumi 0:ff469cc9ac07 25 MPU6050 mpu(p9, p10); //Accelerometer + Gyro
Joeatsumi 0:ff469cc9ac07 26 //(SDA,SCLK)
Joeatsumi 0:ff469cc9ac07 27 DigitalIn logging_terminater(p16);
Joeatsumi 0:ff469cc9ac07 28 InterruptIn reading_port(p18);
Joeatsumi 0:ff469cc9ac07 29 DigitalOut mux_switch(p19);
Joeatsumi 0:ff469cc9ac07 30 PwmOut ELE(p21);
Joeatsumi 0:ff469cc9ac07 31
Joeatsumi 0:ff469cc9ac07 32 Serial pc(USBTX, USBRX); //UART
Joeatsumi 0:ff469cc9ac07 33
Joeatsumi 0:ff469cc9ac07 34 //Pointer of sd card
Joeatsumi 0:ff469cc9ac07 35 FILE *fp;
Joeatsumi 0:ff469cc9ac07 36
Joeatsumi 0:ff469cc9ac07 37 //==================================================
Joeatsumi 0:ff469cc9ac07 38 //Accelerometer and gyro data
Joeatsumi 0:ff469cc9ac07 39 //==================================================
Joeatsumi 0:ff469cc9ac07 40 double acc[3]; //variables for accelerometer
Joeatsumi 0:ff469cc9ac07 41 double gyro[3]; //variables for gyro
Joeatsumi 0:ff469cc9ac07 42
Joeatsumi 0:ff469cc9ac07 43 double offset_gyro_x=0.0;
Joeatsumi 0:ff469cc9ac07 44 double offset_gyro_y=0.0;
Joeatsumi 0:ff469cc9ac07 45
Joeatsumi 0:ff469cc9ac07 46 double sum_gyro_x=0.0;
Joeatsumi 0:ff469cc9ac07 47 double sum_gyro_y=0.0;
Joeatsumi 0:ff469cc9ac07 48
Joeatsumi 0:ff469cc9ac07 49 double threshold_acc,threshold_acc_ini;
Joeatsumi 0:ff469cc9ac07 50
Joeatsumi 0:ff469cc9ac07 51 //==================================================
Joeatsumi 0:ff469cc9ac07 52 //Atitude data
Joeatsumi 0:ff469cc9ac07 53 //==================================================
Joeatsumi 0:ff469cc9ac07 54 double roll_and_pitch_acc[2];//atitude from acceleromter
Joeatsumi 0:ff469cc9ac07 55 double roll_and_pitch[2];//atitude from gyro and acceleromter
Joeatsumi 0:ff469cc9ac07 56
Joeatsumi 0:ff469cc9ac07 57 //==================================================
Joeatsumi 0:ff469cc9ac07 58 //Timer valiables
Joeatsumi 0:ff469cc9ac07 59 //==================================================
Joeatsumi 0:ff469cc9ac07 60 Timer ch_time;//timer for calculate pulse width
Joeatsumi 0:ff469cc9ac07 61 Timer passed_time;//timer for calculate atitude
Joeatsumi 0:ff469cc9ac07 62
Joeatsumi 0:ff469cc9ac07 63 double measured_pre_pulse=0.0;
Joeatsumi 0:ff469cc9ac07 64 double measured_pulse=0.0;
Joeatsumi 0:ff469cc9ac07 65
Joeatsumi 0:ff469cc9ac07 66 double time_new;
Joeatsumi 0:ff469cc9ac07 67 double time_old;
Joeatsumi 0:ff469cc9ac07 68
Joeatsumi 0:ff469cc9ac07 69 double pulse_width_ele,deflection_ele;
Joeatsumi 0:ff469cc9ac07 70
Joeatsumi 0:ff469cc9ac07 71 //=================================================
Joeatsumi 0:ff469cc9ac07 72 //Functions for rising and falind edge interrution
Joeatsumi 0:ff469cc9ac07 73 //=================================================
Joeatsumi 0:ff469cc9ac07 74 //rise edge
Joeatsumi 0:ff469cc9ac07 75 void rising_edge(){
Joeatsumi 0:ff469cc9ac07 76 ch_time.reset();//reset timer counter
Joeatsumi 0:ff469cc9ac07 77 measured_pre_pulse=ch_time.read();
Joeatsumi 0:ff469cc9ac07 78
Joeatsumi 0:ff469cc9ac07 79 }
Joeatsumi 0:ff469cc9ac07 80
Joeatsumi 0:ff469cc9ac07 81 //falling edge
Joeatsumi 0:ff469cc9ac07 82 void falling_edge(){
Joeatsumi 0:ff469cc9ac07 83
Joeatsumi 0:ff469cc9ac07 84 measured_pre_pulse=(ch_time.read()-measured_pre_pulse);
Joeatsumi 0:ff469cc9ac07 85 //pc.printf("The pulse width=%f\r\n",measured_pre_pulse);
Joeatsumi 0:ff469cc9ac07 86 if(measured_pre_pulse>THRESHOLD_PWM){
Joeatsumi 0:ff469cc9ac07 87 mux_switch=1;
Joeatsumi 0:ff469cc9ac07 88 }else{
Joeatsumi 0:ff469cc9ac07 89 mux_switch=0;
Joeatsumi 0:ff469cc9ac07 90 }
Joeatsumi 0:ff469cc9ac07 91 }
Joeatsumi 0:ff469cc9ac07 92
Joeatsumi 0:ff469cc9ac07 93 //terminate logging
Joeatsumi 0:ff469cc9ac07 94 void end_of_log(){
Joeatsumi 0:ff469cc9ac07 95 //flipper.detach();
Joeatsumi 0:ff469cc9ac07 96 fclose(fp);//close "Atitude_angles.csv"
Joeatsumi 0:ff469cc9ac07 97 pc.printf("Logging was terminated.");
Joeatsumi 0:ff469cc9ac07 98
Joeatsumi 0:ff469cc9ac07 99 }
Joeatsumi 0:ff469cc9ac07 100 //==================================================
Joeatsumi 0:ff469cc9ac07 101 //Gyro and accelerometer functions
Joeatsumi 0:ff469cc9ac07 102 //==================================================
Joeatsumi 0:ff469cc9ac07 103 //get data
Joeatsumi 0:ff469cc9ac07 104 void aquisition_sensor_values(double *a,double *g){
Joeatsumi 0:ff469cc9ac07 105
Joeatsumi 0:ff469cc9ac07 106 float ac[3],gy[3];
Joeatsumi 0:ff469cc9ac07 107
Joeatsumi 0:ff469cc9ac07 108 mpu.getAccelero(ac);//get acceleration (Accelerometer)
Joeatsumi 0:ff469cc9ac07 109 //x_axis acc[0]
Joeatsumi 0:ff469cc9ac07 110 //y_axis acc[1]
Joeatsumi 0:ff469cc9ac07 111 //z_axis acc[2]
Joeatsumi 0:ff469cc9ac07 112 mpu.getGyro(gy); //get rate of angle(Gyro)
Joeatsumi 0:ff469cc9ac07 113 //x_axis gyro[0]
Joeatsumi 0:ff469cc9ac07 114 //y_axis gyro[1]
Joeatsumi 0:ff469cc9ac07 115 //z_axis gyro[2]
Joeatsumi 0:ff469cc9ac07 116
Joeatsumi 0:ff469cc9ac07 117 //Invertion for direction of Accelerometer axis
Joeatsumi 0:ff469cc9ac07 118 ac[0]*=(-1.0);
Joeatsumi 0:ff469cc9ac07 119 ac[0]+=ACC_X;
Joeatsumi 0:ff469cc9ac07 120
Joeatsumi 0:ff469cc9ac07 121 ac[2]*=(-1.0);
Joeatsumi 0:ff469cc9ac07 122
Joeatsumi 0:ff469cc9ac07 123 //Unit convertion of rate of angle(radian to degree)
Joeatsumi 0:ff469cc9ac07 124 gy[0]*=RAD_TO_DEG;
Joeatsumi 0:ff469cc9ac07 125 gy[0]*=(-1.0);
Joeatsumi 0:ff469cc9ac07 126
Joeatsumi 0:ff469cc9ac07 127 gy[1]*=RAD_TO_DEG;
Joeatsumi 0:ff469cc9ac07 128
Joeatsumi 0:ff469cc9ac07 129 gy[2]*=RAD_TO_DEG;
Joeatsumi 0:ff469cc9ac07 130 gy[2]*=(-1.0);
Joeatsumi 0:ff469cc9ac07 131
Joeatsumi 0:ff469cc9ac07 132 for(int i=0;i<3;i++){
Joeatsumi 0:ff469cc9ac07 133 a[i]=double(ac[i]);
Joeatsumi 0:ff469cc9ac07 134 g[i]=double(gy[i]);
Joeatsumi 0:ff469cc9ac07 135 }
Joeatsumi 0:ff469cc9ac07 136 g[0]-=offset_gyro_x;//offset rejection
Joeatsumi 0:ff469cc9ac07 137 g[1]-=offset_gyro_y;//offset rejection
Joeatsumi 0:ff469cc9ac07 138
Joeatsumi 0:ff469cc9ac07 139 return;
Joeatsumi 0:ff469cc9ac07 140
Joeatsumi 0:ff469cc9ac07 141 }
Joeatsumi 0:ff469cc9ac07 142
Joeatsumi 0:ff469cc9ac07 143 //calculate offset of gyro
Joeatsumi 0:ff469cc9ac07 144 void offset_calculation_for_gyro(){
Joeatsumi 0:ff469cc9ac07 145
Joeatsumi 0:ff469cc9ac07 146 //Accelerometer and gyro setting
Joeatsumi 0:ff469cc9ac07 147 mpu.setAcceleroRange(0);//acceleration range is +-2G
Joeatsumi 0:ff469cc9ac07 148 mpu.setGyroRange(1);//gyro rate is +-500degree per second(dps)
Joeatsumi 0:ff469cc9ac07 149
Joeatsumi 0:ff469cc9ac07 150 //calculate offset of gyro
Joeatsumi 0:ff469cc9ac07 151 for(int mean_counter=0; mean_counter<MAX_MEAN_COUNTER ;mean_counter++){
Joeatsumi 0:ff469cc9ac07 152 aquisition_sensor_values(acc,gyro);
Joeatsumi 0:ff469cc9ac07 153 sum_gyro_x+=gyro[0];
Joeatsumi 0:ff469cc9ac07 154 sum_gyro_y+=gyro[1];
Joeatsumi 0:ff469cc9ac07 155 wait(0.01);
Joeatsumi 0:ff469cc9ac07 156 }
Joeatsumi 0:ff469cc9ac07 157
Joeatsumi 0:ff469cc9ac07 158 offset_gyro_x=sum_gyro_x/MAX_MEAN_COUNTER;
Joeatsumi 0:ff469cc9ac07 159 offset_gyro_y=sum_gyro_y/MAX_MEAN_COUNTER;
Joeatsumi 0:ff469cc9ac07 160
Joeatsumi 0:ff469cc9ac07 161 return;
Joeatsumi 0:ff469cc9ac07 162 }
Joeatsumi 0:ff469cc9ac07 163
Joeatsumi 0:ff469cc9ac07 164 //atitude calculation from acceleromter
Joeatsumi 0:ff469cc9ac07 165 void atitude_estimation_from_accelerometer(double *a,double *roll_and_pitch){
Joeatsumi 0:ff469cc9ac07 166
Joeatsumi 0:ff469cc9ac07 167 roll_and_pitch[0] = atan(a[1]/a[2])*RAD_TO_DEG;//roll
Joeatsumi 0:ff469cc9ac07 168 roll_and_pitch[1] = atan(a[0]/sqrt( (a[1]*a[1]+a[2]*a[2]) ) )*RAD_TO_DEG;//pitch
Joeatsumi 0:ff469cc9ac07 169
Joeatsumi 0:ff469cc9ac07 170 return;
Joeatsumi 0:ff469cc9ac07 171 }
Joeatsumi 0:ff469cc9ac07 172
Joeatsumi 0:ff469cc9ac07 173 //atitude calculation
Joeatsumi 0:ff469cc9ac07 174 void atitude_update(){
Joeatsumi 0:ff469cc9ac07 175
Joeatsumi 0:ff469cc9ac07 176 aquisition_sensor_values(acc,gyro);
Joeatsumi 0:ff469cc9ac07 177
Joeatsumi 0:ff469cc9ac07 178 roll_and_pitch[0]+=gyro[0]*(time_new-time_old);
Joeatsumi 0:ff469cc9ac07 179 roll_and_pitch[1]+=gyro[1]*(time_new-time_old);
Joeatsumi 0:ff469cc9ac07 180
Joeatsumi 0:ff469cc9ac07 181 threshold_acc=sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
Joeatsumi 0:ff469cc9ac07 182
Joeatsumi 0:ff469cc9ac07 183 if((threshold_acc>=0.9*threshold_acc_ini)
Joeatsumi 0:ff469cc9ac07 184 &&(threshold_acc<=1.1*threshold_acc_ini)){
Joeatsumi 0:ff469cc9ac07 185
Joeatsumi 0:ff469cc9ac07 186 atitude_estimation_from_accelerometer(acc,roll_and_pitch_acc);
Joeatsumi 0:ff469cc9ac07 187 roll_and_pitch[0] = 0.98*roll_and_pitch[0] + 0.02*roll_and_pitch_acc[0];
Joeatsumi 0:ff469cc9ac07 188 roll_and_pitch[1] = 0.98*roll_and_pitch[1] + 0.02*roll_and_pitch_acc[1];
Joeatsumi 0:ff469cc9ac07 189
Joeatsumi 0:ff469cc9ac07 190 }else{}
Joeatsumi 0:ff469cc9ac07 191
Joeatsumi 0:ff469cc9ac07 192 //pc.printf("roll=%f pitch=%f\r\n",roll_and_pitch[0],roll_and_pitch[1]);
Joeatsumi 0:ff469cc9ac07 193 fprintf(fp, "%f,%f,%f\r\n",time_new,roll_and_pitch[0],roll_and_pitch[1]);
Joeatsumi 0:ff469cc9ac07 194
Joeatsumi 0:ff469cc9ac07 195 return;
Joeatsumi 0:ff469cc9ac07 196
Joeatsumi 0:ff469cc9ac07 197 }
Joeatsumi 0:ff469cc9ac07 198
Joeatsumi 0:ff469cc9ac07 199 double deflection_of_ele(double pitch){
Joeatsumi 0:ff469cc9ac07 200
Joeatsumi 0:ff469cc9ac07 201 double add_deflection=(pitch*6.0/1000.0)/1000.0;
Joeatsumi 0:ff469cc9ac07 202
Joeatsumi 0:ff469cc9ac07 203 return add_deflection;
Joeatsumi 0:ff469cc9ac07 204 }
Joeatsumi 0:ff469cc9ac07 205
Joeatsumi 0:ff469cc9ac07 206 //==================================================
Joeatsumi 0:ff469cc9ac07 207 //Main
Joeatsumi 0:ff469cc9ac07 208 //==================================================
Joeatsumi 0:ff469cc9ac07 209 int main() {
Joeatsumi 0:ff469cc9ac07 210
Joeatsumi 0:ff469cc9ac07 211 //UART initialization
Joeatsumi 0:ff469cc9ac07 212 pc.baud(115200);
Joeatsumi 0:ff469cc9ac07 213
Joeatsumi 0:ff469cc9ac07 214 //define servo period
Joeatsumi 0:ff469cc9ac07 215 ELE.period(SERVO_PERIOD); // servo requires a 20ms period
Joeatsumi 0:ff469cc9ac07 216 pulse_width_ele=0.0015;
Joeatsumi 0:ff469cc9ac07 217
Joeatsumi 0:ff469cc9ac07 218 //timer starts
Joeatsumi 0:ff469cc9ac07 219 ch_time.start();
Joeatsumi 0:ff469cc9ac07 220 passed_time.start();
Joeatsumi 0:ff469cc9ac07 221
Joeatsumi 0:ff469cc9ac07 222 time_old=0.0;
Joeatsumi 0:ff469cc9ac07 223
Joeatsumi 0:ff469cc9ac07 224 //declare interrupitons
Joeatsumi 0:ff469cc9ac07 225 reading_port.rise(rising_edge);
Joeatsumi 0:ff469cc9ac07 226 reading_port.fall(falling_edge);
Joeatsumi 0:ff469cc9ac07 227
Joeatsumi 0:ff469cc9ac07 228 mux_switch=0;//set circit as manual mode
Joeatsumi 0:ff469cc9ac07 229
Joeatsumi 0:ff469cc9ac07 230 //gyro and accelerometer initialization
Joeatsumi 0:ff469cc9ac07 231 offset_calculation_for_gyro();
Joeatsumi 0:ff469cc9ac07 232
Joeatsumi 0:ff469cc9ac07 233 //determine initilal atitude
Joeatsumi 0:ff469cc9ac07 234 aquisition_sensor_values(acc,gyro);
Joeatsumi 0:ff469cc9ac07 235 atitude_estimation_from_accelerometer(acc,roll_and_pitch);
Joeatsumi 0:ff469cc9ac07 236
Joeatsumi 0:ff469cc9ac07 237 threshold_acc_ini=sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
Joeatsumi 0:ff469cc9ac07 238
Joeatsumi 0:ff469cc9ac07 239 //create folder(sd) in sd card
Joeatsumi 0:ff469cc9ac07 240 mkdir("/sd", 0777);
Joeatsumi 0:ff469cc9ac07 241 //create "Atitude_angles.csv" in folder(sd)
Joeatsumi 0:ff469cc9ac07 242 fp = fopen("/sd/Atitude_angles.csv", "w");
Joeatsumi 0:ff469cc9ac07 243
Joeatsumi 0:ff469cc9ac07 244 if(fp == NULL) {
Joeatsumi 0:ff469cc9ac07 245 error("Could not open file for write\n");
Joeatsumi 0:ff469cc9ac07 246 }
Joeatsumi 0:ff469cc9ac07 247
Joeatsumi 0:ff469cc9ac07 248 //Logging starts
Joeatsumi 0:ff469cc9ac07 249 pc.printf("Logging starts.");
Joeatsumi 0:ff469cc9ac07 250
Joeatsumi 0:ff469cc9ac07 251
Joeatsumi 0:ff469cc9ac07 252 //while
Joeatsumi 0:ff469cc9ac07 253 while(1){
Joeatsumi 0:ff469cc9ac07 254
Joeatsumi 0:ff469cc9ac07 255 if(logging_terminater==1){
Joeatsumi 0:ff469cc9ac07 256 end_of_log();
Joeatsumi 0:ff469cc9ac07 257 }else{}
Joeatsumi 0:ff469cc9ac07 258
Joeatsumi 0:ff469cc9ac07 259 time_new=passed_time.read();
Joeatsumi 0:ff469cc9ac07 260
Joeatsumi 0:ff469cc9ac07 261 atitude_update();
Joeatsumi 0:ff469cc9ac07 262
Joeatsumi 0:ff469cc9ac07 263 time_old=time_new;
Joeatsumi 0:ff469cc9ac07 264
Joeatsumi 0:ff469cc9ac07 265 //determine deflectionangle of elevator
Joeatsumi 0:ff469cc9ac07 266 /*
Joeatsumi 0:ff469cc9ac07 267 ここから先でサーボの操舵角を姿勢角に応じて変化させる。関数deflection_of_ele
Joeatsumi 0:ff469cc9ac07 268 はpitch角に応じてサーボのパルス幅を返す関数である。
Joeatsumi 0:ff469cc9ac07 269 もし制御則を自ら実装できるのであれば、姿勢角や角速度を引数としてサーボパルス幅を
Joeatsumi 0:ff469cc9ac07 270 戻り値とする関数を作成し、機体を理論的に制御する事が可能である。
Joeatsumi 0:ff469cc9ac07 271 */
Joeatsumi 0:ff469cc9ac07 272
Joeatsumi 0:ff469cc9ac07 273 deflection_ele=deflection_of_ele(roll_and_pitch[1]);
Joeatsumi 0:ff469cc9ac07 274
Joeatsumi 0:ff469cc9ac07 275 ELE.pulsewidth(THRESHOLD_PWM+deflection_ele);
Joeatsumi 0:ff469cc9ac07 276 pc.printf("%f,%f\r\n",THRESHOLD_PWM+deflection_ele,roll_and_pitch[1]);
Joeatsumi 0:ff469cc9ac07 277
Joeatsumi 0:ff469cc9ac07 278 wait(0.002);
Joeatsumi 0:ff469cc9ac07 279
Joeatsumi 0:ff469cc9ac07 280 }//while ends
Joeatsumi 0:ff469cc9ac07 281
Joeatsumi 0:ff469cc9ac07 282 }//main ends