航空研究会
a
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of
Autoflight2018_12
by 
航空研究会
Embed:
(wiki syntax)
Show/hide line numbers
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 "BMP280.h" 00011 #include "hcsr04.h" 00012 //device 00013 #include "sbus.h" 00014 //config 00015 #include "SkipperSv2.h" 00016 #include "falfalla.h" 00017 //other 00018 #include "pid.h" 00019 00020 #define DEBUG_SEMIAUTO 0 00021 #define DEBUG_PRINT_INLOOP 1 00022 00023 #define KP_ELE 15.0 //2.0 00024 #define KI_ELE 0.0 00025 #define KD_ELE 0.0 //0/0 00026 #define KP_RUD 3.0 00027 #define KI_RUD 0.0 00028 #define KD_RUD 0.0 00029 #define KP_AIL 0.1 00030 #define KI_AIL 0.2 00031 #define KD_AIL 0.2 00032 00033 //#define g_AIL_L_Ratio_rightloop 0.5 00034 00035 #define GAIN_CONTROLVALUE_TO_PWM 3.0 00036 00037 #define RIGHT_ROLL -12.0 00038 #define RIGHT_PITCH -10.0 //5.0 00039 #define LEFT_ROLL 12.0 00040 #define LEFT_PITCH -5.0 00041 #define STRAIGHT_ROLL 4.0 00042 #define STRAIGHT_PITCH 3.0 00043 #define TAKEOFF_THR 0.8 00044 #define LOOP_THR 0.6 00045 00046 #define g_rightloopRUD 1500 00047 00048 #define RIGHT_ROLL_SHORT -12.0 00049 #define RIGHT_PITCH_SHORT -5.0 00050 #define LEFT_ROLL_SHORT 12.0 00051 #define LEFT_PITCH_SHORT -5.0 00052 00053 #define rightloopRUD 1300 //1250 00054 #define AIL_R_correctionrightloop 0 00055 #define AIL_L_correctionrightloop -400 00056 #define rightloopshortRUD 1250 00057 #define AIL_L_correctionrightloopshort 0 00058 #define leftloopRUD 1500 00059 #define AIL_L_correctionleftloop -0 00060 #define leftloopshortRUD 1500 00061 #define AIL_L_correctionleftloopshort 0 00062 #define glideloopRUD 1300 00063 00064 #define RIGHTLOOP_RUD 1250 00065 #define RIGHTLOOPSHORT_RUD 1250 00066 #define LEFTLOOP_RUD 1500 00067 #define LEFTLOOPSHORT_RUD 1500 00068 #define GLIDELOOP_RUD 1300 00069 #define AIL_L_CORRECTION_RIGHTLOOP 0 00070 #define AIL_L_CORRECTION_RIGHTLOOPSHORT 0 00071 #define AIL_L_CORRECTION_LEFTLOOP 0 00072 #define AIL_L_CORRECTION_LEFTLOOPSHORT 0 00073 00074 #define GLIDE_ROLL -12.0 00075 #define GLIDE_PITCH -3.0 00076 00077 00078 #define AIL_L_RatioRising 0.5 00079 #define AIL_L_RatioDescent 2 00080 00081 //コンパスキャリブレーション 00082 //SkipperS2基板 00083 /* 00084 #define MAGBIAS_X -35.0 00085 #define MAGBIAS_Y 535.0 00086 #define MAGBIAS_Z -50.0 00087 */ 00088 //S2v2 1番基板 00089 #define MAGBIAS_X 395.0 00090 #define MAGBIAS_Y 505.0 00091 #define MAGBIAS_Z -725.0 00092 //S2v2 2番基板 00093 /* 00094 #define MAGBIAS_X 185.0 00095 #define MAGBIAS_Y 220.0 00096 #define MAGBIAS_Z -350.0 00097 */ 00098 00099 #define ELEMENT 1 00100 #define LIMIT_STRAIGHT_YAW 5.0 00101 #define THRESHOLD_TURNINGRADIUS_YAW 60.0 00102 #define ALLOWHEIGHT 15 00103 00104 #ifndef PI 00105 #define PI 3.14159265358979 00106 #endif 00107 00108 const int16_t lengthdivpwm = 320; 00109 const int16_t changeModeCount = 6; 00110 00111 const float trim[4] = {Trim_Falfalla[0],Trim_Falfalla[1],Trim_Falfalla[2],Trim_Falfalla[3]}; 00112 const float expMax[4] = {ExpMax_Falfalla[0],ExpMax_Falfalla[1],ExpMax_Falfalla[2],ExpMax_Falfalla[3]}; 00113 const float expMin[4] = {ExpMin_Falfalla[0],ExpMin_Falfalla[1],ExpMin_Falfalla[2],ExpMin_Falfalla[3]}; 00114 const int16_t reverce[4] = {Reverce_falfalla[0],Reverce_falfalla[1],Reverce_falfalla[2],Reverce_falfalla[3]}; 00115 00116 SBUS sbus(PA_9, PA_10); //SBUS 00117 00118 PwmOut servo1(PC_6); // TIM3_CH1 //old echo 00119 PwmOut servo2(PC_7); // TIM3_CH2 //PC_7 00120 PwmOut servo3(PB_0); // TIM3_CH3 00121 PwmOut servo4(PB_1); // TIM3_CH4 00122 PwmOut servo5(PB_6); // TIM4_CH1 00123 PwmOut servo6(PB_7); // TIM4_CH2 //old trigger 00124 //PwmOut servo7(PB_8); // TIM4_CH3 //PB_8 new echo 00125 //PwmOut servo8(PB_9); // TIM4_CH4 //new trigger 00126 00127 RawSerial pc(PA_2,PA_3, 115200); //tx,rx.baudrate pin;PA_2=UART2_TX, PA_3=UART2_RX 00128 //RawSerial pc2(PB_6,PB_7, 115200); //sbus確認用 00129 SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd"); 00130 00131 DigitalOut led1(PA_0); //黄色のコネクタ 00132 DigitalOut led2(PA_1); 00133 DigitalOut led3(PB_4); 00134 DigitalOut led4(PB_5); 00135 00136 //InterruptIn switch2(PC_14); 00137 MPU6050DMP6 mpu6050(PC_0,&pc); //割り込みピン，シリアルポインタ i2cのピン指定は MPU6050>>I2Cdev.h 内のdefine 00138 HCSR04 usensor(PB_9,PB_8); //trig,echo 9,8 00139 00140 PID pid_AIL(g_kpAIL,g_kiAIL,g_kdAIL); 00141 PID pid_ELE(g_kpELE,g_kiELE,g_kdELE); 00142 PID pid_RUD(g_kpRUD,g_kiRUD,g_kdRUD); 00143 00144 enum Channel{AIL_R, ELE, THR, RUD, DROP, AIL_L, Ch7, Ch8}; 00145 enum Angle{ROLL, PITCH, YAW}; //yaw:北を0とした絶対角度 00146 enum OperationMode{StartUp, SemiAuto, RightLoop, LeftLoop, GoStraight, BombwithPC, ZERO, Moebius, Glide}; 00147 enum BombingMode{Takeoff, Chicken, Transition, Approach}; 00148 enum OutputStatus{Manual, Auto}; 00149 00150 static OutputStatus output_status = Manual; 00151 OperationMode operation_mode = StartUp; 00152 BombingMode bombing_mode = Takeoff; 00153 static int16_t autopwm[8] = {1500,1500,1180,1500,1392,1500}; 00154 static int16_t trimpwm[6] = {1500,1500,1180,1500,1392,1600}; 00155 int16_t maxpwm[6] = {1820,1820,1820,1820,1820,1820}; 00156 int16_t minpwm[6] = {1180,1180,1180,1180,1180,1180}; 00157 int16_t oldTHR = 1000; 00158 00159 int16_t g_AIL_L_Ratio_rightloop = 0.5; 00160 00161 00162 static float nowAngle[3] = {0,0,0}; 00163 const float trimAngle[3] = {0.0, 0.0, 0.0}; 00164 const float maxAngle[2] = {90, 90}; 00165 const float minAngle[2] = {-90, -90}; 00166 00167 float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0; 00168 00169 unsigned int g_distance; 00170 Ticker USsensor; 00171 static char g_buf[16]; 00172 char g_landingcommand; 00173 float g_SerialTargetYAW; 00174 00175 Timer t; 00176 Timeout RerurnChickenServo1; 00177 Timeout RerurnChickenServo2; 00178 00179 /*-----関数のプロトタイプ宣言-----*/ 00180 void setup(); 00181 void loop(); 00182 00183 void Init_PWM(); 00184 void Init_servo(); //サーボ初期化 00185 void Init_sbus(); //SBUS初期化 00186 void Init_sensors(); 00187 void DisplayClock(); //クロック状態確認 00188 00189 //センサの値取得 00190 void SensingMPU(); 00191 void UpdateDist(); 00192 00193 //void offsetRollPitch(float FirstROLL, float FirstPITCH); 00194 //void TransYaw(float FirstYAW); 00195 float TranslateNewYaw(float beforeYaw, float newzeroYaw); 00196 void UpdateTargetAngle(float targetAngle[3]); 00197 void CalculateControlValue(float targetAngle[3], float controlValue[3]); 00198 void UpdateAutoPWM(float controlValue[3]); 00199 void ConvertPWMintoRAD(float targetAngle[3]); 00200 inline float CalcRatio(float value, float trim, float limit); 00201 bool CheckSW_Up(Channel ch); 00202 int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min); 00203 inline int16_t SetTHRinRatio(float ratio); 00204 00205 //sbus割り込み 00206 void Update_PWM(); //マニュアル・自動モードのpwmデータを整形しpwm変数に入力 00207 void Output_PWM(int16_t pwm[6]); //pwmをサーボへ出力 00208 00209 //シリアル割り込み 00210 void SendSerial(); //1文字きたら送り返す 00211 void SendArray(); 00212 void getSF_Serial(); 00213 float ConvertByteintoFloat(char high, char low); 00214 void ISR_Serial_Rx(); 00215 00216 //SD設定 00217 int GetParameter(FILE *fp, const char *paramName,char parameter[]); 00218 int SetOptions(float *g_kpELE, float *g_kiELE, float *g_kdELE, 00219 float *g_kpRUD, float *g_kiRUD, float *g_kdRUD, 00220 float *g_rightloopROLL, float *g_rightloopPITCH, 00221 float *g_leftloopROLL, float *g_leftloopPITCH, 00222 float *g_gostraightROLL, float *g_gostraightPITCH, 00223 float *g_takeoffTHR, float *g_loopTHR, 00224 float *g_rightloopROLLshort, float *g_rightloopPITCHshort, 00225 float *g_leftloopROLLshort, float *g_leftloopPITCHshort, 00226 float *g_glideloopROLL, float *g_glideloopPITCH); 00227 //switch2割り込み 00228 void ResetTrim(); 00229 00230 //自動操縦 00231 void UpdateTargetAngle_GoStraight(float targetAngle[3]); 00232 void UpdateTargetAngle_Rightloop(float targetAngle[3]); 00233 void UpdateTargetAngle_Rightloop_short(float targetAngle[3]); 00234 void UpdateTargetAngle_Leftloop(float targetAngle[3]); 00235 void UpdateTargetAngle_Leftloop_short(float targetAngle[3]); 00236 void UpdateTargetAngle_Moebius(float targetAngle[3]); 00237 void UpdateTargetAngle_Glide(float targetAngle[3]); 00238 void UpdateTargetAngle_Takeoff(float targetAngle[3]); 00239 void UpdateTargetAngle_Approach(float targetAngle[3]); 00240 void Take_off_and_landing(float targetAngle[3]); 00241 00242 int Rotate(float targetAngle[3], float TargetYAW); 00243 00244 //投下 00245 void Chicken_Drop(); 00246 void ReturnChickenServo1(); 00247 void ReturnChickenServo2(); 00248 00249 //超音波による高度補正 00250 void checkHeight(float targetAngle[3]); 00251 void UpdateTargetAngle_NoseUP(float targetAngle[3]); 00252 void UpdateTargetAngle_NoseDOWN(float targetAngle[3]); 00253 00254 //デバッグ用 00255 void DebugPrint(); 00256 00257 /*---関数のプロトタイプ宣言終わり---*/ 00258 00259 int main() 00260 { 00261 setup(); 00262 00263 00264 while(1){ 00265 00266 loop(); 00267 00268 if(!CheckSW_Up(Ch7)){ 00269 led3=0; 00270 }else{ 00271 led3=1; 00272 } 00273 } 00274 00275 } 00276 00277 void setup(){ 00278 //buzzer = 0; 00279 led1 = 1; 00280 led2 = 1; 00281 led3 = 1; 00282 led4 = 1; 00283 00284 SetOptions(&g_kpELE, &g_kiELE, &g_kdELE, 00285 &g_kpRUD, &g_kiRUD, &g_kdRUD, 00286 &g_rightloopROLL, &g_rightloopPITCH, 00287 &g_leftloopROLL, &g_leftloopPITCH, 00288 &g_gostraightROLL, &g_gostraightPITCH, 00289 &g_takeoffTHR, &g_loopTHR, 00290 &g_rightloopROLLshort, &g_rightloopPITCHshort, 00291 &g_leftloopROLLshort, &g_leftloopPITCHshort, 00292 &g_glideloopROLL, &g_glideloopPITCH); 00293 00294 00295 Init_PWM(); 00296 Init_servo(); 00297 Init_sbus(); 00298 Init_sensors(); 00299 //switch2.rise(ResetTrim); 00300 pc.attach(getSF_Serial, Serial::RxIrq); 00301 USsensor.attach(&UpdateDist, 0.05); 00302 00303 NVIC_SetPriority(USART1_IRQn,0); 00304 NVIC_SetPriority(EXTI0_IRQn,1); 00305 NVIC_SetPriority(TIM5_IRQn,2); 00306 NVIC_SetPriority(EXTI9_5_IRQn,3); 00307 NVIC_SetPriority(USART2_IRQn,4); 00308 DisplayClock(); 00309 t.start(); 00310 00311 00312 pc.printf("MPU calibration start\r\n"); 00313 00314 float offsetstart = t.read(); 00315 while(t.read() - offsetstart < 26){ 00316 SensingMPU(); 00317 for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); 00318 pc.printf("\r\n"); 00319 led1 = !led1; 00320 led2 = !led2; 00321 led3 = !led3; 00322 led4 = !led4; 00323 } 00324 00325 FirstROLL = nowAngle[ROLL]; 00326 FirstPITCH = nowAngle[PITCH]; 00327 nowAngle[ROLL] -=FirstROLL; 00328 nowAngle[PITCH] -=FirstPITCH; 00329 00330 led1 = 0; 00331 led2 = 0; 00332 led3 = 0; 00333 led4 = 0; 00334 wait(0.2); 00335 00336 00337 pc.printf("All initialized\r\n"); 00338 } 00339 00340 void loop(){ 00341 static float targetAngle[3] = {0.0, 0.0, 0.0}, controlValue[2] = {0.0, 0.0}; 00342 00343 SensingMPU(); 00344 UpdateTargetAngle(targetAngle); 00345 //Rotate(targetAngle, 30.0); 00346 CalculateControlValue(targetAngle, controlValue); 00347 UpdateAutoPWM(controlValue); 00348 wait_ms(25); 00349 #if DEBUG_PRINT_INLOOP 00350 DebugPrint(); 00351 #endif 00352 } 00353 00354 //サーボ初期化関数 00355 void Init_servo(){ 00356 00357 servo1.period_ms(14); 00358 servo1.pulsewidth_us(trimpwm[AIL_R]); 00359 00360 servo2.period_ms(14); 00361 servo2.pulsewidth_us(trimpwm[ELE]); 00362 00363 servo3.period_ms(14); 00364 servo3.pulsewidth_us(trimpwm[THR]); 00365 00366 servo4.period_ms(14); 00367 servo4.pulsewidth_us(trimpwm[RUD]); 00368 00369 servo5.period_ms(14); 00370 servo5.pulsewidth_us(1392); 00371 00372 servo6.period_ms(14); 00373 servo6.pulsewidth_us(trimpwm[AIL_L]); 00374 00375 pc.printf("servo initialized\r\n"); 00376 } 00377 00378 //Sbus初期化 00379 void Init_sbus(){ 00380 sbus.initialize(); 00381 sbus.setLastfuncPoint(Update_PWM); 00382 sbus.startInterrupt(); 00383 } 00384 00385 void Init_sensors(){ 00386 if(mpu6050.setup() == -1){ 00387 pc.printf("failed initialize\r\n"); 00388 while(1){ 00389 led1 = 1; led2 = 0; led3 = 1; led4 = 0; 00390 wait(1); 00391 led1 = 0; led2 = 1; led3 = 0; led4 = 1; 00392 wait(1); 00393 } 00394 } 00395 } 00396 00397 void Init_PWM(){ 00398 for (uint8_t i = 0; i < 6; ++i){ 00399 trimpwm[i] = 1500 + (int16_t)(lengthdivpwm * (trim[i]/100)); 00400 maxpwm[i] = 1500 + (int16_t)(lengthdivpwm * (expMax[i]/100)); 00401 minpwm[i] = 1500 - (int16_t)(lengthdivpwm * (expMin[i]/100)); 00402 } 00403 pc.printf("PWM initialized\r\n"); 00404 } 00405 00406 void DisplayClock(){ 00407 pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000); 00408 pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000); 00409 pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000); 00410 pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000); 00411 pc.printf("\r\n"); 00412 } 00413 00414 void UpdateTargetAngle(float targetAngle[3]){ 00415 static int16_t count_op = 0; 00416 #if DEBUG_SEMIAUTO 00417 switch(operation_mode){ 00418 case StartUp: 00419 if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){ 00420 count_op++; 00421 if(count_op > changeModeCount){ 00422 operation_mode = SemiAuto; 00423 pc.printf("Goto SemiAuto mode\r\n"); 00424 count_op = 0; 00425 } 00426 }else count_op = 0; 00427 break; 00428 00429 case SemiAuto: 00430 /* 大会用では以下のif文を入れてoperation_modeを変える 00431 if(CheckSW_Up(Ch6)){ 00432 count_op++; 00433 if(count_op>changeModeCount){ 00434 output_status = XXX; 00435 led2 = 0; 00436 pc.printf("Goto XXX mode\r\n"); 00437 count_op = 0; 00438 }else count_op = 0; 00439 ConvertPWMintoRAD(targetAngle); 00440 } 00441 */ 00442 ConvertPWMintoRAD(targetAngle); 00443 break; 00444 00445 default: 00446 operation_mode = SemiAuto; 00447 break; 00448 } 00449 00450 #else 00451 00452 switch(operation_mode){ 00453 case StartUp: 00454 if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){ //ch7;自動・手動切り替え ch8;自動操縦モード切替 00455 count_op++; 00456 if(count_op > changeModeCount){ 00457 operation_mode = RightLoop; 00458 pc.printf("Goto RightLoop mode\r\n"); 00459 count_op = 0; 00460 } 00461 }else count_op = 0; 00462 break; 00463 00464 case RightLoop: 00465 if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){ 00466 count_op++; 00467 if(count_op > changeModeCount){ 00468 operation_mode = LeftLoop; 00469 pc.printf("Goto LeftLoop mode\r\n"); 00470 count_op = 0; 00471 } 00472 }else count_op = 0; 00473 UpdateTargetAngle_Rightloop(targetAngle); 00474 00475 break; 00476 00477 case LeftLoop: 00478 if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){ 00479 count_op++; 00480 if(count_op > changeModeCount){ 00481 operation_mode = GoStraight; 00482 pc.printf("Goto GoStraight mode\r\n"); 00483 count_op = 0; 00484 } 00485 }else count_op = 0; 00486 UpdateTargetAngle_Leftloop(targetAngle); 00487 break; 00488 00489 case GoStraight: 00490 if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){ 00491 count_op++; 00492 if(count_op > changeModeCount){ 00493 operation_mode = Moebius; 00494 pc.printf("Goto Moebius mode\r\n"); 00495 count_op = 0; 00496 } 00497 }else count_op = 0; 00498 UpdateTargetAngle_GoStraight(targetAngle); 00499 break; 00500 00501 case Moebius: 00502 if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){ 00503 count_op++; 00504 if(count_op > changeModeCount){ 00505 operation_mode = Glide; 00506 pc.printf("Goto Glide mode\r\n"); 00507 count_op = 0; 00508 } 00509 }else count_op = 0; 00510 UpdateTargetAngle_Moebius(targetAngle); 00511 break; 00512 00513 case Glide: 00514 if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){ 00515 count_op++; 00516 if(count_op > changeModeCount){ 00517 operation_mode = BombwithPC; 00518 pc.printf("Goto Bombing mode\r\n"); 00519 count_op = 0; 00520 } 00521 }else count_op = 0; 00522 UpdateTargetAngle_Glide(targetAngle); 00523 break; 00524 00525 case BombwithPC: 00526 if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){ 00527 count_op++; 00528 if(count_op > changeModeCount){ 00529 operation_mode = RightLoop; 00530 pc.printf("Goto RightLoop mode\r\n"); 00531 count_op = 0; 00532 } 00533 }else count_op = 0; 00534 Take_off_and_landing(targetAngle); 00535 break; 00536 00537 default: 00538 operation_mode = StartUp; 00539 break; 00540 } 00541 #endif 00542 00543 if(CheckSW_Up(Ch7)){ 00544 output_status = Auto; 00545 led1 = 1; 00546 }else{ 00547 output_status = Manual; 00548 led1 = 0; 00549 } 00550 } 00551 00552 int GetParameter(FILE *fp, const char *paramName,char parameter[]){ 00553 int i=0, j=0; 00554 int strmax = 200; 00555 char str[strmax]; 00556 00557 rewind(fp); //ファイル位置を先頭に 00558 while(1){ 00559 if (fgets(str, strmax, fp) == NULL) { 00560 return 0; 00561 } 00562 if (!strncmp(str, paramName, strlen(paramName))) { 00563 while (str[i++] != '=') {} 00564 while (str[i] != '\n') { 00565 parameter[j++] = str[i++]; 00566 } 00567 parameter[j] = '\0'; 00568 return 1; 00569 } 00570 } 00571 return 0; 00572 } 00573 00574 00575 //sdによる設定 00576 int SetOptions(float *g_kpELE, float *g_kiELE, float *g_kdELE, 00577 float *g_kpRUD, float *g_kiRUD, float *g_kdRUD, 00578 float *g_rightloopROLL, float *g_rightloopPITCH, 00579 float *g_leftloopROLL, float *g_leftloopPITCH, 00580 float *g_gostraightROLL, float *g_gostraightPITCH, 00581 float *g_takeoffTHR, float *g_loopTHR, 00582 float *g_rightloopROLLshort, float *g_rightloopPITCHshort, 00583 float *g_leftloopROLLshort, float *g_leftloopPITCHshort, 00584 float *g_glideloopROLL, float *g_glideloopPITCH 00585 ){ 00586 00587 pc.printf("SDsetup start.\r\n"); 00588 00589 FILE *fp; 00590 char parameter[30]; //文字列渡す用の配列 00591 int SDerrorcount = 0; //取得できなかった数を返す 00592 const char *paramNames[] = { 00593 "KP_ELEVATOR", 00594 "KI_ELEVATOR", 00595 "KD_ELEVATOR", 00596 "KP_RUDDER", 00597 "KI_RUDDER", 00598 "KD_RUDDER", 00599 "RIGHTLOOP_ROLL", 00600 "RIGHTLOOP_PITCH", 00601 "LEFTLOOP_ROLL", 00602 "LEFTLOOP_PITCH", 00603 "GOSTRAIGHT_ROLL", 00604 "GOSTRAIGHT_PITCH", 00605 "TAKEOFF_THR_RATE", 00606 "LOOP_THR_RATE", 00607 "RIGHTLOOP_ROLL_SHORT", 00608 "RIGHTLOOP_PITCH_SHORT", 00609 "LEFTLOOP_ROLL_SHORT", 00610 "LEFTLOOP_PITCH_SHORT", 00611 "AUTOGLIDE_ROLL", 00612 "AUTOGLIDE PITCH" 00613 }; 00614 00615 fp = fopen("/sd/option.txt","r"); 00616 00617 if(fp != NULL){ //開けたら 00618 pc.printf("File was openned.\r\n"); 00619 if(GetParameter(fp,paramNames[0],parameter)) *g_kpELE = atof(parameter); 00620 else{ *g_kpELE = KP_ELE; 00621 SDerrorcount++; 00622 } 00623 if(GetParameter(fp,paramNames[1],parameter)) *g_kiELE = atof(parameter); 00624 else{ *g_kiELE = KI_ELE; 00625 SDerrorcount++; 00626 } 00627 if(GetParameter(fp,paramNames[2],parameter)) *g_kdELE = atof(parameter); 00628 else{ *g_kdELE = KD_ELE; 00629 SDerrorcount++; 00630 } 00631 if(GetParameter(fp,paramNames[3],parameter)) *g_kpRUD = atof(parameter); 00632 else{ *g_kpRUD = KP_RUD; 00633 SDerrorcount++; 00634 } 00635 if(GetParameter(fp,paramNames[4],parameter)) *g_kiRUD = atof(parameter); 00636 else{ *g_kiRUD = KI_RUD; 00637 SDerrorcount++; 00638 } 00639 if(GetParameter(fp,paramNames[5],parameter)) *g_kdRUD = atof(parameter); 00640 else{ *g_kdRUD = KD_RUD; 00641 SDerrorcount++; 00642 } 00643 if(GetParameter(fp,paramNames[6],parameter)) *g_rightloopROLL = atof(parameter); 00644 else{ *g_rightloopROLL = RIGHT_ROLL; 00645 SDerrorcount++; 00646 } 00647 if(GetParameter(fp,paramNames[7],parameter)) *g_rightloopPITCH = atof(parameter); 00648 else{ *g_rightloopPITCH = RIGHT_PITCH; 00649 SDerrorcount++; 00650 } 00651 if(GetParameter(fp,paramNames[8],parameter)) *g_leftloopROLL = atof(parameter); 00652 else{ *g_leftloopROLL = LEFT_ROLL; 00653 SDerrorcount++; 00654 } 00655 if(GetParameter(fp,paramNames[9],parameter)) *g_leftloopPITCH = atof(parameter); 00656 else{ *g_leftloopPITCH = LEFT_PITCH; 00657 SDerrorcount++; 00658 } 00659 if(GetParameter(fp,paramNames[10],parameter)) *g_gostraightROLL = atof(parameter); 00660 else{ *g_gostraightROLL = STRAIGHT_ROLL; 00661 SDerrorcount++; 00662 } 00663 if(GetParameter(fp,paramNames[11],parameter)) *g_gostraightPITCH = atof(parameter); 00664 else{ *g_gostraightPITCH = STRAIGHT_PITCH; 00665 SDerrorcount++; 00666 } 00667 if(GetParameter(fp,paramNames[12],parameter)) *g_takeoffTHR = atof(parameter); 00668 else{ *g_takeoffTHR = TAKEOFF_THR; 00669 SDerrorcount++; 00670 } 00671 if(GetParameter(fp,paramNames[13],parameter)) *g_loopTHR = atof(parameter); 00672 else{ *g_loopTHR = LOOP_THR; 00673 SDerrorcount++; 00674 } 00675 if(GetParameter(fp,paramNames[14],parameter)) *g_rightloopROLLshort = atof(parameter); 00676 else{ *g_rightloopROLLshort = RIGHT_ROLL_SHORT; 00677 SDerrorcount++; 00678 } 00679 if(GetParameter(fp,paramNames[15],parameter)) *g_rightloopPITCHshort = atof(parameter); 00680 else{ *g_rightloopPITCHshort = RIGHT_PITCH_SHORT; 00681 SDerrorcount++; 00682 } 00683 if(GetParameter(fp,paramNames[16],parameter)) *g_leftloopROLLshort = atof(parameter); 00684 else{ *g_leftloopROLLshort = LEFT_ROLL_SHORT; 00685 SDerrorcount++; 00686 } 00687 if(GetParameter(fp,paramNames[17],parameter)) *g_leftloopPITCHshort = atof(parameter); 00688 else{ *g_leftloopPITCHshort = LEFT_PITCH_SHORT; 00689 SDerrorcount++; 00690 } 00691 if(GetParameter(fp,paramNames[18],parameter)) *g_glideloopROLL = atof(parameter); 00692 else{ *g_glideloopROLL = GLIDE_ROLL; 00693 SDerrorcount++; 00694 } 00695 if(GetParameter(fp,paramNames[19],parameter)) *g_glideloopPITCH = atof(parameter); 00696 else{ *g_glideloopPITCH = GLIDE_PITCH; 00697 SDerrorcount++; 00698 } 00699 fclose(fp); 00700 00701 }else{ //ファイルがなかったら 00702 pc.printf("fp was null.\r\n"); 00703 *g_kpELE = KP_ELE; 00704 *g_kiELE = KI_ELE; 00705 *g_kdELE = KD_ELE; 00706 *g_kpRUD = KP_RUD; 00707 *g_kiRUD = KI_RUD; 00708 *g_kdRUD = KD_RUD; 00709 *g_rightloopROLL = RIGHT_ROLL; 00710 *g_rightloopPITCH = RIGHT_PITCH; 00711 *g_leftloopROLL = LEFT_ROLL; 00712 *g_leftloopPITCH = LEFT_PITCH; 00713 *g_gostraightROLL = STRAIGHT_ROLL; 00714 *g_gostraightPITCH = STRAIGHT_PITCH; 00715 *g_takeoffTHR = TAKEOFF_THR; 00716 *g_loopTHR = LOOP_THR; 00717 SDerrorcount = -1; 00718 } 00719 pc.printf("SDsetup finished.\r\n"); 00720 if(SDerrorcount == 0) pc.printf("setting option is success\r\n"); 00721 else if(SDerrorcount == -1) pc.printf("ERROR 1. cannot open option\r\n"); 00722 else if(SDerrorcount > 0) pc.printf("ERROR 2. reading parameter is failed[%d]\r\n",SDerrorcount); 00723 00724 pc.printf("kpELE = %f, kiELE = %f, kdELE = %f\r\n", *g_kpRUD, *g_kiRUD, *g_kdRUD); 00725 pc.printf("kpRUD = %f, kiRUD = %f, kdRUD = %f\r\n", *g_kpELE, *g_kiELE, *g_kdELE); 00726 pc.printf("rightloopROLL = %f, rightloopPITCH = %f\r\n", *g_rightloopROLL, *g_rightloopPITCH); 00727 pc.printf("leftloopROLL = %f, g_leftloopPITCH = %f\r\n", *g_leftloopROLL, *g_leftloopPITCH); 00728 pc.printf("gostraightROLL = %f, g_gostraightPITCH = %f\r\n", *g_gostraightROLL, *g_gostraightPITCH); 00729 pc.printf("g_takeoffTHR = %f, g_loopTHR = %f\r\n", *g_takeoffTHR, *g_loopTHR); 00730 pc.printf("rightloopROLLshort = %f, rightloopPITCHshort = %f\r\n", *g_rightloopROLLshort, *g_rightloopPITCHshort); 00731 pc.printf("leftloopROLLshort = %f, g_leftloopPITCHshort = %f\r\n", *g_leftloopROLLshort, *g_leftloopPITCHshort); 00732 pc.printf("glideROLL = %f, glidePITCH = %f\r\n", *g_glideloopROLL, *g_glideloopPITCH); 00733 00734 return SDerrorcount; 00735 } 00736 00737 void CalculateControlValue(float targetAngle[3], float controlValue[3]){ 00738 static int t_last; 00739 int t_now; 00740 float dt; 00741 00742 t_now = t.read_us(); 00743 dt = (float)((t_now - t_last)/1000000.0f) ; 00744 t_last = t_now; 00745 00746 00747 //controlValue[ROLL] = pid_RUD.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt); 00748 controlValue[ROLL] = pid_AIL.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt); //エルロンでロール制御 00749 controlValue[PITCH] = pid_ELE.calcPID(nowAngle[PITCH], targetAngle[PITCH], dt); 00750 } 00751 00752 void UpdateAutoPWM(float controlValue[3]){ 00753 int16_t addpwm[2]; //-500~500 00754 addpwm[PITCH] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[PITCH]; //センサ：機首下げ正 レバー:機首上げ正 00755 addpwm[ROLL] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[ROLL]; //センサ：右回転正（8月13日時点；左回転が正！） レバー:右回転正 00756 00757 autopwm[ELE] = trimpwm[ELE] + reverce[ELE] * addpwm[PITCH]; //rewrite 00758 autopwm[AIL_R] = trimpwm[AIL_R] - reverce[AIL_R] * addpwm[ROLL]; 00759 //autopwm[THR] = oldTHR; 00760 00761 autopwm[ELE] = ThresholdMaxMin(autopwm[ELE], maxpwm[ELE], minpwm[ELE]); 00762 autopwm[AIL_R] = ThresholdMaxMin(autopwm[AIL_R], maxpwm[AIL_R], minpwm[AIL_R]); 00763 00764 } 00765 00766 inline float CalcRatio(float value, float trim, float limit){ 00767 return (value - trim) / (limit - trim); 00768 } 00769 00770 bool CheckSW_Up(Channel ch){ 00771 if(SWITCH_CHECK < sbus.manualpwm[ch]){ 00772 return true; 00773 }else{ 00774 return false; 00775 } 00776 } 00777 00778 int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min){ 00779 if(value > max) return max; 00780 if(value < min) return min; 00781 return value; 00782 } 00783 00784 inline int16_t SetTHRinRatio(float ratio){ 00785 return minpwm[THR] + (int16_t)(2 * lengthdivpwm * ratio); 00786 } 00787 00788 00789 00790 /*---SBUS割り込み処理---*/ 00791 00792 //udate_Inputで抽出したpwmデータを整形して各変数に代入する。(マニュアルモード) 00793 //各stabiGylo関数で算出したpwmを各変数に代入する(自動モード) 00794 void Update_PWM() 00795 { 00796 static int16_t pwm[6]; 00797 static int16_t temppwm[6]={trimpwm[0],trimpwm[1],trimpwm[2],trimpwm[3],trimpwm[4],trimpwm[5]}; 00798 if(sbus.flg_ch_update == true){ 00799 switch(output_status){ //マニュアルモード,自動モード,自動着陸もモードを切替 00800 case Manual: 00801 for(uint8_t i=0;i<6;i++){ 00802 pwm[i] = sbus.manualpwm[i]; 00803 } 00804 /*pc.printf("%d ,",pwm[0]);//R 00805 pc.printf("%d ,\r\n",pwm[5]);//L*/ 00806 oldTHR = sbus.manualpwm[THR]; 00807 //pc.printf("update_manual\r\n"); 00808 break; 00809 00810 case Auto: 00811 pwm[AIL_R] = autopwm[AIL_R]; //sbus.manualpwm[AIL]; 00812 pwm[ELE] = autopwm[ELE]; 00813 pwm[THR] = autopwm[THR]; 00814 pwm[RUD] = autopwm[RUD]; 00815 pwm[DROP] = autopwm[DROP]; 00816 pwm[AIL_L] = autopwm[AIL_L]; 00817 pc.printf("%d ,",pwm[AIL_R]);//R 00818 pc.printf("%d ,\r\n",pwm[AIL_L]);//L 00819 //pc.printf("update_auto\r\n"); 00820 00821 break; 00822 00823 default: 00824 for(uint8_t i=0;i<6;i++){ 00825 pwm[i] = sbus.manualpwm[i]; 00826 } //pc.printf("update_manual\r\n"); 00827 break; 00828 } 00829 for(uint8_t i=0;i<6;i++){ 00830 if(pwm[i]<1000 || pwm[i]>2000) pwm[i]=temppwm[i]; 00831 temppwm[i]=pwm[i]; 00832 } 00833 }else{ 00834 pc.printf("0\r\n"); 00835 } 00836 sbus.flg_ch_update = false; 00837 Output_PWM(pwm); 00838 } 00839 00840 00841 //pwmをサーボに出力。 00842 void Output_PWM(int16_t pwm[5]) 00843 { 00844 servo1.pulsewidth_us(pwm[0]); 00845 servo2.pulsewidth_us(pwm[1]); 00846 servo3.pulsewidth_us(pwm[2]); 00847 servo4.pulsewidth_us(pwm[3]); 00848 servo5.pulsewidth_us(pwm[4]); 00849 servo6.pulsewidth_us(pwm[5]); 00850 00851 } 00852 00853 void ResetTrim(){ 00854 for(uint8_t i=0; i<6; i++){ //i=4から書き換え＿投下サーボは入ってない模様 00855 trimpwm[i] = sbus.manualpwm[i]; 00856 } 00857 pc.printf("reset PWM trim\r\n"); 00858 } 00859 00860 00861 void SensingMPU(){ 00862 //static int16_t deltaT = 0, t_start = 0; 00863 //t_start = t.read_us(); 00864 00865 float rpy[3] = {0}, oldrpy[3] = {0}; 00866 static uint16_t count_changeRPY = 0; 00867 static bool flg_checkoutlier = false; 00868 NVIC_DisableIRQ(USART1_IRQn); 00869 NVIC_DisableIRQ(USART2_IRQn); 00870 NVIC_DisableIRQ(TIM5_IRQn); 00871 NVIC_DisableIRQ(EXTI0_IRQn); 00872 NVIC_DisableIRQ(EXTI9_5_IRQn); 00873 00874 mpu6050.getRollPitchYaw_Skipper(rpy); 00875 00876 NVIC_EnableIRQ(USART1_IRQn); 00877 NVIC_EnableIRQ(USART2_IRQn); 00878 NVIC_EnableIRQ(TIM5_IRQn); 00879 NVIC_EnableIRQ(EXTI0_IRQn); 00880 NVIC_EnableIRQ(EXTI9_5_IRQn); 00881 00882 00883 //外れ値対策 00884 for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI; 00885 rpy[ROLL] -= FirstROLL; 00886 rpy[PITCH] -= FirstPITCH; 00887 rpy[YAW] -= FirstYAW; 00888 00889 for(uint8_t i=0; i<3; i++) {if(rpy[i] < nowAngle[i]-10 || rpy[i] > nowAngle[i]+10) {flg_checkoutlier = true;}} 00890 if(!flg_checkoutlier || count_changeRPY >= 2){ 00891 for(uint8_t i=0; i<3; i++){ 00892 nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f; //2つの移動平均 00893 } 00894 count_changeRPY = 0; 00895 }else count_changeRPY++; 00896 flg_checkoutlier = false; 00897 00898 } 00899 00900 float TranslateNewYaw(float beforeYaw, float newzeroYaw){ 00901 float newYaw = beforeYaw - newzeroYaw; 00902 00903 if(newYaw<-180.0f) newYaw += 360.0f; 00904 else if(newYaw>180.0f) newYaw -= 360.0f; 00905 return newYaw; 00906 } 00907 00908 00909 void getSF_Serial(){ 00910 00911 static char SFbuf[16]; 00912 static int bufcounter=0; 00913 00914 SFbuf[bufcounter]=pc.getc(); 00915 if(SFbuf[0]=='S'&&bufcounter<5) bufcounter++; 00916 00917 if(bufcounter==5 && SFbuf[4]=='F'){ 00918 g_landingcommand = SFbuf[1]; 00919 if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]); 00920 bufcounter = 0; 00921 memset(SFbuf, 0, strlen(SFbuf)); 00922 pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW); 00923 } 00924 else if(bufcounter>=5 && g_buf[4]!='F'){ 00925 pc.printf("Communication Falsed.\r\n"); 00926 bufcounter = 0; 00927 } 00928 } 00929 00930 float ConvertByteintoFloat(char high, char low){ 00931 00932 //int16_t intvalue = (int16_t)high*256 + (int16_t)low; 00933 int16_t intvalue = (int16_t)(((int16_t)high << 8) | low); // Turn the MSB and LSB into a signed 16-bit value 00934 float floatvalue = (float)intvalue; 00935 return floatvalue; 00936 } 00937 00938 00939 //超音波割り込み 00940 void UpdateDist(){ 00941 g_distance = usensor.get_dist_cm(); 00942 usensor.start(); 00943 } 00944 00945 //8の字旋回 00946 void UpdateTargetAngle_Moebius(float targetAngle[3]){ 00947 static uint8_t RotateCounter=0; 00948 static bool flg_setInStartAuto = false; 00949 static float FirstYAW_Moebius = 0.0; 00950 float newYaw_Moebius; 00951 00952 if(!flg_setInStartAuto && CheckSW_Up(Ch7)){ 00953 FirstYAW_Moebius = nowAngle[YAW]; 00954 RotateCounter = 0; 00955 flg_setInStartAuto = true; 00956 }else if(!CheckSW_Up(Ch7)){ 00957 flg_setInStartAuto = false; 00958 led2 = 0; 00959 } 00960 autopwm[THR]=oldTHR; 00961 00962 newYaw_Moebius = TranslateNewYaw(nowAngle[YAW], FirstYAW_Moebius); 00963 00964 if(RotateCounter == 0 && newYaw_Moebius >90.0 && newYaw_Moebius < 180.0) {RotateCounter++; led2 = 1; pc.printf("Rotate 90\r\n");} 00965 if(RotateCounter == 1 && newYaw_Moebius >-180.0 && newYaw_Moebius < -90.0) {RotateCounter++; led2 = 0; pc.printf("Rotate 180\r\n");} 00966 if(RotateCounter == 2 && newYaw_Moebius >-90.0 && newYaw_Moebius <-10.0) {RotateCounter++; led2 = 1; pc.printf("Rotate 270\r\n");} 00967 if(RotateCounter == 3 && newYaw_Moebius >0.0 && newYaw_Moebius < 90.0) {RotateCounter++; led2 = 0; pc.printf("Change Rotate direction\r\n");} 00968 00969 00970 if(RotateCounter <= 3) UpdateTargetAngle_Rightloop_short(targetAngle); 00971 else UpdateTargetAngle_Leftloop_short(targetAngle); //左旋回 00972 00973 } 00974 00975 //自動滑空 00976 void UpdateTargetAngle_Glide(float targetAngle[3]){ 00977 static int THRcount = 0; 00978 static int t_start = 0; 00979 static bool flg_tstart = false; 00980 int t_diff = 0; 00981 static int groundcount = 0; 00982 00983 targetAngle[ROLL] = g_glideloopROLL; 00984 targetAngle[PITCH] = g_glideloopPITCH; 00985 00986 // autopwm[THR]=oldTHR; 00987 //シリアル通信受信の割り込みイベント登録 00988 pc.attach(ISR_Serial_Rx, Serial::RxIrq); 00989 00990 00991 //時間計測開始設定 00992 if(!flg_tstart && CheckSW_Up(Ch7)){ 00993 t_start = t.read(); 00994 flg_tstart = true; 00995 pc.printf("timer start\r\n"); 00996 }else if(!CheckSW_Up(Ch7)){ 00997 t_start = 0; 00998 flg_tstart = false; 00999 } 01000 01001 01002 //フラグが偽であれば計測は行わない 01003 if(flg_tstart){ 01004 t_diff = t.read() - t_start; 01005 //一定高度or15秒でled点灯 01006 if((groundcount>5 && g_distance>0) || t_diff > 15){ 01007 led2 = 1; 01008 //pc.printf("Call [Stop!] calling!\r\n"); 01009 } 01010 if(g_distance<180 && g_distance > 0) groundcount++; 01011 }else{ 01012 t_diff = 0; 01013 groundcount = 0; 01014 led2 = 0; 01015 } 01016 01017 if(t_diff > 17){ 01018 autopwm[THR] = SetTHRinRatio(0.5); 01019 }else{ 01020 if(g_distance<150 && g_distance>0 ){ 01021 THRcount++; 01022 if(THRcount>5){ 01023 autopwm[THR] = SetTHRinRatio(0.6); 01024 //pc.printf("throttle ON\r\n"); 01025 } 01026 }else{ 01027 autopwm[THR] = 1180; 01028 THRcount = 0; 01029 } 01030 } 01031 } 01032 //離陸-投下-着陸一連 01033 void Take_off_and_landing(float targetAngle[3]){ 01034 if(!CheckSW_Up(Ch7)) bombing_mode = Takeoff; 01035 01036 switch(bombing_mode){ 01037 case Takeoff: 01038 static bool flg_setFirstYaw = false; 01039 static int TakeoffCount = 0; 01040 01041 if(!flg_setFirstYaw && CheckSW_Up(Ch7)){ 01042 FirstYAW = nowAngle[YAW]; 01043 flg_setFirstYaw = true; 01044 }else if(flg_setFirstYaw && !CheckSW_Up(Ch7)){ 01045 flg_setFirstYaw = false; 01046 } 01047 01048 UpdateTargetAngle_Takeoff(targetAngle); 01049 01050 if(g_distance>150) TakeoffCount++; 01051 else TakeoffCount = 0; 01052 if(TakeoffCount>5){ 01053 autopwm[THR] = 1180+320*2*0.5; 01054 pc.printf("Now go to Approach mode!!"); 01055 bombing_mode = Approach; 01056 } 01057 break; 01058 /* 01059 case Chicken: 01060 break; 01061 */ 01062 case Transition: 01063 static int ApproachCount = 0; 01064 targetAngle[YAW]=180.0; 01065 int Judge = Rotate(targetAngle, g_SerialTargetYAW); 01066 01067 if(Judge==0) ApproachCount++; 01068 if(ApproachCount>5) bombing_mode = Approach; 01069 break; 01070 01071 case Approach: 01072 UpdateTargetAngle_Approach(targetAngle); 01073 break; 01074 01075 default: 01076 bombing_mode = Takeoff; 01077 break; 01078 } 01079 } 01080 01081 //離陸モード 01082 void UpdateTargetAngle_Takeoff(float targetAngle[3]){ 01083 //pc.printf("%d \r\n",g_distance); 01084 targetAngle[ROLL] = g_gostraightROLL; 01085 targetAngle[PITCH] = g_loopTHR; 01086 autopwm[THR] = SetTHRinRatio(g_takeoffTHR); //0.7;スロットルの割合 01087 } 01088 01089 //ヨーを目標値にして許容角度になるまで水平旋回 01090 int Rotate(float targetAngle[3], float TargetYAW){ 01091 float diffYaw = TranslateNewYaw(TargetYAW, nowAngle[YAW]); 01092 01093 if(diffYaw > LIMIT_STRAIGHT_YAW){ 01094 /* 01095 if(diffYaw > THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Rightloop_short(targetAngle); 01096 else UpdateTargetAngle_Rightloop(targetAngle); 01097 */ 01098 UpdateTargetAngle_Rightloop_short(targetAngle); 01099 return 1; 01100 }else if(diffYaw < -LIMIT_STRAIGHT_YAW){ 01101 UpdateTargetAngle_Leftloop_short(targetAngle); 01102 /* 01103 if(diffYaw < -THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Leftloop_short(targetAngle); 01104 else UpdateTargetAngle_Leftloop(targetAngle); 01105 */ 01106 return 1; 01107 }else{ 01108 UpdateTargetAngle_GoStraight(targetAngle); 01109 return 0; 01110 } 01111 } 01112 01113 //チキラー投下 01114 void Chicken_Drop(){ 01115 if(CheckSW_Up(Ch7)){ 01116 autopwm[DROP] = 1512; 01117 pc.printf("Bombed!\r\n"); 01118 RerurnChickenServo1.attach(&ReturnChickenServo1, 3); 01119 //operation_mode = Approach; 01120 //buzzer = 0; 01121 pc.printf("Goto LeftLoop mode\r\n"); 01122 } 01123 } 01124 01125 void ReturnChickenServo1(){ 01126 autopwm[DROP] = 1344; 01127 pc.printf("first reverse\r\n"); 01128 RerurnChickenServo2.attach(&ReturnChickenServo2, 1); 01129 } 01130 01131 void ReturnChickenServo2(){ 01132 autopwm[DROP] = 1392; 01133 pc.printf("second reverse\r\n"); 01134 } 01135 01136 //着陸モード（PCからの指令に従う） 01137 void UpdateTargetAngle_Approach(float targetAngle[3]){ 01138 pc.putc(g_buf[0]); 01139 switch(g_buf[0]){ 01140 case 'R': //右旋回セヨ 01141 UpdateTargetAngle_Rightloop(targetAngle); 01142 break; 01143 01144 case 'L': //左旋回セヨ 01145 UpdateTargetAngle_Leftloop(targetAngle); 01146 break; 01147 01148 case 'G': //直進セヨ 01149 UpdateTargetAngle_GoStraight(targetAngle); 01150 break; 01151 01152 case 'Y': //指定ノヨー方向ニ移動セヨ 01153 Rotate(targetAngle, g_SerialTargetYAW); 01154 break; 01155 01156 case 'B': //ブザーヲ鳴ラセ 01157 //buzzer = 1; 01158 break; 01159 01160 case 'D': //物資ヲ落トセ 01161 Chicken_Drop(); 01162 break; 01163 01164 case 'C': //停止セヨ 01165 targetAngle[ROLL] = 0.0; 01166 targetAngle[PITCH] = -3.0; 01167 autopwm[THR] = minpwm[THR]; 01168 break; 01169 } 01170 } 01171 01172 void checkHeight(float targetAngle[3]){ 01173 01174 static int targetHeight = 200; 01175 01176 if(g_distance < targetHeight + ALLOWHEIGHT){ 01177 UpdateTargetAngle_NoseUP(targetAngle); 01178 if(CheckSW_Up(Ch7)) led2 = 1; 01179 } 01180 else if(g_distance > targetHeight - ALLOWHEIGHT){ 01181 UpdateTargetAngle_NoseDOWN(targetAngle); 01182 if(CheckSW_Up(Ch7)) led2 = 1; 01183 } 01184 else led2=0; 01185 } 01186 01187 void UpdateTargetAngle_NoseUP(float targetAngle[3]){ 01188 01189 //targetAngle[PITCH] += 2.0f; 01190 //if(nowAngle[PITCH]<targetAngle[PITCH]) autopwm[THR] = SetTHRinRatio(0.6); 01191 autopwm[THR] = SetTHRinRatio(g_loopTHR+0.05); 01192 //pc.printf("nose UP"); 01193 } 01194 01195 void UpdateTargetAngle_NoseDOWN(float targetAngle[3]){ 01196 01197 //targetAngle[PITCH] -= 2.0f; 01198 autopwm[THR] = SetTHRinRatio(g_loopTHR-0.05); 01199 //pc.printf("nose DOWN"); 01200 } 01201 01202 //直進 01203 void UpdateTargetAngle_GoStraight(float targetAngle[3]){ 01204 01205 targetAngle[ROLL] = g_gostraightROLL; 01206 targetAngle[PITCH] = g_gostraightPITCH; 01207 autopwm[THR] = SetTHRinRatio(g_loopTHR); 01208 01209 //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); 01210 } 01211 01212 01213 //右旋回 01214 void UpdateTargetAngle_Rightloop(float targetAngle[3]){ //右旋回 01215 01216 targetAngle[ROLL] = g_rightloopROLL; 01217 targetAngle[PITCH] = g_rightloopPITCH ; 01218 autopwm[RUD]=rightloopRUD; //RUD固定 01219 autopwm[THR] = SetTHRinRatio(0.5); //手動スロットル記憶 01220 if(autopwm[AIL_R]>trimpwm[AIL_R]){ //エルロン上がりやすさ調節 01221 autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; 01222 } 01223 else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; 01224 autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop; 01225 01226 //checkHeight(targetAngle); 01227 //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); 01228 } 01229 01230 //右旋回（着陸時スロットル０の時） 01231 void UpdateTargetAngle_Rightloop_zero(float targetAngle[3]){ //右旋回 01232 01233 targetAngle[ROLL] = g_rightloopROLL; 01234 targetAngle[PITCH] = g_rightloopPITCH ; 01235 autopwm[RUD]=rightloopRUD; 01236 autopwm[THR] = minpwm[THR];//SetTHRinRatio(g_loopTHR); 01237 if(autopwm[AIL_R]>trimpwm[AIL_R]){ 01238 autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; 01239 } 01240 else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; 01241 01242 //checkHeight(targetAngle); 01243 //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); 01244 } 01245 01246 void UpdateTargetAngle_Rightloop_short(float targetAngle[3]){ //右旋回 01247 01248 targetAngle[ROLL] = g_rightloopROLLshort; 01249 targetAngle[PITCH] = g_rightloopPITCHshort; 01250 autopwm[RUD]=rightloopshortRUD; 01251 autopwm[THR] = oldTHR; 01252 if(autopwm[AIL_R]>trimpwm[AIL_R]){ 01253 autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; 01254 } 01255 else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; 01256 01257 //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); 01258 } 01259 01260 //左旋回 01261 void UpdateTargetAngle_Leftloop(float targetAngle[3]){ 01262 01263 targetAngle[ROLL] = g_leftloopROLL; 01264 targetAngle[PITCH] = g_leftloopPITCH; 01265 autopwm[RUD]=leftloopRUD; 01266 autopwm[THR] = oldTHR; 01267 if(autopwm[AIL_R]>trimpwm[AIL_R]){ 01268 autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; 01269 } 01270 else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; 01271 //checkHeight(targetAngle); 01272 01273 //printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[], targetAngle[PITCH], autopwm[THR]); 01274 } 01275 01276 //左旋回（着陸時スロットル０のとき） 01277 void UpdateTargetAngle_Leftloop_zero(float targetAngle[3]){ 01278 01279 targetAngle[ROLL] = g_leftloopROLL; 01280 targetAngle[PITCH] = g_leftloopPITCH; 01281 autopwm[RUD]=leftloopRUD; 01282 autopwm[THR] = minpwm[THR]; 01283 if(autopwm[AIL_R]>trimpwm[AIL_R]){ 01284 autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; 01285 } 01286 else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; 01287 //autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_Ratio_leftloop; 01288 //checkHeight(targetAngle); 01289 01290 //printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[], targetAngle[PITCH], autopwm[THR]); 01291 } 01292 01293 void UpdateTargetAngle_Leftloop_short(float targetAngle[3]){ 01294 01295 targetAngle[ROLL] = g_leftloopROLLshort; 01296 targetAngle[PITCH] = g_leftloopPITCHshort; 01297 autopwm[RUD]=leftloopRUD; 01298 autopwm[THR] = oldTHR; 01299 if(autopwm[AIL_R]>trimpwm[AIL_R]){ 01300 autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; 01301 } 01302 else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; 01303 01304 //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); 01305 } 01306 void ISR_Serial_Rx() 01307 { 01308 // シリアルの受信処理 01309 char data = pc.getc(); 01310 01311 if(data=='C'){ 01312 autopwm[THR]=minpwm[2]; 01313 wait(60.0); 01314 } 01315 01316 } 01317 01318 //デバッグ用 01319 void DebugPrint(){ 01320 /* 01321 static int16_t deltaT = 0, t_start = 0; 01322 deltaT = t.read_u2s() - t_start; 01323 pc.printf("t:%d us, ",deltaT); 01324 pc.printf("\r\n"); 01325 t_start = t.read_us(); 01326 */ 01327 //for(uint8_t i=0; i<8; i++) pc.printf("%d ",sbus.manualpwm[i]); 01328 //for(uint8_t i=1; i<4; i++) pc.printf("%d ",sbus.manualpwm[i]); 01329 //pc.printf("\r\n"); 01330 //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); 01331 //for(uint8_t i=0; i<2; i++) pc.printf("%3.2f\t",nowAngle[i]); //ロール、ピッチのみ 小数点以下2ケタ 01332 pc.printf("%d\t",autopwm[AIL_L]); // pc.printf("%d\t",autopwm[RUD]); 01333 //pc.printf("%d",g_distance); 01334 01335 //pc.printf("Mode: %c: ",g_buf[0]); 01336 //if(g_buf[0] == 'Y') pc.printf(" %3.1f",g_SerialTargetYAW); 01337 pc.printf("\r\n"); 01338 }
Generated on Sat Jul 23 2022 07:34:11 by
1.7.2