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.
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