航空研究会
/
Autoflight2018_59
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_58
by 
航空研究会
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 //mbed 00002 #include "mbed.h" 00003 #include "FATFileSystem.h" 00004 #include "SDFileSystem.h" 00005 //C 00006 #include "math.h" 00007 //sensor 00008 #include "MPU6050_DMP6.h" 00009 //#include "MPU9250.h" 00010 //#include "BMP280.h" 00011 #include "hcsr04.h" 00012 //device 00013 #include "sbus.h" 00014 //config 00015 #include "SkipperSv2.h" 00016 #include "falfalla.h" 00017 //other 00018 #include "pid.h" 00019 00020 #define DEBUG_SEMIAUTO 0 00021 #define DEBUG_PRINT_INLOOP 1 00022 00023 #define KP_ELE 15.0 //2.0 00024 #define KI_ELE 0.0 00025 #define KD_ELE 0.0 //0/0 00026 #define KP_RUD 3.0 00027 #define KI_RUD 0.0 00028 #define KD_RUD 0.0 00029 #define KP_AIL 0.1 00030 #define KI_AIL 0.2 00031 #define KD_AIL 0.2 00032 00033 //#define g_AIL_L_Ratio_rightloop 0.5 00034 00035 #define GAIN_CONTROLVALUE_TO_PWM 3.0 00036 00037 #define RIGHT_ROLL -12.0 00038 #define RIGHT_PITCH -10.0 //5.0 00039 #define LEFT_ROLL 12.0 00040 #define LEFT_PITCH -5.0 00041 #define STRAIGHT_ROLL 4.0 00042 #define STRAIGHT_PITCH 3.0 00043 #define TAKEOFF_THR 0.8 00044 #define LOOP_THR 0.6 00045 00046 //#define g_rightloopRUD 1500 00047 00048 #define RIGHT_ROLL_SHORT -12.0 00049 #define RIGHT_PITCH_SHORT -5.0 00050 #define LEFT_ROLL_SHORT 12.0 00051 #define LEFT_PITCH_SHORT -5.0 00052 #define RIGHTLOOPROLL_APPROACH -17.0 00053 #define LEFTLOOPROLL_APPROACH 16.0 00054 #define RIGHTLOOPPITCH_APPROACH -15.0 00055 #define LEFTLOOPPITCH_APPROACH -13.0 00056 00057 #define rightloopROLL2 -10.0 00058 00059 /*#define rightloopRUD 1300 //1250 00060 #define rightloopshortRUD 1250 00061 #define leftloopRUD 1500 00062 #define leftloopshortRUD 1500 00063 #define glideloopRUD 1300 00064 */ 00065 #define AIL_R_correctionrightloop 0 00066 #define AIL_L_correctionrightloop 0 00067 #define AIL_L_correctionrightloopshort 0 00068 #define AIL_L_correctionleftloop -0 00069 #define AIL_L_correctionleftloopshort 0 00070 00071 00072 #define RIGHTLOOP_RUD 1250 00073 #define RIGHTLOOPSHORT_RUD 1250 00074 #define LEFTLOOP_RUD 1500 00075 #define LEFTLOOPSHORT_RUD 1500 00076 #define GLIDELOOP_RUD 1300 00077 #define RIGHTLOOPRUD_APPROACH 1500 00078 #define LEFTLOOPRUD_APPROACH 1500 00079 00080 #define AIL_L_CORRECTION_RIGHTLOOP 0 00081 #define AIL_L_CORRECTION_RIGHTLOOPSHORT 0 00082 #define AIL_L_CORRECTION_LEFTLOOP 0 00083 #define AIL_L_CORRECTION_LEFTLOOPSHORT 0 00084 00085 #define GLIDE_ROLL -12.0 00086 #define GLIDE_PITCH -3.0 00087 00088 00089 #define AIL_L_RatioRising 0.5 00090 #define AIL_L_RatioDescent 2 00091 00092 //コンパスキャリブレーション 00093 //SkipperS2基板 00094 /* 00095 #define MAGBIAS_X -35.0 00096 #define MAGBIAS_Y 535.0 00097 #define MAGBIAS_Z -50.0 00098 */ 00099 //S2v2 1番基板 00100 #define MAGBIAS_X 395.0 00101 #define MAGBIAS_Y 505.0 00102 #define MAGBIAS_Z -725.0 00103 //S2v2 2番基板 00104 /* 00105 #define MAGBIAS_X 185.0 00106 #define MAGBIAS_Y 220.0 00107 #define MAGBIAS_Z -350.0 00108 */ 00109 00110 #define ELEMENT 1 00111 #define LIMIT_STRAIGHT_YAW 5.0 00112 #define THRESHOLD_TURNINGRADIUS_YAW 60.0 00113 #define ALLOWHEIGHT 15 00114 00115 #ifndef PI 00116 #define PI 3.14159265358979 00117 #endif 00118 00119 const int16_t lengthdivpwm = 320; 00120 const int16_t changeModeCount = 6; 00121 00122 00123 SBUS sbus(PA_9, PA_10); //SBUS 00124 00125 PwmOut servo1(PC_6); // TIM3_CH1 //old echo 00126 PwmOut servo2(PC_7); // TIM3_CH2 //PC_7 00127 PwmOut servo3(PB_0); // TIM3_CH3 00128 PwmOut servo4(PB_1); // TIM3_CH4 00129 PwmOut servo5(PB_6); // TIM4_CH1 00130 PwmOut servo6(PB_7); // TIM4_CH2 //old trigger 00131 //PwmOut servo7(PB_8); // TIM4_CH3 //PB_8 new echo 00132 //PwmOut servo8(PB_9); // TIM4_CH4 //new trigger 00133 00134 RawSerial pc(PA_2,PA_3, 115200); //tx,rx.baudrate pin;PA_2=UART2_TX, PA_3=UART2_RX 00135 //RawSerial pc2(PB_6,PB_7, 115200); //sbus確認用 00136 SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd"); 00137 00138 DigitalOut led1(PA_0); //黄色のコネクタ 00139 DigitalOut led2(PA_1); 00140 DigitalOut led3(PB_5); 00141 DigitalOut led4(PB_4); 00142 00143 //InterruptIn switch2(PC_14); 00144 MPU6050DMP6 mpu6050(PC_0,&pc); //割り込みピン，シリアルポインタ i2cのピン指定は MPU6050>>I2Cdev.h 内のdefine 00145 HCSR04 usensor(PB_9,PB_8); //trig,echo 9,8 00146 00147 PID pid_AIL(g_kpAIL,g_kiAIL,g_kdAIL); 00148 PID pid_ELE(g_kpELE,g_kiELE,g_kdELE); 00149 PID pid_RUD(g_kpRUD,g_kiRUD,g_kdRUD); 00150 00151 enum Channel{AIL_R, ELE, THR, RUD, DROP, AIL_L, Ch7, Ch8}; 00152 enum Angle{ROLL, PITCH, YAW}; //yaw:北を0とした絶対角度 00153 enum OperationMode{StartUp, SemiAuto, RightLoop, LeftLoop, GoStraight, BombwithPC, ZERO, Moebius, Glide}; 00154 enum BombingMode{Takeoff, Chicken, Transition, Approach}; 00155 enum OutputStatus{Manual, Auto}; 00156 00157 static OutputStatus output_status = Manual; 00158 OperationMode operation_mode = StartUp; 00159 BombingMode bombing_mode = Takeoff; 00160 00161 static int16_t autopwm[8] = {1453,1448,1176,1626,1417,1450}; 00162 /* 00163 //1号機 00164 static int16_t trimpwm[6] = {1580,1600,1176,1404,1440,1448}; 00165 int16_t maxpwm[6] = {1796,1936,1848,1740,1820,1856}; 00166 int16_t minpwm[6] = {1182,1265,1176,1068,1180,1176}; 00167 const int16_t reverce[4] = {Reverce_falfalla[0],Reverce_falfalla[1],Reverce_falfalla[2],Reverce_falfalla[3]}; 00168 */ 00169 00170 //2号機 00171 00172 static int16_t trimpwm[6] = {1453,1448,1176,1626,1417,1450}; 00173 int16_t maxpwm[6] = {1670,1786,1848,1962,1712,1857}; 00174 int16_t minpwm[6] = {1056,1115,1176,1291,1180,1178}; 00175 const int16_t reverce[4] = {Reverce_falfalla[0],Reverce_falfalla[1],Reverce_falfalla[2],Reverce_falfalla[3]}; 00176 00177 00178 int16_t oldTHR = 1000; 00179 00180 int16_t g_AIL_L_Ratio_rightloop = 0.5; 00181 00182 00183 static float nowAngle[3] = {0,0,0}; 00184 const float trimAngle[3] = {0.0, 0.0, 0.0}; 00185 const float maxAngle[2] = {90, 90}; 00186 const float minAngle[2] = {-90, -90}; 00187 00188 float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0; 00189 00190 unsigned int g_distance; 00191 Ticker USsensor; 00192 static char g_buf[16]; 00193 char g_landingcommand='Z'; 00194 float g_SerialTargetYAW; 00195 00196 Timer t; 00197 Timer t2; 00198 Timeout RerurnChickenServo1; 00199 Timeout RerurnChickenServo2; 00200 00201 /*-----関数のプロトタイプ宣言-----*/ 00202 void setup(); 00203 void loop(); 00204 00205 void Init_PWM(); 00206 void Init_servo(); //サーボ初期化 00207 void Init_sbus(); //SBUS初期化 00208 void Init_sensors(); 00209 void DisplayClock(); //クロック状態確認 00210 00211 //センサの値取得 00212 void SensingMPU(); 00213 void UpdateDist(); 00214 00215 //void offsetRollPitch(float FirstROLL, float FirstPITCH); 00216 //void TransYaw(float FirstYAW); 00217 float TranslateNewYaw(float beforeYaw, float newzeroYaw); 00218 void UpdateTargetAngle(float targetAngle[3]); 00219 void CalculateControlValue(float targetAngle[3], float controlValue[3]); 00220 void UpdateAutoPWM(float controlValue[3]); 00221 void ConvertPWMintoRAD(float targetAngle[3]); 00222 inline float CalcRatio(float value, float trim, float limit); 00223 bool CheckSW_Up(Channel ch); 00224 int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min); 00225 inline int16_t SetTHRinRatio(float ratio); 00226 00227 //sbus割り込み 00228 void Update_PWM(); //マニュアル・自動モードのpwmデータを整形しpwm変数に入力 00229 void Output_PWM(int16_t pwm[6]); //pwmをサーボへ出力 00230 00231 //シリアル割り込み 00232 void getSF_Serial(); 00233 float ConvertByteintoFloat(char high, char low); 00234 00235 00236 //SD設定 00237 int GetParameter(FILE *fp, const char *paramName,char parameter[]); 00238 int SetOptions(float *g_kpELE, float *g_kiELE, float *g_kdELE, 00239 float *g_kpRUD, float *g_kiRUD, float *g_kdRUD, 00240 float *g_rightloopROLL, float *g_rightloopPITCH, 00241 float *g_leftloopROLL, float *g_leftloopPITCH, 00242 float *g_gostraightROLL, float *g_gostraightPITCH, 00243 float *g_takeoffTHR, float *g_loopTHR, 00244 float *g_rightloopROLLshort, float *g_rightloopPITCHshort, 00245 float *g_leftloopROLLshort, float *g_leftloopPITCHshort, 00246 float *g_glideloopROLL, float *g_glideloopPITCH, 00247 float *g_kpAIL, float *g_kiAIL, float *g_kdAIL, 00248 int *g_rightloopRUD, int *g_rightloopshortRUD, 00249 int *g_leftloopRUD, int *g_leftloopshortRUD, 00250 int *g_glideRUD, 00251 float *g_rightloopROLL_approach,float *g_leftloopROLL_approach, 00252 int *g_rightloopRUD_approach,int *g_leftloopRUD_approach, 00253 float *g_rightloopPITCH_approach,float *g_leftloopPITCH_approach 00254 ); 00255 //switch2割り込み 00256 void ResetTrim(); 00257 00258 //自動操縦 00259 void UpdateTargetAngle_GoStraight(float targetAngle[3]); 00260 void UpdateTargetAngle_GoStraight_zero(float targetAngle[3]); //着陸時にスロットルが０の時の直進 00261 void UpdateTargetAngle_Rightloop(float targetAngle[3]); 00262 void UpdateTargetAngle_Rightloop_short(float targetAngle[3]); 00263 void UpdateTargetAngle_Rightloop_zero(float targetAngle[3]); //着陸時にスロットルが０の時の右旋回 00264 void UpdateTargetAngle_Leftloop(float targetAngle[3]); 00265 void UpdateTargetAngle_Leftloop_short(float targetAngle[3]); 00266 void UpdateTargetAngle_Leftloop_zero(float targetAngle[3]); //着陸時にスロットルが０の時の左旋回 00267 void UpdateTargetAngle_Moebius(float targetAngle[3]); 00268 void UpdateTargetAngle_Glide(float targetAngle[3]); 00269 void UpdateTargetAngle_Takeoff(float targetAngle[3]); 00270 void UpdateTargetAngle_Approach(float targetAngle[3]); 00271 void Take_off_and_landing(float targetAngle[3]); 00272 00273 int Rotate(float targetAngle[3], float TargetYAW); 00274 00275 //投下 00276 void Chicken_Drop(); 00277 void ReturnChickenServo1(); 00278 void ReturnChickenServo2(); 00279 00280 //超音波による高度補正 00281 void checkHeight(float targetAngle[3]); 00282 void UpdateTargetAngle_NoseUP(float targetAngle[3]); 00283 void UpdateTargetAngle_NoseDOWN(float targetAngle[3]); 00284 00285 //デバッグ用 00286 void Sbusprintf(); 00287 void DebugPrint(); 00288 00289 /*---関数のプロトタイプ宣言終わり---*/ 00290 00291 int main() 00292 { 00293 setup(); 00294 00295 00296 while(1){ 00297 00298 loop(); 00299 00300 00301 NVIC_DisableIRQ(USART1_IRQn); 00302 if(!CheckSW_Up(Ch7)){ 00303 led3=0; 00304 }else{ 00305 led3=1; 00306 } 00307 NVIC_EnableIRQ(USART1_IRQn); 00308 } 00309 00310 } 00311 00312 void setup(){ 00313 //buzzer = 0; 00314 led1 = 1; 00315 led2 = 1; 00316 led3 = 1; 00317 led4 = 1; 00318 00319 SetOptions(&g_kpELE, &g_kiELE, &g_kdELE, 00320 &g_kpRUD, &g_kiRUD, &g_kdRUD, 00321 &g_rightloopROLL, &g_rightloopPITCH, 00322 &g_leftloopROLL, &g_leftloopPITCH, 00323 &g_gostraightROLL, &g_gostraightPITCH, 00324 &g_takeoffTHR, &g_loopTHR, 00325 &g_rightloopROLLshort, &g_rightloopPITCHshort, 00326 &g_leftloopROLLshort, &g_leftloopPITCHshort, 00327 &g_glideloopROLL, &g_glideloopPITCH, 00328 &g_kpAIL, &g_kiAIL,&g_kdAIL, 00329 &g_rightloopRUD, &g_rightloopshortRUD, 00330 &g_leftloopRUD, &g_leftloopshortRUD, 00331 &g_glideloopRUD, 00332 &g_rightloopROLL_approach,&g_leftloopROLL_approach, 00333 &g_rightloopRUD_approach,&g_leftloopRUD_approach, 00334 &g_rightloopPITCH_approach,&g_leftloopPITCH_approach 00335 ); 00336 00337 00338 Init_PWM(); 00339 Init_servo(); 00340 Init_sbus(); 00341 Init_sensors(); 00342 //switch2.rise(ResetTrim); 00343 00344 USsensor.attach(&UpdateDist, 0.05); 00345 00346 NVIC_SetPriority(USART1_IRQn,0); 00347 NVIC_SetPriority(EXTI0_IRQn,1); 00348 NVIC_SetPriority(TIM5_IRQn,2); 00349 NVIC_SetPriority(EXTI9_5_IRQn,3); 00350 DisplayClock(); 00351 t.start(); 00352 00353 00354 pc.printf("MPU calibration start\r\n"); 00355 00356 float offsetstart = t.read(); 00357 while(t.read() - offsetstart < 26){ 00358 SensingMPU(); 00359 for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); 00360 pc.printf("\r\n"); 00361 led1 = !led1; 00362 led2 = !led2; 00363 led3 = !led3; 00364 led4 = !led4; 00365 } 00366 00367 pc.attach(getSF_Serial, Serial::RxIrq); 00368 NVIC_SetPriority(USART2_IRQn,4); 00369 00370 FirstROLL = nowAngle[ROLL]; 00371 FirstPITCH = nowAngle[PITCH]; 00372 nowAngle[ROLL] -=FirstROLL; 00373 nowAngle[PITCH] -=FirstPITCH; 00374 00375 led1 = 0; 00376 led2 = 0; 00377 led3 = 0; 00378 led4 = 0; 00379 wait(0.2); 00380 00381 00382 pc.printf("All initialized\r\n"); 00383 } 00384 00385 void loop(){ 00386 static float targetAngle[3] = {0.0, 0.0, 0.0}, controlValue[2] = {0.0, 0.0}; 00387 SensingMPU(); 00388 UpdateTargetAngle(targetAngle); 00389 CalculateControlValue(targetAngle, controlValue); 00390 UpdateAutoPWM(controlValue); 00391 00392 00393 //NVIC_SetPriority(TIM5_IRQn,4); 00394 //NVIC_SetPriority(USART2_IRQn,2); 00395 00396 wait_ms(23); 00397 00398 //NVIC_SetPriority(TIM5_IRQn,2); 00399 //NVIC_SetPriority(USART2_IRQn,4); 00400 00401 00402 // pc.printf("6\r\n"); 00403 //NVIC_DisableIRQ(USART2_IRQn); 00404 //pc.printf("%c",g_landingcommand); 00405 //NVIC_EnableIRQ(USART2_IRQn); 00406 #if DEBUG_PRINT_INLOOP 00407 //Sbusprintf(); 00408 DebugPrint(); 00409 #endif 00410 } 00411 00412 //サーボ初期化関数 00413 void Init_servo(){ 00414 00415 servo1.period_ms(14); 00416 servo1.pulsewidth_us(trimpwm[AIL_R]); 00417 00418 servo2.period_ms(14); 00419 servo2.pulsewidth_us(trimpwm[ELE]); 00420 00421 servo3.period_ms(14); 00422 servo3.pulsewidth_us(trimpwm[THR]); 00423 00424 servo4.period_ms(14); 00425 servo4.pulsewidth_us(trimpwm[RUD]); 00426 00427 servo5.period_ms(14); 00428 servo5.pulsewidth_us(trimpwm[DROP]); 00429 00430 servo6.period_ms(14); 00431 servo6.pulsewidth_us(trimpwm[AIL_L]); 00432 00433 pc.printf("servo initialized\r\n"); 00434 } 00435 00436 //Sbus初期化 00437 void Init_sbus(){ 00438 sbus.initialize(); 00439 sbus.setLastfuncPoint(Update_PWM); 00440 sbus.startInterrupt(); 00441 } 00442 00443 void Init_sensors(){ 00444 if(mpu6050.setup() == -1){ 00445 pc.printf("failed initialize\r\n"); 00446 while(1){ 00447 led1 = 1; led2 = 0; led3 = 1; led4 = 0; 00448 wait(1); 00449 led1 = 0; led2 = 1; led3 = 0; led4 = 1; 00450 wait(1); 00451 } 00452 } 00453 } 00454 00455 void Init_PWM(){ 00456 pc.printf("PWM initialized\r\n"); 00457 } 00458 00459 void DisplayClock(){ 00460 pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000); 00461 pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000); 00462 pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000); 00463 pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000); 00464 pc.printf("\r\n"); 00465 } 00466 00467 void UpdateTargetAngle(float targetAngle[3]){ 00468 00469 00470 static int16_t count_op = 0; 00471 #if DEBUG_SEMIAUTO 00472 switch(operation_mode){ 00473 case StartUp: 00474 if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){ 00475 count_op++; 00476 if(count_op > changeModeCount){ 00477 operation_mode = SemiAuto; 00478 pc.printf("Goto SemiAuto mode\r\n"); 00479 count_op = 0; 00480 } 00481 }else count_op = 0; 00482 break; 00483 00484 case SemiAuto: 00485 /* 大会用では以下のif文を入れてoperation_modeを変える 00486 if(CheckSW_Up(Ch6)){ 00487 count_op++; 00488 if(count_op>changeModeCount){ 00489 output_status = XXX; 00490 led2 = 0; 00491 pc.printf("Goto XXX mode\r\n"); 00492 count_op = 0; 00493 }else count_op = 0; 00494 ConvertPWMintoRAD(targetAngle); 00495 } 00496 */ 00497 ConvertPWMintoRAD(targetAngle); 00498 break; 00499 00500 default: 00501 operation_mode = SemiAuto; 00502 break; 00503 } 00504 00505 #else 00506 00507 switch(operation_mode){ 00508 case StartUp: 00509 if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){ //ch7;自動・手動切り替え ch8;自動操縦モード切替 00510 count_op++; 00511 if(count_op > changeModeCount){ 00512 operation_mode = RightLoop; 00513 pc.printf("Goto RightLoop mode\r\n"); 00514 count_op = 0; 00515 } 00516 }else count_op = 0; 00517 break; 00518 00519 case RightLoop: 00520 if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){ 00521 count_op++; 00522 if(count_op > changeModeCount){ 00523 operation_mode = LeftLoop; 00524 pc.printf("Goto LeftLoop mode\r\n"); 00525 count_op = 0; 00526 } 00527 }else count_op = 0; 00528 UpdateTargetAngle_Rightloop(targetAngle); 00529 00530 break; 00531 00532 case LeftLoop: 00533 if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){ 00534 count_op++; 00535 if(count_op > changeModeCount){ 00536 operation_mode = GoStraight; 00537 pc.printf("Goto GoStraight mode\r\n"); 00538 count_op = 0; 00539 } 00540 }else count_op = 0; 00541 UpdateTargetAngle_Leftloop(targetAngle); 00542 break; 00543 00544 case GoStraight: 00545 if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){ 00546 count_op++; 00547 if(count_op > changeModeCount){ 00548 operation_mode = Moebius; 00549 pc.printf("Goto Moebius mode\r\n"); 00550 count_op = 0; 00551 } 00552 }else count_op = 0; 00553 UpdateTargetAngle_GoStraight(targetAngle); 00554 break; 00555 00556 case Moebius: 00557 if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){ 00558 count_op++; 00559 if(count_op > changeModeCount){ 00560 operation_mode = Glide; 00561 pc.printf("Goto Glide mode\r\n"); 00562 count_op = 0; 00563 } 00564 }else count_op = 0; 00565 UpdateTargetAngle_Moebius(targetAngle); 00566 break; 00567 00568 case Glide: 00569 if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){ 00570 count_op++; 00571 if(count_op > changeModeCount){ 00572 operation_mode = BombwithPC; 00573 pc.printf("Goto Bombing mode\r\n"); 00574 pc.attach(getSF_Serial, Serial::RxIrq); 00575 count_op = 0; 00576 } 00577 }else count_op = 0; 00578 UpdateTargetAngle_Glide(targetAngle); 00579 break; 00580 00581 case BombwithPC: 00582 if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){ 00583 count_op++; 00584 if(count_op > changeModeCount){ 00585 operation_mode = RightLoop; 00586 pc.printf("Goto RightLoop mode\r\n"); 00587 pc.attach(NULL, Serial::RxIrq); 00588 count_op = 0; 00589 } 00590 }else count_op = 0; 00591 Take_off_and_landing(targetAngle); 00592 break; 00593 00594 default: 00595 operation_mode = StartUp; 00596 break; 00597 } 00598 #endif 00599 00600 if(CheckSW_Up(Ch7)){ 00601 output_status = Auto; 00602 led1 = 1; 00603 }else{ 00604 output_status = Manual; 00605 led1 = 0; 00606 } 00607 00608 00609 } 00610 00611 int GetParameter(FILE *fp, const char *paramName,char parameter[]){ 00612 int i=0, j=0; 00613 int strmax = 200; 00614 char str[strmax]; 00615 00616 rewind(fp); //ファイル位置を先頭に 00617 while(1){ 00618 if (fgets(str, strmax, fp) == NULL) { 00619 return 0; 00620 } 00621 if (!strncmp(str, paramName, strlen(paramName))) { 00622 while (str[i++] != '=') {} 00623 while (str[i] != '\n') { 00624 parameter[j++] = str[i++]; 00625 } 00626 parameter[j] = '\0'; 00627 return 1; 00628 } 00629 } 00630 } 00631 00632 00633 //sdによる設定 00634 int SetOptions(float *g_kpELE, float *g_kiELE, float *g_kdELE, 00635 float *g_kpRUD, float *g_kiRUD, float *g_kdRUD, 00636 float *g_rightloopROLL, float *g_rightloopPITCH, 00637 float *g_leftloopROLL, float *g_leftloopPITCH, 00638 float *g_gostraightROLL, float *g_gostraightPITCH, 00639 float *g_takeoffTHR, float *g_loopTHR, 00640 float *g_rightloopROLLshort, float *g_rightloopPITCHshort, 00641 float *g_leftloopROLLshort, float *g_leftloopPITCHshort, 00642 float *g_glideloopROLL, float *g_glideloopPITCH, 00643 float *g_kpAIL, float *g_kiAIL, float *g_kdAIL, 00644 int *g_rightloopRUD, int *g_rightloopshortRUD, 00645 int *g_leftloopRUD, int *g_leftloopshortRUD, 00646 int *g_glideloopRUD, 00647 float *g_rightloopROLL_approach,float *g_leftloopROLL_approach, 00648 int *g_rightloopRUD_approach,int *g_leftloopRUD_approach, 00649 float *g_rightloopPITCH_approach,float *g_leftloopPITCH_approach 00650 ){ 00651 00652 pc.printf("SDsetup start.\r\n"); 00653 00654 FILE *fp; 00655 char parameter[40]; //文字列渡す用の配列 00656 int SDerrorcount = 0; //取得できなかった数を返す 00657 const char *paramNames[] = { 00658 "KP_ELEVATOR", 00659 "KI_ELEVATOR", 00660 "KD_ELEVATOR", 00661 "KP_RUDDER", 00662 "KI_RUDDER", 00663 "KD_RUDDER", 00664 "RIGHTLOOP_ROLL", 00665 "RIGHTLOOP_PITCH", 00666 "LEFTLOOP_ROLL", 00667 "LEFTLOOP_PITCH", 00668 "GOSTRAIGHT_ROLL", 00669 "GOSTRAIGHT_PITCH", 00670 "TAKEOFF_THR_RATE", 00671 "LOOP_THR_RATE", 00672 "RIGHTLOOP_ROLL_SHORT", 00673 "RIGHTLOOP_PITCH_SHORT", 00674 "LEFTLOOP_ROLL_SHORT", 00675 "LEFTLOOP_PITCH_SHORT", 00676 "AUTOGLIDE_ROLL", 00677 "AUTOGLIDE PITCH", 00678 "KP_AILERON", 00679 "KI_AILERON", 00680 "KD_AILERON", 00681 "RIGHTLOOP_RUDDER", 00682 "RIGHTLOOPSHORT_RUDDER", 00683 "LEFTLOOP_RUDDER", 00684 "LEFTLOOPSHORT_RUDDER", 00685 "GLIDELOOP_RUDDER", 00686 "RIGHTLOOP_ROLL_APPROACH", 00687 "LEFTLOOP_ROLL_APPROACH", 00688 "RIGHTLOOP_RUDDER_APPROACH", 00689 "LEFTLOOP_RUDDER_APPROACH", 00690 "RIGHTLOOP_PITCH_APPROACH", 00691 "LEFTLOOP_PITCH_APPROACH", 00692 }; 00693 00694 fp = fopen("/sd/option.txt","r"); 00695 00696 if(fp != NULL){ //開けたら 00697 pc.printf("File was openned.\r\n"); 00698 if(GetParameter(fp,paramNames[0],parameter)) *g_kpELE = atof(parameter); 00699 else{ *g_kpELE = KP_ELE; 00700 SDerrorcount++; 00701 } 00702 if(GetParameter(fp,paramNames[1],parameter)) *g_kiELE = atof(parameter); 00703 else{ *g_kiELE = KI_ELE; 00704 SDerrorcount++; 00705 } 00706 if(GetParameter(fp,paramNames[2],parameter)) *g_kdELE = atof(parameter); 00707 else{ *g_kdELE = KD_ELE; 00708 SDerrorcount++; 00709 } 00710 if(GetParameter(fp,paramNames[3],parameter)) *g_kpRUD = atof(parameter); 00711 else{ *g_kpRUD = KP_RUD; 00712 SDerrorcount++; 00713 } 00714 if(GetParameter(fp,paramNames[4],parameter)) *g_kiRUD = atof(parameter); 00715 else{ *g_kiRUD = KI_RUD; 00716 SDerrorcount++; 00717 } 00718 if(GetParameter(fp,paramNames[5],parameter)) *g_kdRUD = atof(parameter); 00719 else{ *g_kdRUD = KD_RUD; 00720 SDerrorcount++; 00721 } 00722 if(GetParameter(fp,paramNames[6],parameter)) *g_rightloopROLL = atof(parameter); 00723 else{ *g_rightloopROLL = RIGHT_ROLL; 00724 SDerrorcount++; 00725 } 00726 if(GetParameter(fp,paramNames[7],parameter)) *g_rightloopPITCH = atof(parameter); 00727 else{ *g_rightloopPITCH = RIGHT_PITCH; 00728 SDerrorcount++; 00729 } 00730 if(GetParameter(fp,paramNames[8],parameter)) *g_leftloopROLL = atof(parameter); 00731 else{ *g_leftloopROLL = LEFT_ROLL; 00732 SDerrorcount++; 00733 } 00734 if(GetParameter(fp,paramNames[9],parameter)) *g_leftloopPITCH = atof(parameter); 00735 else{ *g_leftloopPITCH = LEFT_PITCH; 00736 SDerrorcount++; 00737 } 00738 if(GetParameter(fp,paramNames[10],parameter)) *g_gostraightROLL = atof(parameter); 00739 else{ *g_gostraightROLL = STRAIGHT_ROLL; 00740 SDerrorcount++; 00741 } 00742 if(GetParameter(fp,paramNames[11],parameter)) *g_gostraightPITCH = atof(parameter); 00743 else{ *g_gostraightPITCH = STRAIGHT_PITCH; 00744 SDerrorcount++; 00745 } 00746 if(GetParameter(fp,paramNames[12],parameter)) *g_takeoffTHR = atof(parameter); 00747 else{ *g_takeoffTHR = TAKEOFF_THR; 00748 SDerrorcount++; 00749 } 00750 if(GetParameter(fp,paramNames[13],parameter)) *g_loopTHR = atof(parameter); 00751 else{ *g_loopTHR = LOOP_THR; 00752 SDerrorcount++; 00753 } 00754 if(GetParameter(fp,paramNames[14],parameter)) *g_rightloopROLLshort = atof(parameter); 00755 else{ *g_rightloopROLLshort = RIGHT_ROLL_SHORT; 00756 SDerrorcount++; 00757 } 00758 if(GetParameter(fp,paramNames[15],parameter)) *g_rightloopPITCHshort = atof(parameter); 00759 else{ *g_rightloopPITCHshort = RIGHT_PITCH_SHORT; 00760 SDerrorcount++; 00761 } 00762 if(GetParameter(fp,paramNames[16],parameter)) *g_leftloopROLLshort = atof(parameter); 00763 else{ *g_leftloopROLLshort = LEFT_ROLL_SHORT; 00764 SDerrorcount++; 00765 } 00766 if(GetParameter(fp,paramNames[17],parameter)) *g_leftloopPITCHshort = atof(parameter); 00767 else{ *g_leftloopPITCHshort = LEFT_PITCH_SHORT; 00768 SDerrorcount++; 00769 } 00770 if(GetParameter(fp,paramNames[18],parameter)) *g_glideloopROLL = atof(parameter); 00771 else{ *g_glideloopROLL = GLIDE_ROLL; 00772 SDerrorcount++; 00773 } 00774 if(GetParameter(fp,paramNames[19],parameter)) *g_glideloopPITCH = atof(parameter); 00775 else{ *g_glideloopPITCH = GLIDE_PITCH; 00776 SDerrorcount++; 00777 } 00778 if(GetParameter(fp,paramNames[20],parameter)) *g_kpAIL = atof(parameter); 00779 else{ *g_kpAIL = KP_AIL; 00780 SDerrorcount++; 00781 } 00782 if(GetParameter(fp,paramNames[21],parameter)) *g_kiAIL = atof(parameter); 00783 else{ *g_kiAIL = KI_AIL; 00784 SDerrorcount++; 00785 } 00786 if(GetParameter(fp,paramNames[22],parameter)) *g_kdAIL = atof(parameter); 00787 else{ *g_kdAIL = KP_AIL; 00788 SDerrorcount++; 00789 } 00790 if(GetParameter(fp,paramNames[23],parameter)) *g_rightloopRUD = atof(parameter); 00791 else{ *g_rightloopRUD = RIGHTLOOP_RUD; 00792 SDerrorcount++; 00793 } 00794 if(GetParameter(fp,paramNames[24],parameter)) *g_rightloopshortRUD = atof(parameter); 00795 else{ *g_rightloopshortRUD = RIGHTLOOPSHORT_RUD; 00796 SDerrorcount++; 00797 } 00798 if(GetParameter(fp,paramNames[25],parameter)) *g_leftloopRUD = atof(parameter); 00799 else{ *g_leftloopshortRUD = LEFTLOOP_RUD; 00800 SDerrorcount++; 00801 } 00802 if(GetParameter(fp,paramNames[26],parameter)) *g_leftloopshortRUD = atof(parameter); 00803 else{ *g_leftloopshortRUD = LEFTLOOPSHORT_RUD; 00804 SDerrorcount++; 00805 } 00806 if(GetParameter(fp,paramNames[27],parameter)) *g_glideloopRUD = atof(parameter); 00807 else{ *g_glideloopRUD = GLIDELOOP_RUD; 00808 SDerrorcount++; 00809 } 00810 if(GetParameter(fp,paramNames[28],parameter)) *g_rightloopROLL_approach = atof(parameter); 00811 else{ *g_rightloopROLL_approach = RIGHTLOOPROLL_APPROACH; 00812 SDerrorcount++; 00813 } 00814 if(GetParameter(fp,paramNames[29],parameter)) *g_leftloopROLL_approach= atof(parameter); 00815 else{ *g_leftloopROLL_approach = LEFTLOOPROLL_APPROACH; 00816 SDerrorcount++; 00817 } 00818 if(GetParameter(fp,paramNames[30],parameter)) *g_rightloopRUD_approach = atof(parameter); 00819 else{ *g_rightloopRUD_approach = RIGHTLOOPRUD_APPROACH; 00820 SDerrorcount++; 00821 } 00822 if(GetParameter(fp,paramNames[31],parameter)) *g_leftloopRUD_approach= atof(parameter); 00823 else{ *g_leftloopRUD_approach= LEFTLOOPRUD_APPROACH; 00824 SDerrorcount++; 00825 } 00826 if(GetParameter(fp,paramNames[32],parameter)) *g_rightloopPITCH_approach = atof(parameter); 00827 else{ *g_rightloopPITCH_approach = RIGHTLOOPPITCH_APPROACH; 00828 SDerrorcount++; 00829 } 00830 if(GetParameter(fp,paramNames[33],parameter)) *g_leftloopPITCH_approach = atof(parameter); 00831 else{ *g_leftloopPITCH_approach = LEFTLOOPPITCH_APPROACH; 00832 SDerrorcount++; 00833 } 00834 00835 fclose(fp); 00836 00837 }else{ //ファイルがなかったら 00838 pc.printf("fp was null.\r\n"); 00839 *g_kpELE = KP_ELE; 00840 *g_kiELE = KI_ELE; 00841 *g_kdELE = KD_ELE; 00842 *g_kpRUD = KP_RUD; 00843 *g_kiRUD = KI_RUD; 00844 *g_kdRUD = KD_RUD; 00845 *g_rightloopROLL = RIGHT_ROLL; 00846 *g_rightloopPITCH = RIGHT_PITCH; 00847 *g_leftloopROLL = LEFT_ROLL; 00848 *g_leftloopPITCH = LEFT_PITCH; 00849 *g_gostraightROLL = STRAIGHT_ROLL; 00850 *g_gostraightPITCH = STRAIGHT_PITCH; 00851 *g_takeoffTHR = TAKEOFF_THR; 00852 *g_loopTHR = LOOP_THR; 00853 *g_kpAIL = KP_AIL; //パラメータ変えるのお忘れなく！！ 00854 *g_kiAIL = KI_AIL; 00855 *g_kdAIL = KD_AIL; 00856 *g_rightloopRUD = RIGHTLOOP_RUD; 00857 *g_rightloopshortRUD = RIGHTLOOPSHORT_RUD; 00858 *g_leftloopRUD = LEFTLOOP_RUD; 00859 *g_leftloopshortRUD = LEFTLOOPSHORT_RUD; 00860 *g_glideloopRUD = GLIDELOOP_RUD; 00861 *g_rightloopROLL_approach = RIGHTLOOPROLL_APPROACH; 00862 *g_leftloopROLL_approach = LEFTLOOPROLL_APPROACH; 00863 *g_rightloopRUD_approach = RIGHTLOOPRUD_APPROACH; 00864 *g_leftloopRUD_approach = LEFTLOOPRUD_APPROACH; 00865 *g_rightloopPITCH_approach = RIGHTLOOPPITCH_APPROACH; 00866 *g_leftloopPITCH_approach = LEFTLOOPPITCH_APPROACH; 00867 00868 SDerrorcount = -1; 00869 } 00870 pc.printf("SDsetup finished.\r\n"); 00871 if(SDerrorcount == 0) pc.printf("setting option is success\r\n"); 00872 else if(SDerrorcount == -1) pc.printf("ERROR 1. cannot open option\r\n"); 00873 else if(SDerrorcount > 0) pc.printf("ERROR 2. reading parameter is failed[%d]\r\n",SDerrorcount); 00874 00875 pc.printf("kpELE = %f, kiELE = %f, kdELE = %f\r\n", *g_kpRUD, *g_kiRUD, *g_kdRUD); 00876 pc.printf("kpRUD = %f, kiRUD = %f, kdRUD = %f\r\n", *g_kpELE, *g_kiELE, *g_kdELE); 00877 pc.printf("rightloopROLL = %f, rightloopPITCH = %f\r\n", *g_rightloopROLL, *g_rightloopPITCH); 00878 pc.printf("leftloopROLL = %f, g_leftloopPITCH = %f\r\n", *g_leftloopROLL, *g_leftloopPITCH); 00879 pc.printf("gostraightROLL = %f, g_gostraightPITCH = %f\r\n", *g_gostraightROLL, *g_gostraightPITCH); 00880 pc.printf("g_takeoffTHR = %f, g_loopTHR = %f\r\n", *g_takeoffTHR, *g_loopTHR); 00881 pc.printf("rightloopROLLshort = %f, rightloopPITCHshort = %f\r\n", *g_rightloopROLLshort, *g_rightloopPITCHshort); 00882 pc.printf("leftloopROLLshort = %f, g_leftloopPITCHshort = %f\r\n", *g_leftloopROLLshort, *g_leftloopPITCHshort); 00883 pc.printf("glideROLL = %f, glidePITCH = %f\r\n", *g_glideloopROLL, *g_glideloopPITCH); 00884 pc.printf("kpAIL = %f,kiAIL = %f,kdAIL = %f\r\n ",*g_kpAIL,*g_kiAIL,*g_kdAIL); 00885 pc.printf("RIGHTLOOPRUD = %d,RIGHTLOOPSHORTRUD = %d\r\n",*g_rightloopRUD,*g_rightloopshortRUD); 00886 pc.printf("LEFTTLOOPRUD = %d,LEFTLOOPSHORTRUD = %d\r\n",*g_leftloopRUD,*g_leftloopshortRUD); 00887 pc.printf("GLIDELOOPRUD = %d\r\n",*g_glideloopRUD); 00888 pc.printf("RIGHTLOOP_ROLL_APPROACH = %f, LEFTLOOP_ROLL_APPROACH= %f\r\n",*g_rightloopROLL_approach,*g_leftloopROLL_approach); 00889 pc.printf("RIGHTLOOP_RUD_APPROACH = %d, LEFTLOOP_RUD_APPROACH = %d\r\n",*g_rightloopRUD_approach,*g_leftloopRUD_approach); 00890 pc.printf("RIGHTLOOP_PITCH_APPROACH = %f, LEFTLOOP_PITCH_APPROACH = %f\r\n",*g_rightloopPITCH_approach,*g_leftloopPITCH_approach); 00891 00892 return SDerrorcount; 00893 } 00894 00895 void CalculateControlValue(float targetAngle[3], float controlValue[3]){ 00896 00897 static int t_last; 00898 int t_now; 00899 float dt; 00900 00901 t_now = t.read_us(); 00902 dt = (float)((t_now - t_last)/1000000.0f) ; 00903 t_last = t_now; 00904 00905 00906 //controlValue[ROLL] = pid_RUD.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt); 00907 controlValue[ROLL] = pid_AIL.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt); //エルロンでロール制御 00908 controlValue[PITCH] = pid_ELE.calcPID(nowAngle[PITCH], targetAngle[PITCH], dt); 00909 00910 } 00911 00912 void UpdateAutoPWM(float controlValue[3]){ 00913 NVIC_DisableIRQ(USART1_IRQn); 00914 int16_t addpwm[2]; //-500~500 00915 addpwm[PITCH] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[PITCH]; //センサ：機首下げ正 レバー:機首上げ正 00916 addpwm[ROLL] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[ROLL]; //センサ：右回転正（8月13日時点；左回転が正！） レバー:右回転正 00917 00918 autopwm[ELE] = trimpwm[ELE] + reverce[ELE] * addpwm[PITCH]; //rewrite 00919 autopwm[AIL_R] = trimpwm[AIL_R] - reverce[AIL_R] * addpwm[ROLL]; 00920 //autopwm[THR] = oldTHR; 00921 00922 autopwm[ELE] = ThresholdMaxMin(autopwm[ELE], maxpwm[ELE], minpwm[ELE]); 00923 autopwm[AIL_R] = ThresholdMaxMin(autopwm[AIL_R], maxpwm[AIL_R], minpwm[AIL_R]); 00924 00925 NVIC_EnableIRQ(USART1_IRQn); 00926 } 00927 00928 inline float CalcRatio(float value, float trim, float limit){ 00929 return (value - trim) / (limit - trim); 00930 } 00931 00932 bool CheckSW_Up(Channel ch){ 00933 00934 if(SWITCH_CHECK < sbus.manualpwm[ch]){ 00935 return true; 00936 }else{ 00937 return false; 00938 } 00939 00940 } 00941 00942 int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min){ 00943 if(value > max) return max; 00944 if(value < min) return min; 00945 return value; 00946 } 00947 00948 inline int16_t SetTHRinRatio(float ratio){ 00949 return minpwm[THR] + (int16_t)(2 * lengthdivpwm * ratio); 00950 } 00951 00952 00953 00954 /*---SBUS割り込み処理---*/ 00955 00956 //udate_Inputで抽出したpwmデータを整形して各変数に代入する。(マニュアルモード) 00957 //各stabiGylo関数で算出したpwmを各変数に代入する(自動モード) 00958 void Update_PWM() 00959 { 00960 NVIC_DisableIRQ(USART1_IRQn); 00961 static int16_t pwm[6]; 00962 static int16_t temppwm[6]={trimpwm[0],trimpwm[1],trimpwm[2],trimpwm[3],trimpwm[4],trimpwm[5]}; 00963 static int16_t FailsafeCounter=0; 00964 static int16_t ResetCounter=0; 00965 static int16_t OKCounter=0; 00966 00967 if(sbus.flg_ch_update == true){ 00968 00969 switch(output_status){ //マニュアルモード,自動モード,自動着陸もモードを切替 00970 case Manual: 00971 if(OKCounter!=0) break; 00972 for(uint8_t i=0;i<6;i++){ 00973 pwm[i] = sbus.manualpwm[i]; 00974 } 00975 oldTHR = sbus.manualpwm[THR]; 00976 //pc.printf("update_manual\r\n"); 00977 NVIC_EnableIRQ(USART1_IRQn); 00978 break; 00979 00980 case Auto: 00981 if(OKCounter!=0) break; 00982 pwm[AIL_R] = autopwm[AIL_R]; //sbus.manualpwm[AIL]; 00983 pwm[ELE] = autopwm[ELE]; 00984 pwm[THR] = autopwm[THR]; 00985 pwm[RUD] = autopwm[RUD]; 00986 pwm[DROP] = autopwm[DROP]; 00987 pwm[AIL_L] = autopwm[AIL_L]; 00988 00989 NVIC_EnableIRQ(USART1_IRQn); 00990 break; 00991 00992 default: 00993 if(OKCounter!=0) break; 00994 for(uint8_t i=0;i<6;i++){ 00995 pwm[i] = sbus.manualpwm[i]; 00996 } //pc.printf("update_manual\r\n"); 00997 NVIC_EnableIRQ(USART1_IRQn); 00998 break; 00999 } 01000 01001 for(uint8_t i=0;i<6;i++){ 01002 if(pwm[i]<1000 || pwm[i]>2000) pwm[i]=temppwm[i]; 01003 temppwm[i]=pwm[i]; 01004 } 01005 01006 } 01007 //else(sbus.flg_ch_update == false) pc.printf("0\r\n"); 01008 /* if(sbus.failsafe_status==SBUS_SIGNAL_OK){ 01009 pc.printf("OK\r\n"); 01010 } 01011 */ 01012 //pc.printf("%d\r\n",sbus.failsafe_status); 01013 01014 if(sbus.failsafe_status==SBUS_SIGNAL_LOST||sbus.failsafe_status==SBUS_SIGNAL_FAILSAFE) FailsafeCounter++; 01015 else ResetCounter++; 01016 01017 if(ResetCounter>7){ 01018 ResetCounter=0; 01019 FailsafeCounter=0; 01020 } 01021 01022 if(FailsafeCounter>10){ 01023 ResetCounter=0; 01024 for(uint8_t i=0;i<6;i++) pwm[i] = trimpwm[i]; 01025 01026 if(sbus.failsafe_status==SBUS_SIGNAL_OK&&sbus.flg_ch_update == true) OKCounter++; 01027 else OKCounter=0; 01028 01029 if(OKCounter>10){ 01030 OKCounter=0; 01031 FailsafeCounter=0; 01032 } 01033 //pc.printf("OKCounter=%d, FailsafeCounter=%d, sbus.failsafe_status=%d\r\n",OKCounter,FailsafeCounter,sbus.failsafe_status); 01034 } 01035 //if(sbus.failsafe_status==SBUS_SIGNAL_OK){FailsafeCounter=0;} 01036 01037 01038 sbus.flg_ch_update = false; 01039 Output_PWM(pwm); 01040 } 01041 01042 01043 01044 01045 //pwmをサーボに出力。 01046 void Output_PWM(int16_t pwm[5]) 01047 { 01048 NVIC_DisableIRQ(USART1_IRQn); 01049 servo1.pulsewidth_us(pwm[0]); 01050 servo2.pulsewidth_us(pwm[1]); 01051 servo3.pulsewidth_us(pwm[2]); 01052 servo4.pulsewidth_us(pwm[3]); 01053 servo5.pulsewidth_us(pwm[4]); 01054 servo6.pulsewidth_us(pwm[5]); 01055 NVIC_EnableIRQ(USART1_IRQn); 01056 01057 } 01058 01059 void ResetTrim(){ 01060 for(uint8_t i=0; i<6; i++){ //i=4から書き換え＿投下サーボは入ってない模様 01061 trimpwm[i] = sbus.manualpwm[i]; 01062 } 01063 pc.printf("reset PWM trim\r\n"); 01064 } 01065 01066 01067 void SensingMPU(){ 01068 //static int16_t deltaT = 0, t_start = 0; 01069 //t_start = t.read_us(); 01070 01071 float rpy[3] = {0}, oldrpy[3] = {0}; 01072 static uint16_t count_changeRPY = 0; 01073 static bool flg_checkoutlier = false; 01074 NVIC_DisableIRQ(USART1_IRQn); 01075 NVIC_DisableIRQ(USART2_IRQn); 01076 NVIC_DisableIRQ(TIM5_IRQn); 01077 NVIC_DisableIRQ(EXTI0_IRQn);//MPU割り込み禁止 01078 NVIC_DisableIRQ(EXTI9_5_IRQn);//超音波割り込み禁止 01079 01080 mpu6050.getRollPitchYaw_Skipper(rpy); 01081 01082 NVIC_EnableIRQ(USART1_IRQn); 01083 NVIC_EnableIRQ(USART2_IRQn); 01084 NVIC_EnableIRQ(TIM5_IRQn); 01085 NVIC_EnableIRQ(EXTI0_IRQn); 01086 NVIC_EnableIRQ(EXTI9_5_IRQn); 01087 01088 01089 //外れ値対策 01090 for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI; 01091 rpy[ROLL] -= FirstROLL; 01092 rpy[PITCH] -= FirstPITCH; 01093 rpy[YAW] -= FirstYAW; 01094 01095 for(uint8_t i=0; i<3; i++) {if(rpy[i] < nowAngle[i]-10 || rpy[i] > nowAngle[i]+10) {flg_checkoutlier = true;}} 01096 if(!flg_checkoutlier || count_changeRPY >= 2){ 01097 for(uint8_t i=0; i<3; i++){ 01098 nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f; //2つの移動平均 01099 } 01100 count_changeRPY = 0; 01101 }else count_changeRPY++; 01102 flg_checkoutlier = false; 01103 01104 } 01105 01106 float TranslateNewYaw(float beforeYaw, float newzeroYaw){ 01107 float newYaw = beforeYaw - newzeroYaw; 01108 01109 if(newYaw<-180.0f) newYaw += 360.0f; 01110 else if(newYaw>180.0f) newYaw -= 360.0f; 01111 return newYaw; 01112 } 01113 01114 01115 void getSF_Serial(){ 01116 //NVIC_DisableIRQ(USART1_IRQn); 01117 //NVIC_DisableIRQ(EXTI0_IRQn); 01118 //NVIC_DisableIRQ(TIM5_IRQn); 01119 01120 01121 static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'}; 01122 01123 static int bufcounter=0; 01124 01125 01126 01127 if(pc.readable()) { // 受信確認 01128 01129 SFbuf[bufcounter] = pc.getc(); // 1文字取り出し 01130 if(SFbuf[0]!='S'){ 01131 //pc.printf("x"); 01132 return; 01133 } 01134 01135 01136 01137 //pc.printf("%c",SFbuf[bufcounter]); 01138 01139 if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++; 01140 01141 if(bufcounter==5 && SFbuf[4]=='F'){ 01142 01143 g_landingcommand = SFbuf[1]; 01144 //pc.printf("%c",g_landingcommand); 01145 //wait_ms(20); 01146 //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]); 01147 if(g_landingcommand=='Y')g_SerialTargetYAW =0.0f; 01148 bufcounter = 0; 01149 memset(SFbuf, 0, sizeof(SFbuf)); 01150 NVIC_ClearPendingIRQ(USART2_IRQn); 01151 //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW); 01152 } 01153 01154 else if(bufcounter>=5){ 01155 //pc.printf("Communication Falsed.\r\n"); 01156 memset(SFbuf, 0, sizeof(SFbuf)); 01157 bufcounter = 0; 01158 NVIC_ClearPendingIRQ(USART2_IRQn); 01159 } 01160 } 01161 01162 //NVIC_EnableIRQ(TIM5_IRQn); 01163 //NVIC_EnableIRQ(EXTI0_IRQn); 01164 //NVIC_EnableIRQ(USART1_IRQn); 01165 } 01166 01167 float ConvertByteintoFloat(char high, char low){ 01168 01169 //int16_t intvalue = (int16_t)high*256 + (int16_t)low; 01170 int16_t intvalue = (int16_t)(((int16_t)high << 8) | low); // Turn the MSB and LSB into a signed 16-bit value 01171 float floatvalue = (float)intvalue; 01172 return floatvalue; 01173 } 01174 01175 01176 //超音波割り込み 01177 void UpdateDist(){ 01178 g_distance = usensor.get_dist_cm(); 01179 usensor.start(); 01180 } 01181 01182 //8の字旋回 01183 void UpdateTargetAngle_Moebius(float targetAngle[3]){ 01184 static uint8_t RotateCounter=0; 01185 static bool flg_setInStartAuto = false; 01186 static float FirstYAW_Moebius = 0.0; 01187 float newYaw_Moebius; 01188 01189 if(!flg_setInStartAuto && CheckSW_Up(Ch7)){ 01190 FirstYAW_Moebius = nowAngle[YAW]; 01191 RotateCounter = 0; 01192 flg_setInStartAuto = true; 01193 }else if(!CheckSW_Up(Ch7)){ 01194 flg_setInStartAuto = false; 01195 led4 = 0; 01196 } 01197 autopwm[THR]=oldTHR; 01198 01199 newYaw_Moebius = TranslateNewYaw(nowAngle[YAW], FirstYAW_Moebius); 01200 01201 if(RotateCounter == 0 && newYaw_Moebius >90.0 && newYaw_Moebius < 180.0) {RotateCounter++; led4 = 1; pc.printf("Rotate 90\r\n");} 01202 if(RotateCounter == 1 && newYaw_Moebius >-180.0 && newYaw_Moebius < -90.0) {RotateCounter++; led4 = 0; pc.printf("Rotate 180\r\n");} 01203 if(RotateCounter == 2 && newYaw_Moebius >-90.0 && newYaw_Moebius <-10.0) {RotateCounter++; led4 = 1; pc.printf("Rotate 270\r\n");} 01204 if(RotateCounter == 3 && newYaw_Moebius >0.0 && newYaw_Moebius < 90.0) {RotateCounter++; led4 = 0; pc.printf("Change Rotate direction\r\n");} 01205 01206 01207 if(RotateCounter <= 3) UpdateTargetAngle_Rightloop_short(targetAngle); 01208 else UpdateTargetAngle_Leftloop_short(targetAngle); //左旋回 01209 01210 } 01211 01212 //自動滑空 01213 void UpdateTargetAngle_Glide(float targetAngle[3]){ 01214 static int THRcount = 0; 01215 static int t_start = 0; 01216 static bool flg_tstart = false; 01217 static bool flg_ground = false; 01218 int t_diff = 0; 01219 static int groundcount = 0; 01220 01221 targetAngle[ROLL] = g_glideloopROLL; 01222 01223 autopwm[RUD]=g_glideloopRUD; 01224 // autopwm[THR]=oldTHR; 01225 01226 01227 01228 //時間計測開始設定 01229 if(!flg_tstart && CheckSW_Up(Ch7)){ 01230 t_start = t.read(); 01231 flg_tstart = true; 01232 pc.printf("timer start\r\n"); 01233 }else if(!CheckSW_Up(Ch7)){ 01234 t_start = 0; 01235 flg_tstart = false; 01236 } 01237 01238 01239 //フラグが偽であれば計測は行わない 01240 if(flg_tstart){ 01241 t_diff = t.read() - t_start; 01242 //一定高度or15秒でled点灯 01243 NVIC_DisableIRQ(EXTI9_5_IRQn); 01244 if((groundcount>5 && g_distance>0) || t_diff > 15){ 01245 led4 = 1; 01246 //pc.printf("Call [Stop!] calling!\r\n"); 01247 } 01248 01249 if(g_distance<250 && g_distance > 0) { 01250 groundcount++; 01251 } 01252 NVIC_EnableIRQ(EXTI9_5_IRQn); 01253 }else{ 01254 t_diff = 0; 01255 groundcount = 0; 01256 led4 = 0; 01257 } 01258 01259 NVIC_DisableIRQ(EXTI9_5_IRQn); 01260 if(t_diff > 17) autopwm[THR] = SetTHRinRatio(0.5); 01261 01262 else if(g_distance<150 && g_distance>0 ){ 01263 NVIC_DisableIRQ(EXTI9_5_IRQn); 01264 THRcount++; 01265 if(THRcount>5) flg_ground = true; 01266 } 01267 else THRcount = 0; 01268 NVIC_EnableIRQ(EXTI9_5_IRQn); 01269 01270 if(flg_ground == true) { 01271 autopwm[THR] = SetTHRinRatio(0.6); 01272 targetAngle[PITCH] = g_gostraightPITCH; 01273 } 01274 else { 01275 autopwm[THR] = minpwm[THR]; 01276 targetAngle[PITCH] = g_glideloopPITCH; 01277 } 01278 01279 NVIC_DisableIRQ(USART1_IRQn); 01280 if(!CheckSW_Up(Ch7)){ 01281 flg_ground = false; 01282 } 01283 NVIC_EnableIRQ(USART1_IRQn); 01284 } 01285 01286 //離陸-投下-着陸一連 01287 void Take_off_and_landing(float targetAngle[3]){ 01288 01289 if(!CheckSW_Up(Ch7)) bombing_mode = Takeoff; 01290 01291 switch(bombing_mode){ 01292 case Takeoff: 01293 static bool flg_setFirstYaw = false; 01294 static int TakeoffCount = 0; 01295 01296 if(!flg_setFirstYaw && CheckSW_Up(Ch7)){ 01297 FirstYAW = nowAngle[YAW]; 01298 flg_setFirstYaw = true; 01299 }else if(flg_setFirstYaw && !CheckSW_Up(Ch7)){ 01300 flg_setFirstYaw = false; 01301 } 01302 01303 UpdateTargetAngle_Takeoff(targetAngle); 01304 NVIC_DisableIRQ(EXTI9_5_IRQn); 01305 if(g_distance>100) TakeoffCount++; 01306 else TakeoffCount = 0; 01307 NVIC_EnableIRQ(EXTI9_5_IRQn); 01308 if(TakeoffCount>5){ 01309 01310 //autopwm[THR] = SetTHRinRatio(0.3); 01311 autopwm[THR] = 1180+320*2*0.3; 01312 targetAngle[PITCH]=g_gostraightPITCH; 01313 autopwm[RUD]=trimpwm[RUD]; 01314 //pc.printf("Now go to Approach mode!!"); 01315 bombing_mode = Approach; 01316 } 01317 break; 01318 01319 //case Chicken: 01320 //break; 01321 /* 01322 case Transition: 01323 static int ApproachCount = 0; 01324 targetAngle[YAW]=180.0; 01325 int Judge = Rotate(targetAngle, g_SerialTargetYAW); 01326 01327 if(Judge==0) ApproachCount++; 01328 if(ApproachCount>5) bombing_mode = Approach; 01329 break; 01330 */ 01331 case Approach: 01332 01333 //autopwm[THR] = SetTHRinRatio(g_loopTHR); //0.7;スロットルの割合 01334 autopwm[THR] = 1180+320*2*0.3; 01335 UpdateTargetAngle_Approach(targetAngle); 01336 01337 break; 01338 01339 default: 01340 bombing_mode = Takeoff; 01341 break; 01342 } 01343 01344 } 01345 01346 //離陸モード 01347 void UpdateTargetAngle_Takeoff(float targetAngle[3]){ 01348 //pc.printf("%d \r\n",g_distance); 01349 static int tELE_start = 0; 01350 static bool flg_ELEup = false; 01351 int t_def = 0; 01352 01353 autopwm[RUD] = trimpwm[RUD]; 01354 01355 01356 if(!flg_ELEup && CheckSW_Up(Ch7)){ 01357 tELE_start = t.read_ms(); 01358 flg_ELEup = true; 01359 pc.printf("timer start\r\n"); 01360 }else if(!CheckSW_Up(Ch7)){ 01361 tELE_start = 0; 01362 flg_ELEup = false; 01363 } 01364 if(flg_ELEup){ 01365 t_def = t.read_ms() - tELE_start; 01366 01367 //1.5秒経過すればELE上げ舵へ 01368 if(t_def>500) targetAngle[PITCH]=-35.0; 01369 else{ 01370 t_def = 0; 01371 targetAngle[PITCH]=g_gostraightPITCH; 01372 } 01373 } 01374 targetAngle[ROLL] = g_gostraightROLL; 01375 //targetAngle[PITCH] = g_loopTHR; 01376 autopwm[THR] = SetTHRinRatio(g_takeoffTHR); //0.7;スロットルの割合 01377 } 01378 01379 01380 //ヨーを目標値にして許容角度になるまで水平旋回 01381 int Rotate(float targetAngle[3], float TargetYAW){ 01382 float diffYaw = TranslateNewYaw(TargetYAW, nowAngle[YAW]); 01383 01384 if(diffYaw > LIMIT_STRAIGHT_YAW){ 01385 /* 01386 if(diffYaw > THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Rightloop_short(targetAngle); 01387 else UpdateTargetAngle_Rightloop(targetAngle); 01388 */ 01389 UpdateTargetAngle_Rightloop_short(targetAngle); 01390 return 1; 01391 }else if(diffYaw < -LIMIT_STRAIGHT_YAW){ 01392 UpdateTargetAngle_Leftloop_short(targetAngle); 01393 /* 01394 if(diffYaw < -THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Leftloop_short(targetAngle); 01395 else UpdateTargetAngle_Leftloop(targetAngle); 01396 */ 01397 return 1; 01398 }else{ 01399 UpdateTargetAngle_GoStraight(targetAngle); 01400 return 0; 01401 } 01402 } 01403 01404 //チキラー投下 01405 void Chicken_Drop(){ 01406 if(CheckSW_Up(Ch7)){ 01407 autopwm[DROP] = 1712; 01408 pc.printf("Bombed!\r\n"); 01409 //RerurnChickenServo1.attach(&ReturnChickenServo1, 3); 01410 //operation_mode = Approach; 01411 //buzzer = 0; 01412 pc.printf("Goto LeftLoop mode\r\n"); 01413 } 01414 } 01415 01416 void ReturnChickenServo1(){ 01417 autopwm[DROP] = 1344; 01418 pc.printf("first reverse\r\n"); 01419 RerurnChickenServo2.attach(&ReturnChickenServo2, 1); 01420 } 01421 01422 void ReturnChickenServo2(){ 01423 autopwm[DROP] = 1392; 01424 pc.printf("second reverse\r\n"); 01425 } 01426 01427 //着陸モード（PCからの指令に従う） 01428 void UpdateTargetAngle_Approach(float targetAngle[3]){ 01429 01430 static bool zeroTHR=true;//着陸時のスロットル動作確認用 01431 01432 NVIC_DisableIRQ(USART2_IRQn); 01433 01434 if(CheckSW_Up(Ch7)){ 01435 output_status = Auto; 01436 led1 = 1; 01437 }else{ 01438 output_status = Manual; 01439 led1 = 0; 01440 zeroTHR=true; 01441 //g_landingcommand='G'; 01442 } 01443 01444 01445 01446 switch(g_landingcommand){ 01447 case 'R': //右旋回セヨ 01448 NVIC_EnableIRQ(USART2_IRQn); 01449 if(zeroTHR==false){ 01450 UpdateTargetAngle_Rightloop_zero(targetAngle); 01451 } 01452 else{ 01453 targetAngle[ROLL] = g_rightloopROLL_approach; 01454 targetAngle[PITCH] = g_rightloopPITCH_approach ; 01455 autopwm[RUD]=g_rightloopRUD_approach; //RUD固定 01456 if(autopwm[AIL_R]<trimpwm[AIL_R]){ //エルロン上がりやすさ調節 01457 autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; 01458 } 01459 else {autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; 01460 autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop; 01461 } 01462 } 01463 01464 break; 01465 01466 case 'L': //左旋回セヨ 01467 NVIC_EnableIRQ(USART2_IRQn); 01468 if(zeroTHR==false){ 01469 UpdateTargetAngle_Leftloop_zero(targetAngle); 01470 } 01471 else{ 01472 targetAngle[ROLL] = g_leftloopROLL_approach; 01473 targetAngle[PITCH] = g_leftloopPITCH_approach; 01474 autopwm[RUD]=g_leftloopRUD_approach; 01475 if(autopwm[AIL_R]<trimpwm[AIL_R]){ 01476 autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; 01477 } 01478 else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; 01479 } 01480 01481 break; 01482 01483 case 'G': //直進セヨ 01484 NVIC_EnableIRQ(USART2_IRQn); 01485 if(zeroTHR==false){ 01486 UpdateTargetAngle_GoStraight_zero(targetAngle); 01487 } 01488 else{ 01489 targetAngle[ROLL] = g_gostraightROLL; 01490 targetAngle[PITCH] = g_gostraightPITCH; 01491 } 01492 01493 break; 01494 01495 case 'Y': //指定ノヨー方向ニ移動セヨ 01496 NVIC_EnableIRQ(USART2_IRQn); 01497 Rotate(targetAngle, g_SerialTargetYAW); 01498 if(zeroTHR==false){ 01499 autopwm[THR]=minpwm[THR]; 01500 } 01501 break; 01502 01503 case 'U': //機首ヲ上ゲヨ 01504 NVIC_EnableIRQ(USART2_IRQn); 01505 static int UpCounter=0; 01506 UpCounter++; 01507 if(UpCounter==3){ 01508 while(1){ 01509 //targetAngle[ROLL] = g_gostraightROLL; 01510 autopwm[THR] = minpwm[THR]; 01511 autopwm[ELE] = minpwm[ELE]; 01512 autopwm[RUD]=trimpwm[RUD]; 01513 if(CheckSW_Up(Ch7)){ 01514 output_status = Auto; 01515 led1 = 1; 01516 }else{ 01517 output_status = Manual; 01518 led1 = 0; 01519 zeroTHR=true; 01520 //g_landingcommand='G'; 01521 } 01522 } 01523 01524 } 01525 01526 break; 01527 01528 /*case 'B': //ブザーヲ鳴ラセ 01529 //buzzer = 1; 01530 NVIC_EnableIRQ(USART2_IRQn); 01531 break;*/ 01532 01533 case 'B': //物資ヲ落トセ 01534 NVIC_EnableIRQ(USART2_IRQn); 01535 Chicken_Drop(); 01536 01537 break; 01538 01539 case 'C': //停止セヨ 01540 NVIC_EnableIRQ(USART2_IRQn); 01541 targetAngle[ROLL] = g_gostraightROLL; 01542 targetAngle[PITCH] = -3.0; 01543 autopwm[THR] = minpwm[THR]; 01544 zeroTHR=false; 01545 break; 01546 01547 default : 01548 NVIC_EnableIRQ(USART2_IRQn); 01549 break; 01550 01551 01552 } 01553 01554 } 01555 01556 void checkHeight(float targetAngle[3]){ 01557 01558 static int targetHeight = 200; 01559 01560 NVIC_DisableIRQ(EXTI9_5_IRQn); 01561 if(g_distance < targetHeight + ALLOWHEIGHT){ 01562 UpdateTargetAngle_NoseUP(targetAngle); 01563 if(CheckSW_Up(Ch7)) led2 = 1; 01564 } 01565 else if(g_distance > targetHeight - ALLOWHEIGHT){ 01566 UpdateTargetAngle_NoseDOWN(targetAngle); 01567 if(CheckSW_Up(Ch7)) led2 = 1; 01568 } 01569 else led2=0; 01570 NVIC_EnableIRQ(EXTI9_5_IRQn); 01571 } 01572 01573 void UpdateTargetAngle_NoseUP(float targetAngle[3]){ 01574 01575 //targetAngle[PITCH] += 2.0f; 01576 //if(nowAngle[PITCH]<targetAngle[PITCH]) autopwm[THR] = SetTHRinRatio(0.6); 01577 autopwm[THR] = SetTHRinRatio(g_loopTHR+0.05); 01578 //pc.printf("nose UP"); 01579 } 01580 01581 void UpdateTargetAngle_NoseDOWN(float targetAngle[3]){ 01582 01583 //targetAngle[PITCH] -= 2.0f; 01584 autopwm[THR] = SetTHRinRatio(g_loopTHR-0.05); 01585 //pc.printf("nose DOWN"); 01586 } 01587 01588 //直進 01589 void UpdateTargetAngle_GoStraight(float targetAngle[3]){ 01590 01591 autopwm[RUD] = trimpwm[RUD]; 01592 targetAngle[ROLL] = g_gostraightROLL; 01593 targetAngle[PITCH] = g_gostraightPITCH; 01594 autopwm[THR] = SetTHRinRatio(g_loopTHR); 01595 01596 //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); 01597 } 01598 01599 //直進(着陸時スロットル０のとき） 01600 void UpdateTargetAngle_GoStraight_zero(float targetAngle[3]){ 01601 01602 autopwm[RUD] = trimpwm[RUD]; 01603 targetAngle[ROLL] = g_gostraightROLL; 01604 targetAngle[PITCH] = g_gostraightPITCH; 01605 autopwm[THR] = minpwm[THR]; 01606 01607 //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); 01608 } 01609 01610 //右旋回 01611 void UpdateTargetAngle_Rightloop(float targetAngle[3]){ //右旋回 01612 01613 targetAngle[PITCH] = g_rightloopPITCH ; 01614 autopwm[RUD]=g_rightloopRUD; //RUD固定 01615 autopwm[THR] = SetTHRinRatio(0.45); //手動スロットル記憶 01616 01617 /* 01618 if (nowAngle[ROLL]>20.0||nowAngle[ROLL]<-20.0){ 01619 t2.start(); 01620 pc.printf("Timer start."); 01621 } 01622 if(0.0<t2.read()<5.0){ 01623 //pc.printf("tagetAngle is changed."); 01624 targetAngle[ROLL] = rightloopROLL2; 01625 } 01626 else { 01627 t2.stop(); 01628 t2.reset(); 01629 pc.printf("Timer stopped."); 01630 targetAngle[ROLL] = g_rightloopROLL; 01631 } 01632 */ 01633 if(autopwm[AIL_R]<trimpwm[AIL_R]){ //エルロン上がりやすさ調節 01634 autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; 01635 } 01636 else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; 01637 autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop; 01638 01639 01640 //checkHeight(targetAngle); 01641 //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); 01642 } 01643 01644 //右旋回（着陸時スロットル０の時） 01645 void UpdateTargetAngle_Rightloop_zero(float targetAngle[3]){ //右旋回 01646 autopwm[THR]=minpwm[THR]; 01647 targetAngle[ROLL] = g_rightloopROLL_approach; 01648 targetAngle[PITCH] = g_rightloopPITCH_approach ; 01649 autopwm[RUD]=g_rightloopRUD_approach; //RUD固定 01650 if(autopwm[AIL_R]<trimpwm[AIL_R]){ //エルロン上がりやすさ調節 01651 autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; 01652 } 01653 else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; 01654 autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop; 01655 01656 //checkHeight(targetAngle); 01657 //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); 01658 } 01659 01660 void UpdateTargetAngle_Rightloop_short(float targetAngle[3]){ //右旋回 01661 01662 targetAngle[ROLL] = g_rightloopROLLshort; 01663 targetAngle[PITCH] = g_rightloopPITCHshort; 01664 autopwm[RUD]=g_rightloopshortRUD; 01665 autopwm[THR] = SetTHRinRatio(0.45); 01666 if(autopwm[AIL_R]<trimpwm[AIL_R]){ 01667 autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; 01668 } 01669 else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; 01670 01671 //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); 01672 } 01673 01674 //左旋回 01675 void UpdateTargetAngle_Leftloop(float targetAngle[3]){ 01676 01677 targetAngle[ROLL] = g_leftloopROLL; 01678 targetAngle[PITCH] = g_leftloopPITCH; 01679 autopwm[RUD]=g_leftloopRUD; 01680 autopwm[THR] = SetTHRinRatio(0.45); 01681 if(autopwm[AIL_R]<trimpwm[AIL_R]){ 01682 autopwm[AIL_L]=trimpwm[AIL_L]+g_AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; 01683 } 01684 else autopwm[AIL_L]=trimpwm[AIL_L]+g_AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; 01685 //checkHeight(targetAngle); 01686 01687 //printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[], targetAngle[PITCH], autopwm[THR]); 01688 } 01689 01690 //左旋回（着陸時スロットル０のとき） 01691 void UpdateTargetAngle_Leftloop_zero(float targetAngle[3]){ 01692 01693 targetAngle[ROLL] = g_leftloopROLL_approach; 01694 targetAngle[PITCH] = g_leftloopPITCH_approach; 01695 autopwm[RUD]=g_leftloopRUD_approach; 01696 autopwm[THR] = minpwm[THR]; 01697 if(autopwm[AIL_R]<trimpwm[AIL_R]){ 01698 autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; 01699 } 01700 else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; 01701 //autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_Ratio_leftloop; 01702 //checkHeight(targetAngle); 01703 01704 //printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[], targetAngle[PITCH], autopwm[THR]); 01705 } 01706 01707 void UpdateTargetAngle_Leftloop_short(float targetAngle[3]){ 01708 01709 targetAngle[ROLL] = g_leftloopROLLshort; 01710 targetAngle[PITCH] = g_leftloopPITCHshort; 01711 autopwm[RUD]=g_leftloopRUD; 01712 autopwm[THR] = SetTHRinRatio(0.45); 01713 if(autopwm[AIL_R]<trimpwm[AIL_R]){ 01714 autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; 01715 } 01716 else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; 01717 01718 //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); 01719 } 01720 01721 void Sbusprintf(){ 01722 01723 for(uint8_t i=0; i<8; i++) pc.printf("ch.%d = %d ",i+1,sbus.manualpwm[i]); 01724 pc.printf("\r\n"); 01725 01726 } 01727 01728 01729 01730 //デバッグ用 01731 void DebugPrint(){ 01732 /* 01733 static int16_t deltaT = 0, t_start = 0; 01734 deltaT = t.read_u2s() - t_start; 01735 pc.printf("t:%d us, ",deltaT); 01736 pc.printf("\r\n"); 01737 t_start = t.read_us(); 01738 */ 01739 //for(uint8_t i=0; i<8; i++) pc.printf("%d ",sbus.manualpwm[i]); 01740 //for(uint8_t i=1; i<4; i++) pc.printf("%d ",sbus.manualpwm[i]); 01741 //pc.printf("\r\n"); 01742 //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); 01743 //for(uint8_t i=0; i<2; i++) pc.printf("%3.2f\t",nowAngle[i]); //ロール、ピッチのみ 小数点以下2ケタ 01744 //pc.printf("%d\t",autopwm[AIL_L]); 01745 //pc.printf("%d\t",autopwm[RUD]); 01746 //pc.printf("%d",g_distance); 01747 //NVIC_DisableIRQ(EXTI9_5_IRQn); 01748 //pc.printf("g_distance = %d",g_distance); 01749 //NVIC_EnableIRQ(EXTI9_5_IRQn); 01750 //pc.printf("Mode: %c: ",g_buf[0]); 01751 //if(g_buf[0] == 'Y') pc.printf(" %3.1f",g_SerialTargetYAW); 01752 //pc.printf("\r\n"); 01753 }
Generated on Wed Jul 20 2022 05:24:47 by
1.7.2