フェイルセーフ完成版
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of Autoflight2018_53 by
Diff: main.cpp
- Revision:
- 43:4413ee61bc39
- Parent:
- 42:74d72339a8a8
diff -r 74d72339a8a8 -r 4413ee61bc39 main.cpp --- a/main.cpp Wed Sep 26 04:35:47 2018 +0000 +++ b/main.cpp Wed Sep 26 11:26:28 2018 +0000 @@ -961,9 +961,10 @@ static int16_t pwm[6]; static int16_t temppwm[6]={trimpwm[0],trimpwm[1],trimpwm[2],trimpwm[3],trimpwm[4],trimpwm[5]}; static int16_t FailsafeCounter=0; + static int16_t ResetCounter=0; static int16_t OKCounter=0; - if(sbus.flg_ch_update == true && sbus.failsafe_status==SBUS_SIGNAL_OK){ + if(sbus.flg_ch_update == true){ switch(output_status){ //マニュアルモード,自動モード,自動着陸もモードを切替 case Manual: @@ -1011,20 +1012,25 @@ //pc.printf("%d\r\n",sbus.failsafe_status); if(sbus.failsafe_status==SBUS_SIGNAL_LOST||sbus.failsafe_status==SBUS_SIGNAL_FAILSAFE) FailsafeCounter++; + else ResetCounter++; + if(ResetCounter>7){ + ResetCounter=0; + FailsafeCounter=0; + } + if(FailsafeCounter>10){ + ResetCounter=0; for(uint8_t i=0;i<6;i++) pwm[i] = trimpwm[i]; - if(sbus.failsafe_status==SBUS_SIGNAL_OK) OKCounter++; + if(sbus.failsafe_status==SBUS_SIGNAL_OK&&sbus.flg_ch_update == true) OKCounter++; else OKCounter=0; - if(OKCounter>5){ + if(OKCounter>10){ OKCounter=0; - FailsafeCounter=0; + FailsafeCounter=0; } - //pc.printf("%d\r\n",sbus.failsafe_status); - //if(sbus.failsafe_status!=SBUS_SIGNAL_FAILSAFE)break; - + pc.printf("OKCounter=%d, FailsafeCounter=%d, sbus.failsafe_status=%d\r\n",OKCounter,FailsafeCounter,sbus.failsafe_status); } //if(sbus.failsafe_status==SBUS_SIGNAL_OK){FailsafeCounter=0;} @@ -1034,6 +1040,8 @@ } + + //pwmをサーボに出力。 void Output_PWM(int16_t pwm[5]) { @@ -1299,6 +1307,7 @@ if(TakeoffCount>5){ autopwm[THR] = 1180+320*2*0.5; targetAngle[PITCH]=g_gostraightPITCH; + autopwm[RUD]=trimpwm[RUD]; //pc.printf("Now go to Approach mode!!"); bombing_mode = Approach; }