Atsumi Toda
UAVの姿勢推定に使用するプログラム。
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /* 00002 This system is vertion 2.0. Thsi program enable us to read signals from receiver ,measure balance of circit, and actuate servos. 00003 */ 00004 00005 #include "mbed.h" 00006 #include "string.h" 00007 #include "MPU6050.h" 00008 00009 #include <stdio.h> 00010 #include <errno.h> 00011 00012 #include "SDBlockDevice.h" 00013 #include "FATFileSystem.h" 00014 00015 #include "myConstants.h" 00016 00017 #define READBUFFERSIZE 70 00018 #define DELIMITER "," 00019 #define M_PI 3.141592 00020 #define PI2 (2*M_PI) 00021 00022 /*ジャイロセンサの補正値（引く）*/ 00023 #define GYRO_FIX_X 290.5498 00024 #define GYRO_FIX_Y 99.29363 00025 #define GYRO_FIX_Z 61.41011 00026 00027 /*---------ピン指定-------------------*/ 00028 DigitalOut myled(LED1); 00029 00030 DigitalIn switch_off(p30);//sdカードへの書き込みを終了させる 00031 00032 InterruptIn button(p5);//GPSモジュールからの1pps信号を読み取る（立ち下がり割り込み） 00033 00034 InterruptIn input_from_Aileron_1(p11); 00035 InterruptIn input_from_elevator(p12);// 00036 InterruptIn input_from_throttle(p13);// 00037 InterruptIn input_from_rudder(p14);// 00038 InterruptIn input_from_gear(p15);// 00039 InterruptIn input_from_Aileron_2(p16);// 00040 InterruptIn input_from_pitch(p17);// 00041 InterruptIn input_from_AUX5(p18);// 00042 00043 00044 MPU6050 mpu(p28, p27);//sda scl 00045 00046 Serial pc(USBTX, USBRX); // tx, rx 00047 Serial gps(p9,p10);// gpsとの接続 tx, rx 00048 SDBlockDevice blockDevice(p5, p6, p7, p8); // mosi, miso, sck, cs 00049 00050 //declare PWM pins here 00051 PwmOut ESC (p21); 00052 PwmOut Elevator_servo(p22); 00053 PwmOut Rudder_servo(p23); 00054 PwmOut Aileron_R_servo(p25); 00055 PwmOut Aileron_L_servo(p26); 00056 00057 /*-----------------------------------*/ 00058 00059 /*-----ファイルポインタ-----*/ 00060 // File system declaration 00061 FATFileSystem fileSystem("fs"); 00062 /*-----------------------*/ 00063 00064 /*------------------------GPSの変数------------------------*/ 00065 int GPS_flag_update=0; 00066 int GPS_interruptIn=0; 00067 00068 float longtude_gps,latitude_gps,azimuth_gps,speed_gps; 00069 00070 //char NMEA_sentense[READBUFFERSIZE]="$GPRMC,222219.000,A,3605.4010,N,14006.8240,E,0.11,109.92,191016,,,A"; 00071 char NMEA_sentense[READBUFFERSIZE]; 00072 00073 /*--------------------------------------------------------*/ 00074 00075 00076 /*--------------------------角度とセンサ--------------------------*/ 00077 float sensor_array[6]; 00078 float Acc_Pitch_Roll[2]; 00079 float Gyro_Pitch_Roll[2];//pitch and roll 00080 float omega_z; 00081 /*--------------------------------------------------------------*/ 00082 00083 /*----------PID コントロール-----------------*/ 00084 #define Gain_Kp 0.8377 00085 #define Gain_Ki 0.4450 00086 #define Gain_Kd 0.3890 00087 00088 double Prop_p,Int_p,Dif_p; 00089 double Pre_Prop_p; 00090 /*-----------------------------------------*/ 00091 00092 /*-----------サーボコントロール----------------*/ 00093 //define period of servo here 00094 #define SERVO_PERIOD 0.020 // servo requires a 20ms period 00095 #define NUTRAL 0.0015 00096 #define UPPER_DUTY 0.0020 00097 #define LOWER_DUTY 0.0010 00098 00099 #define THRESHOLD 0.0018 00100 00101 double pitch_duty,roll_duty,yaw_duty; 00102 00103 double first_period,second_period,third_period,fourth_period; 00104 double fifth_period,sixth_period,seventh_period,eighth_period; 00105 00106 double first_period_old=0.0,second_period_old=0.0,third_period_old=0.0,fourth_period_old=0.0; 00107 double fifth_period_old=0.0,sixth_period_old=0.0,seventh_period_old=0.0,eighth_period_old=0.0; 00108 00109 double throttle_duty,elevator_duty,rudder_duty,switch_duty,aileron_duty; 00110 00111 /*------------------------------------------*/ 00112 00113 /*-----タイマー---------*/ 00114 Timer passed_time;//経過時間の計算 00115 Timer ch_time;// 00116 00117 float Time_new,Time_old=0.0; 00118 float Servo_command=0.002; 00119 /*---------------------*/ 00120 /*---------PWM割り込みフラグ-------*/ 00121 int PWM_flag_update=0; 00122 /*-------------------------------*/ 00123 00124 /*----------プロトタイプ宣言----------*/ 00125 void gps_rx(); 00126 void gps_read(); 00127 00128 double Degree_to_PWM_duty(double degree); 00129 double PID_duty_pitch(double target,double vol,float dt); 00130 double PID_duty_rudder(double target,double vol,float dt,float yaw_rate); 00131 double PID_duty_roll(double target,double vol,float dt); 00132 00133 void Acc_to_Pitch_Roll(float x_acc,float y_acc,float z_acc,float qua[2]); 00134 void scan_update(float box_sensor[6]); 00135 void sensor_update(); 00136 00137 //gpsの1pps信号の割り込み 00138 void flip(); 00139 00140 00141 void Initialize_ESC(); 00142 void Activate_ESC(); 00143 00144 void Input_Aileron_1(); 00145 void Input_elevator(); 00146 void Input_throttle(); 00147 void Input_rudder(); 00148 void Input_gear(); 00149 void Input_Aileron_2(); 00150 void Input_pitch(); 00151 void Input_AUX5(); 00152 00153 void Input_Aileron_1_fall(); 00154 void Input_elevator_fall(); 00155 void Input_throttle_fall(); 00156 void Input_rudder_fall(); 00157 void Input_gear_fall(); 00158 void Input_Aileron_2_fall(); 00159 void Input_pitch_fall(); 00160 void Input_AUX5_fall(); 00161 /*---------------------------------*/ 00162 00163 00164 int main() { 00165 00166 wait(3); 00167 00168 00169 /*sdカードにファイルを構成する そのファイルを書き込み状態にする*/ 00170 00171 00172 00173 pc.printf("--- Mbed OS filesystem example ---\n"); 00174 00175 // Try to mount the filesystem 00176 pc.printf("Mounting the filesystem... "); 00177 fflush(stdout); 00178 00179 int err = fileSystem.mount(&blockDevice); 00180 pc.printf("%s\n", (err ? "Fail :(" : "OK")); 00181 if (err) { 00182 // Reformat if we can't mount the filesystem 00183 // this should only happen on the first boot 00184 pc.printf("No filesystem found, formatting... "); 00185 fflush(stdout); 00186 err = fileSystem.reformat(&blockDevice); 00187 pc.printf("%s\n", (err ? "Fail :(" : "OK")); 00188 if (err) { 00189 error("error: %s (%d)\n", strerror(-err), err); 00190 } 00191 } 00192 00193 // Open the numbers file 00194 pc.printf("Opening \"/fs/log.csv\"... "); 00195 fflush(stdout); 00196 00197 FILE* f = fopen("/fs/log.csv", "r+"); 00198 FILE* fp = fopen("/fs/log.txt", "r+"); 00199 00200 pc.printf("%s\n", (!f ? "Fail :(" : "OK")); 00201 00202 if (!f) { 00203 // Create the numbers file if it doesn't exist 00204 pc.printf("No file found, creating a new file... "); 00205 fflush(stdout); 00206 f = fopen("/fs/log.csv", "w+"); 00207 fp = fopen("/fs/log.txt", "w+"); 00208 00209 pc.printf("%s\n", (!f ? "Fail :(" : "OK")); 00210 } 00211 00212 00213 00214 00215 /*define period of serve and ESC*/ 00216 ESC.period(SERVO_PERIOD); 00217 Elevator_servo.period(SERVO_PERIOD); 00218 Rudder_servo.period(SERVO_PERIOD); 00219 Aileron_R_servo.period(SERVO_PERIOD); 00220 Aileron_L_servo.period(SERVO_PERIOD); 00221 /*------------------------------*/ 00222 Elevator_servo.pulsewidth(NUTRAL); // servo position determined by a pulsewidth between 1-2ms 00223 Rudder_servo.pulsewidth(NUTRAL); 00224 Aileron_R_servo.pulsewidth(NUTRAL); 00225 Aileron_L_servo.pulsewidth(NUTRAL); 00226 00227 Activate_ESC();//Activate ESC 00228 00229 /*--------------ピン変化割り込みに対応した関数の宣言--------------*/ 00230 button.fall(&flip);//GPSモジュールからの1pps信号を立ち下がり割り込みで読み取る。 00231 00232 input_from_throttle.rise(&Input_throttle);// 00233 00234 input_from_Aileron_1.fall(&Input_Aileron_1_fall); 00235 input_from_elevator.fall(&Input_elevator_fall);// 00236 input_from_throttle.fall(&Input_throttle_fall);// 00237 input_from_rudder.fall(&Input_rudder_fall);// 00238 input_from_gear.fall(&Input_gear_fall); 00239 input_from_Aileron_2.fall(&Input_Aileron_2_fall);// 00240 input_from_pitch.fall(&Input_pitch_fall);// 00241 input_from_AUX5.fall(&Input_AUX5_fall);// 00242 00243 ch_time.start();//時間計測スタート 00244 /*----------------------------------------------------------*/ 00245 00246 pc.baud(115200); 00247 gps.baud(115200); 00248 00249 //gps.attach(gps_rx, Serial::RxIrq);//シリアル割り込み 00250 //pc.attach(gps_rx, Serial::RxIrq);//シリアル割り込み 00251 00252 passed_time.start();//タイマー開始 00253 Time_old=float(passed_time.read()); 00254 00255 /*初期姿勢角の算出*/ 00256 scan_update(sensor_array);//角速度、加速度を求める 00257 Acc_to_Pitch_Roll(sensor_array[3],sensor_array[4],sensor_array[5],Acc_Pitch_Roll); 00258 Gyro_Pitch_Roll[0]=Acc_Pitch_Roll[0];//初期姿勢角の算出 Pitch 00259 Gyro_Pitch_Roll[1]=Acc_Pitch_Roll[1];//初期姿勢角の算出 Roll 00260 00261 pc.printf("Start!!\r\n"); 00262 00263 while(1) { 00264 00265 switch (GPS_interruptIn) { 00266 case 1: 00267 00268 gps_rx(); 00269 gps_read(); 00270 GPS_interruptIn--; 00271 break; 00272 00273 default: 00274 00275 sensor_update(); 00276 break; 00277 }//switch end 00278 00279 /* 00280 ここでプロポの信号のパルス幅を読み取って、手動or 自動の切り替えを行う必要がある。 00281 以下のpitch control等は自動操縦とみなしてよい。 00282 */ 00283 00284 00285 if(switch_duty>THRESHOLD){//オートパイロット 00286 00287 //Pitch control 00288 //pitch_duty=NUTRAL+PID_duty_pitch(0.0,Gyro_Pitch_Roll[0],(Time_new-Time_old)); 00289 elevator_duty=NUTRAL+PID_duty_pitch(0.0,Gyro_Pitch_Roll[0],(Time_new-Time_old)); 00290 Elevator_servo.pulsewidth(elevator_duty); 00291 00292 //roll control 00293 //roll_duty=NUTRAL+PID_duty_roll(0.0,Gyro_Pitch_Roll[1],(Time_new-Time_old)) 00294 aileron_duty=NUTRAL+PID_duty_roll(0.0,Gyro_Pitch_Roll[1],(Time_new-Time_old)); 00295 00296 //角速度だけフィードバック 00297 rudder_duty=NUTRAL+PID_duty_rudder(0.0,0.0,(Time_new-Time_old),omega_z); 00298 ///pc.printf("%f\r\n",rudder_duty); 00299 00300 00301 //スロットルはマニュアル 00302 //Servo_command 00303 if( (float(passed_time.read())-Servo_command)>0.003 ){ 00304 00305 ESC.pulsewidth(throttle_duty); 00306 Elevator_servo.pulsewidth(elevator_duty); 00307 Rudder_servo.pulsewidth(rudder_duty); 00308 Aileron_R_servo.pulsewidth(aileron_duty); 00309 Aileron_L_servo.pulsewidth(aileron_duty); 00310 00311 Servo_command=float(passed_time.read());//サーボを動かしたタイミングの更新 00312 }else{} 00313 00314 }else{//マニュアルモード 00315 00316 if( (float(passed_time.read())-Servo_command)>0.003 ){ 00317 00318 ESC.pulsewidth(throttle_duty); 00319 Elevator_servo.pulsewidth(elevator_duty); 00320 Rudder_servo.pulsewidth(rudder_duty); 00321 Aileron_R_servo.pulsewidth(aileron_duty); 00322 Aileron_L_servo.pulsewidth(aileron_duty); 00323 00324 Servo_command=float(passed_time.read());//サーボを動かしたタイミングの更新 00325 00326 }else{} 00327 00328 } 00329 00330 pc.printf("%f,%f,%f,%f,%f,%f\r\n",Time_new,Gyro_Pitch_Roll[0],Gyro_Pitch_Roll[1],azimuth_gps,latitude_gps,longtude_gps); 00331 err = fprintf(f,"%f,%f,%f,%f\r\n",Time_new,Gyro_Pitch_Roll[0],Gyro_Pitch_Roll[1],azimuth_gps); 00332 err = fprintf(fp,"%f,%f\r\n",latitude_gps,longtude_gps); 00333 00334 Time_old=Time_new;//時間更新 00335 00336 if(switch_off==1){ 00337 //fclose(fp); 00338 //flipper.detach(); 00339 00340 err = fclose(f); 00341 err = fclose(fp); 00342 00343 pc.printf("Writing finish!"); 00344 myled=0; 00345 break; 00346 00347 } 00348 00349 //wait(0.002);//この値は10倍されると考える 00350 }//while ends 00351 00352 }//main ends 00353 00354 void gps_rx(){ 00355 00356 // __disable_irq(); // 割り込み禁止 00357 00358 int i=0; 00359 00360 while(gps.getc()!='$'){ 00361 } 00362 while( (NMEA_sentense[i]=gps.getc()) != '\r'){ 00363 //while( (NMEA_sentense[i]=pc.getc()) != '\r'){ 00364 //pc.putc(NMEA_sentense[i]); 00365 i++; 00366 if(i==READBUFFERSIZE){ 00367 i=(READBUFFERSIZE-1); 00368 break; 00369 } 00370 } 00371 NMEA_sentense[i]='\0'; 00372 //pc.printf("\r\n"); 00373 00374 if(GPS_flag_update==0){ 00375 GPS_flag_update++; 00376 } 00377 00378 //__enable_irq(); // 割り込み許可 00379 00380 }//void gps_rx 00381 00382 void gps_read(){ 00383 /*------gps のNMEAフォーマットの処理プログラム----------------*/ 00384 float temp=0.0, deg=0.0, min=0.0; 00385 00386 char *pszTime;//UTC時刻 00387 char* pszLat;////緯度 00388 char* pszLong;//経度 00389 char* pszSp;//速度 00390 char* pszSpd;//速度方向 00391 char* pszDate;//日付 00392 00393 //strtokで処理した文字列は内容がNullになるので、テストでは、毎回NMEA_sentense[128]を初期化しないといけない 00394 //char NMEA_sentense[128]="$GPRMC,222219.000,A,3605.4010,N,14006.8240,E,0.11,109.92,191016,,,A"; 00395 00396 //Time_old=float(passed_time.read()); 00397 00398 00399 00400 if((GPS_flag_update==1)&&(0==strncmp( "GPRMC", NMEA_sentense, 5 ))){//GPS_flag_updateも判定に加えた方がいい 00401 //if(0==strncmp( "$GPRMC", NMEA_sentense, 6 )){//GPS_flag_updateも判定に加えた方がいい 00402 00403 strtok( NMEA_sentense, DELIMITER ); // $GPRMC 00404 pszTime = strtok( NULL, DELIMITER ); // UTC時刻 00405 strtok( NULL, DELIMITER ); // ステータス 00406 pszLat = strtok( NULL, DELIMITER ); // 緯度(dddmm.mmmm) 00407 strtok( NULL, DELIMITER ); // 北緯か南緯か 00408 pszLong = strtok( NULL, DELIMITER ); // 経度(dddmm.mmmm) 00409 strtok( NULL, DELIMITER ); // 東経か西経か 00410 00411 pszSp = strtok( NULL, DELIMITER ); // 速度（vvv.v） 00412 pszSpd = strtok( NULL, DELIMITER ); // 速度方向（ddd.d） 00413 pszDate = strtok( NULL, DELIMITER ); // 日付 00414 00415 if( NULL != pszLong ){//pszLongがnullでないのならば 00416 temp = atof(pszLat); 00417 deg = (int)(temp/100); 00418 min = temp - deg * 100; 00419 latitude_gps = deg + min / 60;//latitudeの算出 00420 00421 temp = atof(pszLong); 00422 deg = (int)(temp/100); 00423 min = temp - deg * 100; 00424 longtude_gps = deg + min / 60;//longtudeの算出 00425 00426 speed_gps=atof(pszSp)*KNOT_TO_METER_PER_SEC; 00427 00428 azimuth_gps=atof(pszSpd); 00429 00430 GPS_flag_update--; 00431 00432 //pc.printf("%f,%f\r\n",longtude_gps,latitude_gps); 00433 00434 }else{}//end of ( NULL != pszLong ) 00435 00436 }else{}//end of if(0==strncmp( "$GPRMC", NMEA_sentense, 6 )){ 00437 } 00438 00439 00440 void sensor_update(){ 00441 scan_update(sensor_array);//角速度、加速度を求める 00442 00443 Time_new=float(passed_time.read());//時間の更新 00444 00445 Gyro_Pitch_Roll[0]+=sensor_array[1]*(Time_new-Time_old);//初期姿勢角の算出 Pitch 00446 Gyro_Pitch_Roll[1]+=sensor_array[0]*(Time_new-Time_old);//初期姿勢角の算出 Roll 00447 00448 00449 /*--------------------加速センサでジャイロの値を補正する --------------------*/ 00450 00451 00452 00453 if(9.8065*1.05 > sqrt((sensor_array[3]*sensor_array[3]+sensor_array[4]*sensor_array[4]+sensor_array[5]*sensor_array[5]))) 00454 { 00455 Acc_to_Pitch_Roll(sensor_array[3],sensor_array[4],sensor_array[5],Acc_Pitch_Roll); 00456 Gyro_Pitch_Roll[0]*=0.95;// 00457 Gyro_Pitch_Roll[1]*=0.95;// 00458 Gyro_Pitch_Roll[0]+=(0.05*Acc_Pitch_Roll[0]);// 00459 Gyro_Pitch_Roll[1]+=(0.05*Acc_Pitch_Roll[1]);// 00460 00461 00462 }else{} 00463 00464 //pc.printf("%f,%f,%f\r\n",Time_new,Acc_Pitch_Roll[0]*RAD_TO_DEG,Acc_Pitch_Roll[1]*RAD_TO_DEG); 00465 //pc.printf("%f,%f,%f\r\n",Time_new,Gyro_Pitch_Roll[0],Gyro_Pitch_Roll[1]); 00466 00467 //Time_old=Time_new; 00468 00469 } 00470 double Degree_to_PWM_duty(double degree){ 00471 00472 double duty=0.0; 00473 00474 duty=(0.2/1000)/20.0*degree; 00475 00476 if((duty+NUTRAL)>=UPPER_DUTY){ 00477 duty=UPPER_DUTY-NUTRAL; 00478 }else if((duty+NUTRAL)<=LOWER_DUTY){ 00479 duty=NUTRAL-LOWER_DUTY; 00480 }else{} 00481 00482 return duty; 00483 00484 } 00485 00486 double PID_duty_pitch(double target,double vol,float dt){ 00487 //ここで角度の単位で偏差は入力される 00488 //出力はPWMで 00489 double duty=0.0; 00490 double add_duty=0.0; 00491 00492 Prop_p=target-vol; 00493 00494 //pc.printf("%f/r/n",Prop_p); 00495 00496 Int_p+=Prop_p*double(dt); 00497 Dif_p=(Prop_p - Pre_Prop_p) / double(dt); 00498 00499 Pre_Prop_p=Prop_p; 00500 00501 //add_duty=Gain_Kp*Prop_p+Gain_Ki*Int_p+Gain_Kd*Dif_p; 00502 00503 add_duty=1.0*Prop_p; 00504 00505 duty+=add_duty; 00506 duty=Degree_to_PWM_duty(duty); 00507 00508 return duty; 00509 00510 } 00511 00512 double PID_duty_rudder(double target,double vol,float dt,float yaw_rate){ 00513 //ここで角度の単位で偏差は入力される 00514 //出力はPWMで 00515 double duty=0.0; 00516 double add_duty=0.0; 00517 00518 Prop_p=target-vol; 00519 00520 //pc.printf("%f/r/n",Prop_p); 00521 00522 Int_p+=Prop_p*double(dt); 00523 Dif_p=(Prop_p - Pre_Prop_p) / double(dt); 00524 00525 Pre_Prop_p=Prop_p; 00526 00527 //add_duty=Gain_Kp*Prop_p+Gain_Ki*Int_p+Gain_Kd*Dif_p; 00528 00529 //add_duty=1.0*Prop_p; 00530 //今回は角速度に反応させてラダーを動かす 00531 add_duty=double(yaw_rate)*0.1; 00532 00533 duty+=add_duty; 00534 duty=Degree_to_PWM_duty(duty); 00535 00536 return duty; 00537 00538 } 00539 00540 double PID_duty_roll(double target,double vol,float dt){ 00541 //ここで角度の単位で偏差は入力される 00542 //出力はPWMで 00543 double duty=0.0; 00544 double add_duty=0.0; 00545 00546 Prop_p=target-vol; 00547 00548 //pc.printf("%f/r/n",Prop_p); 00549 00550 Int_p+=Prop_p*double(dt); 00551 Dif_p=(Prop_p - Pre_Prop_p) / double(dt); 00552 00553 Pre_Prop_p=Prop_p; 00554 00555 //add_duty=Gain_Kp*Prop_p+Gain_Ki*Int_p+Gain_Kd*Dif_p; 00556 00557 add_duty=1.0*Prop_p; 00558 00559 duty+=add_duty; 00560 duty=Degree_to_PWM_duty(duty); 00561 00562 return duty; 00563 00564 } 00565 void Initialize_ESC(){ 00566 00567 ESC.pulsewidth(0.002); 00568 wait(3); 00569 00570 ESC.pulsewidth(0.001); 00571 wait(5); 00572 00573 } 00574 00575 00576 void Activate_ESC(){ 00577 00578 ESC.pulsewidth(0.001); 00579 wait(5); 00580 00581 } 00582 00583 void Acc_to_Pitch_Roll(float x_acc,float y_acc,float z_acc,float qua[2]){ 00584 00585 float Roll_before = (y_acc)/(z_acc); 00586 float Roll = atan(Roll_before); 00587 00588 float Pitch_before = (x_acc/sqrt((y_acc*y_acc+z_acc*z_acc)) ); 00589 float Pitch = -atan(Pitch_before); 00590 00591 qua[0]=Pitch*RAD_TO_DEG; 00592 qua[1]=Roll*RAD_TO_DEG; 00593 //float Acc_Pitch_Roll[2]; 00594 00595 } 00596 00597 void scan_update(float box_sensor[6]){ 00598 00599 float a[3];//加速度の値 00600 float g[3];//ジャイロの値[rad/s] 00601 00602 //int16_t raw_compass[3];//地磁気センサの値 00603 00604 mpu.setAcceleroRange(0);//acceleration range is +-4G 00605 mpu.setGyroRange(0);//gyro rate is +-degree per second(dps) 00606 00607 mpu.getAccelero(a);// 加速度を格納する配列[m/s2] a[0] is x axis,a[1] is y axis,a[2] is z axis; 00608 mpu.getGyro(g);// 角速度を格納する配列[rad/s] 00609 00610 //pc.printf("%f,%f,%f\r\n",g[0],g[1],g[2]); 00611 00612 //compass.getXYZ(raw_compass);//地磁気の値を格納する配列 00613 00614 box_sensor[0]=((-g[0])-(GYRO_FIX_X/7505.7))*RAD_TO_DEG; 00615 box_sensor[1]=((g[1])-(GYRO_FIX_Y/7505.7))*RAD_TO_DEG; 00616 box_sensor[2]=((-g[2])-(GYRO_FIX_Z/7505.7))*RAD_TO_DEG; 00617 00618 omega_z=box_sensor[2]; 00619 00620 box_sensor[3]=a[0]; 00621 box_sensor[4]=-a[1]; 00622 box_sensor[5]=a[2]; 00623 00624 //pc.printf("%f,%f,%f\r\n",box_sensor[0],box_sensor[1],box_sensor[2]); 00625 } 00626 00627 void flip(){ 00628 00629 GPS_interruptIn++; 00630 00631 }; 00632 00633 00634 //ch1 00635 void Input_Aileron_1(){ 00636 00637 first_period_old=ch_time.read(); 00638 //pc.printf("ch1=%f",second_period); 00639 }; 00640 00641 void Input_Aileron_1_fall(){ 00642 first_period=ch_time.read(); 00643 aileron_duty=first_period-first_period_old; 00644 }; 00645 00646 00647 //ch2 00648 void Input_elevator(){ 00649 second_period_old=ch_time.read(); 00650 }; 00651 00652 void Input_elevator_fall(){ 00653 second_period=ch_time.read(); 00654 elevator_duty=second_period-second_period_old; 00655 }; 00656 00657 00658 //ch3 00659 void Input_throttle(){ 00660 00661 first_period_old=ch_time.read(); 00662 second_period_old=ch_time.read(); 00663 third_period_old=ch_time.read(); 00664 fourth_period_old=ch_time.read(); 00665 fifth_period_old=ch_time.read(); 00666 sixth_period_old=ch_time.read(); 00667 00668 00669 } 00670 00671 void Input_throttle_fall(){ 00672 third_period=ch_time.read(); 00673 00674 /*デコード*/ 00675 throttle_duty=third_period-third_period_old; 00676 //rudder_duty=fourth_period-fourth_period_old; 00677 //elevator_duty=second_period-second_period_old; 00678 //aileron_duty=first_period-first_period_old; 00679 00680 //pc.printf("throttle_duty=%f:rudder_duty=%f:elevator_duty=%f\r\n",throttle_duty,rudder_duty,elevator_duty); 00681 } 00682 00683 //ch4 00684 void Input_rudder(){ 00685 fourth_period_old=ch_time.read(); 00686 } 00687 00688 void Input_rudder_fall(){ 00689 fourth_period=ch_time.read(); 00690 rudder_duty=fourth_period-fourth_period_old; 00691 } 00692 00693 //ch5 00694 void Input_gear(){ 00695 fifth_period_old=ch_time.read(); 00696 00697 } 00698 00699 void Input_gear_fall(){ 00700 fifth_period=ch_time.read(); 00701 switch_duty=fifth_period-fifth_period_old; 00702 } 00703 00704 //ch6 00705 void Input_Aileron_2(){ 00706 sixth_period_old=ch_time.read(); 00707 } 00708 00709 void Input_Aileron_2_fall(){ 00710 sixth_period=ch_time.read(); 00711 }; 00712 00713 //ch7 00714 void Input_pitch(){ 00715 seventh_period_old=ch_time.read(); 00716 } 00717 00718 void Input_pitch_fall(){ 00719 seventh_period=ch_time.read(); 00720 } 00721 00722 //ch8 00723 void Input_AUX5(){ 00724 eighth_period_old=ch_time.read(); 00725 } 00726 00727 void Input_AUX5_fall(){ 00728 eighth_period=ch_time.read(); 00729 }
Generated on Thu Jul 28 2022 14:41:32 by
1.7.2