勇輝 関本
/
Estrela_v10
4項目一応成功
Fork of Estrela_v01 by
Revision 9:182021b1df79, committed 2016-08-22
- Comitter:
- motoseki
- Date:
- Mon Aug 22 09:03:15 2016 +0000
- Parent:
- 8:887d448db359
- Commit message:
- ??????
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sun Aug 21 10:37:09 2016 +0000 +++ b/main.cpp Mon Aug 22 09:03:15 2016 +0000 @@ -30,7 +30,7 @@ DigitalOut led1(PA_0); //緑 DigitalOut led2(PA_1); //赤 -DigitalOut led3(PB_4); +DigitalOut led3(PB_4); //赤外線 DigitalOut led4(PB_5); volatile uint8_t buf_sbus[25]; @@ -462,8 +462,8 @@ //pc.printf("first%d\r\n",jj); jj++; }else{ //旋回中 - Auto_RUD = RUD_N + -80 + int((-23-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); - Auto_ELE = ELE_N + -110 + int( (-28 - pitch)*ELEVATOR_GN - gx*ELE_DGN); + Auto_RUD = RUD_N + -70 + int((-23-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); + Auto_ELE = ELE_N + -100 + int((-25 - pitch)*ELEVATOR_GN - gx*ELE_DGN); Auto_THR = oldTHL; } @@ -488,30 +488,30 @@ void StabiGyro_Mobius() { if(jj<=25){ //右旋回導入 - Auto_RUD = RUD_N + int((-25-roll)*RUDDER_GN - gy*RUD_DGN); - Auto_ELE = ELE_N + int( (LAND_ANGLE + 3 - pitch)*ELEVATOR_GN - gx*ELE_DGN); - Auto_THR = THR_N + 50; + Auto_RUD = RUD_N + -105 + int((-30-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); //roll -> gy 3.5 + Auto_ELE = ELE_N + -75 + int((-15 - pitch)*ELEVATOR_GN - gx*ELE_DGN); //pitch -> -gx + Auto_THR = oldTHL + 50; //pc.printf("first%d\r\n",jj); jj++; }else if((25<jj) && (jj<=112)){ //右旋回途中 - Auto_RUD = RUD_N + int((-20-roll)*RUDDER_GN - gy*RUD_DGN); - Auto_ELE = ELE_N + int( (LAND_ANGLE - 1 - pitch)*ELEVATOR_GN - gx*ELE_DGN); - Auto_THR = THR_N; + Auto_RUD = RUD_N + -80 + int((-23-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); + Auto_ELE = ELE_N + -110 + int((-28 - pitch)*ELEVATOR_GN - gx*ELE_DGN); + Auto_THR = oldTHL; jj++; }else if((112<jj) && (jj<=117)){ //旋回遷移1 - Auto_RUD = RUD_N + int((-roll)*RUDDER_GN - gy*RUD_DGN); - Auto_ELE = ELE_N + int( (LAND_ANGLE + 2 - pitch)*ELEVATOR_GN - gx*ELE_DGN); + Auto_RUD = RUD_N + int((0 -roll)*RUDDER_GN - (gy-100)*RUD_DGN); + Auto_ELE = ELE_N + -30 + int((-10 - pitch)*ELEVATOR_GN - gx*ELE_DGN); Auto_THR = THR_N - 100; //pc.printf("first%d\r\n",jj); jj++; }else if((117<jj) && (jj<=132)){ //旋回遷移2 - Auto_RUD = RUD_N + int((25-roll)*RUDDER_GN - gy*RUD_DGN); - Auto_ELE = ELE_N + int( (LAND_ANGLE + 6 - pitch)*ELEVATOR_GN - gx*ELE_DGN); - Auto_THR = THR_N - 20; + Auto_RUD = RUD_N + 105 + int((30-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); + Auto_ELE = ELE_N + -75 + int((-15 - pitch)*ELEVATOR_GN - gx*ELE_DGN); + Auto_THR = oldTHL - 20; jj++; }else{ //左旋回途中 - Auto_RUD = RUD_N + int((20-roll)*RUDDER_GN - gy*RUD_DGN); - Auto_ELE = ELE_N + int( (LAND_ANGLE - pitch)*ELEVATOR_GN - gx*ELE_DGN); + Auto_RUD = RUD_N + 80 + int((23-roll)*RUDDER_GN - gy*RUD_DGN); + Auto_ELE = ELE_N + -110 + int((-28 - pitch)*ELEVATOR_GN - gx*ELE_DGN); Auto_THR = THR_N + 30; } @@ -535,15 +535,15 @@ void StabiGyro_Climb() { if(jj<=25){ //右旋回導入 - Auto_RUD = RUD_N + int((-25-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); //roll -> gy - Auto_ELE = ELE_N + int( (LAND_ANGLE + 3 - pitch)*ELEVATOR_GN - gx*ELE_DGN); //pitch -> -gx - Auto_THR = THR_N + 50; + Auto_RUD = RUD_N + -105 + int((-30-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); //roll -> gy 3.5 + Auto_ELE = ELE_N + -75 + int((-15 - pitch)*ELEVATOR_GN - gx*ELE_DGN); //pitch -> -gx + Auto_THR = oldTHL + 50; //pc.printf("first%d\r\n",jj); jj++; }else if((25<jj) && (jj<=224)){ //右旋回途中 - Auto_RUD = RUD_N + int((-20-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); - Auto_ELE = ELE_N + int( (LAND_ANGLE - pitch)*ELEVATOR_GN - gx*ELE_DGN); - Auto_THR = THR_N; + Auto_RUD = RUD_N + -80 + int((-23-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); + Auto_ELE = ELE_N + -110 + int((-28 - pitch)*ELEVATOR_GN - gx*ELE_DGN); + Auto_THR = oldTHL; jj++; /*}else if((224<jj) && (jj<=234)){ //上昇遷移 Auto_RUD = RUD_N + 160 - int((g_HpfGyro[0]+25)*0.75); @@ -552,14 +552,14 @@ //pc.printf("first%d\r\n",jj); jj++;*/ }else if((224<jj) && (jj<=314)){ //上昇中 - Auto_RUD = RUD_N + int((-10-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); - Auto_ELE = ELE_N + int( (LAND_ANGLE + 8 - pitch)*ELEVATOR_GN - gx*ELE_DGN); - Auto_THR = THR_N + 200; + Auto_RUD = RUD_N + -60 + int((-20-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); + Auto_ELE = ELE_N + -140 + int((-30 - pitch)*ELEVATOR_GN - gx*ELE_DGN); + Auto_THR = oldTHL + 220; jj++; }else{ //水平旋回 - Auto_RUD = RUD_N + int((-15-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); //roll -> gy - Auto_ELE = ELE_N + int( (LAND_ANGLE + 3 - pitch)*ELEVATOR_GN - gx*ELE_DGN); //pitch -> -gx - Auto_THR = THR_N; + Auto_RUD = RUD_N + -80 + int((-23-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); + Auto_ELE = ELE_N + -110 + int((-28 - pitch)*ELEVATOR_GN - gx*ELE_DGN); + Auto_THR = oldTHL; } if(Auto_RUD > RUD_MAX){ @@ -588,8 +588,8 @@ //pc.printf("first%d\r\n",jj); // jj++; //}else{ - Auto_RUD = RUD_N + int((-10-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); - Auto_ELE = ELE_N + int( (LAND_ANGLE + 3 - pitch)*ELEVATOR_GN - gx*ELE_DGN); + Auto_RUD = RUD_N + -60 + int((-18-roll)*RUDDER_GN - gy*RUD_DGN); //*0.75); + Auto_ELE = ELE_N + -110 + int((-24 - pitch)*ELEVATOR_GN - gx*ELE_DGN); Auto_THR = 1000; //スロットルoff if(Auto_RUD > RUD_MAX){ @@ -607,6 +607,12 @@ //Thread::wait(G_MS); } +void StabiGyro_Landing() +{ + Auto_ELE = ELE_N + -80 + int((-20 - pitch)*ELEVATOR_GN - gx*ELE_DGN); + //Auto_THR = +} + void Auto_Loop() { //while(true){ @@ -655,8 +661,11 @@ } void Auto_Landing() -{ - update_mode = UPD_MANUAL; +{ + sensing(); + StabiGyro_Landing(); + update_mode = UPD_LANDING; + wait_ms(50); } int main() @@ -752,9 +761,9 @@ if(!CheckSW_up(7)&&!CheckSW_up(8)){ //チャンネル7,8が両方offなら OperationMode=AUTOLOOP; //変数OperationModeにAUTOLOOP(定数なので2(SkipperS.hで定義))を代入 pc.printf("go to autoloop\r\n"); - }//else if(CheckSW_up(7)&&!CheckSW_up(8)){ //チャンネル7だけonなら - //CalibrateCompass(); - //} + }/*else if(CheckSW_up(7)&&!CheckSW_up(8)){ //チャンネル7だけonなら + CalibrateCompass(); + }*/ //変数update_modeはSBus(=プロポ=操縦者)からのpwmをそのまま垂れ流すか、自動制御で計算したpwmをサーボに流すかを切り替えるための変数 update_mode = UPD_MANUAL; //マニュアルモード(そのまま垂れ流す) // pc.printf("groundcheck mode\r\n");