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