main.cpp

00001 //skipper動作確認用プログラム
00002 //加速度，ジャイロ，地磁気，気圧，の４つのセンサを確認可能
00003 //シリアル受信側にはuarttestの受信のプログラムを使用する
00004 
00005 #include "mbed.h"
00006 //#include "rtos.h"
00007 #include "MPU9250.h"
00008 #include "BMP280.h"
00009 #include "SkipperSv2.h"
00010 #include "Estrela.h"
00011 #include "math.h"
00012 //#include "stm32f4xx_hal_iwdg.h"
00013 
00014 
00015 #define UPD_MANUAL  0
00016 #define UPD_AUTO    1
00017 #define UPD_LANDING 2
00018 
00019 #define SBUS_SIGNAL_OK          0x00
00020 #define SBUS_SIGNAL_LOST        0x01
00021 #define SBUS_SIGNAL_FAILSAFE    0x03
00022 
00023 
00024 
00025 
00026 
00027 
00028 
00029 
00030 
00031 
00032 //Serial pc(TX, RX);
00033 RawSerial sbus_(PA_9, PA_10); //SBUS
00034 
00035 PwmOut servo1(PC_6); // TIM3_CH1
00036 PwmOut servo2(PC_7); // TIM3_CH2    //PC_7
00037 PwmOut servo3(PB_0); // TIM3_CH3
00038 PwmOut servo4(PB_1); // TIM3_CH4
00039 PwmOut servo5(PB_6); // TIM4_CH1
00040 PwmOut servo6(PB_7); // TIM4_CH2
00041 PwmOut servo7(PB_8); // TIM4_CH3    //PB_8
00042 PwmOut servo8(PB_9); // TIM4_CH4
00043 
00044 RawSerial pc(PA_2, PA_3);
00045 
00046 DigitalOut led1(PA_0);  //緑
00047 DigitalOut led2(PA_1);  //赤
00048 DigitalOut led3(PB_4);  //赤外線
00049 DigitalOut led4(PB_5);
00050 
00051 volatile uint8_t buf_sbus[25]; 
00052 volatile int cnt_sbus = 0;
00053 static int16_t channels[18];
00054 uint8_t  failsafe_status = SBUS_SIGNAL_FAILSAFE;
00055 static bool flg_ch_update = false;
00056 //uint8_t i,j,k;
00057 uint8_t update_mode = 0;
00058 static uint16_t pwm[8];
00059 static uint16_t oldTHL;
00060 
00061 //9軸センサー関連
00062 float sum = 0;
00063 uint32_t sumCount = 0;
00064 MPU9250 mpu9250;
00065 Timer t;
00066 
00067 /*
00068 float g_BefGyro[3] = {0,0,0};
00069 double g_HpfGyro[3] = {0,0,0};
00070 double g_HpfGyro1[3] = {0,0,0};
00071 double g_LpfAccel[3] = {0,0,0};
00072 */
00073 
00074 static int jj = 0;
00075 
00076 //IWDG_HandleTypeDef hiwdg;
00077 
00078 void disp_clock(){
00079     pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000);
00080     pc.printf("HCLK Clock   = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000);
00081     pc.printf("PCLK1 Clock  = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000);
00082     pc.printf("PCLK2 Clock  = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000);
00083     pc.printf("\r\n");
00084     }
00085 
00086 //サーボ初期化関数
00087 void init_Servo()
00088 {
00089     uint8_t j = 0;
00090     
00091     servo1.period_ms(14);
00092     servo1.pulsewidth_us(1500);
00093     
00094     servo2.period_ms(14);
00095     servo2.pulsewidth_us(1500);
00096     
00097     servo3.period_ms(14);
00098     servo3.pulsewidth_us(1000);
00099     
00100     servo4.period_ms(14);
00101     servo4.pulsewidth_us(1500);
00102     
00103     servo5.period_ms(14);
00104     servo5.pulsewidth_us(1500);
00105     
00106     servo6.period_ms(14);
00107     servo6.pulsewidth_us(1500);
00108     
00109     servo7.period_ms(14);
00110     servo7.pulsewidth_us(1500);
00111     
00112     servo8.period_ms(14);
00113     servo8.pulsewidth_us(1500);
00114     
00115     for (j=0; j<8; j++) {pwm[j] = 1500;}
00116     pc.printf("servo initialized\r\n");
00117 }
00118 
00119 //---setall_servo---
00120 //pwmをサーボに出力する関数。
00121 void setall_servo()
00122 {
00123     uint8_t j = 0;
00124     
00125     //if(failsafe_status == SBUS_SIGNAL_OK){
00126     servo1.pulsewidth_us(pwm[0]);
00127     servo2.pulsewidth_us(pwm[1]);
00128     servo3.pulsewidth_us(pwm[2]);
00129     servo4.pulsewidth_us(pwm[3]);
00130     servo5.pulsewidth_us(pwm[4]);
00131     servo6.pulsewidth_us(pwm[5]);
00132     servo7.pulsewidth_us(pwm[6]);
00133     servo8.pulsewidth_us(pwm[7]);   
00134       /*  }else{
00135         servo1.pulsewidth_us(1000);
00136         servo2.pulsewidth_us(1000);
00137         servo3.pulsewidth_us(1000);
00138         servo4.pulsewidth_us(1000);
00139         servo5.pulsewidth_us(1000);
00140         servo6.pulsewidth_us(1000);
00141         servo7.pulsewidth_us(1000);
00142         servo8.pulsewidth_us(1000);
00143     }*/
00144 
00145     for(j=0;j<8;j++){
00146         pc.printf("ch%d=%d ", j+1, pwm[j]);
00147     }
00148     //pc.printf("time = %d", frq.read_ms()); 
00149     pc.printf("\r\n");
00150 
00151     //frq.reset();
00152 
00153 }
00154 
00155 
00156 //---update_pwm---
00157 //udate_Inputで抽出したpwmデータを整形して各変数に代入する。(マニュアルモード)
00158 //各stabiGylo関数で算出したpwmを各変数に代入する(自動モード)
00159 void update_pwm()
00160 {
00161     uint8_t j = 0;
00162     
00163     if(flg_ch_update == true){
00164     switch(update_mode){    //マニュアルモード,自動モード,自動着陸もモードを切替
00165         case UPD_MANUAL:    //マニュアルモード
00166             for(j=0;j<8;j++){
00167                 pwm[j] = (channels[j]>>1) + 1000;
00168             }
00169             oldTHL = pwm[2];
00170             jj=0;
00171             //pc.printf("update_manual\r\n");
00172             break;
00173         
00174         case UPD_AUTO:      //自動モード
00175             pwm[0] = (channels[0]>>1) + 1000;
00176             pwm[1] = Auto_ELE;
00177             pwm[2] = Auto_THR;
00178             pwm[3] = Auto_RUD;
00179             pwm[4] = (channels[4]>>1) + 1000;
00180             pwm[5] = (channels[5]>>1) + 1000;
00181             pwm[6] = (channels[6]>>1) + 1000;
00182             pwm[7] = (channels[7]>>1) + 1000;
00183  
00184             //pc.printf("update_auto\r\n");
00185             break;
00186         
00187         case UPD_LANDING:   //自動着陸モード
00188             pwm[0] = (channels[0]>>1) + 1000;
00189             pwm[1] = Auto_ELE;
00190             pwm[2] = (channels[2]>>1) + 1000;
00191             pwm[3] = (channels[3]>>1) + 1000;
00192             pwm[4] = (channels[4]>>1) + 1000;
00193             pwm[5] = (channels[5]>>1) + 1000;
00194             pwm[6] = (channels[6]>>1) + 1000;
00195             pwm[7] = (channels[7]>>1) + 1000;
00196             break;
00197             
00198         default:            //デフォはマニュアルモード
00199             for(j=0;j<8;j++){
00200                 pwm[j] = (channels[j]>>1) + 1000;
00201             }
00202 
00203             jj=0;
00204             //pc.printf("update_manual\r\n");
00205             break;
00206     }
00207     }else{
00208         pc.printf("0\r\n");
00209     }
00210     flg_ch_update = false;
00211     ///oldTHL = pwm[2]; 
00212           setall_servo();  
00213 }
00214 
00215 /*void update_auto()
00216 {
00217     pwm[0] = (channels[0]>>1) + 1000;
00218     pwm[1] = Auto_ELE;
00219     pwm[2] = Auto_THR;
00220     pwm[3] = Auto_RUD;
00221     pwm[4] = (channels[4]>>1) + 1000;
00222     pwm[5] = (channels[5]>>1) + 1000;
00223     pwm[6] = (channels[6]>>1) + 1000;
00224     pwm[7] = (channels[7]>>1) + 1000;
00225     
00226     setall_servo();
00227     //pc.printf("update_auto\r\n");
00228 }*/
00229 
00230 //---update_Input---
00231 //catch_sbusで取得したSbusの生データから各チャンネルのpwmを抽出する。
00232 void update_Input()
00233 {     
00234     uint8_t i = 0;
00235 
00236 /*  for (i=0; i<25; i++){
00237         pc.printf("%x ", buf_sbus[i]);
00238         }pc.printf("\n"); 
00239 */
00240     // clear channels[]
00241     for (i=0; i<18; i++) {channels[i] = 0;}
00242 
00243     // reset counters
00244     uint8_t byte_in_sbus = 1;
00245     uint8_t bit_in_sbus = 0;
00246     uint8_t ch = 0;
00247     uint8_t bit_in_channel = 0;
00248 
00249     // process actual sbus data
00250     for (i=0; i<176; i++) {
00251         if (buf_sbus[byte_in_sbus] & (1<<bit_in_sbus)) {
00252             channels[ch] |= (1<<bit_in_channel);
00253         }
00254         bit_in_sbus++;
00255         bit_in_channel++;
00256 
00257         if (bit_in_sbus == 8) {
00258             bit_in_sbus =0;
00259             byte_in_sbus++;
00260         }
00261         if (bit_in_channel == 11) {
00262             bit_in_channel =0;
00263             ch++;
00264         }
00265     }
00266     // DigiChannel 1
00267     if (buf_sbus[23] & (1<<0)) {
00268         channels[16] = 1;
00269     }else{
00270         channels[16] = 0;
00271     }
00272     // DigiChannel 2
00273     if (buf_sbus[23] & (1<<1)) {
00274         channels[17] = 1;
00275     }else{
00276         channels[17] = 0;
00277     }
00278     // Failsafe
00279     failsafe_status = SBUS_SIGNAL_OK;
00280     if (buf_sbus[23] & (1<<2)) {
00281         failsafe_status = SBUS_SIGNAL_LOST;
00282     }
00283     if (buf_sbus[23] & (1<<3)) {
00284         failsafe_status = SBUS_SIGNAL_FAILSAFE;
00285     }
00286     //channels[i] = channels[i]>>1;
00287     for (i=0; i<25; i++) {buf_sbus[i] = 0;}
00288     cnt_sbus = 0;
00289     flg_ch_update = true;
00290     
00291     update_pwm();
00292         
00293 
00294 }
00295 
00296 //---catch_sbus---
00297 //受信機から送られてくるSBus信号(シリアル)を取得する関数
00298 void catch_sbus()
00299 {
00300     buf_sbus[cnt_sbus] = sbus_.getc();
00301     if(buf_sbus[0] == 0x0f)
00302         {
00303             cnt_sbus ++;
00304         }
00305     if(cnt_sbus >=25){  
00306         if(buf_sbus[24] == 0x00){
00307             update_Input();     //update_Input実行
00308         }else{
00309             cnt_sbus = 0;
00310         }
00311     }
00312 return;
00313 }
00314 
00315 //---init_sbus---
00316 //Sbusを初期化する関数
00317 void init_sbus(){
00318     // Set Baudrate
00319     sbus_.baud(100000);
00320     // Set Datalenght & Frame
00321     sbus_.format(8, Serial::Even, 2);
00322     //start sbus irq
00323     sbus_.attach(&catch_sbus, RawSerial::RxIrq);
00324     }
00325 
00326 //---CheckSW_up---
00327 //プロポのスイッチがonかoffか返す関数
00328 //ch 1~8
00329 bool CheckSW_up(int8_t ch)
00330 {
00331     uint16_t checkpwm;
00332     checkpwm = (channels[ch-1]>>1) + 1000;
00333     //pc.printf("ch%d=%d ", ch, checkpwm);
00334     if(SWITCH_CHECK > checkpwm){
00335         return true;
00336     }else{
00337         return false;
00338     }
00339 }
00340 
00341 
00342         
00343 //---sensing---
00344 //センサーの値を読み込み、各種フィルタをかける(認知)
00345 void sensing()
00346 {
00347     // If intPin goes high, all data registers have new data
00348     if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
00349 
00350         mpu9250.readAccelData(accelCount);  // Read the x/y/z adc values   
00351         // Now we'll calculate the accleration value into actual g's
00352         ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
00353         ay = (float)accelCount[1]*aRes - accelBias[1];   
00354         az = (float)accelCount[2]*aRes - accelBias[2];  
00355    
00356         mpu9250.readGyroData(gyroCount);  // Read the x/y/z adc values
00357         // Calculate the gyro value into actual degrees per second
00358         gx = (float)gyroCount[0]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
00359         gy = (float)gyroCount[1]*gRes - gyroBias[1];  
00360         gz = (float)gyroCount[2]*gRes - gyroBias[2];   
00361   
00362         mpu9250.readMagData(magCount);  // Read the x/y/z adc values   
00363         // Calculate the magnetometer values in milliGauss
00364         // Include factory calibration per data sheet and user environmental corrections
00365         mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
00366         my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];  
00367         mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];   
00368     }
00369    
00370     Now = t.read_us();
00371     deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
00372     lastUpdate = Now;
00373     
00374     sum += deltat;
00375     sumCount++;
00376     
00377 //  if(lastUpdate - firstUpdate > 10000000.0f) {
00378 //      beta = 0.04;  // decrease filter gain after stabilized
00379 //      zeta = 0.015; // increasey bias drift gain after stabilized
00380 //  }
00381     
00382    // Pass gyro rate as rad/s
00383     mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
00384     mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
00385 
00386     /*  Auto_Loop()で50ms待ってるから要らないと思う
00387     // Serial print and/or display at 0.5 s rate independent of data rates
00388     delt_t = t.read_ms() - count;
00389     if (delt_t > 500) { // update LCD once per half-second independent of read rate
00390     */
00391 
00392     //pc.printf("ax = %f", 1000*ax); 
00393     //pc.printf(" ay = %f", 1000*ay); 
00394     //pc.printf(" az = %f  mg\n\r", 1000*az); 
00395 
00396     //pc.printf("gx = %f", gx); 
00397     //pc.printf(" gy = %f", gy); 
00398     //pc.printf(" gz = %f  deg/s\n\r", gz); 
00399     
00400     //pc.printf("mx = %f,", mx); 
00401     //pc.printf(" my = %f,", my); 
00402     //pc.printf(" mz = %f  mG\n\r", mz); 
00403     
00404     tempCount = mpu9250.readTempData();  // Read the adc values
00405     temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
00406     //pc.printf(" temperature = %f  C\n\r", temperature); 
00407     
00408     //pc.printf("q0 = %f\n\r", q[0]);
00409     //pc.printf("q1 = %f\n\r", q[1]);
00410     //pc.printf("q2 = %f\n\r", q[2]);
00411     //pc.printf("q3 = %f\n\r", q[3]);      
00412     
00413       
00414   // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
00415   // In this coordinate system, the positive z-axis is down toward Earth. 
00416   // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
00417   // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
00418   // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
00419   // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
00420   // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
00421   // applied in the correct order which for this configuration is yaw, pitch, and then roll.
00422   // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
00423     yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);   
00424     roll = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
00425     pitch  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
00426     pitch *= 180.0f / PI;
00427     yaw   *= 180.0f / PI; 
00428     yaw   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
00429     roll  *= 180.0f / PI;
00430 
00431     //pc.printf("$%d %d %d;",(int)(yaw*100),(int)(pitch*100),(int)(roll*100));
00432     //pc.printf("Yaw: %f  Pitch:%f  Roll:%f\n\r",  yaw,   pitch,   roll);
00433     //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
00434  
00435     count = t.read_ms(); 
00436     sum = 0;
00437     sumCount = 0; 
00438     //}           
00439 }
00440 
00441 
00442 //---CalibrateCompass---
00443 //グラウンドチェック時にスイッチ7だけをonにしておくことで実行 リポ抜いたらやり直ししなくてはならない
00444 //使わない予定
00445 void CalibrateCompass(void)
00446 {
00447     led1 = 1;
00448     sensing();
00449     uint8_t ii;
00450     float mxMAX=mx, mxMIN=mx;
00451     float myMAX=my, myMIN=my;
00452     magbias[0] = 0;    //初期化
00453     magbias[1] = 0;
00454     //magbias[2] = 0;   
00455     ii=0;
00456     while(1){   //キャリブレーション実行   led2が点滅時に機体を一回転させる xy方向のみ
00457         sensing();
00458         if(mxMAX < mx) mxMAX = mx;
00459         if(mxMIN > mx) mxMIN = mx;
00460         if(myMAX < my) myMAX = my;
00461         if(myMIN > my) myMIN = my;
00462         led2 = !led2;
00463         ++ii;
00464         wait(50);
00465         
00466         if(!CheckSW_up(7)) break;   //スイッチ7を下げて終了
00467     }
00468     led2 = 0;
00469     if(ii<20){  //チャンネル7をすぐ切った場合 元に戻す
00470         magbias[0] = MAGBIASX;
00471         magbias[1] = MAGBIASY;
00472     }else{  //しっかりと値を入れた場合
00473         magbias[0] = (mxMAX+mxMIN)/2;
00474         magbias[1] = (myMAX+myMIN)/2;
00475     }
00476     led1 = 0;
00477     t.reset();  //タイマーをリセットして終了
00478 }  
00479 
00480     
00481 //---StabiGyro---
00482 //水平旋回のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断)
00483 //右旋回
00484 void StabiGyro()
00485 {   
00486     if(0  /*oldTHL < 1200*/){  //機体審査用
00487         Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN-gy*RUD_DGN);
00488         Auto_ELE = ELE_N + -50 + int((-15 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
00489         Auto_THR = 1500;
00490         }else{
00491             if(jj<=0){     //旋回し始め  15  
00492                 Auto_RUD = RUD_N + -65 + int((-12 -roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75);   //roll -> gy        3.5
00493                 Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - gx*ELE_DGN);     //pitch  -> -gx
00494                 Auto_THR = oldTHL + 30;     
00495                 //pc.printf("first%d\r\n",jj);
00496                 jj++;
00497             }else{          //旋回中
00498                 Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75); 
00499                 Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
00500                 Auto_THR = oldTHL ;
00501             }
00502         }
00503     
00504     //pwmの値が最小値と最大値の間に入ってなかったら、強制的に最大値or最小値にする
00505     if(Auto_RUD > RUD_MAX){
00506         Auto_RUD = RUD_MAX;
00507     }else if(Auto_RUD < RUD_MIN){
00508         Auto_RUD = RUD_MIN;
00509     }
00510     if(Auto_ELE > ELE_MAX){
00511         Auto_ELE = ELE_MAX;
00512     }else if(Auto_ELE < ELE_MIN){
00513         Auto_ELE = ELE_MIN;
00514     }
00515     
00516 
00517         
00518     //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE);
00519     //Thread::wait(G_MS);
00520 }
00521 
00522 //---StabiGyro_Mobius---
00523 //8の字旋回のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断)
00524 void StabiGyro_Mobius()
00525 {
00526     if(jj<=15){     //右旋回導入
00527         Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - (gy + 130)*0.75);    //*0.75);   //roll -> gy        3.5
00528         Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - (gx + 30)*0.8);     //pitch  -> -gx
00529         Auto_THR = oldTHL;       
00530         //pc.printf("first%d\r\n",jj);
00531         jj++;
00532     }else if((15<jj) && (jj<=70)){     //右旋回途中
00533         Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75); 
00534         Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
00535         Auto_THR = oldTHL;
00536         jj++;
00537     }else if((70<jj) && (jj<=75)){    //旋回遷移1
00538         Auto_RUD = RUD_N + 100 + int((16 -roll)*RUDDER_GN - (gy-100)*0.75);  
00539         Auto_ELE = ELE_N + 20 + int((0 -pitch)*ELEVATOR_GN - (gx + 0)*0.6);   
00540         Auto_THR = THR_N - 100;      
00541         //pc.printf("first%d\r\n",jj);
00542         jj++;
00543     }else if((75<jj) && (jj<=90)){    //旋回遷移2
00544         Auto_RUD = RUD_N + 65 + int((12-roll)*RUDDER_GN - (gy-100)*0.75);    // 105 30 
00545         Auto_ELE = ELE_N + -15 + int((-5 - pitch)*ELEVATOR_GN - (gx+30)*0.6);   //-75 -15
00546         Auto_THR = oldTHL - 20;     //-20
00547         jj++;
00548     }else{                              //左旋回途中
00549         Auto_RUD = RUD_N + 65 + int((12-roll)*RUDDER_GN - gy*RUD_DGN);  
00550         Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - gx*ELE_DGN);   
00551         Auto_THR = oldTHL;   
00552     }
00553         
00554     if(Auto_RUD > RUD_MAX){
00555         Auto_RUD = RUD_MAX;
00556     }else if(Auto_RUD < RUD_MIN){
00557         Auto_RUD = RUD_MIN;
00558     }
00559     if(Auto_ELE > ELE_MAX){
00560         Auto_ELE = ELE_MAX;
00561     }else if(Auto_ELE < ELE_MIN){
00562         Auto_ELE = ELE_MIN;
00563     }
00564         
00565     //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE);
00566     //Thread::wait(G_MS);
00567 }
00568 
00569 //---StabiGyro_Climb---
00570 //上昇旋回のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断)
00571 void StabiGyro_Climb()
00572 {
00573     if(jj<=15){     //右旋回導入
00574         Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - (gy+130)*RUD_DGN);    //*0.75);   //roll -> gy        3.5
00575         Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - (gx+30)*ELE_DGN);     //pitch  -> -gx
00576         Auto_THR = oldTHL;            
00577         //pc.printf("first%d\r\n",jj);
00578         jj++;
00579     }else if((15<jj) && (jj<=136)){     //右旋回途中
00580         Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75); 
00581         Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
00582         Auto_THR = oldTHL;
00583         jj++;
00584     /*}else if((136<jj) && (jj<=146)){    //上昇遷移
00585         Auto_RUD = RUD_N + -65 - int((-12-roll)*RUDDER_GN - gy*0);
00586         Auto_ELE = ELE_N + -80 + int((-22-pitch)*ELEVATOR_GN - gx*0);
00587         Auto_THR = THR_N + 100;     
00588         //pc.printf("first%d\r\n",jj);
00589         jj++;*/
00590     }else if((136<jj) && (jj<=200)){    //上昇中
00591         Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75); 
00592         Auto_ELE = ELE_N + -140 + int((-25 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
00593         Auto_THR = oldTHL + 220;
00594         jj++;
00595     }else{                              //水平旋回
00596         Auto_RUD = RUD_N + -65 + int((-12-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75); 
00597         Auto_ELE = ELE_N + -65 + int((-16 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
00598         Auto_THR = oldTHL;
00599     }
00600         
00601     if(Auto_RUD > RUD_MAX){
00602         Auto_RUD = RUD_MAX;
00603     }else if(Auto_RUD < RUD_MIN){
00604         Auto_RUD = RUD_MIN;
00605     }
00606     if(Auto_ELE > ELE_MAX){
00607         Auto_ELE = ELE_MAX;
00608     }else if(Auto_ELE < ELE_MIN){
00609         Auto_ELE = ELE_MIN;
00610     }
00611         
00612     //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE);
00613     //Thread::wait(G_MS);
00614 }
00615 
00616 //---StabiGyro_Glide---
00617 //自動滑空のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断)
00618 void StabiGyro_Glide()
00619 {
00620     if(jj<=15){
00621         Auto_RUD = RUD_N;
00622         Auto_ELE = ELE_N + -5 + int (( - pitch)*ELEVATOR_GN - gx*0);
00623         Auto_THR = 1000;
00624         //pc.printf("first%d\r\n",jj);
00625         jj++;
00626     }else{
00627         Auto_RUD = RUD_N + -30 + int((-8 - roll)*RUDDER_GN - gy*RUD_DGN);    
00628         Auto_ELE = ELE_N + -25 + int((-5- pitch)*ELEVATOR_GN - gx*ELE_DGN);
00629         Auto_THR = 1000;    //スロットルoff
00630     }
00631     if(Auto_RUD > RUD_MAX){
00632         Auto_RUD = RUD_MAX;
00633     }else if(Auto_RUD < RUD_MIN){
00634         Auto_RUD = RUD_MIN;
00635     }
00636     if(Auto_ELE > ELE_MAX){
00637         Auto_ELE = ELE_MAX;
00638     }else if(Auto_ELE < ELE_MIN){
00639         Auto_ELE = ELE_MIN;
00640     }
00641         
00642     //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE);
00643     //Thread::wait(G_MS);
00644 }
00645 
00646 void StabiGyro_Landing()
00647 {
00648     if(pwm[0]<1300){
00649         Auto_ELE = ELE_N + -10 + int((-5 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
00650         Auto_THR = oldTHL;
00651     }else if((1300<=pwm[0])&&(pwm[0]<1800)){
00652         Auto_ELE = ELE_N + -15 + int((-8 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
00653         Auto_THR = oldTHL - 30;
00654     }else if(1800<=pwm[0]){
00655         Auto_ELE = ELE_N + -25 + int((-12 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
00656         Auto_THR = 1000;
00657     }
00658 }
00659 
00660 void Auto_Loop()
00661 {
00662     //while(true){
00663     //for(k=0;k<100;++k){
00664         //sensing();      //センサーの値を読み込む(認知)
00665         StabiGyro();    //水平旋回のためのラダー、エレベーターのpwmを計算(判断)
00666         update_mode = UPD_AUTO;     //オートモード
00667         //StabiAccel();
00668         //Auto_ELE+=145;
00669         //Auto_RUD-=80;
00670         //Servos.Auto2Servo();
00671         //Thread::wait(50);
00672     /*
00673         if(!CheckSW_up(7)){
00674             pc.printf("go to manual\n");
00675             break;
00676         }*/
00677         //wait_ms(50);        
00678     //}   
00679     //k=0; //for debug
00680     //pwm[6]=1000;
00681 }
00682 
00683 void Auto_Mobius()
00684 {
00685     //sensing();
00686     StabiGyro_Mobius();
00687     update_mode = UPD_AUTO;
00688     //wait_ms(50);
00689 }
00690 
00691 void Auto_Climb()
00692 {
00693     //sensing();
00694     StabiGyro_Climb();
00695     update_mode = UPD_AUTO;
00696     //wait_ms(50);   
00697 }
00698 
00699 void Auto_Glide()
00700 {
00701     //sensing();
00702     StabiGyro_Glide();
00703     update_mode = UPD_AUTO;
00704     //wait_ms(50);
00705 }
00706 
00707 void Auto_Landing()
00708 {   
00709     //sensing();
00710     StabiGyro_Landing();
00711     update_mode = UPD_MANUAL;
00712     //wait_ms(50);
00713 }
00714 
00715 /*動作確認用uart通信*/
00716 Serial g_Serial1(USBTX,USBRX);
00717 Serial g_serial2(PA_2, PA_3);
00718 /*気圧センサ*/
00719 BMP280 bmp(PC_9, PA_8);
00720 
00721 
00722 int main()
00723 {
00724     
00725     
00726     //SBus用バッファ初期化
00727     //pc.baud(115200);    //パソコン通信用シリアルのボードレート
00728            
00729     //frq.start();     
00730     
00731    
00732     //---9軸センサー(MPU9250)初期化--- 
00733     
00734     //Set up I2C
00735     i2c.frequency(400000);  // use fast (400 kHz) I2C  
00736     pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);   
00737     t.start();
00738     
00739     // Read the WHO_AM_I register, this is a good test of communication
00740     uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
00741     pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
00742   
00743 
00744 
00745     mpu9250.getAres(); // Get accelerometer sensitivity
00746     mpu9250.getGres(); // Get gyro sensitivity
00747     mpu9250.getMres(); // Get magnetometer sensitivity
00748     //pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
00749     //pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
00750     //pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
00751     magbias[0] = MAGBIASX;       //+470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
00752     magbias[1] = MAGBIASY;       //+120.;  // User environmental y-axis correction in milliGauss
00753     magbias[2] = MAGBIASZ;       //+125.;  // User environmental z-axis correction in milliGauss
00754     
00755     t.start();
00756     pc.printf("mpu9250 initialized\r\n");
00757     
00758     //---9軸センサー(MPU9250)初期化終わり---   
00759           
00760     
00761     
00762     
00763     
00764     /*ここからskippre test*/
00765     g_serial2.baud(9600);
00766      
00767     
00768        //BMP280を初期化　引数にenum型のFilterselectionを入れると
00769     //それに合わせた初期化をしてくれる Readme参照
00770     //引数いれないとフィルタなしで初期化する
00771     bmp.initialize(BMP280::INDOOR_NAVIGATION);
00772 
00773     while(1){
00774         //気圧を測定　気圧測定時には温度補正のために先に温度を測る必要があるが，
00775         //getPressureは勝手に測るのでこれだけでよい
00776         //単位はhPa
00777         float press = bmp.getPressure();
00778 
00779         //もし温度自分で取得する場合，getPressureの引数をtrueにすれば処理を早くできるが，
00780         //その場合getPressureの直前に温度を測る必要がある
00781         //float temp = bmp.getTemperature();
00782         //float press = bmp.getPressure(true);
00783 
00784 
00785         //気圧と高度の近似換算式　Readme参照
00786         //float height = (float)(44.331514 - 11.880516 * pow(press, 0.1902632)) * 1000);
00787 
00788     
00789     
00790     
00791     
00792     sensing();
00793     
00794     g_serial2.printf("ax,%f ay,%f az,%f\r\n",ax,ay,az);
00795     g_serial2.printf("gx,%f gy,%f gz,%f\r\n",gx,gy,gz);
00796     g_serial2.printf("mx,%f my,%f mz,%f\r\n",mx,my,mz);
00797     g_serial2.printf("press,%f\r\n\r\n",press);
00798     
00799     wait(1.0);
00800     
00801     }
00802     
00803     
00804 }