Atsumi Toda / Mbed 2 deprecated Auto_pilot_prototype_2

ドローン用計測制御基板の作り方vol.2で使用したピッチ制御プログラムです。

Dependencies:   mbed MPU6050_alter SDFileSystem

main.cpp@0:e647f6de3d26, 2020-03-06 (annotated)

Committer:
Joeatsumi
Date:
Fri Mar 06 15:03:57 2020 +0000
Revision:
0:e647f6de3d26
An software for autopilot(ver2)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joeatsumi 0:e647f6de3d26 1 //==================================================
Joeatsumi 0:e647f6de3d26 2 //Auto pilot(prototype2)
Joeatsumi 0:e647f6de3d26 3 //
Joeatsumi 0:e647f6de3d26 4 //MPU board: mbed LPC1768
Joeatsumi 0:e647f6de3d26 5 //Multiplexer TC74HC157AP
Joeatsumi 0:e647f6de3d26 6 //Accelerometer +Gyro sensor : GY-521
Joeatsumi 0:e647f6de3d26 7 //2019/11/17 A.Toda
Joeatsumi 0:e647f6de3d26 8 //==================================================
Joeatsumi 0:e647f6de3d26 9 #include "mbed.h"
Joeatsumi 0:e647f6de3d26 10 #include "MPU6050.h"
Joeatsumi 0:e647f6de3d26 11 #include "SDFileSystem.h"
Joeatsumi 0:e647f6de3d26 12
Joeatsumi 0:e647f6de3d26 13 #include "Vector.h"
Joeatsumi 0:e647f6de3d26 14 #include "Matrix.h"
Joeatsumi 0:e647f6de3d26 15 #include "Vector_Matrix_operator.h"
Joeatsumi 0:e647f6de3d26 16
Joeatsumi 0:e647f6de3d26 17 #include "math.h"
Joeatsumi 0:e647f6de3d26 18
Joeatsumi 0:e647f6de3d26 19 //==================================================
Joeatsumi 0:e647f6de3d26 20 #define RAD_TO_DEG 57.2957795f // 180 / π
Joeatsumi 0:e647f6de3d26 21 #define MAX_MEAN_COUNTER 500
Joeatsumi 0:e647f6de3d26 22
Joeatsumi 0:e647f6de3d26 23 #define ACC_X -1.205178682//offset of x-axi accelerometer
Joeatsumi 0:e647f6de3d26 24 #define ACC_Y -0.141728488//offset of y-axi accelerometer
Joeatsumi 0:e647f6de3d26 25 #define ACC_Z -0.339272785//offset of z-axi accelerometer
Joeatsumi 0:e647f6de3d26 26
Joeatsumi 0:e647f6de3d26 27 #define ACC_GAIN_X 1.0
Joeatsumi 0:e647f6de3d26 28 #define ACC_GAIN_Y 0.995906146
Joeatsumi 0:e647f6de3d26 29 #define ACC_GAIN_Z 1.017766039
Joeatsumi 0:e647f6de3d26 30
Joeatsumi 0:e647f6de3d26 31 #define THRESHOLD_PWM 0.0015
Joeatsumi 0:e647f6de3d26 32 #define M_A_PWM 0.0017
Joeatsumi 0:e647f6de3d26 33 #define SERVO_PERIOD 0.020
Joeatsumi 0:e647f6de3d26 34
Joeatsumi 0:e647f6de3d26 35 #define PITCH_TARGET 0.0
Joeatsumi 0:e647f6de3d26 36
Joeatsumi 0:e647f6de3d26 37 /*
Joeatsumi 0:e647f6de3d26 38 8/2/2020でのゲイン
Joeatsumi 0:e647f6de3d26 39 #define P_P_GAIN 3.0
Joeatsumi 0:e647f6de3d26 40 #define P_I_GAIN 0.5
Joeatsumi 0:e647f6de3d26 41 #define P_D_GAIN 1.0
Joeatsumi 0:e647f6de3d26 42 */
Joeatsumi 0:e647f6de3d26 43
Joeatsumi 0:e647f6de3d26 44 #define OFFSET_PWM_ELE 0.0015
Joeatsumi 0:e647f6de3d26 45 #define MAX_PWM_ELE 0.00175
Joeatsumi 0:e647f6de3d26 46 #define MIN_PWM_ELE 0.00125
Joeatsumi 0:e647f6de3d26 47
Joeatsumi 0:e647f6de3d26 48 #define LPF_ELE_K 0.110//この値が小さいとローパスフィルタが強くなる 0.050は小さすぎる
Joeatsumi 0:e647f6de3d26 49 //0.200は動きが機敏過ぎる。
Joeatsumi 0:e647f6de3d26 50 //==================================================
Joeatsumi 0:e647f6de3d26 51
Joeatsumi 0:e647f6de3d26 52 //Port Setting
Joeatsumi 0:e647f6de3d26 53 SDFileSystem sd(p5, p6, p7, p8, "sd");//pins for sd slot
Joeatsumi 0:e647f6de3d26 54 MPU6050 mpu(p9, p10); //Accelerometer + Gyro
Joeatsumi 0:e647f6de3d26 55 //(SDA,SCLK)
Joeatsumi 0:e647f6de3d26 56 DigitalIn logging_terminater(p16);
Joeatsumi 0:e647f6de3d26 57 InterruptIn reading_port(p18);
Joeatsumi 0:e647f6de3d26 58 DigitalOut mux_switch(p19);
Joeatsumi 0:e647f6de3d26 59 PwmOut ELE(p21);
Joeatsumi 0:e647f6de3d26 60
Joeatsumi 0:e647f6de3d26 61 Serial pc(USBTX, USBRX); //UART
Joeatsumi 0:e647f6de3d26 62
Joeatsumi 0:e647f6de3d26 63 //Pointer of sd card
Joeatsumi 0:e647f6de3d26 64 FILE *fp;
Joeatsumi 0:e647f6de3d26 65
Joeatsumi 0:e647f6de3d26 66 //==================================================
Joeatsumi 0:e647f6de3d26 67 //Accelerometer and gyro data
Joeatsumi 0:e647f6de3d26 68 //==================================================
Joeatsumi 0:e647f6de3d26 69 double acc[3]; //variables for accelerometer
Joeatsumi 0:e647f6de3d26 70 double gyro[3]; //variables for gyro
Joeatsumi 0:e647f6de3d26 71
Joeatsumi 0:e647f6de3d26 72 double offset_gyro_x=0.0;
Joeatsumi 0:e647f6de3d26 73 double offset_gyro_y=0.0;
Joeatsumi 0:e647f6de3d26 74
Joeatsumi 0:e647f6de3d26 75 double sum_gyro_x=0.0;
Joeatsumi 0:e647f6de3d26 76 double sum_gyro_y=0.0;
Joeatsumi 0:e647f6de3d26 77
Joeatsumi 0:e647f6de3d26 78 double threshold_acc,threshold_acc_ini;
Joeatsumi 0:e647f6de3d26 79
Joeatsumi 0:e647f6de3d26 80 double dev =0.0;
Joeatsumi 0:e647f6de3d26 81 double i_dev=0.0;
Joeatsumi 0:e647f6de3d26 82 double d_dev=0.0;
Joeatsumi 0:e647f6de3d26 83
Joeatsumi 0:e647f6de3d26 84 double old_i_dev=0.0;
Joeatsumi 0:e647f6de3d26 85 //==================================================
Joeatsumi 0:e647f6de3d26 86 //Atitude data
Joeatsumi 0:e647f6de3d26 87 //==================================================
Joeatsumi 0:e647f6de3d26 88 double roll_and_pitch_acc[2];//atitude from acceleromter
Joeatsumi 0:e647f6de3d26 89 double roll_and_pitch[2];//atitude from gyro and acceleromter
Joeatsumi 0:e647f6de3d26 90
Joeatsumi 0:e647f6de3d26 91 /*--------------------------行列、ベクトル-----------------------------*/
Joeatsumi 0:e647f6de3d26 92 Matrix rate_angle_matrix(4,4);//角速度行列 クォータニオンの更新に使う
Joeatsumi 0:e647f6de3d26 93 Vector quaternion(4),pre_quaternion(4),dump_1(4);;//クォータニオン
Joeatsumi 0:e647f6de3d26 94 /*-------------------------------------------------------------------*/
Joeatsumi 0:e647f6de3d26 95
Joeatsumi 0:e647f6de3d26 96 //==================================================
Joeatsumi 0:e647f6de3d26 97 //Timer valiables
Joeatsumi 0:e647f6de3d26 98 //==================================================
Joeatsumi 0:e647f6de3d26 99 Timer ch_time;//timer for calculate pulse width
Joeatsumi 0:e647f6de3d26 100 Timer passed_time;//timer for calculate atitude
Joeatsumi 0:e647f6de3d26 101
Joeatsumi 0:e647f6de3d26 102 double measured_pre_pulse=0.0;
Joeatsumi 0:e647f6de3d26 103 double measured_pulse=0.0;
Joeatsumi 0:e647f6de3d26 104
Joeatsumi 0:e647f6de3d26 105 double time_new;
Joeatsumi 0:e647f6de3d26 106 double time_old;
Joeatsumi 0:e647f6de3d26 107
Joeatsumi 0:e647f6de3d26 108 double pulse_width_ele,deflection_ele,old_deflection_ele;
Joeatsumi 0:e647f6de3d26 109
Joeatsumi 0:e647f6de3d26 110 //=================================================
Joeatsumi 0:e647f6de3d26 111 //PID Gain
Joeatsumi 0:e647f6de3d26 112 //==================================================
Joeatsumi 0:e647f6de3d26 113 float P_P_GAIN,P_I_GAIN,P_D_GAIN;
Joeatsumi 0:e647f6de3d26 114 int P_GAIN,I_GAIN,D_GAIN;
Joeatsumi 0:e647f6de3d26 115
Joeatsumi 0:e647f6de3d26 116 //=================================================
Joeatsumi 0:e647f6de3d26 117 //エレベータの舵角
Joeatsumi 0:e647f6de3d26 118 //=================================================
Joeatsumi 0:e647f6de3d26 119 double pitch_command;
Joeatsumi 0:e647f6de3d26 120
Joeatsumi 0:e647f6de3d26 121 //=================================================
Joeatsumi 0:e647f6de3d26 122 //手動か自動かのインジケータ0なら手動、1なら自動
Joeatsumi 0:e647f6de3d26 123 //=================================================
Joeatsumi 0:e647f6de3d26 124 int m_a_indicater;
Joeatsumi 0:e647f6de3d26 125 //=================================================
Joeatsumi 0:e647f6de3d26 126 //Functions for rising and falind edge interrution
Joeatsumi 0:e647f6de3d26 127 //=================================================
Joeatsumi 0:e647f6de3d26 128 //rise edge
Joeatsumi 0:e647f6de3d26 129 void rising_edge(){
Joeatsumi 0:e647f6de3d26 130 ch_time.reset();//reset timer counter
Joeatsumi 0:e647f6de3d26 131 measured_pre_pulse=ch_time.read();
Joeatsumi 0:e647f6de3d26 132
Joeatsumi 0:e647f6de3d26 133 }
Joeatsumi 0:e647f6de3d26 134
Joeatsumi 0:e647f6de3d26 135 //falling edge
Joeatsumi 0:e647f6de3d26 136 void falling_edge(){
Joeatsumi 0:e647f6de3d26 137
Joeatsumi 0:e647f6de3d26 138 measured_pre_pulse=(ch_time.read()-measured_pre_pulse);
Joeatsumi 0:e647f6de3d26 139 //pc.printf("The pulse width=%f\r\n",measured_pre_pulse);
Joeatsumi 0:e647f6de3d26 140 if(measured_pre_pulse>M_A_PWM){
Joeatsumi 0:e647f6de3d26 141 mux_switch=1;
Joeatsumi 0:e647f6de3d26 142 m_a_indicater=1;//set indicater as auto
Joeatsumi 0:e647f6de3d26 143 }else{
Joeatsumi 0:e647f6de3d26 144 mux_switch=0;
Joeatsumi 0:e647f6de3d26 145 m_a_indicater=0;//set indicater as manual
Joeatsumi 0:e647f6de3d26 146 }
Joeatsumi 0:e647f6de3d26 147 }
Joeatsumi 0:e647f6de3d26 148
Joeatsumi 0:e647f6de3d26 149 //terminate logging
Joeatsumi 0:e647f6de3d26 150 void end_of_log(){
Joeatsumi 0:e647f6de3d26 151 //flipper.detach();
Joeatsumi 0:e647f6de3d26 152 fclose(fp);//close "Atitude_angles.csv"
Joeatsumi 0:e647f6de3d26 153 pc.printf("Logging was terminated.");
Joeatsumi 0:e647f6de3d26 154
Joeatsumi 0:e647f6de3d26 155 }
Joeatsumi 0:e647f6de3d26 156 //==================================================
Joeatsumi 0:e647f6de3d26 157 //Gyro and accelerometer functions
Joeatsumi 0:e647f6de3d26 158 //==================================================
Joeatsumi 0:e647f6de3d26 159 //get data
Joeatsumi 0:e647f6de3d26 160 void aquisition_sensor_values(double *a,double *g){
Joeatsumi 0:e647f6de3d26 161
Joeatsumi 0:e647f6de3d26 162 float ac[3],gy[3];
Joeatsumi 0:e647f6de3d26 163
Joeatsumi 0:e647f6de3d26 164 mpu.getAccelero(ac);//get acceleration (Accelerometer)
Joeatsumi 0:e647f6de3d26 165 //x_axis acc[0]
Joeatsumi 0:e647f6de3d26 166 //y_axis acc[1]
Joeatsumi 0:e647f6de3d26 167 //z_axis acc[2]
Joeatsumi 0:e647f6de3d26 168 mpu.getGyro(gy); //get rate of angle(Gyro)
Joeatsumi 0:e647f6de3d26 169 //x_axis gyro[0]
Joeatsumi 0:e647f6de3d26 170 //y_axis gyro[1]
Joeatsumi 0:e647f6de3d26 171 //z_axis gyro[2]
Joeatsumi 0:e647f6de3d26 172
Joeatsumi 0:e647f6de3d26 173 //Invertion for direction of Accelerometer axis
Joeatsumi 0:e647f6de3d26 174 ac[0]*=(-1.0);
Joeatsumi 0:e647f6de3d26 175 ac[2]*=(-1.0);
Joeatsumi 0:e647f6de3d26 176
Joeatsumi 0:e647f6de3d26 177 ac[0]=(ac[0]-ACC_X)/ACC_GAIN_X;
Joeatsumi 0:e647f6de3d26 178 ac[1]=(ac[1]-ACC_Y)/ACC_GAIN_Y;
Joeatsumi 0:e647f6de3d26 179 ac[2]=(ac[2]-ACC_Z)/ACC_GAIN_Z;
Joeatsumi 0:e647f6de3d26 180
Joeatsumi 0:e647f6de3d26 181 //Unit convertion of rate of angle(radian to degree)
Joeatsumi 0:e647f6de3d26 182 gy[0]*=RAD_TO_DEG;
Joeatsumi 0:e647f6de3d26 183 gy[0]*=(-1.0);
Joeatsumi 0:e647f6de3d26 184
Joeatsumi 0:e647f6de3d26 185 gy[1]*=RAD_TO_DEG;
Joeatsumi 0:e647f6de3d26 186 gy[2]*=RAD_TO_DEG;
Joeatsumi 0:e647f6de3d26 187 gy[2]*=(-1.0);
Joeatsumi 0:e647f6de3d26 188
Joeatsumi 0:e647f6de3d26 189 for(int i=0;i<3;i++){
Joeatsumi 0:e647f6de3d26 190 a[i]=double(ac[i]);
Joeatsumi 0:e647f6de3d26 191 g[i]=double(gy[i]);
Joeatsumi 0:e647f6de3d26 192 }
Joeatsumi 0:e647f6de3d26 193 g[0]-=offset_gyro_x;//offset rejection
Joeatsumi 0:e647f6de3d26 194 g[1]-=offset_gyro_y;//offset rejection
Joeatsumi 0:e647f6de3d26 195
Joeatsumi 0:e647f6de3d26 196 return;
Joeatsumi 0:e647f6de3d26 197
Joeatsumi 0:e647f6de3d26 198 }
Joeatsumi 0:e647f6de3d26 199
Joeatsumi 0:e647f6de3d26 200 //calculate offset of gyro
Joeatsumi 0:e647f6de3d26 201 void offset_calculation_for_gyro(){
Joeatsumi 0:e647f6de3d26 202
Joeatsumi 0:e647f6de3d26 203 //Accelerometer and gyro setting
Joeatsumi 0:e647f6de3d26 204 mpu.setAcceleroRange(0);//acceleration range is +-2G
Joeatsumi 0:e647f6de3d26 205 mpu.setGyroRange(1);//gyro rate is +-500degree per second(dps)
Joeatsumi 0:e647f6de3d26 206
Joeatsumi 0:e647f6de3d26 207 //calculate offset of gyro
Joeatsumi 0:e647f6de3d26 208 for(int mean_counter=0; mean_counter<MAX_MEAN_COUNTER ;mean_counter++){
Joeatsumi 0:e647f6de3d26 209 aquisition_sensor_values(acc,gyro);
Joeatsumi 0:e647f6de3d26 210 sum_gyro_x+=gyro[0];
Joeatsumi 0:e647f6de3d26 211 sum_gyro_y+=gyro[1];
Joeatsumi 0:e647f6de3d26 212 wait(0.01);
Joeatsumi 0:e647f6de3d26 213 }
Joeatsumi 0:e647f6de3d26 214
Joeatsumi 0:e647f6de3d26 215 offset_gyro_x=sum_gyro_x/MAX_MEAN_COUNTER;
Joeatsumi 0:e647f6de3d26 216 offset_gyro_y=sum_gyro_y/MAX_MEAN_COUNTER;
Joeatsumi 0:e647f6de3d26 217
Joeatsumi 0:e647f6de3d26 218 return;
Joeatsumi 0:e647f6de3d26 219 }
Joeatsumi 0:e647f6de3d26 220
Joeatsumi 0:e647f6de3d26 221 //atitude calculation from acceleromter
Joeatsumi 0:e647f6de3d26 222 void atitude_estimation_from_accelerometer(double *a,double *roll_and_pitch){
Joeatsumi 0:e647f6de3d26 223
Joeatsumi 0:e647f6de3d26 224 roll_and_pitch[0] = atan(a[1]/a[2])*RAD_TO_DEG;//roll
Joeatsumi 0:e647f6de3d26 225 roll_and_pitch[1] = atan(a[0]/sqrt( (a[1]*a[1]+a[2]*a[2]) ) )*RAD_TO_DEG;//pitch
Joeatsumi 0:e647f6de3d26 226
Joeatsumi 0:e647f6de3d26 227 return;
Joeatsumi 0:e647f6de3d26 228 }
Joeatsumi 0:e647f6de3d26 229
Joeatsumi 0:e647f6de3d26 230 //quaternion to euler
Joeatsumi 0:e647f6de3d26 231 void quaternion_to_euler(Vector& qua,double *roll_and_pitch ){
Joeatsumi 0:e647f6de3d26 232
Joeatsumi 0:e647f6de3d26 233 double q0=double (qua.GetComp(1));
Joeatsumi 0:e647f6de3d26 234 double q1=double (qua.GetComp(2));
Joeatsumi 0:e647f6de3d26 235 double q2=double (qua.GetComp(3));
Joeatsumi 0:e647f6de3d26 236 double q3=double (qua.GetComp(4));
Joeatsumi 0:e647f6de3d26 237
Joeatsumi 0:e647f6de3d26 238 roll_and_pitch[0]=atan((q2*q3+q0*q1)/(q0*q0-q1*q1-q2*q2+q3*q3)) ;//roll
Joeatsumi 0:e647f6de3d26 239 roll_and_pitch[1]=-asin(2*(q1*q3-q0*q2));
Joeatsumi 0:e647f6de3d26 240
Joeatsumi 0:e647f6de3d26 241 return;
Joeatsumi 0:e647f6de3d26 242 }
Joeatsumi 0:e647f6de3d26 243
Joeatsumi 0:e647f6de3d26 244 //quaternion to euler
Joeatsumi 0:e647f6de3d26 245 void euler_to_quaternion(Vector& qua,double *roll_and_pitch ){
Joeatsumi 0:e647f6de3d26 246
Joeatsumi 0:e647f6de3d26 247 double roll_rad_2=(roll_and_pitch[0]/57.3)/2.0;
Joeatsumi 0:e647f6de3d26 248 double pitch_rad_2=(roll_and_pitch[1]/57.3)/2.0;
Joeatsumi 0:e647f6de3d26 249 double yaw_rad_2=0.0;
Joeatsumi 0:e647f6de3d26 250
Joeatsumi 0:e647f6de3d26 251 float q0= cos(roll_rad_2)*cos(pitch_rad_2)*cos(yaw_rad_2)
Joeatsumi 0:e647f6de3d26 252 +sin(roll_rad_2)*sin(pitch_rad_2)*sin(yaw_rad_2);
Joeatsumi 0:e647f6de3d26 253
Joeatsumi 0:e647f6de3d26 254 float q1= sin(roll_rad_2)*cos(pitch_rad_2)*cos(yaw_rad_2)
Joeatsumi 0:e647f6de3d26 255 -cos(roll_rad_2)*sin(pitch_rad_2)*sin(yaw_rad_2);
Joeatsumi 0:e647f6de3d26 256
Joeatsumi 0:e647f6de3d26 257 float q2= cos(roll_rad_2)*sin(pitch_rad_2)*cos(yaw_rad_2)
Joeatsumi 0:e647f6de3d26 258 +sin(roll_rad_2)*cos(pitch_rad_2)*sin(yaw_rad_2);
Joeatsumi 0:e647f6de3d26 259
Joeatsumi 0:e647f6de3d26 260 float q3= cos(roll_rad_2)*cos(pitch_rad_2)*sin(yaw_rad_2)
Joeatsumi 0:e647f6de3d26 261 -sin(roll_rad_2)*sin(pitch_rad_2)*cos(yaw_rad_2);
Joeatsumi 0:e647f6de3d26 262
Joeatsumi 0:e647f6de3d26 263 //クォータニオン行列の作成（クォータニオンの演算に用いる）
Joeatsumi 0:e647f6de3d26 264 float quaternion_elements[4]={q0,q1,q2,q3};
Joeatsumi 0:e647f6de3d26 265 qua.SetComps(quaternion_elements);
Joeatsumi 0:e647f6de3d26 266
Joeatsumi 0:e647f6de3d26 267
Joeatsumi 0:e647f6de3d26 268 return;
Joeatsumi 0:e647f6de3d26 269 }
Joeatsumi 0:e647f6de3d26 270
Joeatsumi 0:e647f6de3d26 271 //atitude calculation
Joeatsumi 0:e647f6de3d26 272 void atitude_update(){
Joeatsumi 0:e647f6de3d26 273
Joeatsumi 0:e647f6de3d26 274 //慣性センサの計測
Joeatsumi 0:e647f6de3d26 275 aquisition_sensor_values(acc,gyro);
Joeatsumi 0:e647f6de3d26 276 //角速度行列の作成（クォータニオンの演算に用いる）
Joeatsumi 0:e647f6de3d26 277 float rate_angles[16]={0.0,-float(gyro[0]),-float(gyro[1]),-float(gyro[2])
Joeatsumi 0:e647f6de3d26 278 ,float(gyro[0]),0.0,float(gyro[2]),-float(gyro[1])
Joeatsumi 0:e647f6de3d26 279 ,float(gyro[1]),-float(gyro[2]),0.0,float(gyro[0])
Joeatsumi 0:e647f6de3d26 280 ,float(gyro[0]),float(gyro[1]),-float(gyro[2]),0.0};
Joeatsumi 0:e647f6de3d26 281
Joeatsumi 0:e647f6de3d26 282 rate_angle_matrix.SetComps(rate_angles);
Joeatsumi 0:e647f6de3d26 283 //クォータニオンの演算
Joeatsumi 0:e647f6de3d26 284 //pc.printf("クォータニオンの演算\r\n");
Joeatsumi 0:e647f6de3d26 285 float coefficents=0.5*(time_new-time_old);
Joeatsumi 0:e647f6de3d26 286 float coefficents_elements[16]={coefficents,0.0,0.0,0.0
Joeatsumi 0:e647f6de3d26 287 ,0.0,coefficents,0.0,0.0
Joeatsumi 0:e647f6de3d26 288 ,0.0,0.0,coefficents,0.0
Joeatsumi 0:e647f6de3d26 289 ,0.0,0.0,0.0,coefficents};
Joeatsumi 0:e647f6de3d26 290 Vector coefficents_vector(4);
Joeatsumi 0:e647f6de3d26 291 coefficents_vector.SetComps(coefficents_elements);
Joeatsumi 0:e647f6de3d26 292
Joeatsumi 0:e647f6de3d26 293 dump_1=rate_angle_matrix*coefficents_vector;
Joeatsumi 0:e647f6de3d26 294 quaternion=dump_1+pre_quaternion;
Joeatsumi 0:e647f6de3d26 295
Joeatsumi 0:e647f6de3d26 296 //正規化
Joeatsumi 0:e647f6de3d26 297 //pc.printf("正規化\r\n");
Joeatsumi 0:e647f6de3d26 298 quaternion=quaternion.Normalize();
Joeatsumi 0:e647f6de3d26 299 //クォータニオンからオイラー角への変換
Joeatsumi 0:e647f6de3d26 300 quaternion_to_euler(quaternion,roll_and_pitch_acc );
Joeatsumi 0:e647f6de3d26 301
Joeatsumi 0:e647f6de3d26 302 threshold_acc=sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
Joeatsumi 0:e647f6de3d26 303
Joeatsumi 0:e647f6de3d26 304 if((threshold_acc>=0.9*threshold_acc_ini)
Joeatsumi 0:e647f6de3d26 305 &&(threshold_acc<=1.1*threshold_acc_ini)){
Joeatsumi 0:e647f6de3d26 306
Joeatsumi 0:e647f6de3d26 307 atitude_estimation_from_accelerometer(acc,roll_and_pitch_acc);
Joeatsumi 0:e647f6de3d26 308 roll_and_pitch[0] = 0.98*roll_and_pitch[0] + 0.02*roll_and_pitch_acc[0];
Joeatsumi 0:e647f6de3d26 309 roll_and_pitch[1] = 0.98*roll_and_pitch[1] + 0.02*roll_and_pitch_acc[1];
Joeatsumi 0:e647f6de3d26 310
Joeatsumi 0:e647f6de3d26 311 }else{}
Joeatsumi 0:e647f6de3d26 312 //補正したオイラー角をクォータニオンへ変換
Joeatsumi 0:e647f6de3d26 313 euler_to_quaternion(pre_quaternion,roll_and_pitch_acc );
Joeatsumi 0:e647f6de3d26 314
Joeatsumi 0:e647f6de3d26 315 //microSDに記録する
Joeatsumi 0:e647f6de3d26 316 //経過時間,ロール角,ピッチ角,操縦方式,慣性センサの値
Joeatsumi 0:e647f6de3d26 317 fprintf(fp, "%f,%f,%f,%d,%f,%f,%f,%f,%f,%f\r\n"
Joeatsumi 0:e647f6de3d26 318 ,time_new,roll_and_pitch[0],roll_and_pitch[1],m_a_indicater,gyro[0],gyro[1],acc[0],acc[1],acc[2],pitch_command);
Joeatsumi 0:e647f6de3d26 319
Joeatsumi 0:e647f6de3d26 320 return;
Joeatsumi 0:e647f6de3d26 321
Joeatsumi 0:e647f6de3d26 322 }
Joeatsumi 0:e647f6de3d26 323
Joeatsumi 0:e647f6de3d26 324 //elevation commnad to PWM
Joeatsumi 0:e647f6de3d26 325 double elevation_to_PWM(double elevation_command){
Joeatsumi 0:e647f6de3d26 326
Joeatsumi 0:e647f6de3d26 327 /*
Joeatsumi 0:e647f6de3d26 328 PWM信号0.25msが舵角の16.45度に相当する.
Joeatsumi 0:e647f6de3d26 329 */
Joeatsumi 0:e647f6de3d26 330
Joeatsumi 0:e647f6de3d26 331 double PWM_pitch = (elevation_command*1.519)/100000+ OFFSET_PWM_ELE;
Joeatsumi 0:e647f6de3d26 332 //double PWM_pitch = ( ((elevation_command)*6.0/1000.0)/1000.0 )+ OFFSET_PWM_ELE;
Joeatsumi 0:e647f6de3d26 333
Joeatsumi 0:e647f6de3d26 334 /*PWMコマンドの上限と下限の設定*/
Joeatsumi 0:e647f6de3d26 335 if(PWM_pitch>MAX_PWM_ELE){
Joeatsumi 0:e647f6de3d26 336 PWM_pitch=MAX_PWM_ELE;
Joeatsumi 0:e647f6de3d26 337
Joeatsumi 0:e647f6de3d26 338 }else if(PWM_pitch<MIN_PWM_ELE){
Joeatsumi 0:e647f6de3d26 339 PWM_pitch=MIN_PWM_ELE;
Joeatsumi 0:e647f6de3d26 340 }
Joeatsumi 0:e647f6de3d26 341
Joeatsumi 0:e647f6de3d26 342 return PWM_pitch;
Joeatsumi 0:e647f6de3d26 343
Joeatsumi 0:e647f6de3d26 344 }
Joeatsumi 0:e647f6de3d26 345
Joeatsumi 0:e647f6de3d26 346 double deflection_of_ele(double pitch){
Joeatsumi 0:e647f6de3d26 347
Joeatsumi 0:e647f6de3d26 348 double add_deflection=((pitch-PITCH_TARGET)*6.0/1000.0)/1000.0;
Joeatsumi 0:e647f6de3d26 349
Joeatsumi 0:e647f6de3d26 350 return add_deflection;
Joeatsumi 0:e647f6de3d26 351 }
Joeatsumi 0:e647f6de3d26 352
Joeatsumi 0:e647f6de3d26 353 //PID controller
Joeatsumi 0:e647f6de3d26 354 double pitch_PID_controller(double pitch,double target,double gyro_pitch){
Joeatsumi 0:e647f6de3d26 355
Joeatsumi 0:e647f6de3d26 356 dev=target-pitch;
Joeatsumi 0:e647f6de3d26 357
Joeatsumi 0:e647f6de3d26 358 //アンチワインドアップ
Joeatsumi 0:e647f6de3d26 359 i_dev=old_i_dev+dev*(time_new-time_old);
Joeatsumi 0:e647f6de3d26 360 if(i_dev>=25.0){
Joeatsumi 0:e647f6de3d26 361 i_dev=25.0;
Joeatsumi 0:e647f6de3d26 362 }else if(i_dev<=-25.0){
Joeatsumi 0:e647f6de3d26 363 i_dev=-25.0;
Joeatsumi 0:e647f6de3d26 364 }
Joeatsumi 0:e647f6de3d26 365 //アンチワインドアップ終わり
Joeatsumi 0:e647f6de3d26 366
Joeatsumi 0:e647f6de3d26 367 old_i_dev=i_dev;
Joeatsumi 0:e647f6de3d26 368
Joeatsumi 0:e647f6de3d26 369 d_dev=-gyro_pitch;
Joeatsumi 0:e647f6de3d26 370
Joeatsumi 0:e647f6de3d26 371 pitch_command = double(P_P_GAIN*dev+P_I_GAIN*i_dev+P_D_GAIN*d_dev);
Joeatsumi 0:e647f6de3d26 372
Joeatsumi 0:e647f6de3d26 373 double pwm_command = elevation_to_PWM(pitch_command);//pwm信号に変換
Joeatsumi 0:e647f6de3d26 374
Joeatsumi 0:e647f6de3d26 375 return pwm_command;
Joeatsumi 0:e647f6de3d26 376
Joeatsumi 0:e647f6de3d26 377 }
Joeatsumi 0:e647f6de3d26 378
Joeatsumi 0:e647f6de3d26 379 //LPF
Joeatsumi 0:e647f6de3d26 380
Joeatsumi 0:e647f6de3d26 381 double LPF_pitch(double c_com,double old_com){
Joeatsumi 0:e647f6de3d26 382
Joeatsumi 0:e647f6de3d26 383 double lpf_output=(1-LPF_ELE_K)*old_com+LPF_ELE_K*c_com;
Joeatsumi 0:e647f6de3d26 384
Joeatsumi 0:e647f6de3d26 385 return lpf_output;
Joeatsumi 0:e647f6de3d26 386 }
Joeatsumi 0:e647f6de3d26 387
Joeatsumi 0:e647f6de3d26 388 //==================================================
Joeatsumi 0:e647f6de3d26 389 //Main
Joeatsumi 0:e647f6de3d26 390 //==================================================
Joeatsumi 0:e647f6de3d26 391 int main() {
Joeatsumi 0:e647f6de3d26 392
Joeatsumi 0:e647f6de3d26 393 wait(5.0);
Joeatsumi 0:e647f6de3d26 394
Joeatsumi 0:e647f6de3d26 395 //UART initialization
Joeatsumi 0:e647f6de3d26 396 pc.baud(115200);
Joeatsumi 0:e647f6de3d26 397
Joeatsumi 0:e647f6de3d26 398 //define servo period
Joeatsumi 0:e647f6de3d26 399 ELE.period(SERVO_PERIOD); // servo requires a 20ms period
Joeatsumi 0:e647f6de3d26 400 pulse_width_ele=0.0015;
Joeatsumi 0:e647f6de3d26 401
Joeatsumi 0:e647f6de3d26 402 //timer starts
Joeatsumi 0:e647f6de3d26 403 ch_time.start();
Joeatsumi 0:e647f6de3d26 404 passed_time.start();
Joeatsumi 0:e647f6de3d26 405
Joeatsumi 0:e647f6de3d26 406 time_old=0.0;
Joeatsumi 0:e647f6de3d26 407
Joeatsumi 0:e647f6de3d26 408 //declare interrupitons
Joeatsumi 0:e647f6de3d26 409 reading_port.rise(rising_edge);
Joeatsumi 0:e647f6de3d26 410 reading_port.fall(falling_edge);
Joeatsumi 0:e647f6de3d26 411
Joeatsumi 0:e647f6de3d26 412 /*
Joeatsumi 0:e647f6de3d26 413 mux_switch=0;//set circit as manual mode
Joeatsumi 0:e647f6de3d26 414 m_a_indicater=0;//set indicater as manual
Joeatsumi 0:e647f6de3d26 415 */
Joeatsumi 0:e647f6de3d26 416
Joeatsumi 0:e647f6de3d26 417 //gyro and accelerometer initialization
Joeatsumi 0:e647f6de3d26 418 offset_calculation_for_gyro();
Joeatsumi 0:e647f6de3d26 419
Joeatsumi 0:e647f6de3d26 420 //determine initilal atitude
Joeatsumi 0:e647f6de3d26 421 aquisition_sensor_values(acc,gyro);
Joeatsumi 0:e647f6de3d26 422 atitude_estimation_from_accelerometer(acc,roll_and_pitch);
Joeatsumi 0:e647f6de3d26 423 euler_to_quaternion(pre_quaternion,roll_and_pitch);
Joeatsumi 0:e647f6de3d26 424
Joeatsumi 0:e647f6de3d26 425 threshold_acc_ini=sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
Joeatsumi 0:e647f6de3d26 426
Joeatsumi 0:e647f6de3d26 427 /*
Joeatsumi 0:e647f6de3d26 428 PIDゲインをsdカードのテキストファイルから読み取る
Joeatsumi 0:e647f6de3d26 429 int型しか読みとれないので、希望する値の10倍をテキストファイルに書き込んでいる。
Joeatsumi 0:e647f6de3d26 430 Pゲインを3.0,Iゲインを0.5,Dゲインを1.0とする場合
Joeatsumi 0:e647f6de3d26 431
Joeatsumi 0:e647f6de3d26 432 30,5,10
Joeatsumi 0:e647f6de3d26 433
Joeatsumi 0:e647f6de3d26 434 のようにテキストデータをsdカードに用意する。
Joeatsumi 0:e647f6de3d26 435 */
Joeatsumi 0:e647f6de3d26 436 //open PID gain file in sd card
Joeatsumi 0:e647f6de3d26 437 FILE*ga = fopen("/sd/Gain_data.txt", "r");
Joeatsumi 0:e647f6de3d26 438 if(ga == NULL) {
Joeatsumi 0:e647f6de3d26 439 error("Could not open file for write\n");
Joeatsumi 0:e647f6de3d26 440 }
Joeatsumi 0:e647f6de3d26 441
Joeatsumi 0:e647f6de3d26 442 while (!feof(ga)) {
Joeatsumi 0:e647f6de3d26 443 int n = fscanf(ga, "%d,%d,%d",&P_GAIN, &I_GAIN, &D_GAIN);
Joeatsumi 0:e647f6de3d26 444
Joeatsumi 0:e647f6de3d26 445 if(n!= 3){
Joeatsumi 0:e647f6de3d26 446 error("Could not read 3 elements");
Joeatsumi 0:e647f6de3d26 447 }
Joeatsumi 0:e647f6de3d26 448 }
Joeatsumi 0:e647f6de3d26 449
Joeatsumi 0:e647f6de3d26 450 P_P_GAIN=P_GAIN/10.0;
Joeatsumi 0:e647f6de3d26 451 P_I_GAIN=I_GAIN/10.0;
Joeatsumi 0:e647f6de3d26 452 P_D_GAIN=D_GAIN/10.0;
Joeatsumi 0:e647f6de3d26 453
Joeatsumi 0:e647f6de3d26 454 fclose(ga);
Joeatsumi 0:e647f6de3d26 455
Joeatsumi 0:e647f6de3d26 456 //create folder(sd) in sd card
Joeatsumi 0:e647f6de3d26 457 mkdir("/sd", 0777);
Joeatsumi 0:e647f6de3d26 458 //create "Atitude_angles.csv" in folder(sd)
Joeatsumi 0:e647f6de3d26 459 fp = fopen("/sd/Atitude_angles.csv", "a");//ファイルがある場合は追加で書き込み
Joeatsumi 0:e647f6de3d26 460
Joeatsumi 0:e647f6de3d26 461 if(fp == NULL) {
Joeatsumi 0:e647f6de3d26 462 error("Could not open file for write\n");
Joeatsumi 0:e647f6de3d26 463 }
Joeatsumi 0:e647f6de3d26 464
Joeatsumi 0:e647f6de3d26 465 //Logging starts
Joeatsumi 0:e647f6de3d26 466 pc.printf("Logging starts.");
Joeatsumi 0:e647f6de3d26 467
Joeatsumi 0:e647f6de3d26 468 //write PID gain on sd card
Joeatsumi 0:e647f6de3d26 469 fprintf(fp,"%f,%f,%f\r\n",P_P_GAIN,P_I_GAIN,P_D_GAIN);
Joeatsumi 0:e647f6de3d26 470
Joeatsumi 0:e647f6de3d26 471 //while
Joeatsumi 0:e647f6de3d26 472 while(1){
Joeatsumi 0:e647f6de3d26 473
Joeatsumi 0:e647f6de3d26 474 if(logging_terminater==1){
Joeatsumi 0:e647f6de3d26 475 end_of_log();
Joeatsumi 0:e647f6de3d26 476 }else{}
Joeatsumi 0:e647f6de3d26 477
Joeatsumi 0:e647f6de3d26 478 time_new=passed_time.read();
Joeatsumi 0:e647f6de3d26 479
Joeatsumi 0:e647f6de3d26 480 atitude_update();
Joeatsumi 0:e647f6de3d26 481
Joeatsumi 0:e647f6de3d26 482 time_old=time_new;
Joeatsumi 0:e647f6de3d26 483
Joeatsumi 0:e647f6de3d26 484 /*
Joeatsumi 0:e647f6de3d26 485 ここから先でサーボの操舵角を姿勢角に応じて変化させる。関数pitch_PID_controller
Joeatsumi 0:e647f6de3d26 486 はpitch角とpitch角速度に応じてサーボのパルス幅を返す関数である。
Joeatsumi 0:e647f6de3d26 487 */
Joeatsumi 0:e647f6de3d26 488
Joeatsumi 0:e647f6de3d26 489 deflection_ele = pitch_PID_controller(roll_and_pitch[1],PITCH_TARGET,gyro[1]);
Joeatsumi 0:e647f6de3d26 490
Joeatsumi 0:e647f6de3d26 491 //LPF
Joeatsumi 0:e647f6de3d26 492 deflection_ele = LPF_pitch(deflection_ele,old_deflection_ele);
Joeatsumi 0:e647f6de3d26 493 old_deflection_ele = deflection_ele;
Joeatsumi 0:e647f6de3d26 494
Joeatsumi 0:e647f6de3d26 495 //servo output
Joeatsumi 0:e647f6de3d26 496 ELE.pulsewidth(deflection_ele);
Joeatsumi 0:e647f6de3d26 497
Joeatsumi 0:e647f6de3d26 498 //PCにつないでデバッグを行う際に表示する
Joeatsumi 0:e647f6de3d26 499 pc.printf("%f,%f,%f,%f,%d\r\n",time_new,deflection_ele,roll_and_pitch[0],roll_and_pitch[1],m_a_indicater);
Joeatsumi 0:e647f6de3d26 500
Joeatsumi 0:e647f6de3d26 501 wait(0.002);
Joeatsumi 0:e647f6de3d26 502
Joeatsumi 0:e647f6de3d26 503 }//while ends
Joeatsumi 0:e647f6de3d26 504
Joeatsumi 0:e647f6de3d26 505 }//main ends