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