語型受信機フェイルセーフ付き(要修正) 離陸時のELE角度以外動作確認完了
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of Autoflight2018_43 by
Revision 33:0f7a35d55316, committed 2018-09-23
- Comitter:
- taknokolat
- Date:
- Sun Sep 23 18:17:50 2018 +0000
- Parent:
- 32:48d5d3f77c41
- Commit message:
- a
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sat Sep 22 10:41:41 2018 +0000 +++ b/main.cpp Sun Sep 23 18:17:50 2018 +0000 @@ -937,10 +937,14 @@ NVIC_DisableIRQ(USART1_IRQn); static int16_t pwm[6]; static int16_t temppwm[6]={trimpwm[0],trimpwm[1],trimpwm[2],trimpwm[3],trimpwm[4],trimpwm[5]}; - if(sbus.flg_ch_update == true){ - + static int16_t FailsafeCounter=0; + static int16_t OKCounter=0; + + if(sbus.flg_ch_update == true && sbus.failsafe_status==SBUS_SIGNAL_OK){ + switch(output_status){ //マニュアルモード,自動モード,自動着陸もモードを切替 - case Manual: + case Manual: + if(OKCounter!=0) break; for(uint8_t i=0;i<6;i++){ pwm[i] = sbus.manualpwm[i]; } @@ -950,37 +954,62 @@ break; case Auto: + if(OKCounter!=0) break; pwm[AIL_R] = autopwm[AIL_R]; //sbus.manualpwm[AIL]; pwm[ELE] = autopwm[ELE]; pwm[THR] = autopwm[THR]; pwm[RUD] = autopwm[RUD]; pwm[DROP] = autopwm[DROP]; pwm[AIL_L] = autopwm[AIL_L]; - + NVIC_EnableIRQ(USART1_IRQn); break; default: + if(OKCounter!=0) break; for(uint8_t i=0;i<6;i++){ pwm[i] = sbus.manualpwm[i]; } //pc.printf("update_manual\r\n"); NVIC_EnableIRQ(USART1_IRQn); break; - } for(uint8_t i=0;i<6;i++){ - if(pwm[i]<1000 || pwm[i]>2000) pwm[i]=temppwm[i]; - temppwm[i]=pwm[i]; + if(pwm[i]<1000 || pwm[i]>2000) pwm[i]=temppwm[i]; + temppwm[i]=pwm[i]; + } + + } + //else(sbus.flg_ch_update == false) pc.printf("0\r\n"); + /* if(sbus.failsafe_status==SBUS_SIGNAL_OK){ + pc.printf("OK\r\n"); } - }else{ - pc.printf("0\r\n"); - } + */ + //pc.printf("%d\r\n",sbus.failsafe_status); + + if(sbus.failsafe_status==SBUS_SIGNAL_LOST||sbus.failsafe_status==SBUS_SIGNAL_FAILSAFE) FailsafeCounter++; + else FailsafeCounter=0; + + if(FailsafeCounter>10){ + for(uint8_t i=0;i<6;i++) pwm[i] = trimpwm[i]; + + if(sbus.failsafe_status==SBUS_SIGNAL_OK) OKCounter++; + if(OKCounter>10) { + OKCounter=0; + } + + //pc.printf("%d\r\n",sbus.failsafe_status); + //if(sbus.failsafe_status!=SBUS_SIGNAL_FAILSAFE)break; + + } + //if(sbus.failsafe_status==SBUS_SIGNAL_OK){FailsafeCounter=0;} + + sbus.flg_ch_update = false; Output_PWM(pwm); } - - + + //pwmをサーボに出力。 void Output_PWM(int16_t pwm[5]) { @@ -1217,7 +1246,7 @@ //離陸-投下-着陸一連 void Take_off_and_landing(float targetAngle[3]){ - /* + if(!CheckSW_Up(Ch7)) bombing_mode = Takeoff; switch(bombing_mode){ @@ -1233,9 +1262,10 @@ } UpdateTargetAngle_Takeoff(targetAngle); - + NVIC_DisableIRQ(EXTI9_5_IRQn); if(g_distance>150) TakeoffCount++; else TakeoffCount = 0; + NVIC_EnableIRQ(EXTI9_5_IRQn); if(TakeoffCount>5){ autopwm[THR] = 1180+320*2*0.5; pc.printf("Now go to Approach mode!!"); @@ -1245,7 +1275,7 @@ //case Chicken: //break; - + /* case Transition: static int ApproachCount = 0; targetAngle[YAW]=180.0; @@ -1254,19 +1284,19 @@ if(Judge==0) ApproachCount++; if(ApproachCount>5) bombing_mode = Approach; break; - + */ case Approach: - */ + autopwm[THR] = SetTHRinRatio(g_loopTHR); //0.7;スロットルの割合 UpdateTargetAngle_Approach(targetAngle); - /* + break; default: bombing_mode = Takeoff; break; } - */ + } //離陸モード @@ -1421,6 +1451,7 @@ void checkHeight(float targetAngle[3]){ static int targetHeight = 200; + NVIC_DisableIRQ(EXTI9_5_IRQn); if(g_distance < targetHeight + ALLOWHEIGHT){ UpdateTargetAngle_NoseUP(targetAngle); @@ -1606,10 +1637,10 @@ //for(uint8_t i=0; i<2; i++) pc.printf("%3.2f\t",nowAngle[i]); //ロール、ピッチのみ 小数点以下2ケタ //pc.printf("%d\t",autopwm[AIL_L]); // pc.printf("%d\t",autopwm[RUD]); //pc.printf("%d",g_distance); - NVIC_DisableIRQ(EXTI9_5_IRQn); - pc.printf("g_distance = %d",g_distance); - NVIC_EnableIRQ(EXTI9_5_IRQn); + //NVIC_DisableIRQ(EXTI9_5_IRQn); + //pc.printf("g_distance = %d",g_distance); + //NVIC_EnableIRQ(EXTI9_5_IRQn); //pc.printf("Mode: %c: ",g_buf[0]); //if(g_buf[0] == 'Y') pc.printf(" %3.1f",g_SerialTargetYAW); - pc.printf("\r\n"); + //pc.printf("\r\n"); } \ No newline at end of file