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