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