syouichi imamori
/
MulticopterQuadX
Quad X Type Multicopter
Diff: main.cpp
- Revision:
- 7:16bf0085d914
- Parent:
- 6:a50e6d3924f1
- Child:
- 8:1db19b529b22
diff -r a50e6d3924f1 -r 16bf0085d914 main.cpp --- a/main.cpp Tue Feb 24 09:28:29 2015 +0000 +++ b/main.cpp Tue Apr 28 01:48:21 2015 +0000 @@ -533,7 +533,7 @@ void PWM_Out(bool mode) { int reg[3]; - int i; + int i,j,k; float gain; // float cur_height; @@ -563,9 +563,17 @@ M5 = THR + conf.Throttl_Trim; break; case Quad_3D: - i = (int)throLimit.calc((float)THR); - if ( abs(i-conf.Reverse_Point) > 100 ) i = THR; - M1 = M2 = M3 = M4 = i + conf.Throttl_Trim; + j = THR + conf.Throttl_Trim; + if ( Stick[GAIN] < 0 ) { + k = THR - conf.Reverse_Point; + if ( abs(k) < THR_MIDRANGE ) { + if ( k > 0 ) j = conf.Reverse_Point + THR_MIDRANGE; + else j = conf.Reverse_Point - THR_MIDRANGE; + } + } +// i = (int)throLimit.calc((float)THR); + // if ( abs(i-conf.Reverse_Point) > 100 ) i = THR; + M1 = M2 = M3 = M4 = j; break; case Delta: case Delta_TW: @@ -655,32 +663,28 @@ M4 -= reg[YAW]; break; } - int h = conf.Model_Type; - for ( int i=0; i<6; i++ ) { + j = conf.Model_Type; + for ( i=0; i<6; i++ ) { if ( M[i] > Pulse_Max ) M[i] = Pulse_Max; if ( M[i] < Pulse_Min ) M[i] = Pulse_Min; - if ( Servo_idx[h][i] == 1 ) + if ( Servo_idx[j][i] == 1 ) { M[i] = conf.Stick_Ref[i] + ( ( M[i] - conf.Stick_Ref[i] ) * conf.Servo_Dir[i] ); } else { - if ( h == Quad_3D ) { + if ( j == Quad_3D ) { if ( Stick[GAIN] > 0 ) { if ( THR < conf.Reverse_Point ) M[i] = conf.Reverse_Point; } - else { - if ( abs(THR - conf.Reverse_Point ) < 10 ) - M[i] = conf.Reverse_Point; - } } else { - // if ( Stick[COL] < Thro_Zero ) M[i] = conf.Stick_Ref[COL]; + if ( Stick[COL] < Thro_Zero ) M[i] = conf.Stick_Ref[COL]; } } } if ( mode ) { - h = conf.Stick_Ref[THR]; +// h = conf.Stick_Ref[THR]; for ( i=0;i<6;i++ ) { // while ( !pwmpin[i] ); if ( conf.PWM_Mode == 1 )