main.cpp

00001 //mbed
00002 #include "mbed.h"
00003 #include "FATFileSystem.h"
00004 #include "SDFileSystem.h"
00005 //C
00006 #include "math.h"
00007 //sensor
00008 #include "MPU6050_DMP6.h"
00009 //#include "MPU9250.h"
00010 #include "MPU9250_BMP.h"
00011 #include "BMP280.h"
00012 #include "hcsr04.h"
00013 //device
00014 #include "sbus.h"
00015 //config
00016 #include "SkipperSv2.h"
00017 #include "falfalla.h"
00018 //other
00019 #include "pid.h"
00020 
00021 #define DEBUG_SEMIAUTO 0
00022 #define DEBUG_PRINT_INLOOP 1
00023 
00024 #define KP_ELE 16.5//15.0  //2.0
00025 #define KI_ELE 0.0
00026 #define KD_ELE 0.1  //0/0
00027 #define KP_RUD 3.0
00028 #define KI_RUD 0.0
00029 #define KD_RUD 0.0
00030 #define KP_AIL 0.11//0.1
00031 #define KI_AIL 0.2
00032 #define KD_AIL 0.2
00033 
00034 //#define g_AIL_L_Ratio_rightloop 0.5
00035 
00036 #define GAIN_CONTROLVALUE_TO_PWM 3.0
00037 
00038 #define RIGHT_ROLL 5.0//10
00039 #define RIGHT_PITCH 45//-20.0//-22//-24.0  //5.0
00040 #define LEFT_ROLL 15//-38.0//30
00041 
00042 #define LEFT_PITCH 10//-15.0
00043 #define STRAIGHT_ROLL 0//12.0//8.0
00044 #define STRAIGHT_PITCH 3.0
00045 #define TAKEOFF_THR 0.8
00046 #define LOOP_THR 0.6
00047 
00048 //#define g_rightloopRUD 1500
00049 
00050 #define RIGHT_ROLL_SHORT 10.0
00051 #define RIGHT_PITCH_SHORT -25.0
00052 #define LEFT_ROLL_SHORT -30.0
00053 #define LEFT_PITCH_SHORT -20.0
00054 #define RIGHTLOOPROLL_APPROACH 10.0
00055 #define LEFTLOOPROLL_APPROACH -30.0
00056 #define RIGHTLOOPPITCH_APPROACH -25.0
00057 #define LEFTLOOPPITCH_APPROACH -30.0
00058 
00059 #define ASCENDING_PITCH -35.0
00060 #define TAKE_OFF_THROTTLE 1650
00061 #define TAKE_OFF_PITCH 50//-40.0
00062 
00063 #define rightloopROLL2 -10.0
00064 
00065 /*#define rightloopRUD 1300  //1250
00066 #define rightloopshortRUD 1250
00067 #define leftloopRUD  1500
00068 #define leftloopshortRUD 1500
00069 #define glideloopRUD 1300
00070 */
00071 #define AIL_R_correctionrightloop 0
00072 #define AIL_L_correctionrightloop 0
00073 #define AIL_L_correctionrightloopshort 0
00074 #define AIL_L_correctionleftloop -0
00075 #define AIL_L_correctionleftloopshort 0
00076 
00077 
00078 #define RIGHTLOOP_RUD 1190//1150 //1200
00079 #define RIGHTLOOPSHORT_RUD 1250
00080 #define LEFTLOOP_RUD 1560 //1680//1750//1700
00081 #define LEFTLOOPSHORT_RUD 1500
00082 #define GLIDELOOP_RUD 1300
00083 #define RIGHTLOOPRUD_APPROACH 1500
00084 #define LEFTLOOPRUD_APPROACH  1500
00085 #define RIGHTLOOP_THR_RATE 0.64//0.66
00086 #define LEFTLOOP_THR_RATE 0.49//0.62//0.66
00087 #define ASCENDING_THR_RATE 0.75
00088 #define ASCENDING_ROLL 16.0
00089 #define ASCENDING_RUD 1200
00090 
00091 
00092 #define AIL_L_CORRECTION_RIGHTLOOP 0
00093 #define AIL_L_CORRECTION_RIGHTLOOPSHORT 0
00094 #define AIL_L_CORRECTION_LEFTLOOP 0
00095 #define AIL_L_CORRECTION_LEFTLOOPSHORT 0
00096 
00097 
00098 
00099 #define TRIM1 1590
00100 #define TRIM2 1579
00101 #define TRIM3 1177
00102 #define TRIM4 1392
00103 #define TRIM5 1176
00104 #define TRIM6 1560
00105 #define TRIM_CHECK 0
00106 
00107 #define GLIDE_ROLL -12.0
00108 #define GLIDE_PITCH -3.0
00109 
00110 
00111 #define AIL_L_RatioRising 0.5
00112 #define AIL_L_RatioDescent 2
00113 
00114 //コンパスキャリブレーション
00115 //SkipperS2基板
00116 /*
00117 #define MAGBIAS_X -35.0
00118 #define MAGBIAS_Y 535.0
00119 #define MAGBIAS_Z -50.0
00120 */
00121 //S2v2 1番基板
00122 #define MAGBIAS_X 395.0
00123 #define MAGBIAS_Y 505.0
00124 #define MAGBIAS_Z -725.0
00125 //S2v2 2番基板
00126 /*
00127 #define MAGBIAS_X 185.0
00128 #define MAGBIAS_Y 220.0
00129 #define MAGBIAS_Z -350.0
00130 */
00131 
00132 #define ELEMENT 1
00133 #define LIMIT_STRAIGHT_YAW 5.0
00134 #define THRESHOLD_TURNINGRADIUS_YAW 60.0
00135 #define ALLOWHEIGHT 15
00136 
00137 //気圧用
00138 #define SBUS_SIGNAL_OK          0x00
00139 #define SBUS_SIGNAL_LOST        0x01
00140 #define SBUS_SIGNAL_FAILSAFE    0x03
00141 MPU9250 mpu9250;
00142 float g_sum = 0;      //グローバルなので要注意!!
00143 uint32_t g_sumCount = 0;
00144 float g_grav = 9.80665;
00145 float g_st_press = 1018.5;
00146 float g_press, g_BMPheight;
00147 int g_BMPheight_int = 0;
00148 float g_InitPress[101];
00149 float g_InitPressAve = 0;
00150 float g_InitBMPHeight[101];
00151 float g_InitBMPHeightAve = 0;
00152 int g_temporary;
00153 float g_dt = 0.0;
00154 float g_v_prev = 0.0;
00155 float g_z_prev; 
00156 float g_z_PressHeight, g_z_AccelHeight;
00157 float g_z_comp;
00158 float g_tempaz[2];
00159 
00160 BMP280 bmp(PC_9, PA_8);
00161 Timer tBMP;
00162 
00163 
00164 #ifndef PI
00165 #define PI 3.14159265358979
00166 #endif
00167 
00168 const int16_t lengthdivpwm = 320;
00169 const int16_t changeModeCount = 6;
00170 
00171 
00172 SBUS sbus(PA_9, PA_10); //SBUS
00173 
00174 PwmOut servo1(PC_6); // TIM3_CH1  //old echo
00175 PwmOut servo2(PC_7); // TIM3_CH2    //PC_7
00176 PwmOut servo3(PB_0); // TIM3_CH3
00177 PwmOut servo4(PB_1); // TIM3_CH4
00178 PwmOut servo5(PB_6); // TIM4_CH1
00179 PwmOut servo6(PB_7); // TIM4_CH2  //old trigger
00180 //PwmOut servo7(PB_8); // TIM4_CH3    //PB_8 new echo
00181 //PwmOut servo8(PB_9); // TIM4_CH4   //new trigger
00182 
00183 RawSerial pc(PA_2,PA_3, 115200);    //tx,rx.baudrate pin;PA_2=UART2_TX, PA_3=UART2_RX
00184 //RawSerial pc2(PB_6,PB_7, 115200);   //sbus確認用
00185 SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd");
00186 
00187 DigitalOut led1(PA_0);  //黄色のコネクタ
00188 DigitalOut led2(PA_1);
00189 DigitalOut led3(PB_5);
00190 DigitalOut led4(PB_4);
00191 
00192 //InterruptIn switch2(PC_14);
00193 MPU6050DMP6 mpu6050(PC_0,&pc); //割り込みピン，シリアルポインタ i2cのピン指定は MPU6050>>I2Cdev.h 内のdefine
00194 HCSR04 usensor(PB_9,PB_8); //trig,echo  9,8
00195 
00196 PID pid_AIL(g_kpAIL,g_kiAIL,g_kdAIL);
00197 PID pid_ELE(g_kpELE,g_kiELE,g_kdELE);
00198 PID pid_RUD(g_kpRUD,g_kiRUD,g_kdRUD);
00199 
00200 enum Channel {AIL_R, ELE, THR, RUD, DROP, AIL_L, Ch7, Ch8};
00201 enum Angle {ROLL, PITCH, YAW};  //yaw:北を0とした絶対角度
00202 enum OperationMode {StartUp, SemiAuto, RightLoop, LeftLoop, GoStraight, BombwithPC, ZERO, Moebius, Glide};
00203 enum BombingMode {Takeoff, Chicken, Transition, Approach};
00204 enum OutputStatus {Manual, Auto};
00205 
00206 static OutputStatus output_status = Manual;
00207 OperationMode operation_mode = StartUp;
00208 BombingMode bombing_mode = Takeoff;
00209 
00210 static int16_t autopwm[8] = {1590,1579,1177,1392,1176,1560,1178,1178};//6:1848
00211 
00212 //1号機14SG
00213 
00214 static int16_t trimpwm[6] = {1590,1579,1177,1392,1176,1560};//1500
00215 int16_t maxpwm[6] = {1921,1895,1848,1728,1780,1891};
00216 int16_t minpwm[6] = {1249,1234,1177,1056,1176,1219};
00217 const int16_t reverce[4] = {Reverce_falfalla[0],Reverce_falfalla[1],Reverce_falfalla[2],Reverce_falfalla[3]};
00218 
00219 
00220 //2号機
00221 /*
00222 static int16_t trimpwm[6] = {1508,1559,1184,1664,1072,1952};
00223 int16_t maxpwm[6] = {1840,1884,1840,1992,1728,1952};
00224 int16_t minpwm[6] = {1183,1228,1176,1420,1420,1072};
00225 const int16_t reverce[4] = {Reverce_falfalla[0],Reverce_falfalla[1],Reverce_falfalla[2],Reverce_falfalla[3]};
00226 */
00227 
00228 int16_t oldTHR = 1000;
00229 
00230 int16_t g_AIL_L_Ratio_rightloop = 0.5;
00231 
00232 
00233 static float nowAngle[3] = {0,0,0};
00234 const float trimAngle[3] = {0.0, 0.0, 0.0};
00235 const float maxAngle[2] = {90, 90};
00236 const float minAngle[2] = {-90, -90};
00237 
00238 float FirstROLL = 0.0, FirstPITCH = 0.0,FirstYAW = 0.0;
00239 
00240 unsigned int g_distance;
00241 Ticker USsensor;
00242 static char g_buf[16];
00243 char g_landingcommand='T';
00244 float g_SerialTargetYAW;
00245 
00246 
00247 Timer t;
00248 Timer t2;
00249 Timeout RerurnChickenServo1;
00250 Timeout RerurnChickenServo2;
00251 
00252 /*-----関数のプロトタイプ宣言-----*/
00253 void setup();
00254 void loop();
00255 
00256 void Init_PWM();
00257 void Init_servo();              //サーボ初期化
00258 void Init_sbus();               //SBUS初期化
00259 void Init_sensors();
00260 void DisplayClock();            //クロック状態確認
00261 void SetupBMP280();
00262 void Set_trim();
00263 
00264 //センサの値取得
00265 void SensingMPU();
00266 void UpdateDist();
00267 void sensing();
00268 void getBMP280();
00269 
00270 //void offsetRollPitch(float FirstROLL, float FirstPITCH);
00271 //void TransYaw(float FirstYAW);
00272 float TranslateNewYaw(float beforeYaw,  float newzeroYaw);
00273 void UpdateTargetAngle(float targetAngle[3]);
00274 void CalculateControlValue(float targetAngle[3], float controlValue[3]);
00275 void UpdateAutoPWM(float controlValue[3]);
00276 void ConvertPWMintoRAD(float targetAngle[3]);
00277 inline float CalcRatio(float value, float trim, float limit);
00278 bool CheckSW_Up(Channel ch);
00279 int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min);
00280 inline int16_t SetTHRinRatio(float ratio);
00281 void Ascending(float targetAngle[3]);
00282 
00283 //sbus割り込み
00284 void Update_PWM();              //マニュアル・自動モードのpwmデータを整形しpwm変数に入力
00285 void Output_PWM(int16_t pwm[6]);    //pwmをサーボへ出力
00286 
00287 //シリアル割り込み
00288 void getSF_Serial();
00289 float ConvertByteintoFloat(char high, char low);
00290 
00291 bool g_Finflag = false;
00292 
00293 
00294 //SD設定
00295 int GetParameter(FILE *fp, const char *paramName,char parameter[]);
00296 int SetOptions(float *g_kpELE, float *g_kiELE, float *g_kdELE,
00297                float *g_kpRUD, float *g_kiRUD, float *g_kdRUD,
00298                float *g_rightloopROLL, float *g_rightloopPITCH,
00299                float *g_leftloopROLL, float *g_leftloopPITCH,
00300                float *g_gostraightROLL, float *g_gostraightPITCH,
00301                float *g_takeoffTHR, float *g_loopTHR,
00302                float *g_rightloopROLLshort, float *g_rightloopPITCHshort,
00303                float *g_leftloopROLLshort, float *g_leftloopPITCHshort,
00304                float *g_glideloopROLL, float *g_glideloopPITCH,
00305                float *g_kpAIL, float *g_kiAIL, float *g_kdAIL,
00306                int *g_rightloopRUD, int *g_rightloopshortRUD,
00307                int *g_leftloopRUD, int *g_leftloopshortRUD,
00308                int *g_glideRUD,
00309                float *g_rightloopROLL_approach, float *g_leftloopROLL_approach,
00310                int *g_rightloopRUD_approach, int *g_leftloopRUD_approach,
00311                float *g_rightloopPITCH_approach, float *g_leftloopPITCH_approach,
00312                float *g_ascendingPITCH,
00313                int *g_take_off_THROTTLE, float *g_take_off_PITCH,
00314                float *g_rightloop_THR_rate, float *g_leftloop_THR_rate,
00315                float *g_ascendding_thr_rate, 
00316                float *g_ascending_ROLL, int *g_ascending_RUD,
00317                int *g_trim1, int *g_trim2, int *g_trim3,
00318                int *g_trim4, int *g_trim5, int *g_trim6,
00319                bool *g_trim_check
00320               );
00321 //switch2割り込み
00322 void ResetTrim();
00323 
00324 //自動操縦
00325 void UpdateTargetAngle_GoStraight(float targetAngle[3]);
00326 void UpdateTargetAngle_GoStraight_zero(float targetAngle[3]);       //着陸時にスロットルが０の時の直進
00327 void UpdateTargetAngle_Rightloop(float targetAngle[3]);
00328 void UpdateTargetAngle_Rightloop_short(float targetAngle[3]);
00329 void UpdateTargetAngle_Rightloop_zero(float targetAngle[3]);      //着陸時にスロットルが０の時の右旋回
00330 void UpdateTargetAngle_Rightloop_UP(float targetAngle[3]);
00331 void UpdateTargetAngle_Leftloop(float targetAngle[3]);
00332 void UpdateTargetAngle_Leftloop_short(float targetAngle[3]);
00333 void UpdateTargetAngle_Leftloop_zero(float targetAngle[3]);     //着陸時にスロットルが０の時の左旋回
00334 void UpdateTargetAngle_Moebius(float targetAngle[3]);
00335 void UpdateTargetAngle_Takeoff(float targetAngle[3]);
00336 void UpdateTargetAngle_Approach(float targetAngle[3]);
00337 void Take_off_and_landing(float targetAngle[3]);
00338 
00339 
00340 
00341 int Rotate(float targetAngle[3], float TargetYAW);
00342 int Rotate_zero(float targetAngle[3], float TargetYAW);
00343 
00344 //投下
00345 void Chicken_Drop();
00346 void ReturnChickenServo1();
00347 void ReturnChickenServo2();
00348 
00349 //超音波による高度補正
00350 void checkHeight(float targetAngle[3]);
00351 void UpdateTargetAngle_NoseUP(float targetAngle[3]);
00352 void UpdateTargetAngle_NoseDOWN(float targetAngle[3]);
00353 
00354 //デバッグ用
00355 void Sbusprintf();
00356 void DebugPrint();
00357 
00358 /*---関数のプロトタイプ宣言終わり---*/
00359 
00360 int main()
00361 {
00362     setup();
00363 
00364 
00365     while(1) {
00366 
00367         loop();
00368 
00369 
00370         NVIC_DisableIRQ(USART1_IRQn);
00371         if(!CheckSW_Up(Ch7)) {
00372             led3=0;
00373 
00374         } else {
00375             led3=1;
00376         }
00377         NVIC_EnableIRQ(USART1_IRQn);
00378     }
00379 
00380 }
00381 
00382 void setup()
00383 {
00384     //buzzer = 0;
00385     led1 = 1;
00386     led2 = 1;
00387     led3 = 1;
00388     led4 = 1;
00389 
00390     SetOptions(&g_kpELE, &g_kiELE, &g_kdELE,
00391                &g_kpRUD, &g_kiRUD, &g_kdRUD,
00392                &g_rightloopROLL, &g_rightloopPITCH,
00393                &g_leftloopROLL, &g_leftloopPITCH,
00394                &g_gostraightROLL, &g_gostraightPITCH,
00395                &g_takeoffTHR, &g_loopTHR,
00396                &g_rightloopROLLshort, &g_rightloopPITCHshort,
00397                &g_leftloopROLLshort, &g_leftloopPITCHshort,
00398                &g_glideloopROLL, &g_glideloopPITCH,
00399                &g_kpAIL, &g_kiAIL,&g_kdAIL,
00400                &g_rightloopRUD, &g_rightloopshortRUD,
00401                &g_leftloopRUD, &g_leftloopshortRUD,
00402                &g_glideloopRUD,
00403                &g_rightloopROLL_approach,&g_leftloopROLL_approach,
00404                &g_rightloopRUD_approach,&g_leftloopRUD_approach,
00405                &g_rightloopPITCH_approach,&g_leftloopPITCH_approach,
00406                &g_ascendingPITCH,
00407                &g_take_off_THROTTLE, &g_take_off_PITCH,
00408                &g_rightloop_THR_rate, &g_leftloop_THR_rate,
00409                &g_ascending_THR_rate, 
00410                &g_ascending_ROLL, &g_ascending_RUD,
00411                &g_trim1, &g_trim2, &g_trim3,
00412                &g_trim4, &g_trim5, &g_trim6,
00413                &g_trim_check
00414               );
00415               
00416     Set_trim();
00417         
00418 
00419 
00420     Init_PWM();
00421     Init_servo();
00422     Init_sbus();
00423     Init_sensors();
00424     //switch2.rise(ResetTrim);
00425 
00426     USsensor.attach(&UpdateDist, 0.05);
00427 
00428     NVIC_SetPriority(USART1_IRQn,0);//プロポ
00429     NVIC_SetPriority(EXTI0_IRQn,1);//MPU割り込み禁止
00430     NVIC_SetPriority(TIM5_IRQn,2);
00431     NVIC_SetPriority(EXTI9_5_IRQn,3);
00432     NVIC_SetPriority(EXTI4_IRQn,5);
00433     DisplayClock();
00434     t.start();
00435     t.start();
00436 
00437 
00438     pc.printf("MPU calibration start\r\n");
00439 
00440     float offsetstart = t.read();
00441     while(t.read() - offsetstart < 26) {
00442         SensingMPU();
00443         for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
00444         pc.printf("\r\n");
00445         led1 = !led1;
00446         led2 = !led2;
00447         led3 = !led3;
00448         led4 = !led4;
00449     }
00450     //pc.attach(getSF_Serial, Serial::RxIrq);
00451     NVIC_SetPriority(USART2_IRQn,4);//シリアル
00452 
00453     FirstROLL = nowAngle[ROLL];
00454     FirstPITCH = nowAngle[PITCH];
00455     nowAngle[ROLL] -=FirstROLL;
00456     nowAngle[PITCH] -=FirstPITCH;
00457     
00458     SetupBMP280();
00459     
00460 //    NVIC_EnableIRQ(USART2_IRQn);
00461 //    
00462 //    if(pc.readable()) {    // 受信確認
00463 //        
00464 //        static char SFbuf[16]= {'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'}; 
00465 //        SFbuf[0] = pc.getc();    // 1文字取り出し
00466 //        if(SFbuf[0]=='S') {
00467 //            for(int i=0;i<3;i++){
00468 //                led1 = 1;
00469 //                led2 = 1;
00470 //                led3 = 1;
00471 //                led4 = 1;
00472 //                wait(0.2);
00473 //            }
00474 //        }
00475 //    }
00476 //    NVIC_DisableIRQ(USART2_IRQn);
00477 
00478 
00479     led1 = 0;
00480     led2 = 0;
00481     led3 = 0;
00482     led4 = 0;
00483     wait(0.2);
00484 
00485 
00486     pc.printf("All initialized\r\n");
00487 }
00488 
00489 void Set_trim(){
00490     trimpwm[0] = g_trim1;
00491     trimpwm[1] = g_trim2;
00492     trimpwm[2] = g_trim3;
00493     trimpwm[3] = g_trim4;
00494     trimpwm[4] = g_trim5;
00495     trimpwm[5] = g_trim6;
00496     
00497     maxpwm[0] = 1920;//trimpwm[0] + 330;1768
00498     maxpwm[1] = 1909;//trimpwm[1] + 330;1964
00499     maxpwm[2] = 1833;//trimpwm[2] + 660;1848
00500     maxpwm[3] = 1722;//trimpwm[3] + 330;1825
00501     maxpwm[4] = 1836;//trimpwm[4] + 660;1914
00502     maxpwm[5] = 1890;//trimpwm[5] + 330;1816
00503     
00504     minpwm[0]= 1260;//trimpwm[0] - 330;1186
00505     minpwm[1]= 1249;//trimpwm[1] - 330;1314
00506     minpwm[2]= trimpwm[2] - 0;
00507     minpwm[3]= 1062;//trimpwm[3] - 330;1154
00508     minpwm[4]= 1176;//trimpwm[4] - 0;1344
00509     minpwm[5]= 846;//trimpwm[5] - 330;1144
00510 }
00511 
00512 void loop()
00513 {
00514     static float targetAngle[3] = {0.0, 0.0, 0.0}, controlValue[2] = {0.0, 0.0};
00515     SensingMPU();
00516     UpdateTargetAngle(targetAngle);
00517     CalculateControlValue(targetAngle, controlValue);
00518     UpdateAutoPWM(controlValue);
00519     
00520 
00521     //NVIC_SetPriority(TIM5_IRQn,4);
00522     //NVIC_SetPriority(USART2_IRQn,2);
00523 
00524     //wait_ms(23);
00525     wait_ms(30);
00526 
00527     //NVIC_SetPriority(TIM5_IRQn,2);
00528     //NVIC_SetPriority(USART2_IRQn,4);
00529 
00530 
00531     // pc.printf("6\r\n");
00532     //NVIC_DisableIRQ(USART2_IRQn);
00533     //pc.printf("%c",g_landingcommand);
00534     //NVIC_EnableIRQ(USART2_IRQn);
00535 #if DEBUG_PRINT_INLOOP
00536     //Sbusprintf();
00537     DebugPrint();
00538 #endif
00539 }
00540 
00541 //サーボ初期化関数
00542 void Init_servo()
00543 {
00544     servo1.period_ms(16);
00545     servo1.pulsewidth_us(trimpwm[AIL_R]);
00546 
00547     servo2.period_ms(16);
00548     servo2.pulsewidth_us(trimpwm[ELE]);
00549 
00550     servo3.period_ms(16);
00551     servo3.pulsewidth_us(trimpwm[THR]);
00552 
00553     servo4.period_ms(16);
00554     servo4.pulsewidth_us(trimpwm[RUD]);
00555 
00556     servo5.period_ms(16);
00557     servo5.pulsewidth_us(trimpwm[DROP]);
00558 
00559     servo6.period_ms(16);
00560     servo6.pulsewidth_us(trimpwm[AIL_L]);
00561     
00562     int i = 0;
00563     
00564     for(i = 0;i<6;i++ ){
00565         autopwm[i]=trimpwm[i];
00566         }
00567 
00568     pc.printf("servo initialized\r\n");
00569 }
00570 
00571 //Sbus初期化
00572 void Init_sbus()
00573 {
00574     sbus.initialize();
00575     sbus.setLastfuncPoint(Update_PWM);
00576     sbus.startInterrupt();
00577 }
00578 
00579 void Init_sensors()
00580 {
00581     if(mpu6050.setup() == -1) {
00582         pc.printf("failed initialize\r\n");
00583         while(1) {
00584             led1 = 1;
00585             led2 = 0;
00586             led3 = 1;
00587             led4 = 0;
00588             wait(1);
00589             led1 = 0;
00590             led2 = 1;
00591             led3 = 0;
00592             led4 = 1;
00593             wait(1);
00594         }
00595     }
00596 }
00597 
00598 void Init_PWM()
00599 {
00600     pc.printf("PWM initialized\r\n");
00601 }
00602 
00603 void DisplayClock()
00604 {
00605     pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000);
00606     pc.printf("HCLK Clock   = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000);
00607     pc.printf("PCLK1 Clock  = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000);
00608     pc.printf("PCLK2 Clock  = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000);
00609     pc.printf("\r\n");
00610 }
00611 
00612 void UpdateTargetAngle(float targetAngle[3])
00613 {
00614 
00615 
00616     static int16_t count_op = 0;
00617 #if DEBUG_SEMIAUTO
00618     switch(operation_mode) {
00619         case StartUp:
00620             if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)) {
00621                 count_op++;B
00622                 if(count_op > changeModeCount) {
00623                     operation_mode = SemiAuto;
00624                     pc.printf("Goto SemiAuto mode\r\n");
00625                     count_op = 0;
00626                 }
00627             } else count_op = 0;
00628             break;
00629 
00630         case SemiAuto:
00631             /*  大会用では以下のif文を入れてoperation_modeを変える
00632                 if(CheckSW_Up(Ch6)){
00633                     count_op++;
00634                     if(count_op>changeModeCount){
00635                         output_status = XXX;
00636                         led2 = 0;
00637                         pc.printf("Goto XXX mode\r\n");
00638                         count_op = 0;
00639                     }else count_op = 0;
00640                     ConvertPWMintoRAD(targetAngle);
00641                 }
00642             */
00643             ConvertPWMintoRAD(targetAngle);
00644             break;
00645 
00646         default:
00647             operation_mode = SemiAuto;
00648             break;
00649     }
00650 
00651 #else
00652 
00653     switch(operation_mode) {
00654         case StartUp:
00655             if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)) {       //ch7;自動・手動切り替え　ch8;自動操縦モード切替
00656                 count_op++;
00657                 if(count_op > changeModeCount) {
00658                     operation_mode = LeftLoop;
00659                     pc.printf("Goto LeftLoop mode\r\n");
00660                     count_op = 0;
00661                 }
00662             } else count_op = 0;
00663             break;
00664 
00665         case LeftLoop:
00666             if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)) {
00667                 count_op++;
00668                 if(count_op > changeModeCount) {
00669                     operation_mode = GoStraight;
00670                     pc.printf("Goto GoStraight mode\r\n");
00671                     count_op = 0;
00672                 }
00673             } else count_op = 0;
00674             UpdateTargetAngle_Leftloop(targetAngle);
00675             //UpdateTargetAngle_GoStraight_zero(targetAngle);
00676             break;
00677 
00678         case GoStraight:
00679             if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)) {
00680                 count_op++;
00681                 if(count_op > changeModeCount) {
00682                     operation_mode = Moebius;
00683                     pc.printf("Goto Moebius mode\r\n");
00684                     count_op = 0;
00685                 }
00686             } else count_op = 0;
00687             //UpdateTargetAngle_GoStraight(targetAngle);
00688             UpdateTargetAngle_GoStraight_zero(targetAngle);
00689             //UpdateTargetAngle_Leftloop_short(targetAngle);
00690             //UpdateTargetAngle_Leftloop_zero(targetAngle);
00691         
00692             
00693             break;
00694 
00695         case Moebius:
00696             if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)) {
00697                 count_op++;
00698                 if(count_op > changeModeCount) {
00699                     operation_mode = Glide;
00700                     pc.printf("Goto Glide mode\r\n");
00701                     count_op = 0;
00702                 }
00703             } else count_op = 0;
00704             UpdateTargetAngle_Moebius(targetAngle);
00705             break;
00706 
00707         case Glide:
00708             if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)) {
00709                 count_op++;
00710                 if(count_op > changeModeCount) {
00711                     operation_mode = RightLoop;
00712                     pc.printf("Goto RightLoop mode\r\n");
00713                     count_op = 0;
00714                 }
00715             } else count_op = 0;
00716             //UpdateTargetAngle_Rightloop_short(targetAngle);
00717             //UpdateTargetAngle_Rightloop_zero(targetAngle);
00718             UpdateTargetAngle_GoStraight_zero(targetAngle);
00719             
00720             break;
00721 
00722         case RightLoop:
00723             if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)) {
00724                 count_op++;
00725                 if(count_op > changeModeCount) {
00726                     operation_mode = BombwithPC;
00727                     pc.attach(getSF_Serial, Serial::RxIrq);
00728                     autopwm[DROP]=trimpwm[DROP];
00729                     pc.printf("Goto BombwithPC mode\r\n");
00730                     //g_AscendingFlag = false;
00731                     count_op = 0;
00732                 }
00733             } else count_op = 0;
00734             Ascending(targetAngle);
00735             break;
00736 
00737         case BombwithPC:
00738             if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)) {
00739                 count_op++;
00740                 if(count_op > changeModeCount) {
00741                     operation_mode = LeftLoop;
00742                     pc.printf("Goto Left mode\r\n");
00743                     pc.attach(getSF_Serial, Serial::RxIrq);
00744                     count_op = 0;
00745                 }
00746             } else count_op = 0;
00747             Take_off_and_landing(targetAngle);
00748             break;
00749 
00750         default:
00751             operation_mode = StartUp;
00752             break;
00753     }
00754 #endif
00755 
00756     if(CheckSW_Up(Ch7)) {
00757         output_status = Auto;
00758         led1 = 1;
00759     } else {
00760         output_status = Manual;
00761         led1 = 0;
00762     }
00763 
00764 
00765 }
00766 
00767 int GetParameter(FILE *fp, const char *paramName,char parameter[])
00768 {
00769     int i=0, j=0;
00770     int strmax = 200;
00771     char str[strmax];
00772 
00773     rewind(fp); //ファイル位置を先頭に
00774     while(1) {
00775         if (fgets(str, strmax, fp) == NULL) {
00776             return 0;
00777         }
00778         if (!strncmp(str, paramName, strlen(paramName))) {
00779             while (str[i++] != '=') {}
00780             while (str[i] != '\n') {
00781                 parameter[j++] = str[i++];
00782             }
00783             parameter[j] = '\0';
00784             return 1;
00785         }
00786     }
00787 }
00788 
00789 
00790 //sdによる設定
00791 int SetOptions(float *g_kpELE, float *g_kiELE, float *g_kdELE,
00792                float *g_kpRUD, float *g_kiRUD, float *g_kdRUD,
00793                float *g_rightloopROLL, float *g_rightloopPITCH,
00794                float *g_leftloopROLL, float *g_leftloopPITCH,
00795                float *g_gostraightROLL, float *g_gostraightPITCH,
00796                float *g_takeoffTHR, float *g_loopTHR,
00797                float *g_rightloopROLLshort, float *g_rightloopPITCHshort,
00798                float *g_leftloopROLLshort, float *g_leftloopPITCHshort,
00799                float *g_glideloopROLL, float *g_glideloopPITCH,
00800                float *g_kpAIL, float *g_kiAIL, float *g_kdAIL,
00801                int *g_rightloopRUD, int *g_rightloopshortRUD,
00802                int *g_leftloopRUD, int *g_leftloopshortRUD,
00803                int *g_glideloopRUD,
00804                float *g_rightloopROLL_approach,float *g_leftloopROLL_approach,
00805                int *g_rightloopRUD_approach,int *g_leftloopRUD_approach,
00806                float *g_rightloopPITCH_approach,float *g_leftloopPITCH_approach,
00807                float *g_ascendingPITCH,
00808                int *g_take_off_THROTTLE, float *g_take_off_PITCH,
00809                float *g_rightloop_THR_rate, float *g_leftloop_THR_rate,
00810                float *g_ascending_THR_rate, 
00811                float *g_ascending_ROLL, int *g_ascending_RUD,
00812                int *g_trim1, int *g_trim2, int *g_trim3,
00813                int *g_trim4, int *g_trim5, int *g_trim6,
00814                bool *g_trim_check
00815               )
00816 {
00817 
00818     pc.printf("SDsetup start.\r\n");
00819 
00820     FILE *fp;
00821     char parameter[50]; //文字列渡す用の配列
00822     int SDerrorcount = 0;  //取得できなかった数を返す
00823     const char *paramNames[] = {
00824         "KP_ELEVATOR",
00825         "KI_ELEVATOR",
00826         "KD_ELEVATOR",
00827         "KP_RUDDER",
00828         "KI_RUDDER",
00829         "KD_RUDDER",
00830         "RIGHTLOOP_ROLL",
00831         "RIGHTLOOP_PITCH",
00832         "LEFTLOOP_ROLL",
00833         "LEFTLOOP_PITCH",
00834         "GOSTRAIGHT_ROLL",
00835         "GOSTRAIGHT_PITCH",
00836         "TAKEOFF_THR_RATE",
00837         "LOOP_THR_RATE",
00838         "RIGHTLOOP_ROLL_SHORT",
00839         "RIGHTLOOP_PITCH_SHORT",
00840         "LEFTLOOP_ROLL_SHORT",
00841         "LEFTLOOP_PITCH_SHORT",
00842         "AUTOGLIDE_ROLL",
00843         "AUTOGLIDE PITCH",
00844         "KP_AILERON",
00845         "KI_AILERON",
00846         "KD_AILERON",
00847         "RIGHTLOOP_RUDDER",
00848         "RIGHTLOOPSHORT_RUDDER",
00849         "LEFTLOOP_RUDDER",
00850         "LEFTLOOPSHORT_RUDDER",
00851         "GLIDELOOP_RUDDER",
00852         "RIGHTLOOP_ROLL_APPROACH",
00853         "LEFTLOOP_ROLL_APPROACH",
00854         "RIGHTLOOP_RUDDER_APPROACH",
00855         "LEFTLOOP_RUDDER_APPROACH",
00856         "RIGHTLOOP_PITCH_APPROACH",
00857         "LEFTLOOP_PITCH_APPROACH",
00858         "ASCENDING_PITCH",
00859         "TAKE_OFF_THROTTLE",
00860         "TAKE_OFF_PITCH",
00861         "RIGHTLOOP_THR_RATE",
00862         "LEFTLOOP_THR_RATE",
00863         "ASCENDING_THR_RATE",
00864         "ASCENDING_ROLL",
00865         "ASCENDING_RUD",
00866         "TRIM1",
00867         "TRIM2",
00868         "TRIM3",
00869         "TRIM4",
00870         "TRIM5",
00871         "TRIM6",
00872         "TRIM_CHECK"
00873     };
00874 
00875     fp = fopen("/sd/option.txt","r");
00876 
00877     if(fp != NULL) { //開けたら
00878         pc.printf("File was openned.\r\n");
00879         if(GetParameter(fp,paramNames[0],parameter)) *g_kpELE = atof(parameter);
00880         else {
00881             *g_kpELE = KP_ELE;
00882             SDerrorcount++;
00883         }
00884         if(GetParameter(fp,paramNames[1],parameter)) *g_kiELE = atof(parameter);
00885         else {
00886             *g_kiELE = KI_ELE;
00887             SDerrorcount++;
00888         }
00889         if(GetParameter(fp,paramNames[2],parameter)) *g_kdELE = atof(parameter);
00890         else {
00891             *g_kdELE = KD_ELE;
00892             SDerrorcount++;
00893         }
00894         if(GetParameter(fp,paramNames[3],parameter)) *g_kpRUD = atof(parameter);
00895         else {
00896             *g_kpRUD = KP_RUD;
00897             SDerrorcount++;
00898         }
00899         if(GetParameter(fp,paramNames[4],parameter)) *g_kiRUD = atof(parameter);
00900         else {
00901             *g_kiRUD = KI_RUD;
00902             SDerrorcount++;
00903         }
00904         if(GetParameter(fp,paramNames[5],parameter)) *g_kdRUD = atof(parameter);
00905         else {
00906             *g_kdRUD = KD_RUD;
00907             SDerrorcount++;
00908         }
00909         if(GetParameter(fp,paramNames[6],parameter)) *g_rightloopROLL = atof(parameter);
00910         else {
00911             *g_rightloopROLL = RIGHT_ROLL;
00912             SDerrorcount++;
00913         }
00914         if(GetParameter(fp,paramNames[7],parameter)) *g_rightloopPITCH = atof(parameter);
00915         else {
00916             *g_rightloopPITCH = RIGHT_PITCH;
00917             SDerrorcount++;
00918         }
00919         if(GetParameter(fp,paramNames[8],parameter)) *g_leftloopROLL = atof(parameter);
00920         else {
00921             *g_leftloopROLL = LEFT_ROLL;
00922             SDerrorcount++;
00923         }
00924         if(GetParameter(fp,paramNames[9],parameter)) *g_leftloopPITCH = atof(parameter);
00925         else {
00926             *g_leftloopPITCH = LEFT_PITCH;
00927             SDerrorcount++;
00928         }
00929         if(GetParameter(fp,paramNames[10],parameter)) *g_gostraightROLL = atof(parameter);
00930         else {
00931             *g_gostraightROLL = STRAIGHT_ROLL;
00932             SDerrorcount++;
00933         }
00934         if(GetParameter(fp,paramNames[11],parameter)) *g_gostraightPITCH = atof(parameter);
00935         else {
00936             *g_gostraightPITCH = STRAIGHT_PITCH;
00937             SDerrorcount++;
00938         }
00939         if(GetParameter(fp,paramNames[12],parameter)) *g_takeoffTHR = atof(parameter);
00940         else {
00941             *g_takeoffTHR = TAKEOFF_THR;
00942             SDerrorcount++;
00943         }
00944         if(GetParameter(fp,paramNames[13],parameter)) *g_loopTHR = atof(parameter);
00945         else {
00946             *g_loopTHR = LOOP_THR;
00947             SDerrorcount++;
00948         }
00949         if(GetParameter(fp,paramNames[14],parameter)) *g_rightloopROLLshort = atof(parameter);
00950         else {
00951             *g_rightloopROLLshort = RIGHT_ROLL_SHORT;
00952             SDerrorcount++;
00953         }
00954         if(GetParameter(fp,paramNames[15],parameter)) *g_rightloopPITCHshort = atof(parameter);
00955         else {
00956             *g_rightloopPITCHshort = RIGHT_PITCH_SHORT;
00957             SDerrorcount++;
00958         }
00959         if(GetParameter(fp,paramNames[16],parameter)) *g_leftloopROLLshort = atof(parameter);
00960         else {
00961             *g_leftloopROLLshort = LEFT_ROLL_SHORT;
00962             SDerrorcount++;
00963         }
00964         if(GetParameter(fp,paramNames[17],parameter)) *g_leftloopPITCHshort = atof(parameter);
00965         else {
00966             *g_leftloopPITCHshort = LEFT_PITCH_SHORT;
00967             SDerrorcount++;
00968         }
00969         if(GetParameter(fp,paramNames[18],parameter)) *g_glideloopROLL = atof(parameter);
00970         else {
00971             *g_glideloopROLL = GLIDE_ROLL;
00972             SDerrorcount++;
00973         }
00974         if(GetParameter(fp,paramNames[19],parameter)) *g_glideloopPITCH = atof(parameter);
00975         else {
00976             *g_glideloopPITCH = GLIDE_PITCH;
00977             SDerrorcount++;
00978         }
00979         if(GetParameter(fp,paramNames[20],parameter)) *g_kpAIL = atof(parameter);
00980         else {
00981             *g_kpAIL = KP_AIL;
00982             SDerrorcount++;
00983         }
00984         if(GetParameter(fp,paramNames[21],parameter)) *g_kiAIL = atof(parameter);
00985         else {
00986             *g_kiAIL = KI_AIL;
00987             SDerrorcount++;
00988         }
00989         if(GetParameter(fp,paramNames[22],parameter)) *g_kdAIL = atof(parameter);
00990         else {
00991             *g_kdAIL = KP_AIL;
00992             SDerrorcount++;
00993         }
00994         if(GetParameter(fp,paramNames[23],parameter)) *g_rightloopRUD = atof(parameter);
00995         else {
00996             *g_rightloopRUD = RIGHTLOOP_RUD;
00997             SDerrorcount++;
00998         }
00999         if(GetParameter(fp,paramNames[24],parameter)) *g_rightloopshortRUD = atof(parameter);
01000         else {
01001             *g_rightloopshortRUD = RIGHTLOOPSHORT_RUD;
01002             SDerrorcount++;
01003         }
01004         if(GetParameter(fp,paramNames[25],parameter)) *g_leftloopRUD = atof(parameter);
01005         else {
01006             *g_leftloopshortRUD = LEFTLOOP_RUD;
01007             SDerrorcount++;
01008         }
01009         if(GetParameter(fp,paramNames[26],parameter)) *g_leftloopshortRUD = atof(parameter);
01010         else {
01011             *g_leftloopshortRUD = LEFTLOOPSHORT_RUD;
01012             SDerrorcount++;
01013         }
01014         if(GetParameter(fp,paramNames[27],parameter)) *g_glideloopRUD = atof(parameter);
01015         else {
01016             *g_glideloopRUD = GLIDELOOP_RUD;
01017             SDerrorcount++;
01018         }
01019         if(GetParameter(fp,paramNames[28],parameter)) *g_rightloopROLL_approach = atof(parameter);
01020         else {
01021             *g_rightloopROLL_approach  = RIGHTLOOPROLL_APPROACH;
01022             SDerrorcount++;
01023         }
01024         if(GetParameter(fp,paramNames[29],parameter)) *g_leftloopROLL_approach= atof(parameter);
01025         else {
01026             *g_leftloopROLL_approach = LEFTLOOPROLL_APPROACH;
01027             SDerrorcount++;
01028         }
01029         if(GetParameter(fp,paramNames[30],parameter)) *g_rightloopRUD_approach = atof(parameter);
01030         else {
01031             *g_rightloopRUD_approach = RIGHTLOOPRUD_APPROACH;
01032             SDerrorcount++;
01033         }
01034         if(GetParameter(fp,paramNames[31],parameter)) *g_leftloopRUD_approach= atof(parameter);
01035         else {
01036             *g_leftloopRUD_approach= LEFTLOOPRUD_APPROACH;
01037             SDerrorcount++;
01038         }
01039         if(GetParameter(fp,paramNames[32],parameter)) *g_rightloopPITCH_approach = atof(parameter);
01040         else {
01041             *g_rightloopPITCH_approach = RIGHTLOOPPITCH_APPROACH;
01042             SDerrorcount++;
01043         }
01044         if(GetParameter(fp,paramNames[33],parameter)) *g_leftloopPITCH_approach = atof(parameter);
01045         else {
01046             *g_leftloopPITCH_approach = LEFTLOOPPITCH_APPROACH;
01047             SDerrorcount++;
01048         }
01049         if(GetParameter(fp,paramNames[34],parameter)) *g_ascendingPITCH = atof(parameter);
01050         else {
01051             *g_ascendingPITCH = ASCENDING_PITCH;
01052             SDerrorcount++;
01053         }
01054         if(GetParameter(fp,paramNames[35],parameter)) *g_take_off_THROTTLE = atof(parameter);
01055         else {
01056             *g_take_off_THROTTLE = TAKE_OFF_THROTTLE;
01057             SDerrorcount++;
01058         }
01059         if(GetParameter(fp,paramNames[36],parameter)) *g_take_off_PITCH = atof(parameter);
01060         else {
01061             *g_take_off_PITCH = TAKE_OFF_PITCH;
01062             SDerrorcount++;
01063         }
01064         if(GetParameter(fp,paramNames[37],parameter)) *g_rightloop_THR_rate = atof(parameter);
01065         else {
01066             *g_rightloop_THR_rate = RIGHTLOOP_THR_RATE;
01067             SDerrorcount++;
01068         }
01069         if(GetParameter(fp,paramNames[38],parameter)) *g_leftloop_THR_rate = atof(parameter);
01070         else {
01071             *g_leftloop_THR_rate = LEFTLOOP_THR_RATE;
01072             SDerrorcount++;
01073         }
01074         if(GetParameter(fp,paramNames[39],parameter)) *g_ascending_THR_rate = atof(parameter);
01075         else {
01076             *g_ascending_THR_rate = ASCENDING_THR_RATE;
01077             SDerrorcount++;
01078         }
01079         if(GetParameter(fp,paramNames[40],parameter)) *g_ascending_ROLL = atof(parameter);
01080         else {
01081             *g_ascending_ROLL = ASCENDING_ROLL;
01082             SDerrorcount++;
01083         }
01084         if(GetParameter(fp,paramNames[41],parameter)) *g_ascending_RUD= atof(parameter);
01085         else {
01086             *g_ascending_RUD = ASCENDING_RUD;
01087             SDerrorcount++;
01088         }
01089         if(GetParameter(fp,paramNames[42],parameter)) *g_trim1 = atof(parameter);
01090         else {
01091             *g_trim1 = TRIM1;
01092             SDerrorcount++;
01093         }
01094         if(GetParameter(fp,paramNames[43],parameter)) *g_trim2 = atof(parameter);
01095         else {
01096             *g_trim2 = TRIM2;
01097             SDerrorcount++;
01098         }
01099         if(GetParameter(fp,paramNames[44],parameter)) *g_trim3 = atof(parameter);
01100         else {
01101             *g_trim3 = TRIM3;
01102             SDerrorcount++;
01103         }
01104         if(GetParameter(fp,paramNames[45],parameter)) *g_trim4 = atof(parameter);
01105         else {
01106             *g_trim4 = TRIM4;
01107             SDerrorcount++;
01108         }
01109         if(GetParameter(fp,paramNames[46],parameter)) *g_trim5 = atof(parameter);
01110         else {
01111             *g_trim5 = TRIM5;
01112             SDerrorcount++;
01113         }
01114         if(GetParameter(fp,paramNames[47],parameter)) *g_trim6 = atof(parameter);
01115         else {
01116             *g_trim6 = TRIM6;
01117             SDerrorcount++;
01118         }
01119         if(GetParameter(fp,paramNames[48],parameter)) *g_trim_check = atof(parameter);
01120         else {
01121             *g_trim_check = TRIM_CHECK;
01122             SDerrorcount++;
01123         }
01124 
01125 
01126         fclose(fp);
01127 
01128     } else { //ファイルがなかったら
01129         pc.printf("fp was null.\r\n");
01130         *g_kpELE = KP_ELE;
01131         *g_kiELE = KI_ELE;
01132         *g_kdELE = KD_ELE;
01133         *g_kpRUD = KP_RUD;
01134         *g_kiRUD = KI_RUD;
01135         *g_kdRUD = KD_RUD;
01136         *g_rightloopROLL = RIGHT_ROLL;
01137         *g_rightloopPITCH = RIGHT_PITCH;
01138         *g_leftloopROLL = LEFT_ROLL;
01139         *g_leftloopPITCH = LEFT_PITCH;
01140         *g_gostraightROLL = STRAIGHT_ROLL;
01141         *g_gostraightPITCH = STRAIGHT_PITCH;
01142         *g_takeoffTHR = TAKEOFF_THR;
01143         *g_loopTHR = LOOP_THR;
01144         *g_kpAIL = KP_AIL; //パラメータ変えるのお忘れなく！！
01145         *g_kiAIL = KI_AIL;
01146         *g_kdAIL = KD_AIL;
01147         *g_rightloopRUD = RIGHTLOOP_RUD;
01148         *g_rightloopshortRUD = RIGHTLOOPSHORT_RUD;
01149         *g_leftloopRUD = LEFTLOOP_RUD;
01150         *g_leftloopshortRUD = LEFTLOOPSHORT_RUD;
01151         *g_glideloopRUD = GLIDELOOP_RUD;
01152         *g_rightloopROLL_approach = RIGHTLOOPROLL_APPROACH;
01153         *g_leftloopROLL_approach = LEFTLOOPROLL_APPROACH;
01154         *g_rightloopRUD_approach = RIGHTLOOPRUD_APPROACH;
01155         *g_leftloopRUD_approach = LEFTLOOPRUD_APPROACH;
01156         *g_rightloopPITCH_approach = RIGHTLOOPPITCH_APPROACH;
01157         *g_leftloopPITCH_approach = LEFTLOOPPITCH_APPROACH;
01158         *g_ascendingPITCH = ASCENDING_PITCH;
01159         *g_take_off_THROTTLE = TAKE_OFF_THROTTLE;
01160         *g_take_off_PITCH = TAKE_OFF_PITCH;
01161         *g_rightloop_THR_rate = RIGHTLOOP_THR_RATE;
01162         *g_leftloop_THR_rate = LEFTLOOP_THR_RATE;
01163         *g_ascending_THR_rate = ASCENDING_THR_RATE;
01164         *g_ascending_ROLL = ASCENDING_ROLL;
01165         *g_ascending_RUD = ASCENDING_RUD; 
01166         *g_trim1 = TRIM1;
01167         *g_trim2 = TRIM2;
01168         *g_trim3 = TRIM3;
01169         *g_trim4 = TRIM4;
01170         *g_trim5 = TRIM5;
01171         *g_trim6 = TRIM6;
01172         *g_trim_check = TRIM_CHECK;
01173         
01174         SDerrorcount = -1;
01175     }
01176     pc.printf("SDsetup finished.\r\n");
01177     if(SDerrorcount == 0)  pc.printf("setting option is success\r\n");
01178     else if(SDerrorcount == -1) pc.printf("ERROR 1. cannot open option\r\n");
01179     else if(SDerrorcount > 0)  pc.printf("ERROR 2. reading parameter is failed[%d]\r\n",SDerrorcount);
01180 
01181     pc.printf("kpELE = %f, kiELE = %f, kdELE = %f\r\n", *g_kpELE, *g_kiELE, *g_kdELE);
01182     pc.printf("kpRUD = %f, kiRUD = %f, kdRUD = %f\r\n", *g_kpRUD, *g_kiRUD, *g_kdRUD);
01183     pc.printf("rightloopROLL = %f, rightloopPITCH = %f\r\n", *g_rightloopROLL, *g_rightloopPITCH);
01184     pc.printf("leftloopROLL = %f, g_leftloopPITCH =  %f\r\n", *g_leftloopROLL, *g_leftloopPITCH);
01185     pc.printf("gostraightROLL = %f, g_gostraightPITCH = %f\r\n", *g_gostraightROLL, *g_gostraightPITCH);
01186     pc.printf("g_takeoffTHR = %f, g_loopTHR = %f\r\n", *g_takeoffTHR, *g_loopTHR);
01187     pc.printf("rightloopROLLshort = %f, rightloopPITCHshort = %f\r\n", *g_rightloopROLLshort, *g_rightloopPITCHshort);
01188     pc.printf("leftloopROLLshort = %f, g_leftloopPITCHshort =  %f\r\n", *g_leftloopROLLshort, *g_leftloopPITCHshort);
01189     pc.printf("glideROLL = %f, glidePITCH = %f\r\n", *g_glideloopROLL, *g_glideloopPITCH);
01190     pc.printf("kpAIL = %f,kiAIL = %f,kdAIL = %f\r\n ",*g_kpAIL,*g_kiAIL,*g_kdAIL);
01191     pc.printf("RIGHTLOOPRUD = %d,RIGHTLOOPSHORTRUD = %d\r\n",*g_rightloopRUD,*g_rightloopshortRUD);
01192     pc.printf("LEFTTLOOPRUD = %d,LEFTLOOPSHORTRUD = %d\r\n",*g_leftloopRUD,*g_leftloopshortRUD);
01193     pc.printf("GLIDELOOPRUD = %d\r\n",*g_glideloopRUD);
01194     pc.printf("RIGHTLOOP_ROLL_APPROACH = %f, LEFTLOOP_ROLL_APPROACH= %f\r\n",*g_rightloopROLL_approach,*g_leftloopROLL_approach);
01195     pc.printf("RIGHTLOOP_RUD_APPROACH = %d, LEFTLOOP_RUD_APPROACH = %d\r\n",*g_rightloopRUD_approach,*g_leftloopRUD_approach);
01196     pc.printf("RIGHTLOOP_PITCH_APPROACH = %f, LEFTLOOP_PITCH_APPROACH = %f\r\n",*g_rightloopPITCH_approach,*g_leftloopPITCH_approach);
01197     pc.printf("ASCENDING_PITCH = %f\r\n",*g_ascendingPITCH);
01198     pc.printf("TAKE_OFF_THROTTLE = %d, TAKE_OFF_PITCH = %f\r\n",*g_take_off_THROTTLE,*g_take_off_PITCH);
01199     pc.printf("RIGHTLOOP_THR_RATE = %f, LEFTLOOP_THR_RATE = %f, ASCENDING_THR_RATE = %f\r\n", *g_rightloop_THR_rate, *g_leftloop_THR_rate, *g_ascending_THR_rate);
01200     pc.printf("ASCENDING_ROLL = %f,ASCENDING_RUD = %d\r\n",*g_ascending_ROLL,*g_ascending_RUD);
01201     pc.printf("TRIM1 = %d, TRIM2 = %d, TRIM3 = %d\r\n",*g_trim1,*g_trim2,*g_trim3);
01202     pc.printf("TRIM4 = %d, TRIM5 = %d, TRIM6 = %d\r\n",*g_trim4,*g_trim5,*g_trim6);
01203     pc.printf("TRIM_CHECK = %d\r\n",*g_trim_check);
01204 
01205     return SDerrorcount;
01206 }
01207 
01208 void CalculateControlValue(float targetAngle[3], float controlValue[3])
01209 {
01210 
01211     static int t_last;
01212     int t_now;
01213     float dt;
01214 
01215     t_now = t.read_us();
01216     dt = (float)((t_now - t_last)/1000000.0f) ;
01217     t_last = t_now;
01218 
01219 
01220     //controlValue[ROLL] = pid_RUD.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt);
01221     controlValue[ROLL] = pid_AIL.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt);       //エルロンでロール制御
01222     controlValue[PITCH] = pid_ELE.calcPID(nowAngle[PITCH], targetAngle[PITCH], dt);
01223 
01224 }
01225 
01226 void UpdateAutoPWM(float controlValue[3])
01227 {
01228     NVIC_DisableIRQ(USART1_IRQn);
01229     int16_t addpwm[2]; //-500~500
01230     addpwm[PITCH] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[PITCH];    //センサ：機首下げ正    レバー:機首上げ正
01231     addpwm[ROLL] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[ROLL];           //センサ：右回転正（8月13日時点；左回転が正！）     レバー:右回転正
01232 
01233     autopwm[ELE] = trimpwm[ELE] + reverce[ELE] * addpwm[PITCH];                 //rewrite
01234     autopwm[AIL_R] = trimpwm[AIL_R] - reverce[AIL_R] * addpwm[ROLL];
01235     //autopwm[THR] = oldTHR;
01236 
01237     autopwm[ELE] = ThresholdMaxMin(autopwm[ELE], maxpwm[ELE], minpwm[ELE]);
01238     autopwm[AIL_R] = ThresholdMaxMin(autopwm[AIL_R], maxpwm[AIL_R], minpwm[AIL_R]);
01239 
01240     NVIC_EnableIRQ(USART1_IRQn);
01241 }
01242 
01243 inline float CalcRatio(float value, float trim, float limit)
01244 {
01245     return  (value - trim) / (limit - trim);
01246 }
01247 
01248 bool CheckSW_Up(Channel ch)
01249 {
01250 
01251     if(SWITCH_CHECK < sbus.manualpwm[ch]) {
01252         return true;
01253     } else {
01254         return false;
01255     }
01256 
01257 }
01258 
01259 int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min)
01260 {
01261     if(value > max) return max;
01262     if(value < min) return min;
01263     return value;
01264 }
01265 
01266 inline int16_t SetTHRinRatio(float ratio)
01267 {
01268     return minpwm[THR] + (int16_t)(2 * lengthdivpwm * ratio);
01269 }
01270 
01271 
01272 
01273 /*---SBUS割り込み処理---*/
01274 
01275 //udate_Inputで抽出したpwmデータを整形して各変数に代入する。(マニュアルモード)
01276 //各stabiGylo関数で算出したpwmを各変数に代入する(自動モード)
01277 void Update_PWM()
01278 {
01279     NVIC_DisableIRQ(USART1_IRQn);
01280     static int16_t pwm[6];
01281     static int16_t temppwm[6]= {trimpwm[0],trimpwm[1],trimpwm[2],trimpwm[3],trimpwm[4],trimpwm[5]};
01282     static int16_t FailsafeCounter=0;
01283     static int16_t ResetCounter=0;
01284     static int16_t OKCounter=0;
01285 
01286     if(sbus.flg_ch_update == true) {
01287 
01288         switch(output_status) {   //マニュアルモード,自動モード,自動着陸もモードを切替
01289             case Manual:
01290                 if(OKCounter!=0) break;
01291                 for(uint8_t i=0; i<6; i++) {
01292                     pwm[i] = sbus.manualpwm[i];
01293                 }
01294                 oldTHR = sbus.manualpwm[THR];
01295                 //pc.printf("update_manual\r\n");
01296                 NVIC_EnableIRQ(USART1_IRQn);
01297                 break;
01298 
01299             case Auto:
01300                 if(OKCounter!=0) break;
01301                 pwm[AIL_R] = autopwm[AIL_R];               //sbus.manualpwm[AIL];
01302                 pwm[ELE] = autopwm[ELE];
01303                 pwm[THR] = autopwm[THR];
01304                 pwm[RUD] = autopwm[RUD];
01305                 pwm[DROP] = autopwm[DROP];
01306                 pwm[AIL_L] = autopwm[AIL_L];
01307 
01308                 NVIC_EnableIRQ(USART1_IRQn);
01309                 break;
01310 
01311             default:
01312                 if(OKCounter!=0) break;
01313                 for(uint8_t i=0; i<6; i++) {
01314                     pwm[i] = sbus.manualpwm[i];
01315                 } //pc.printf("update_manual\r\n");
01316                 NVIC_EnableIRQ(USART1_IRQn);
01317                 break;
01318         }
01319 
01320         for(uint8_t i=0; i<6; i++) {
01321             if(pwm[i]<1000 || pwm[i]>2000) pwm[i]=temppwm[i];
01322             temppwm[i]=pwm[i];
01323         }
01324 
01325     }
01326     //else(sbus.flg_ch_update == false) pc.printf("0\r\n");
01327     /* if(sbus.failsafe_status==SBUS_SIGNAL_OK){
01328         pc.printf("OK\r\n");
01329     }
01330     */
01331     //pc.printf("%d\r\n",sbus.failsafe_status);
01332 
01333     if(sbus.failsafe_status==SBUS_SIGNAL_LOST||sbus.failsafe_status==SBUS_SIGNAL_FAILSAFE) FailsafeCounter++;
01334     else ResetCounter++;
01335 
01336     if(ResetCounter>7) {
01337         ResetCounter=0;
01338         FailsafeCounter=0;
01339     }
01340 
01341     if(FailsafeCounter>10) {
01342         ResetCounter=0;
01343         for(uint8_t i=0; i<6; i++) pwm[i] = trimpwm[i];
01344 
01345         if(sbus.failsafe_status==SBUS_SIGNAL_OK&&sbus.flg_ch_update == true) OKCounter++;
01346         else OKCounter=0;
01347 
01348         if(OKCounter>10) {
01349             OKCounter=0;
01350             FailsafeCounter=0;
01351         }
01352         //pc.printf("OKCounter=%d,  FailsafeCounter=%d,  sbus.failsafe_status=%d\r\n",OKCounter,FailsafeCounter,sbus.failsafe_status);
01353     }
01354     //if(sbus.failsafe_status==SBUS_SIGNAL_OK){FailsafeCounter=0;}
01355 
01356 
01357     sbus.flg_ch_update = false;
01358     Output_PWM(pwm);
01359 }
01360 
01361 
01362 
01363 
01364 //pwmをサーボに出力。
01365 void Output_PWM(int16_t pwm[5])
01366 {
01367     NVIC_DisableIRQ(USART1_IRQn);
01368     servo1.pulsewidth_us(pwm[0]);
01369     servo2.pulsewidth_us(pwm[1]);
01370     servo3.pulsewidth_us(pwm[2]);
01371     servo4.pulsewidth_us(pwm[3]);
01372     servo5.pulsewidth_us(pwm[4]);
01373     servo6.pulsewidth_us(pwm[5]);
01374     NVIC_EnableIRQ(USART1_IRQn);
01375 
01376 }
01377 
01378 void ResetTrim()
01379 {
01380     for(uint8_t i=0; i<6; i++) {           //i=4から書き換え＿投下サーボは入ってない模様
01381         trimpwm[i] = sbus.manualpwm[i];
01382     }
01383     pc.printf("reset PWM trim\r\n");
01384 }
01385 
01386 
01387 void SensingMPU()
01388 {
01389     //static int16_t deltaT = 0, t_start = 0;
01390     //t_start = t.read_us();
01391 
01392     float rpy[3] = {0}, oldrpy[3] = {0};
01393     static uint16_t count_changeRPY = 0;
01394     static bool flg_checkoutlier = false;
01395     NVIC_DisableIRQ(USART1_IRQn);
01396     NVIC_DisableIRQ(USART2_IRQn);
01397     NVIC_DisableIRQ(TIM5_IRQn);
01398     NVIC_DisableIRQ(EXTI0_IRQn);//MPU割り込み禁止
01399     NVIC_DisableIRQ(EXTI9_5_IRQn);//超音波割り込み禁止
01400 
01401     mpu6050.getRollPitchYaw_Skipper(rpy);
01402 
01403     NVIC_EnableIRQ(USART1_IRQn);
01404     NVIC_EnableIRQ(USART2_IRQn);
01405     NVIC_EnableIRQ(TIM5_IRQn);
01406     NVIC_EnableIRQ(EXTI0_IRQn);
01407     NVIC_EnableIRQ(EXTI9_5_IRQn);
01408 
01409 
01410     //外れ値対策
01411     for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI;
01412     rpy[ROLL] -= FirstROLL;
01413     rpy[PITCH] -= FirstPITCH;
01414     rpy[YAW] -= FirstYAW;
01415 
01416     for(uint8_t i=0; i<3; i++) {
01417         if(rpy[i] < nowAngle[i]-10 || rpy[i] > nowAngle[i]+10) {
01418             flg_checkoutlier = true;
01419         }
01420     }
01421     if(!flg_checkoutlier || count_changeRPY >= 2) {
01422         for(uint8_t i=0; i<3; i++) {
01423             nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f;  //2つの移動平均
01424         }
01425         count_changeRPY = 0;
01426     } else   count_changeRPY++;
01427     flg_checkoutlier = false;
01428 
01429 }
01430 
01431 float TranslateNewYaw(float beforeYaw,  float newzeroYaw)
01432 {
01433     float newYaw = beforeYaw - newzeroYaw;
01434 
01435     if(newYaw<-180.0f) newYaw += 360.0f;
01436     else if(newYaw>180.0f) newYaw -= 360.0f;
01437     return newYaw;
01438 }
01439 
01440 
01441 void getSF_Serial()
01442 {
01443     //NVIC_DisableIRQ(USART1_IRQn);
01444     //NVIC_DisableIRQ(EXTI0_IRQn);
01445     //NVIC_DisableIRQ(TIM5_IRQn);
01446 
01447 
01448     static char SFbuf[16]= {'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
01449 
01450     static int bufcounter=0;
01451     
01452     float targetyaw = 0;
01453 
01454 
01455 
01456     if(pc.readable()) {    // 受信確認
01457 
01458         SFbuf[bufcounter] = pc.getc();    // 1文字取り出し
01459         if(SFbuf[0]!='S') {
01460             //pc.printf("x");
01461             return;
01462         }
01463 
01464 
01465 
01466         pc.printf("%c",SFbuf[bufcounter]);
01467 
01468         if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++;
01469 
01470         if(bufcounter==5 && SFbuf[4]=='F') {
01471             pc.printf("%s\r\n",SFbuf);
01472             g_landingcommand = SFbuf[1];
01473             //pc.printf("%c",g_landingcommand);
01474             wait_ms(10);
01475             if(g_landingcommand=='Y'){
01476                 targetyaw = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
01477                 if(targetyaw>=0 && targetyaw<=360){
01478                     g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
01479                 }
01480             }
01481             //if(g_landingcommand=='Y')g_SerialTargetYAW =0.0f;
01482             bufcounter = 0;
01483             memset(SFbuf, 0, sizeof(SFbuf));
01484             NVIC_ClearPendingIRQ(USART2_IRQn);
01485             //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW);
01486         }
01487 
01488         else if(bufcounter>=5) {
01489             //pc.printf("Communication Falsed.\r\n");
01490             memset(SFbuf, 0, sizeof(SFbuf));
01491             bufcounter = 0;
01492             NVIC_ClearPendingIRQ(USART2_IRQn);
01493         }
01494     }
01495 
01496     //NVIC_EnableIRQ(TIM5_IRQn);
01497     //NVIC_EnableIRQ(EXTI0_IRQn);
01498     //NVIC_EnableIRQ(USART1_IRQn);
01499 }
01500 
01501 float ConvertByteintoFloat(char high, char low)
01502 {
01503 
01504     //int16_t intvalue = (int16_t)high*256 + (int16_t)low;
01505     int16_t intvalue = (int16_t)(((int16_t)high << 8) | low);  // Turn the MSB and LSB into a signed 16-bit value
01506     float floatvalue = (float)intvalue-256;
01507     return floatvalue;
01508 }
01509 
01510 
01511 //超音波割り込み
01512 
01513 void UpdateDist()
01514 {
01515     g_distance = usensor.get_dist_cm();
01516     usensor.start();
01517 }
01518 
01519 //8の字旋回
01520 void UpdateTargetAngle_Moebius(float targetAngle[3])
01521 {
01522     static uint8_t RotateCounter=0;
01523     static bool flg_setInStartAuto = false;
01524     static float FirstYAW_Moebius = 0.0;
01525     float newYaw_Moebius;
01526 
01527     if(!flg_setInStartAuto && CheckSW_Up(Ch7)) {
01528         FirstYAW_Moebius = nowAngle[YAW];
01529         RotateCounter = 0;
01530         flg_setInStartAuto = true;
01531     } else if(!CheckSW_Up(Ch7)) {
01532         flg_setInStartAuto = false;
01533         led4 = 0;
01534     }
01535     autopwm[THR]=oldTHR;
01536 
01537     newYaw_Moebius = TranslateNewYaw(nowAngle[YAW], FirstYAW_Moebius);
01538 
01539     if(RotateCounter == 0 && newYaw_Moebius >90.0 && newYaw_Moebius < 180.0)    {
01540         RotateCounter++;
01541         led4 = 1;
01542         pc.printf("Rotate 90\r\n");
01543     }
01544     if(RotateCounter == 1 && newYaw_Moebius >-180.0 && newYaw_Moebius < -90.0)  {
01545         RotateCounter++;
01546         led4 = 0;
01547         pc.printf("Rotate 180\r\n");
01548     }
01549     if(RotateCounter == 2 && newYaw_Moebius >-90.0 && newYaw_Moebius <-20.0)   {
01550         RotateCounter++;
01551         led4 = 1;
01552         pc.printf("Rotate 270\r\n");
01553     }
01554     if(RotateCounter == 3 && newYaw_Moebius >-10.0 && newYaw_Moebius < 90.0)     {
01555         RotateCounter++;
01556         led4 = 0;
01557         pc.printf("Change Rotate direction\r\n");
01558     }
01559 
01560 
01561     if(RotateCounter <= 3) UpdateTargetAngle_Rightloop(targetAngle);
01562     else UpdateTargetAngle_Leftloop(targetAngle);   //左旋回
01563 
01564 }
01565 
01566 
01567 
01568 //離陸-投下-着陸一連
01569 void Take_off_and_landing(float targetAngle[3])
01570 {
01571 
01572     if(!CheckSW_Up(Ch7)) bombing_mode = Takeoff;
01573 
01574     switch(bombing_mode) {
01575         case Takeoff:
01576             static bool flg_setFirstYaw = false;
01577             static int TakeoffCount = 0;
01578 
01579             if(!flg_setFirstYaw && CheckSW_Up(Ch7)) {
01580                 FirstYAW = nowAngle[YAW];
01581                 flg_setFirstYaw = true;
01582             } else if(flg_setFirstYaw && !CheckSW_Up(Ch7)) {
01583                 flg_setFirstYaw = false;
01584             }
01585 
01586             UpdateTargetAngle_Takeoff(targetAngle);
01587             NVIC_DisableIRQ(EXTI9_5_IRQn);
01588             
01589             
01590              //if(g_distance>120) TakeoffCount++;
01591 //            else TakeoffCount = 0;
01592 //             if(TakeoffCount>3) Chicken_Drop();
01593 
01594             //if(g_distance>180)TakeoffCount++;
01595             //else TakeoffCount = 0;
01596             
01597             //if(TakeoffCount>2)Chicken_Drop();
01598 
01599             
01600             //if(g_distance>210)Chicken_Drop();
01601             
01602             
01603             
01604             if(g_distance>120) TakeoffCount++;
01605             else TakeoffCount = 0;
01606             //Chicken_Drop();
01607             NVIC_EnableIRQ(EXTI9_5_IRQn);
01608             if(g_landingcommand == 'B'){
01609                 Chicken_Drop();
01610             }
01611             if(TakeoffCount>5) {
01612 
01613                 //autopwm[THR] = SetTHRinRatio(0.3);
01614                 autopwm[THR] = 1700;//g_take_off_THROTTLE;//1180+320*2*0.3;
01615                 targetAngle[PITCH]=g_gostraightPITCH;
01616                 autopwm[RUD]=1370;//trimpwm[RUD];
01617                 targetAngle[ROLL]=g_gostraightROLL;
01618                 Chicken_Drop();
01619                 if(autopwm[AIL_R]<trimpwm[AIL_R]){                                                                                  //エルロン上がりやすさ調節
01620                     autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;  //エルロンサーボ二つ使用時
01621                 }
01622                 else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
01623         
01624                 //pc.printf("Now go to Approach mode!!");
01625                 bombing_mode = Approach;
01626                 g_landingcommand = 'T';
01627             }
01628             
01629              
01630              
01631             break;
01632 
01633         //case Chicken:
01634         //break;
01635         /*
01636         case Transition:
01637             static int ApproachCount = 0;
01638             targetAngle[YAW]=180.0;
01639             int Judge = Rotate(targetAngle, g_SerialTargetYAW);
01640 
01641             if(Judge==0) ApproachCount++;
01642             if(ApproachCount>5) bombing_mode = Approach;
01643             break;
01644             */
01645         case Approach:
01646 
01647             //autopwm[THR] = SetTHRinRatio(g_loopTHR); //0.7;スロットルの割合
01648             autopwm[THR] = minpwm[THR];//1180+320*2*0.3;
01649             if(g_Finflag == true){
01650                 autopwm[THR] = minpwm[THR];
01651                 autopwm[ELE] = minpwm[ELE];
01652                 autopwm[RUD]=trimpwm[RUD];
01653                 autopwm[AIL_R] = minpwm[AIL_R];
01654                 autopwm[AIL_L]= maxpwm[AIL_L];
01655                 targetAngle[PITCH] = -80;
01656                 targetAngle[ROLL] = -80;
01657             }else{
01658                 UpdateTargetAngle_Approach(targetAngle);
01659             }
01660 
01661             break;
01662 
01663         default:
01664             bombing_mode = Takeoff;
01665             break;
01666     }
01667 
01668 }
01669 
01670 //離陸モード
01671 void UpdateTargetAngle_Takeoff(float targetAngle[3])
01672 {
01673     //pc.printf("%d \r\n",g_distance);
01674     static int tELE_start = 0;
01675     static bool flg_ELEup = false;
01676     int t_def = 0;
01677 
01678     autopwm[RUD] = 1390;//trimpwm[RUD];
01679     targetAngle[ROLL]=g_gostraightROLL;
01680                 
01681     if(autopwm[AIL_R]<trimpwm[AIL_R]){                                                                                  //エルロン上がりやすさ調節
01682         autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;  //エルロンサーボ二つ使用時
01683     }
01684     else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
01685 
01686 
01687     if(!flg_ELEup && CheckSW_Up(Ch7)) {
01688         tELE_start = t.read_ms();
01689         flg_ELEup = true;
01690         pc.printf("timer start!!!!\r\n");
01691     } else if(!CheckSW_Up(Ch7)) {
01692         tELE_start = 0;
01693         flg_ELEup = false;
01694         //pc.printf("lost!!!!\r\n");
01695     }
01696     if(flg_ELEup) {
01697         t_def = t.read_ms() - tELE_start;
01698 
01699         //1.5秒経過すればELE上げ舵へ
01700         if(t_def>500) targetAngle[PITCH]= g_take_off_PITCH;
01701         else {
01702             t_def = 0;
01703             targetAngle[PITCH]=g_gostraightPITCH;
01704         }
01705     }
01706     targetAngle[ROLL] = g_gostraightROLL;
01707     //targetAngle[PITCH] = g_loopTHR;
01708     autopwm[THR] = g_take_off_THROTTLE;//SetTHRinRatio(g_takeoffTHR); //0.7;スロットルの割合
01709 }
01710 
01711 
01712 //ヨーを目標値にして許容角度になるまで水平旋回
01713 int Rotate(float targetAngle[3], float TargetYAW)
01714 {
01715     float diffYaw = TranslateNewYaw(TargetYAW, nowAngle[YAW]);
01716 
01717     if(diffYaw > LIMIT_STRAIGHT_YAW) {
01718         
01719         if(diffYaw > THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Rightloop_short(targetAngle);
01720         else UpdateTargetAngle_Rightloop(targetAngle);
01721         
01722         //UpdateTargetAngle_Rightloop_short(targetAngle);
01723         return 1;
01724     } else if(diffYaw < -LIMIT_STRAIGHT_YAW) {
01725         
01726         if(diffYaw < -THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Leftloop_short(targetAngle);
01727         else UpdateTargetAngle_Leftloop(targetAngle);
01728         
01729         //UpdateTargetAngle_Leftloop_short(targetAngle);
01730         
01731         return 1;
01732     } else {
01733         UpdateTargetAngle_GoStraight(targetAngle);
01734         return 0;
01735     }
01736 }
01737 
01738 int Rotate_zero(float targetAngle[3], float TargetYAW){
01739     float diffYaw = TranslateNewYaw(TargetYAW, nowAngle[YAW]);
01740 
01741     if(diffYaw > LIMIT_STRAIGHT_YAW) {
01742         
01743         /*if(diffYaw > THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Rightloop_short(targetAngle);
01744         else UpdateTargetAngle_Rightloop(targetAngle);
01745         */
01746         UpdateTargetAngle_Rightloop_zero(targetAngle);
01747         return 1;
01748     } else if(diffYaw < -LIMIT_STRAIGHT_YAW) {
01749         
01750         /*if(diffYaw < -THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Leftloop_short(targetAngle);
01751         else UpdateTargetAngle_Leftloop(targetAngle);
01752         */
01753         UpdateTargetAngle_Leftloop_zero(targetAngle);
01754         
01755         return 1;
01756     } else {
01757         UpdateTargetAngle_GoStraight_zero(targetAngle);
01758         return 0;
01759     }
01760 }
01761 
01762 //チキラー投下
01763 void Chicken_Drop()
01764 {
01765     //if(CheckSW_Up(Ch7)) {
01766         autopwm[DROP] = maxpwm[DROP];
01767         //pc.printf("Bombed!\r\n");
01768         //RerurnChickenServo1.attach(&ReturnChickenServo1, 3);
01769         //operation_mode = Approach;
01770         //buzzer = 0;
01771         //pc.printf("Goto LeftLoop mode\r\n");
01772     //}
01773 }
01774 
01775 void ReturnChickenServo1()
01776 {
01777     autopwm[DROP] = 1344;
01778     pc.printf("first reverse\r\n");
01779     RerurnChickenServo2.attach(&ReturnChickenServo2, 1);
01780 }
01781 
01782 void ReturnChickenServo2()
01783 {
01784     autopwm[DROP] = 1392;
01785     pc.printf("second reverse\r\n");
01786 }
01787 
01788 //着陸モード（PCからの指令に従う）
01789 void UpdateTargetAngle_Approach(float targetAngle[3])
01790 {
01791 
01792     static bool Zeroflag=false;//着陸時のスロットル動作確認用
01793 
01794     NVIC_DisableIRQ(USART2_IRQn);
01795 
01796     if(CheckSW_Up(Ch7)) {
01797         output_status = Auto;
01798         led1 = 1;
01799     } else {
01800         output_status = Manual;
01801         led1 = 0;
01802         Zeroflag=true;
01803         //g_landingcommand='G';
01804     }
01805 
01806     //pc.printf("Zeroflag = %d\r\n",Zeroflag);
01807 
01808     switch(g_landingcommand) {
01809         case 'R':   //右旋回セヨ
01810             NVIC_EnableIRQ(USART2_IRQn);
01811             if(Zeroflag==true){
01812             UpdateTargetAngle_Rightloop_zero(targetAngle);
01813             }
01814             else{
01815                 UpdateTargetAngle_Rightloop(targetAngle);
01816                 /*
01817                 targetAngle[ROLL] = g_rightloopROLL_approach;
01818                 targetAngle[PITCH] = g_rightloopPITCH_approach ;
01819                 autopwm[RUD]=g_rightloopRUD_approach; //RUD固定
01820                 if(autopwm[AIL_R]<trimpwm[AIL_R]){                                                                                  //エルロン上がりやすさ調節
01821                     autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
01822                 }
01823                 else {autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
01824                 autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop;
01825                 */
01826             }
01827 
01828             break;
01829 
01830         case 'L':   //左旋回セヨ
01831             NVIC_EnableIRQ(USART2_IRQn);
01832             if(Zeroflag==true){
01833                 UpdateTargetAngle_Leftloop_zero(targetAngle);
01834                 }else{
01835                 UpdateTargetAngle_Leftloop(targetAngle);
01836                 /*targetAngle[ROLL] = g_leftloopROLL_approach;
01837                 targetAngle[PITCH] = g_leftloopPITCH_approach;
01838                 autopwm[RUD]=g_leftloopRUD_approach;
01839                 if(autopwm[AIL_R]<trimpwm[AIL_R]){
01840                     autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
01841                 }
01842                 else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
01843                 */
01844             }
01845 
01846             break;
01847 
01848         case 'G':   //直進セヨ
01849             NVIC_EnableIRQ(USART2_IRQn);
01850             if(Zeroflag==true){
01851             UpdateTargetAngle_GoStraight_zero(targetAngle);
01852             }
01853             else{
01854                 UpdateTargetAngle_GoStraight(targetAngle);
01855                 /*
01856                 targetAngle[ROLL] = g_gostraightROLL;
01857                 targetAngle[PITCH] = g_gostraightPITCH;
01858                 */
01859             }
01860 
01861             break;
01862 
01863         case 'Y':   //指定ノヨー方向ニ移動セヨ
01864             NVIC_EnableIRQ(USART2_IRQn);
01865             if(Zeroflag==true){
01866                 Rotate_zero(targetAngle, g_SerialTargetYAW);
01867                 //autopwm[THR]=minpwm[THR];
01868             }else{
01869                 Rotate(targetAngle, g_SerialTargetYAW);
01870                 //autopwm[THR]=SetTHRinRatio(g_loopTHR+0.05);
01871                 }
01872             break;
01873             
01874         case 'T':   //0度ノヨー方向ニ移動セヨ
01875             NVIC_EnableIRQ(USART2_IRQn);
01876             if(Zeroflag==true){
01877                 Rotate_zero(targetAngle, 0);
01878                 //autopwm[THR]=minpwm[THR];
01879             }else{
01880                 Rotate(targetAngle, 0);
01881                 //autopwm[THR]=SetTHRinRatio(g_loopTHR+0.05);
01882                 }
01883             break;    
01884 
01885         case 'U':   //機首ヲ上ゲヨ
01886             NVIC_EnableIRQ(USART2_IRQn);
01887             static int UpCounter=0;
01888             UpCounter++;
01889             if(UpCounter==3) {
01890                 g_Finflag = true;
01891             }
01892 
01893             break;
01894             
01895         case 'Z':   //ヤレバワカル
01896             NVIC_EnableIRQ(USART2_IRQn);
01897             autopwm[THR] = minpwm[THR];
01898             autopwm[ELE] = minpwm[ELE];
01899             autopwm[RUD]=trimpwm[RUD];
01900             autopwm[AIL_R] = minpwm[AIL_R];
01901             autopwm[AIL_L]= maxpwm[AIL_L];
01902             targetAngle[ROLL] = -80;
01903 
01904             break;
01905 
01906         /*case 'B':   //ブザーヲ鳴ラセ
01907             //buzzer = 1;
01908             NVIC_EnableIRQ(USART2_IRQn);
01909             break;*/
01910 
01911         case 'B':   //物資ヲ落トセ
01912             NVIC_EnableIRQ(USART2_IRQn);
01913             Chicken_Drop();
01914             if(Zeroflag==true){
01915                 Rotate_zero(targetAngle, 0);
01916                 //autopwm[THR]=minpwm[THR];
01917             }else{
01918                 Rotate(targetAngle, 0);
01919                 //autopwm[THR]=SetTHRinRatio(g_loopTHR+0.05);
01920                 }
01921 
01922             break;
01923 
01924         case 'C':   //停止セヨ
01925             NVIC_EnableIRQ(USART2_IRQn);
01926             /*targetAngle[ROLL] = g_gostraightROLL;
01927             targetAngle[PITCH] = g_gostraightPITCH;
01928             autopwm[THR] = minpwm[THR];
01929             */
01930             Zeroflag = true;
01931             if(Zeroflag==true){
01932                 Rotate_zero(targetAngle, 0);
01933                 //autopwm[THR]=minpwm[THR];
01934             }else{
01935                 Rotate(targetAngle, 0);
01936                 //autopwm[THR]=SetTHRinRatio(g_loopTHR+0.05);
01937                 }
01938             
01939             break;
01940             
01941         case 'X':   //再起動セヨ
01942             NVIC_EnableIRQ(USART2_IRQn);
01943             /*
01944             targetAngle[ROLL] = g_gostraightROLL;
01945             targetAngle[PITCH] = g_gostraightPITCH;
01946             autopwm[THR] = minpwm[THR];
01947             */
01948             Zeroflag = false;
01949             if(Zeroflag==true){
01950                 Rotate_zero(targetAngle, 0);
01951                 //autopwm[THR]=minpwm[THR];
01952             }else{
01953                 Rotate(targetAngle, 0);
01954                 //autopwm[THR]=SetTHRinRatio(g_loopTHR+0.05);
01955                 }
01956                 
01957             break;
01958 
01959         default :
01960             NVIC_EnableIRQ(USART2_IRQn);
01961             break;
01962 
01963 
01964     }
01965 
01966 }
01967 
01968 void checkHeight(float targetAngle[3])
01969 {
01970 
01971     static int targetHeight = 200;
01972 
01973     NVIC_DisableIRQ(EXTI9_5_IRQn);
01974     if(g_distance < targetHeight + ALLOWHEIGHT) {
01975         UpdateTargetAngle_NoseUP(targetAngle);
01976         if(CheckSW_Up(Ch7)) led2 = 1;
01977     } else if(g_distance > targetHeight - ALLOWHEIGHT) {
01978         UpdateTargetAngle_NoseDOWN(targetAngle);
01979         if(CheckSW_Up(Ch7)) led2 = 1;
01980     } else led2=0;
01981     NVIC_EnableIRQ(EXTI9_5_IRQn);
01982 }
01983 
01984 void UpdateTargetAngle_NoseUP(float targetAngle[3])
01985 {
01986 
01987     //targetAngle[PITCH] += 2.0f;
01988     //if(nowAngle[PITCH]<targetAngle[PITCH]) autopwm[THR] = SetTHRinRatio(0.6);
01989     autopwm[THR] = SetTHRinRatio(g_loopTHR+0.05);
01990     //pc.printf("nose UP");
01991 }
01992 
01993 void UpdateTargetAngle_NoseDOWN(float targetAngle[3])
01994 {
01995 
01996     //targetAngle[PITCH] -= 2.0f;
01997     autopwm[THR] = SetTHRinRatio(g_loopTHR-0.05);
01998     //pc.printf("nose DOWN");
01999 }
02000 
02001 //直進
02002 void UpdateTargetAngle_GoStraight(float targetAngle[3])
02003 {
02004 
02005     autopwm[RUD] = trimpwm[RUD];
02006     targetAngle[ROLL] = g_gostraightROLL;
02007     targetAngle[PITCH] = g_gostraightPITCH;
02008     autopwm[THR] = SetTHRinRatio(g_loopTHR);
02009     
02010     if(autopwm[AIL_R]<trimpwm[AIL_R]){                                                                                  //エルロン上がりやすさ調節
02011             autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;  //エルロンサーボ二つ使用時
02012         }
02013         else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
02014     
02015     autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop;
02016 
02017     //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
02018 }
02019 
02020 //直進(着陸時スロットル０のとき）
02021 void UpdateTargetAngle_GoStraight_zero(float targetAngle[3])
02022 {
02023 
02024     autopwm[RUD] =  trimpwm[RUD];
02025     targetAngle[ROLL] = g_gostraightROLL;
02026     targetAngle[PITCH] = -7.0;//g_gostraightPITCH;
02027     autopwm[THR] = minpwm[THR];
02028     
02029     if(autopwm[AIL_R]<trimpwm[AIL_R]){                                                                                  //エルロン上がりやすさ調節
02030             autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;  //エルロンサーボ二つ使用時
02031         }
02032         else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
02033 
02034     //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
02035 }
02036 
02037 //右旋回
02038 void UpdateTargetAngle_Rightloop(float targetAngle[3])  //右旋回
02039 {
02040     targetAngle[ROLL] = g_rightloopROLL;
02041     targetAngle[PITCH] = g_rightloopPITCH;
02042     autopwm[RUD]=g_rightloopRUD;              //RUD固定
02043     autopwm[THR] = SetTHRinRatio(g_rightloop_THR_rate);                  //手動スロットル記憶
02044 
02045     /*
02046     if (nowAngle[ROLL]>20.0||nowAngle[ROLL]<-20.0){
02047         t2.start();
02048         pc.printf("Timer start.");
02049     }
02050     if(0.0<t2.read()<5.0){
02051             //pc.printf("tagetAngle is changed.");
02052             targetAngle[ROLL] = rightloopROLL2;
02053             }
02054     else {
02055         t2.stop();
02056         t2.reset();
02057         pc.printf("Timer stopped.");
02058         targetAngle[ROLL] = g_rightloopROLL;
02059     }
02060     */
02061     if(autopwm[AIL_R]<trimpwm[AIL_R]){                                                                                  //エルロン上がりやすさ調節
02062             autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;  //エルロンサーボ二つ使用時
02063         }
02064         else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
02065     
02066     autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop;
02067 
02068 
02069     //checkHeight(targetAngle);
02070     //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
02071 }
02072 
02073 void UpdateTargetAngle_Rightloop_UP(float targetAngle[3])  //右上昇旋回
02074 { 
02075     //autopwm[ELE]=maxpwm[ELE];
02076     targetAngle[ROLL] = g_ascending_ROLL;
02077     targetAngle[PITCH] = g_ascendingPITCH;
02078     autopwm[RUD]=g_ascending_RUD;              //RUD固定
02079     autopwm[THR] = SetTHRinRatio(g_ascending_THR_rate);                  //手動スロットル記憶
02080 
02081     /*
02082     if (nowAngle[ROLL]>20.0||nowAngle[ROLL]<-20.0){
02083         t2.start();
02084         pc.printf("Timer start.");
02085     }
02086     if(0.0<t2.read()<5.0){
02087             //pc.printf("tagetAngle is changed.");
02088             targetAngle[ROLL] = rightloopROLL2;
02089             }
02090     else {
02091         t2.stop();
02092         t2.reset();
02093         pc.printf("Timer stopped.");
02094         targetAngle[ROLL] = g_rightloopROLL;
02095     }
02096     */
02097     if(autopwm[AIL_R]<trimpwm[AIL_R]){                                                                                  //エルロン上がりやすさ調節
02098             autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;  //エルロンサーボ二つ使用時
02099         }
02100         else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
02101     
02102     autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop;
02103 
02104 
02105     //checkHeight(targetAngle);
02106     //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
02107 }
02108 
02109 //右旋回（着陸時スロットル０の時）
02110 void UpdateTargetAngle_Rightloop_zero(float targetAngle[3])  //右旋回
02111 {
02112     autopwm[THR]=minpwm[THR];
02113     targetAngle[ROLL] = g_rightloopROLL_approach;
02114     targetAngle[PITCH] = g_rightloopPITCH_approach ;
02115     autopwm[RUD]=g_rightloopRUD_approach;              //RUD固定
02116     if(autopwm[AIL_R]<trimpwm[AIL_R]){                                                                                  //エルロン上がりやすさ調節
02117             autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;  //エルロンサーボ二つ使用時
02118         }
02119         else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
02120     
02121     autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop;
02122 
02123     //checkHeight(targetAngle);
02124     //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
02125 }
02126 
02127 void UpdateTargetAngle_Rightloop_short(float targetAngle[3])  //右旋回
02128 {
02129 
02130     targetAngle[ROLL] = g_rightloopROLLshort;
02131     targetAngle[PITCH] = g_rightloopPITCHshort;
02132     autopwm[RUD]=g_rightloopshortRUD;
02133     autopwm[THR] = SetTHRinRatio(g_rightloop_THR_rate);
02134     if(autopwm[AIL_R]<trimpwm[AIL_R]){
02135             autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
02136             }
02137         else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;  //エルロンサーボ二つ使用時
02138     
02139     //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
02140 }
02141 
02142 
02143 //左旋回
02144 void UpdateTargetAngle_Leftloop(float targetAngle[3])
02145 {
02146 
02147     targetAngle[ROLL] = g_leftloopROLL;
02148     targetAngle[PITCH] = g_leftloopPITCH;
02149     //autopwm[ELE] = 1700;
02150     autopwm[RUD]=g_leftloopRUD;
02151     autopwm[THR] = SetTHRinRatio(g_leftloop_THR_rate);
02152     if(autopwm[AIL_R]<trimpwm[AIL_R]){
02153             autopwm[AIL_L]=trimpwm[AIL_L]+g_AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
02154             }
02155         else autopwm[AIL_L]=trimpwm[AIL_L]+g_AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;  //エルロンサーボ二つ使用時
02156     
02157     //checkHeight(targetAngle);
02158 
02159     //printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[], targetAngle[PITCH], autopwm[THR]);
02160 }
02161 
02162 //左旋回（着陸時スロットル０のとき）
02163 void UpdateTargetAngle_Leftloop_zero(float targetAngle[3])
02164 {
02165 
02166     targetAngle[ROLL] = g_leftloopROLL_approach;
02167     targetAngle[PITCH] = g_leftloopPITCH_approach;
02168     autopwm[RUD]=g_leftloopRUD_approach;
02169     autopwm[THR] = minpwm[THR];
02170     if(autopwm[AIL_R]<trimpwm[AIL_R]){
02171             autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
02172             }
02173         else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;  //エルロンサーボ二つ使用時
02174     
02175     //autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_Ratio_leftloop;
02176     //checkHeight(targetAngle);
02177 
02178     //printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[], targetAngle[PITCH], autopwm[THR]);
02179 }
02180 
02181 void UpdateTargetAngle_Leftloop_short(float targetAngle[3])
02182 {
02183 
02184     targetAngle[ROLL] = g_leftloopROLLshort;
02185     targetAngle[PITCH] = g_leftloopPITCHshort;
02186     autopwm[RUD]=g_leftloopRUD;
02187     autopwm[THR] = SetTHRinRatio(g_leftloop_THR_rate);
02188     if(autopwm[AIL_R]<trimpwm[AIL_R]){
02189             autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
02190             }
02191         else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;  //エルロンサーボ二つ使用時
02192     
02193     //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
02194 }
02195 
02196 void Ascending(float targetAngle[3]){
02197     static uint8_t RotateCounter=0;
02198     static bool LapCounter = false;
02199     static bool UpFlag = false;
02200     static bool PressFlag = false;
02201     static float FirstHeight = 0;
02202     static bool flg_setInStartAuto = false;
02203     //static float Time = 0.0;
02204     static float FirstYaw_Ascending = 0;
02205     float newYaw_Ascending;
02206     
02207     //pc.printf("FirstHeight = %f  g_z_comp = %f   g_AscendingFlag = %d\r\n",FirstHeight,g_z_comp,g_AscendingFlag);  
02208     
02209     getBMP280();
02210     
02211     if(!flg_setInStartAuto && CheckSW_Up(Ch7)) {
02212         FirstYaw_Ascending = nowAngle[YAW];
02213         RotateCounter = 0;
02214         flg_setInStartAuto = true;
02215         LapCounter = false;
02216         UpFlag = false;
02217         PressFlag = false;
02218         //tBMP.reset();
02219     } else if(!CheckSW_Up(Ch7)) {
02220         flg_setInStartAuto = false;
02221         led4 = 0;
02222     }
02223     
02224     newYaw_Ascending = TranslateNewYaw(nowAngle[YAW], FirstYaw_Ascending);
02225 
02226     if(RotateCounter == 0 && newYaw_Ascending >90.0 && newYaw_Ascending < 180.0)    {
02227         RotateCounter++;
02228         led4 = 1;
02229         pc.printf("Rotate 90\r\n");
02230     }
02231     if(RotateCounter == 1 && newYaw_Ascending >-180.0 && newYaw_Ascending < -90.0)  {
02232         RotateCounter++;
02233         led4 = 0;
02234         pc.printf("Rotate 180\r\n");
02235     }
02236     if(RotateCounter == 2 && newYaw_Ascending >-90.0 && newYaw_Ascending <-20.0)   {
02237         RotateCounter++;
02238         led4 = 1;
02239         pc.printf("Rotate 270\r\n");
02240     }
02241     if(RotateCounter == 3 && newYaw_Ascending >-10.0 && newYaw_Ascending < 90.0)     {
02242         RotateCounter++;
02243         led4 = 0;
02244         if(LapCounter == false){
02245             RotateCounter = 0;
02246             LapCounter = true;
02247             pc.printf("Lap2\r\n");
02248             }
02249     }
02250     
02251     if(RotateCounter <= 3){
02252         UpdateTargetAngle_Rightloop(targetAngle);
02253     }else if(3 < RotateCounter && UpFlag == false){
02254         if(PressFlag == false){
02255             FirstHeight = g_z_comp;
02256             //tBMP.start();
02257             PressFlag = true;
02258             led4 = 1;
02259             }
02260         //Time = tBMP.read();    
02261         if(g_z_comp > FirstHeight+3000){
02262             UpFlag = true;
02263             led4 = 0;
02264             }
02265         UpdateTargetAngle_Rightloop_UP(targetAngle);
02266         }else{
02267      UpdateTargetAngle_Rightloop(targetAngle);
02268      }
02269          
02270     
02271 }    
02272 
02273 void Sbusprintf() 
02274 {
02275 
02276     for(uint8_t i=0; i<8; i++) pc.printf("ch.%d = %d ",i+1,sbus.manualpwm[i]);
02277     pc.printf("\r\n");
02278 
02279 }
02280 
02281 
02282 //---sensing---
02283 //センサーの値を読み込み、各種フィルタをかける(認知)
02284 void sensing()
02285 {
02286     // If intPin goes high, all data registers have new data
02287     if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
02288 
02289         mpu9250.readAccelData(accelCount);  // Read the x/y/z adc values   
02290         // Now we'll calculate the accleration value into actual g's
02291         ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
02292         ay = (float)accelCount[1]*aRes - accelBias[1];   
02293         az = (float)accelCount[2]*aRes - accelBias[2];  
02294    
02295         mpu9250.readGyroData(gyroCount);  // Read the x/y/z adc values
02296         // Calculate the gyro value into actual degrees per second
02297         gx = (float)gyroCount[0]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
02298         gy = (float)gyroCount[1]*gRes - gyroBias[1];  
02299         gz = (float)gyroCount[2]*gRes - gyroBias[2];   
02300   
02301         mpu9250.readMagData(magCount);  // Read the x/y/z adc values   
02302         // Calculate the magnetometer values in milliGauss
02303         // Include factory calibration per data sheet and user environmental corrections
02304         mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
02305         my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];  
02306         mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];   
02307     }
02308    
02309     Now = t.read_us();
02310     deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
02311     lastUpdate = Now;
02312     
02313     g_sum += deltat;
02314     g_sumCount++;
02315     
02316 //  if(lastUpdate - firstUpdate > 10000000.0f) {
02317 //      beta = 0.04;  // decrease filter gain after stabilized
02318 //      zeta = 0.015; // increasey bias drift gain after stabilized
02319 //  }
02320     
02321    // Pass gyro rate as rad/s
02322     mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  mx,  my, mz);
02323    // mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
02324     
02325     mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);
02326    // mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
02327 
02328     /*  Auto_Loop()で50ms待ってるから要らないと思う
02329     // Serial print and/or display at 0.5 s rate independent of data rates
02330     delt_t = t.read_ms() - count;
02331     if (delt_t > 500) { // update LCD once per half-second independent of read rate
02332     */
02333 
02334     //pc.printf("ax = %f", 1000*ax); 
02335     //pc.printf(" ay = %f", 1000*ay); 
02336     //pc.printf(" az = %f  mg\n\r", 1000*az); 
02337 
02338     //pc.printf("gx = %f", gx); 
02339     //pc.printf(" gy = %f", gy); 
02340     //pc.printf(" gz = %f  deg/s\n\r", gz); 
02341     
02342     //pc.printf("mx = %f,", mx); 
02343     //pc.printf(" my = %f,", my); 
02344     //pc.printf(" mz = %f  mG\n\r", mz); 
02345     
02346     tempCount = mpu9250.readTempData();  // Read the adc values
02347     temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
02348     //pc.printf(" temperature = %f  C\n\r", temperature); 
02349     
02350     //pc.printf("q0 = %f\n\r", q[0]);
02351     //pc.printf("q1 = %f\n\r", q[1]);
02352     //pc.printf("q2 = %f\n\r", q[2]);
02353     //pc.printf("q3 = %f\n\r", q[3]);      
02354     
02355       
02356   // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
02357   // In this coordinate system, the positive z-axis is down toward Earth. 
02358   // 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.
02359   // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
02360   // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
02361   // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
02362   // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
02363   // applied in the correct order which for this configuration is yaw, pitch, and then roll.
02364   // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
02365     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]);   
02366     roll = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
02367     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]);
02368     pitch *= 180.0f / PI;
02369     yaw   *= 180.0f / PI; 
02370     yaw   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
02371     roll  *= 180.0f / PI;
02372 
02373     //pc.printf("$%d %d %d;",(int)(yaw*100),(int)(pitch*100),(int)(roll*100));
02374     //pc.printf("Yaw: %f  Pitch:%f  Roll:%f\n\r",  yaw,   pitch,   roll);
02375     //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
02376  
02377     count = t.read_ms(); 
02378     g_sum = 0;
02379     g_sumCount = 0; 
02380     //}           
02381 }
02382 
02383 void SetupBMP280(){
02384     bmp.initialize(BMP280::INDOOR_NAVIGATION);
02385     
02386     for(int sn = 0; sn < 100; sn++){
02387         g_press = bmp.getPressure();
02388         if(sn > 0){
02389             g_InitPress[sn] = g_press;
02390             g_InitPressAve += g_InitPress[sn];
02391             float press_ratio = g_st_press / g_press;
02392             g_InitBMPHeight[sn] = ((powf(press_ratio, 0.19) - 1) * 298.15 * 1000) / 0.0065;
02393             g_InitBMPHeightAve += g_InitBMPHeight[sn];
02394 
02395         }
02396         pc.printf("%f,%f\r\n", g_InitBMPHeight[sn],g_InitPress[sn]);
02397         wait(0.05);
02398     }
02399     
02400     g_InitPressAve /= 100.0;
02401 
02402     g_InitBMPHeightAve /= 100.0;
02403     
02404     pc.printf("\r\n%f,%f\r\n\r\n", g_InitBMPHeightAve,g_InitPressAve);
02405         
02406     pc.printf("bmp280 initialized\r\n");    
02407     
02408     g_BMPheight = g_InitBMPHeightAve;
02409     
02410     tBMP.start();//dtを計測
02411    
02412     sensing();//最初だけ空回し
02413     
02414     g_tempaz[0] = az * g_grav * 1000;
02415     
02416 }
02417 
02418 void getBMP280(){
02419         float z_angle, gyro_ver, z_angle_prev;
02420         static float press_rel_coef = 0.03;
02421         float press_ratio = g_st_press / g_press;
02422         
02423         g_z_PressHeight = g_BMPheight;
02424         
02425         z_angle = atan2(sqrt(ax * ax + ay * ay), az);
02426         gyro_ver = cos(z_angle);   //垂直方向の加速度
02427         
02428         g_dt = tBMP.read_us();
02429         g_dt /= 1000000;
02430         tBMP.reset();
02431         tBMP.start();
02432         
02433         
02434         g_z_AccelHeight = g_z_prev - g_v_prev * g_dt - (az - gyro_ver) * g_grav *1000* g_dt * g_dt / 2;
02435             
02436         g_z_comp = g_z_PressHeight* press_rel_coef + g_z_AccelHeight* (1 - press_rel_coef);//係数は足して1になるように適当に設定,単位はmm
02437         
02438         //pc.printf("%f\r\n",g_z_comp/1000);
02439         
02440         g_BMPheight_int = g_z_comp;
02441         
02442         g_press = bmp.getPressure();
02443     
02444         g_BMPheight = ((powf(press_ratio, 0.19) - 1) * 298.15 *1000) / 0.0065 - g_InitBMPHeightAve;
02445               
02446         NVIC_DisableIRQ(USART1_IRQn);
02447         NVIC_DisableIRQ(USART2_IRQn);
02448         NVIC_DisableIRQ(TIM5_IRQn);
02449         NVIC_DisableIRQ(EXTI0_IRQn);//MPU割り込み禁止
02450         NVIC_DisableIRQ(EXTI9_5_IRQn);//超音波割り込み禁止
02451         sensing();      
02452         NVIC_EnableIRQ(USART1_IRQn);
02453         NVIC_EnableIRQ(USART2_IRQn);
02454         NVIC_EnableIRQ(TIM5_IRQn);
02455         NVIC_EnableIRQ(EXTI0_IRQn);
02456         NVIC_EnableIRQ(EXTI9_5_IRQn);
02457         
02458         g_v_prev = (g_z_comp - g_z_prev) / g_dt / 1000000;
02459         
02460         g_z_prev = g_z_comp;
02461         
02462         
02463         //wait_ms(1.0);
02464         
02465 }
02466 
02467 
02468 
02469 //デバッグ用=
02470 void DebugPrint()
02471 {
02472  /*
02473     static int16_t deltaT = 0, t_start = 0;
02474     //deltaT = t.read_u2s() - t_start;
02475     pc.printf("t:%d us, ",deltaT);
02476     pc.printf("\r\n");
02477     t_start = t.read_us();
02478     
02479     for(uint8_t i=0; i<6; i++) pc.printf("sbus.manualpwm[%d]:%d ",i,sbus.manualpwm[i]);
02480     if(g_trim_check){
02481         for(uint8_t i=0; i<6; i++) pc.printf("sbus.manualpwm[%d]:%d ",i,sbus.manualpwm[i]);
02482         pc.printf("\r\n");
02483         }
02484  */
02485     //pc.printf("trimpwm[AIL_R]:%d ELE:%d THR:%d RUD:%d 4:%d AIL_L:%d\n",trimpwm[AIL_R],trimpwm[ELE],trimpwm[THR],trimpwm[RUD],trimpwm[4],trimpwm[AIL_L]);
02486     //for(uint8_t i=0; i<8; i++) pc.printf("sbus.manualpwm[%d]:%d ",i,sbus.manualpwm[i]);
02487     /*for(uint8_t i=1; i<4; i++) pc.printf("sbus.manualpwm[%d]:%d ",i,sbus.manualpwm[i]);
02488     pc.printf("\r\n");
02489     for(uint8_t i=0; i<3; i++) pc.printf("nowangle[%d]:%3.2f\t",i,nowAngle[i]);
02490     for(uint8_t i=0; i<2; i++) pc.printf("nowangle[%d]:%3.2f\t",i,nowAngle[i]); //ロール、ピッチのみ　小数点以下2ケタ
02491     pc.printf("nowangle[pitch]:%3.2f\t\r\n",nowAngle[2]);
02492     pc.printf("autopwm[ELE]:%d\t\r\n",autopwm[ELE]);
02493     pc.printf("autpwm[RUD]:%d\t",autopwm[RUD]);*/
02494     //NVIC_DisableIRQ(EXTI9_5_IRQn);
02495     pc.printf("g_distance:%d\t",g_distance);
02496     //NVIC_EnableIRQ(EXTI9_5_IRQn);
02497     /*pc.printf("Mode: %c: ",g_buf[0]);
02498     if(g_buf[0] == 'Y') pc.printf(" %3.1f",g_SerialTargetYAW);
02499     pc.printf("%f",g_z_comp);
02500     pc.printf("\r\n%f,%f\r\n", g_InitBMPHeightAve,g_InitPressAve);*/
02501     pc.printf("\r\n");
02502     
02503 }