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