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