Atsumi Toda
/
UAV_Logger4
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: MPU6050_alter HMC5883L
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include <stdio.h> 00003 #include <errno.h> 00004 #include "MPU6050.h" 00005 #include "HMC5883L.h" 00006 00007 #include "Vector.h" 00008 #include "Matrix.h" 00009 #include "Vector_Matrix_operator.h" 00010 00011 // Block devices 00012 //#include "SPIFBlockDevice.h" 00013 //#include "DataFlashBlockDevice.h" 00014 #include "SDBlockDevice.h" 00015 //#include "HeapBlockDevice.h" 00016 00017 // File systems 00018 00019 //#include "LittleFileSystem.h" 00020 #include "FATFileSystem.h" 00021 00022 #define M_PI 3.141592 00023 #define PI2 (2*M_PI) 00024 00025 /*地磁気センサの補正値（足し合わせる）*/ 00026 #define MAG_FIX_X 338 00027 #define MAG_FIX_Y 20 00028 #define MAG_FIX_Z -50 00029 00030 /*ジャイロセンサの補正値（引く）*/ 00031 #define GYRO_FIX_X 290.5498 00032 #define GYRO_FIX_Y 99.29363 00033 #define GYRO_FIX_Z 61.41011 00034 00035 // File system declaration 00036 FATFileSystem fileSystem("fs"); 00037 00038 /*-----割り込み--------*/ 00039 Ticker flipper; 00040 /*--------------------*/ 00041 /*-----タイマー---------*/ 00042 Timer passed_time; 00043 /*---------------------*/ 00044 /*-------ピン指定------------*/ 00045 //declare PWM pins here 00046 PwmOut ESC (p21); 00047 PwmOut Elevator_servo(p22); 00048 PwmOut Rudder_servo(p23); 00049 00050 00051 SDBlockDevice blockDevice(p5, p6, p7, p8); // mosi, miso, sck, cs 00052 MPU6050 mpu(p28, p27);//sda scl 00053 HMC5883L compass(p28, p27);//sda scl 00054 00055 RawSerial gps(p9,p10); //serial port for gps_module 00056 RawSerial pc(USBTX, USBRX); 00057 00058 /*--------------------------*/ 00059 00060 DigitalIn switch_off(p30); 00061 DigitalOut led2(LED2); 00062 00063 /*-----------グローバル変数-----------*/ 00064 double Euler_angle[3]; 00065 double Euler_angle_Initiated[3]; 00066 00067 float g_hokui,g_tokei; 00068 int fp_count=0; 00069 int gps_flag=0; 00070 int gps_flag_conversion=0; 00071 00072 char gps_data[10]="000000000";//末尾にnullがあるので、要素が9でも配列では10箱を用意している 00073 char gps_data_2[10]="000000000";//末尾にnullがあるので、要素が9でも配列では10の箱を用意している 00074 00075 unsigned int tokei_int_receive; 00076 unsigned int hokui_int_receive; 00077 00078 float tokei_float_receive; 00079 float hokui_float_receive; 00080 00081 float a[3];//加速度の値 00082 float g[3];//ジャイロの値[rad/s] 00083 float dpsX,dpsY,dpsZ; 00084 float AccX,AccY,AccZ; 00085 00086 int sensor_array[6]; 00087 int16_t mag[3]; 00088 00089 double Roll_Acc,Pitch_Acc; 00090 double Yaw; 00091 double Azi_mag;//地磁気からの方位 00092 double Quaternion_from_acc[4];//加速度と地磁気からのQuaternion 00093 00094 /*----------------------------------*/ 00095 /*--------------------------行列、ベクトル-----------------------------*/ 00096 00097 Vector Quaternion_atitude_from_omega(4); 00098 Matrix Omega_matrix(4,4),Half_matrix(4,4); 00099 Matrix Complement_matrix_1(4,4),Complement_matrix_2(4,4); 00100 00101 /*-------------------------------------------------------------------*/ 00102 00103 /*----------PID コントロール-----------------*/ 00104 #define Gain_Kp 0.8377 00105 #define Gain_Ki 0.4450 00106 #define Gain_Kd 0.3890 00107 00108 double Prop_p,Int_p,Dif_p; 00109 double Pre_Prop_p; 00110 /*-----------------------------------------*/ 00111 00112 /*-----------サーボコントロール----------------*/ 00113 //define period of servo here 00114 #define SERVO_PERIOD 0.020 // servo requires a 20ms period 00115 #define NUTRAL 0.0015 00116 #define UPPER_DUTY 0.0020 00117 #define LOWER_DUTY 0.0010 00118 00119 double pitch_duty,roll_duty,yaw_duty; 00120 00121 /*------------------------------------------*/ 00122 00123 00124 /*プロトタイプ宣言*/ 00125 void toString_V(Vector& v); // ベクトルデバッグ用 00126 void gps_rx(); 00127 void gps_convertion(); 00128 /*--------------*/ 00129 00130 void toString_V(Vector& v) 00131 { 00132 00133 for(int i=0; i<v.GetDim(); i++) { 00134 pc.printf("%.6f\t", v.GetComp(i+1)); 00135 } 00136 pc.printf("\r\n"); 00137 00138 } 00139 00140 void scan_update(int box_sensor[6],int16_t m[3]){ 00141 00142 int a[3];//加速度の値 00143 int g[3];//角速度の値 00144 00145 int16_t raw_compass[3];//地磁気センサの値 00146 00147 mpu.setAcceleroRange(0);//acceleration range is +-4G 00148 mpu.setGyroRange(0);//gyro rate is +-degree per second(dps) 00149 00150 mpu.getAcceleroRaw(a);// 加速度を格納する配列[m/s2] a[0] is x axis,a[1] is y axis,a[2] is z axis; 00151 mpu.getGyroRaw(g); // 角速度を格納する配列[rad/s] 00152 compass.getXYZ(raw_compass);//地磁気の値を格納する配列 00153 00154 box_sensor[0]=-g[0]; 00155 box_sensor[1]=g[1]; 00156 box_sensor[2]=-g[2]; 00157 00158 box_sensor[3]=a[0]; 00159 box_sensor[4]=-a[1]; 00160 box_sensor[5]=a[2]; 00161 00162 m[0]=(raw_compass[0]);//x 00163 m[1]=(raw_compass[2]);//y 00164 m[2]=(raw_compass[1]);//z 00165 00166 } 00167 00168 void gps_rx(){ 00169 00170 if((gps.getc()=='$')&&(gps_flag==0)){ 00171 for(int i=0;i<9;i++){ 00172 gps_data[i]=gps.getc(); 00173 //pc.putc(gps_data[i]); 00174 } 00175 00176 gps_data[9]='\0'; 00177 00178 for(int i=0;i<9;i++){ 00179 gps_data_2[i]=gps.getc(); 00180 //pc.putc(gps_data[i]); 00181 } 00182 00183 gps_data_2[9]='\0'; 00184 00185 }//if(twe.getc()==':') 00186 00187 //pc.printf("%s,%s\r\n",gps_data,gps_data_2); 00188 00189 gps_flag=1; 00190 00191 }//gps_rx ends 00192 00193 void gps_convertion(){ 00194 while(1){ 00195 if(gps_flag==1){ 00196 tokei_int_receive=atoi(gps_data); 00197 hokui_int_receive=atoi(gps_data_2); 00198 00199 //pc.printf("%d,%d\r\n",tokei_int_receive,hokui_int_receive); 00200 00201 tokei_float_receive=float(tokei_int_receive)/pow(10.0,6.0)-100.0; 00202 hokui_float_receive=float(hokui_int_receive)/pow(10.0,6.0)-100.0; 00203 00204 gps_flag_conversion=1; 00205 00206 }else{gps_flag_conversion=0;} 00207 }//while ends 00208 00209 }//ends 00210 00211 void Acc_to_Quaternion(int x_acc,int y_acc,int z_acc,double Yaw,double qua[4]){ 00212 00213 double Roll_before = double(y_acc)/double(z_acc); 00214 double Roll = atan(Roll_before); 00215 00216 double Pitch_before = double(x_acc/sqrt(double(y_acc*y_acc+z_acc*z_acc)) ); 00217 double Pitch = -atan(Pitch_before); 00218 00219 //Yaw=0.0; 00220 00221 /*Quaternionの要素へオイラー角を変換する*/ 00222 qua[0]=cos(Roll/2.0)*cos(Pitch/2.0)*cos(Yaw/2.0)+sin(Roll/2.0)*sin(Pitch/2.0)*sin(Yaw/2.0); 00223 qua[1]=sin(Roll/2.0)*cos(Pitch/2.0)*cos(Yaw/2.0)-cos(Roll/2.0)*sin(Pitch/2.0)*sin(Yaw/2.0); 00224 qua[2]=cos(Roll/2.0)*sin(Pitch/2.0)*cos(Yaw/2.0)+sin(Roll/2.0)*cos(Pitch/2.0)*sin(Yaw/2.0); 00225 qua[3]=cos(Roll/2.0)*cos(Pitch/2.0)*sin(Yaw/2.0)-sin(Roll/2.0)*sin(Pitch/2.0)*cos(Yaw/2.0); 00226 00227 00228 } 00229 00230 void Acc_to_Pitch_Roll(int x_acc,int y_acc,int z_acc,double qua[2]){ 00231 00232 double Roll_before = double(y_acc)/double(z_acc); 00233 double Roll = atan(Roll_before); 00234 00235 double Pitch_before = double(x_acc/sqrt(double(y_acc*y_acc+z_acc*z_acc)) ); 00236 double Pitch = -atan(Pitch_before); 00237 00238 00239 qua[0]=Pitch; 00240 qua[1]=Roll; 00241 00242 } 00243 00244 void Correct_n(int x_acc,int y_acc,int z_acc,double n[4]){ 00245 00246 //y_acc*=-1.0; 00247 00248 n[0]=-double(y_acc)/sqrt(double(y_acc*y_acc+x_acc*x_acc)); 00249 n[1]=double(x_acc)/sqrt(double(y_acc*y_acc+x_acc*x_acc)); 00250 n[2]=0.0; 00251 00252 n[3]=sqrt(double(x_acc*x_acc+y_acc*y_acc))/double(z_acc); 00253 00254 } 00255 00256 double Geomagnetism_correction(int x_acc,int y_acc,int z_acc,int16_t magnet[3]){ 00257 00258 Vector Mag_from_sensor(3),Mag_fixed_sensor(3); //座標系 00259 Matrix Rotation(3, 3); //検算の行列 00260 00261 double Correct_vector_n[4];//回転行列の成分 00262 Correct_n(x_acc,y_acc,z_acc, Correct_vector_n); 00263 00264 double n_x=Correct_vector_n[0]; 00265 double n_y=Correct_vector_n[1]; 00266 double n_z=Correct_vector_n[2]; 00267 double theta=-Correct_vector_n[3]; 00268 00269 float Rotate_element[9] = { 00270 n_x*n_x*(1-cos(theta))+cos(theta) , n_x*n_y*(1-cos(theta)) , n_y*sin(theta), 00271 n_x*n_y*(1-cos(theta)) ,n_y*n_y*(1-cos(theta))+cos(theta) , -n_x*sin(theta), 00272 n_y*sin(theta) , n_x*sin(theta) , cos(theta) 00273 }; 00274 00275 Rotation.SetComps(Rotate_element); 00276 //x y z 00277 float Geomagnetism[3]={ float(magnet[1]+MAG_FIX_X) 00278 , float(magnet[0]+MAG_FIX_Y) 00279 , float( (-1.0)*(magnet[2]+MAG_FIX_Z ))}; 00280 00281 Mag_from_sensor.SetComps(Geomagnetism); 00282 00283 Mag_fixed_sensor=Rotation*Mag_from_sensor;//地磁気センサの値を加速度センサによる傾きで補正。 00284 00285 float Mag_fixed_x= Mag_fixed_sensor.GetComp(1); 00286 float Mag_fixed_y= Mag_fixed_sensor.GetComp(2); 00287 00288 double Azi=atan2(double(Mag_fixed_y),double(Mag_fixed_x)); 00289 00290 /* 00291 if(Azi < 0.0) // fix sign 00292 Azi += PI2; 00293 00294 if(Azi > PI2) // fix overflow 00295 Azi -= PI2; 00296 */ 00297 00298 return Azi; 00299 00300 } 00301 00302 //void led2_thread(void const *argument) { 00303 void imu_thread() { 00304 while (true) { 00305 }//while ends 00306 } 00307 00308 void Quaternion_to_Euler(float q0,float q1,float q2,float q3,double Euler[3]){ 00309 00310 //float pitch,roll,yaw; 00311 00312 Euler[0]=RAD_TO_DEG*atan2(double( 2*(q2*q3+q0*q1)), double (q0*q0-q1*q1-q2*q2+q3*q3) ); 00313 Euler[1]=-RAD_TO_DEG*asin(double(2*(q1*q3-q0*q2))); 00314 Euler[2]=RAD_TO_DEG*atan2(double(2*(q1*q2+q0*q3)), double(q0*q0+q1*q1-q2*q2-q3*q3)); 00315 00316 } 00317 00318 00319 double Degree_to_PWM_duty(double degree){ 00320 00321 double duty=0.0; 00322 00323 duty=(0.2/1000)/20.0*degree; 00324 00325 if((duty+NUTRAL)>=UPPER_DUTY){ 00326 duty=UPPER_DUTY-NUTRAL; 00327 }else if((duty+NUTRAL)<=LOWER_DUTY){ 00328 duty=NUTRAL-LOWER_DUTY; 00329 }else{} 00330 00331 return duty; 00332 00333 } 00334 00335 double PID_duty(double target,double vol,float dt){ 00336 //ここで角度の単位で偏差は入力される 00337 //出力はPWMで 00338 double duty=0.0; 00339 double add_duty=0.0; 00340 00341 Prop_p=target-vol; 00342 00343 pc.printf("%f/r/n",Prop_p); 00344 00345 Int_p+=Prop_p*double(dt); 00346 Dif_p=(Prop_p - Pre_Prop_p) / double(dt); 00347 00348 Pre_Prop_p=Prop_p; 00349 00350 //add_duty=Gain_Kp*Prop_p+Gain_Ki*Int_p+Gain_Kd*Dif_p; 00351 00352 add_duty=1.0*Prop_p; 00353 00354 duty+=add_duty; 00355 duty=Degree_to_PWM_duty(duty); 00356 00357 return duty; 00358 00359 } 00360 00361 void Initialize_ESC(){ 00362 00363 ESC.pulsewidth(0.002); 00364 wait(3); 00365 00366 ESC.pulsewidth(0.001); 00367 wait(5); 00368 00369 } 00370 00371 00372 void Activate_ESC(){ 00373 00374 ESC.pulsewidth(0.001); 00375 wait(5); 00376 00377 } 00378 00379 // Entry point for the example 00380 int main() 00381 { 00382 gps.baud(115200);//GT-720Fボーレート 00383 pc.baud(115200); 00384 //imu_thread 00385 00386 compass.init();//hmc5883の起動 00387 00388 Thread thread1 (gps_convertion); 00389 gps.attach(gps_rx, Serial::RxIrq); 00390 00391 /*define period of serve and ESC*/ 00392 ESC.period(SERVO_PERIOD); 00393 Elevator_servo.period(SERVO_PERIOD); 00394 Rudder_servo.period(SERVO_PERIOD); 00395 /*------------------------------*/ 00396 Elevator_servo.pulsewidth(NUTRAL); // servo position determined by a pulsewidth between 1-2ms 00397 Rudder_servo.pulsewidth(NUTRAL); 00398 00399 Activate_ESC(); 00400 00401 float Time_old=0.0;//時間初期化 00402 passed_time.start();//タイマー開始 00403 00404 pc.printf("--- Mbed OS filesystem example ---\n"); 00405 00406 // Try to mount the filesystem 00407 pc.printf("Mounting the filesystem... "); 00408 fflush(stdout); 00409 00410 int err = fileSystem.mount(&blockDevice); 00411 pc.printf("%s\n", (err ? "Fail :(" : "OK")); 00412 if (err) { 00413 // Reformat if we can't mount the filesystem 00414 // this should only happen on the first boot 00415 pc.printf("No filesystem found, formatting... "); 00416 fflush(stdout); 00417 err = fileSystem.reformat(&blockDevice); 00418 pc.printf("%s\n", (err ? "Fail :(" : "OK")); 00419 if (err) { 00420 error("error: %s (%d)\n", strerror(-err), err); 00421 } 00422 } 00423 00424 // Open the numbers file 00425 pc.printf("Opening \"/fs/numbers.txt\"... "); 00426 fflush(stdout); 00427 00428 FILE* f = fopen("/fs/numbers.txt", "r+"); 00429 pc.printf("%s\n", (!f ? "Fail :(" : "OK")); 00430 00431 if (!f) { 00432 // Create the numbers file if it doesn't exist 00433 pc.printf("No file found, creating a new file... "); 00434 fflush(stdout); 00435 f = fopen("/fs/numbers.txt", "w+"); 00436 pc.printf("%s\n", (!f ? "Fail :(" : "OK")); 00437 } 00438 00439 /*初期姿勢のQuaternionの設定*/ 00440 scan_update(sensor_array,mag);//９軸の値の取得 mag[0]=y mag[1]=x mag[2]=-Z 00441 Azi_mag=Geomagnetism_correction(sensor_array[3],sensor_array[4],sensor_array[5],mag); 00442 00443 Acc_to_Quaternion(sensor_array[3],sensor_array[4],sensor_array[5],Azi_mag,Quaternion_from_acc); 00444 00445 float Initialize_atitude[4]; 00446 Initialize_atitude[0]=Quaternion_from_acc[0]; 00447 Initialize_atitude[1]=Quaternion_from_acc[1]; 00448 Initialize_atitude[2]=Quaternion_from_acc[2]; 00449 Initialize_atitude[3]=Quaternion_from_acc[3]; 00450 00451 Quaternion_atitude_from_omega.SetComps(Initialize_atitude);//ジャイロで計算するQuaternionの初期化 00452 00453 00454 while(1){ 00455 00456 /*gpsから来た文字列の処理 00457 if((gps_flag==1)&&(gps_flag_conversion==1)){ 00458 00459 err = fprintf(f,"%f,%f\r\n",tokei_float_receive,hokui_float_receive); 00460 00461 gps_flag=0; 00462 gps_flag_conversion=0; 00463 00464 }else{} 00465 */ 00466 00467 scan_update(sensor_array,mag);//９軸の値の取得 mag[0]=y mag[1]=x mag[2]=-Z 00468 float Time_new=float(passed_time.read());//時間の取得 00469 00470 //地磁気の補正用に使う。 00471 //pc.printf("%d,%d,%d\r\n",mag[1],mag[0],mag[2]); 00472 00473 //ジャイロの補正用に使う 00474 //pc.printf("%d,%d,%d\r\n",sensor_array[0],sensor_array[1],sensor_array[2]); 00475 00476 /*----------------ジャイロセンサからQuaternionを求める----------------*/ 00477 float omega_x=float(( float(sensor_array[0])-GYRO_FIX_X )/ 7505.7); 00478 float omega_y=float(( float(sensor_array[1])-GYRO_FIX_Y )/ 7505.7); 00479 float omega_z=float(( float(sensor_array[2])-GYRO_FIX_Z )/ 7505.7); 00480 00481 float omega[16] = { 00482 00483 0.0 , -omega_x , -omega_y, -omega_z, 00484 omega_x ,0.0 ,omega_z ,-omega_y , 00485 omega_y , -omega_z , 0.0 , omega_x , 00486 omega_z , omega_y ,-omega_x , 0.0 00487 00488 }; 00489 00490 Omega_matrix.SetComps(omega); 00491 00492 float half[16] ={ 00493 (Time_new-Time_old)*0.5,0.0,0.0,0.0, 00494 0.0,(Time_new-Time_old)*0.5,0.0,0.0, 00495 0.0,0.0,(Time_new-Time_old)*0.5,0.0, 00496 0.0,0.0,0.0,(Time_new-Time_old)*0.5 00497 }; 00498 00499 00500 Half_matrix.SetComps(half); 00501 //Quaternion_atitude_from_omega+=(Time_new-Time_old)* 00502 Quaternion_atitude_from_omega +=Half_matrix*Omega_matrix*Quaternion_atitude_from_omega; 00503 00504 Quaternion_atitude_from_omega=Quaternion_atitude_from_omega.Normalize(); 00505 00506 00507 //Time_old=Time_new;//時間の更新 00508 00509 /* 00510 pc.printf("%f,%f,%f,%f\r\n" 00511 ,Quaternion_atitude_from_omega.GetComp(1),Quaternion_atitude_from_omega.GetComp(2) 00512 ,Quaternion_atitude_from_omega.GetComp(3),Quaternion_atitude_from_omega.GetComp(4)); 00513 */ 00514 00515 /*----------------------------------------------------------------*/ 00516 00517 /* 00518 err = fprintf(f,"A,%f,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n" 00519 ,val,sensor_array[0],sensor_array[1],sensor_array[2],sensor_array[3],sensor_array[4],sensor_array[5] 00520 ,sensor_array[6],sensor_array[7],sensor_array[8]); 00521 */ 00522 00523 /*--------------------加速センサで地磁気の値を補正する --------------------*/ 00524 00525 if(9.8065*1.1 > 00526 sqrt(double(sensor_array[3]*sensor_array[3]+sensor_array[4]*sensor_array[4]+sensor_array[5]*sensor_array[5])) 00527 / 16384.0 * 9.81) 00528 { 00529 00530 Azi_mag=Geomagnetism_correction(sensor_array[3],sensor_array[4],sensor_array[5],mag); 00531 00532 //pc.printf("Azimath=%f\r\n",Azi_mag*RAD_TO_DEG); 00533 00534 double pitch_and_roll[2]; 00535 Acc_to_Pitch_Roll(sensor_array[3],sensor_array[4],sensor_array[5],pitch_and_roll); 00536 00537 //pc.printf("Pitch %f,Roll %f\r\n",pitch_and_roll[0]*RAD_TO_DEG,pitch_and_roll[1]*RAD_TO_DEG); 00538 00539 Acc_to_Quaternion(sensor_array[3],sensor_array[4],sensor_array[5],Azi_mag,Quaternion_from_acc); 00540 00541 /*Quaternion_from_acc_fixedの要素をfloatへ変換する*/ 00542 float Quaternion_from_acc_fixed[4]; 00543 Quaternion_from_acc_fixed[0]=float(Quaternion_from_acc[0]); 00544 Quaternion_from_acc_fixed[1]=float(Quaternion_from_acc[1]); 00545 Quaternion_from_acc_fixed[2]=float(Quaternion_from_acc[2]); 00546 Quaternion_from_acc_fixed[3]=float(Quaternion_from_acc[3]); 00547 00548 Vector Qua_acc(4); 00549 00550 Qua_acc.SetComps(Quaternion_from_acc_fixed); 00551 /*---------------------------------------------*/ 00552 00553 /*---------------------相補フィルタのゲインに用いる---------------------*/ 00554 float comp_1[16]={0.95,0.0,0.0,0.0, 00555 0.0,0.95,0.0,0.0, 00556 0.0,0.0,0.95,0.0, 00557 0.0,0.0,0.0,0.95 00558 }; 00559 /*--------------------------------------------------------------------*/ 00560 /*---------------------quaternionの時間微分に用いる---------------------*/ 00561 float comp_2[16]={0.05,0.0,0.0,0.0, 00562 0.0,0.05,0.0,0.0, 00563 0.0,0.0,0.05,0.0, 00564 0.0,0.0,0.0,0.05 00565 }; 00566 00567 Complement_matrix_1.SetComps(comp_1); 00568 Complement_matrix_2.SetComps(comp_2); 00569 00570 /*--------------------------------------------------------------------*/ 00571 00572 00573 /*-------------------加速度と地磁気でジャイロデータを補正する------------------------*/ 00574 00575 Quaternion_atitude_from_omega=Complement_matrix_1*Quaternion_atitude_from_omega 00576 +Complement_matrix_2*Qua_acc; 00577 00578 /*----------------------------------------------------------------------------*/ 00579 00580 00581 /* 00582 pc.printf("%f,%f,%f,%f,%f,%f,%f,%f\r\n" 00583 ,Quaternion_from_acc[0],Quaternion_atitude_from_omega.GetComp(1) 00584 ,Quaternion_from_acc[1],Quaternion_atitude_from_omega.GetComp(2) 00585 ,Quaternion_from_acc[2],Quaternion_atitude_from_omega.GetComp(3) 00586 ,Quaternion_from_acc[3],Quaternion_atitude_from_omega.GetComp(4)); 00587 */ 00588 00589 }else{} 00590 00591 00592 /* 00593 pc.printf("%f,%f,%f,%f,%f\r\n" 00594 ,Time_new 00595 ,Quaternion_atitude_from_omega.GetComp(1) 00596 ,Quaternion_atitude_from_omega.GetComp(2) 00597 ,Quaternion_atitude_from_omega.GetComp(3) 00598 ,Quaternion_atitude_from_omega.GetComp(4)); 00599 */ 00600 /*-----------Quaternionからオイラー角への変換-----------*/ 00601 Quaternion_to_Euler(Quaternion_atitude_from_omega.GetComp(1) 00602 ,Quaternion_atitude_from_omega.GetComp(2) 00603 ,Quaternion_atitude_from_omega.GetComp(3) 00604 ,Quaternion_atitude_from_omega.GetComp(4) 00605 ,Euler_angle); 00606 00607 /*-----------オイラー角の表示----------------------*/ 00608 00609 pc.printf("%f,%f,%f,%f,%f\r\n" 00610 ,Time_new 00611 ,Euler_angle[0] 00612 ,Euler_angle[1] 00613 ,Euler_angle[2] 00614 ); 00615 00616 /*--------------------------------------------------*/ 00617 //Pitch control 00618 pitch_duty=NUTRAL+PID_duty(0.0,Euler_angle[1],(Time_new-Time_old)); 00619 Elevator_servo.pulsewidth(pitch_duty); 00620 00621 00622 Time_old=Time_new;//時間の更新 00623 00624 wait(0.001); 00625 00626 /*----------------------------------------------------------------*/ 00627 00628 if(switch_off==1){ 00629 // Close the file which also flushes any cached writes 00630 pc.printf("Closing \"/fs/numbers.txt\"... "); 00631 err = fclose(f); 00632 break; 00633 } 00634 00635 }//while(1) ends 00636 }//main ends
Generated on Tue Jul 12 2022 19:08:26 by
1.7.2