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 }