syouichi imamori
/
MulticopterQuadX
Quad X Type Multicopter
Diff: main.cpp
- Revision:
- 6:a50e6d3924f1
- Parent:
- 5:7b02775787a9
- Child:
- 7:16bf0085d914
--- a/main.cpp Fri Oct 17 03:56:56 2014 +0000 +++ b/main.cpp Tue Feb 24 09:28:29 2015 +0000 @@ -3,13 +3,14 @@ ------------+-----------+-----------+-----------+-----------+--------------------- Quad-X |FL Thr ESC |FR Thr ESC |BL Thr ESC |BR Thr ESC | - | - Quad-H |FL VP Ser |FR VP Ser |BL VP Ser |BR VP Ser |Thr ESC | - - Delta |FL Tro ESC |L Ale Ser |R Ale Ser |Rud Ser | - | - - Delta-TW |FL Tro ESC |L Ale Ser |R Ale Ser |Rud Ser |FR Thr ESC | - + Quad-3D |FL Thr ESC |FR Thr ESC |BL Thr ESC |BR Thr ESC | - | - + Delta |FL Tro ESC |L Alv Ser |R Alv Ser |Rud Ser | - | - + Delta-TW |FL Tro ESC |L Alv Ser |R Alv Ser |Rud Ser |FR Thr ESC | - Airplane |Thr ESC |Ail Ser |Ele Ser |Rud Ser |Ail Ser | - - Ele:Elevator, Rud:Rudder, Ail:Aileron, Thr:Throttle, Ale:Alevon + F:Front, B:Back, L:Left, R:Right + Ele:Elevator, Rud:Rudder, Ail:Aileron, Thr:Throttle, Alv:Alevon ESC:for ESC, Ser:for Servo, VP:Variable Pitch - F:Front, B:Back, L:Left, R:Right */ #include "mbed.h" @@ -33,7 +34,7 @@ #if defined(TARGET_LPC1768) DigitalInOut pwmpin[] = { p21,p22,p23,p24 }; // #ifdef LPCXpresso -// #define LED1 P0_22 + #define LED1 P0_22 // #endif DigitalOut led1(LED1); // DigitalOut led2(LED2); @@ -101,6 +102,7 @@ Timer FlyghtTime; config conf; //PID pid[4]; +Limiter throLimit = 100; Limiter gyroLimit[3] = {300,300,300}; Limiter accLimit[3] = {0.5,0.5,0.5}; Limiter pwmLimit[4] = {50,50,50,50}; @@ -119,12 +121,20 @@ volatile float Gyro_Save[3]; volatile int Stick[6]; volatile int Stick_Save[6]; -int PWM_Init[5][6] = { 1080,1080,1080,1080,1080,1080, //Quad_X - 1500,1500,1500,1500,1080,1080, //Quad_H +int PWM_Init[6][6] = { 1080,1080,1080,1080,1080,1080, //Quad_X + 1500,1500,1500,1500,1080,1080, //Quad_VP + 1500,1500,1500,1500,1080,1080, //Quad_3D 1080,1500,1500,1500,1500,1080, //Delta 1080,1500,1500,1500,1080,1500, //Delta_TW 1080,1500,1500,1500,1500,1080 //AirPlane }; +char Servo_idx[6][6] = { 0,0,0,0,0,0, //Quad_X + 1,1,1,1,0,0, //Quad_VP + 0,0,0,0,0,0, //Quad_3D + 0,1,1,1,1,1, //Delta + 0,1,1,1,0,1, //Delta_TW + 0,1,1,1,1,1 //AirPlane + }; //int Stick_Max[3]; float Press; float Base_Press; @@ -137,8 +147,8 @@ int loop_cnt; float target_height; float cuurent_height; -float base_throttol; -int throttol; +float base_Throttl; +int Throttl; bool hov_control; float Rdata; @@ -162,7 +172,6 @@ void LCD_printf(char *); void LCD_cls(); void LCD_locate(int,int); -void Servo_Reverse(int,int); #ifdef SOFTPWM void Tpwm_interrupt() { @@ -197,7 +206,8 @@ initialize(); Get_Stick_Pos(); - while ( Stick[COL] > Thro_Zero || conf.StartMode == 'C' ) //Shrottol Low + while ( Stick[COL] > Thro_Zero || conf.StartMode == 'C' + || ( conf.Model_Type == Quad_3D && Stick[GAIN] < 0 ) ) //Shrottol Low { // if ( Stick[COL] > 890 && -Stick[YAW] < Stick_Limit ) // Shrottle High // ESC_SetUp(); @@ -207,8 +217,8 @@ SetUpPrompt(conf,i2c); break; } - FlashLED(3); - wait(0.3); + FlashLED(2); + wait(0.5); Get_Stick_Pos(); } Flight_SetUp(); @@ -263,6 +273,8 @@ else InPulseMode = 'P'; led1 = 0; CycleTime.start(); + throLimit.differential(conf.Thro_Limit_Val); + throLimit.rate(conf.Thro_Limit_Rate); #ifdef SOFTPWM Tpwm.attach_us(&Tpwm_interrupt,conf.PWM_Interval); #endif @@ -439,13 +451,9 @@ if ( err == false ) return false; //pc.printf("%6.4f,%6.4f,%6.4f\r\n",x,y,z); - x -= conf.Accel_Ref[0]; - y -= conf.Accel_Ref[1]; - z -= conf.Accel_Ref[2]; - - Accel[ROL] = accLimit[0].calc(x); - Accel[PIT] = accLimit[1].calc(y); - Accel[YAW] = accLimit[2].calc(z); + x = accLimit[0].calc(x) - conf.Accel_Ref[0]; + y = accLimit[1].calc(y) - conf.Accel_Ref[1]; + z = accLimit[2].calc(z) - conf.Accel_Ref[2]; //pc.printf("%6.4f,%6.4f,%6.4f\r\n",Accel[ROL],Accel[PIT],Accel[YAW]); Accel[ROL] = atan(x/sqrtf( powf(y,2)+powf(z,2)))*180/3.14f; Accel[PIT] = atan(y/sqrtf( powf(x,2)+powf(z,2)))*180/3.14f; @@ -456,7 +464,7 @@ void Get_Angle(float interval) { float x,y,z; - Get_Accel(interval); +// Get_Accel(interval); Get_Gyro(interval); x = ( Gyro[ROL] + Gyro_Save[ROL] ) * 0.5f; @@ -531,7 +539,7 @@ interval = CycleTime.read(); CycleTime.reset(); - if ( interval > 0.2f ) return; + if ( interval > 0.005f && mode ) return; TotalTime += interval; if ( TotalTime > 0.5f ) { led1 = !led1; @@ -548,22 +556,27 @@ Get_Stick_Pos(); switch ( conf.Model_Type ) { case Quad_X: - M1 = M2 = M3 = M4 = Stick[COL]; + M1 = M2 = M3 = M4 = THR + conf.Throttl_Trim; break; - case Quad_H: - M1 = M2 = M3 = M4 = Stick[AUX2]; - M5 = Stick[COL]; + case Quad_VP: + M1 = M2 = M3 = M4 = Stick[AUX2] + conf.Stick_Ref[COL]; + 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; break; case Delta: case Delta_TW: case AirPlane: - M1 = Stick[COL]; - M2 = conf.Stick_Ref[ROL] - conf.Stick_Ref[COL]; - M3 = conf.Stick_Ref[PIT] - conf.Stick_Ref[COL]; - M4 = conf.Stick_Ref[YAW] - conf.Stick_Ref[COL]; + M1 = THR + conf.Throttl_Trim; + M2 = conf.Stick_Ref[ROL]; + M3 = conf.Stick_Ref[PIT]; + M4 = conf.Stick_Ref[YAW]; if ( conf.Model_Type == AirPlane ) - M5 = conf.Stick_Ref[ROL] - conf.Stick_Ref[COL]; - else M5 = Stick[COL]; + M5 = conf.Stick_Ref[ROL]; + else M5 = M1; break; } @@ -571,11 +584,12 @@ // Stick Angle Mixing if ( conf.Gyro_Gain_Setting == 1 ) gain = conf.Gyro_Gain[i] * conf.Gyro_Dir[i]; - else gain = ( (float)abs(Stick[GAIN])/100 + conf.Gyro_Gain[i+3] ) * conf.Gyro_Dir[i]; - if ( fabsf(Stick[i]) > 0 ) gain -= gain * ( fabsf(Stick[i]) / 500 ) * conf.Active_Jyro_Gain; + else gain = ( (float)fabsf(Stick[GAIN])/100 + conf.Gyro_Gain[i+3] ) * conf.Gyro_Dir[i]; + if ( Stick[i] > 0 ) gain -= gain * ( fabsf(Stick[i]) / 400 ) * conf.Active_Gyro_Gain; if ( Stick[GAIN] > 0 || i == YAW || conf.Model_Type > 0 + || i2c.GetAddr(ACCEL_ADDR) == 0 ) { reg[i] = Stick[i] * conf.Stick_Mix[i]; reg[i] += Gyro[i] * gain * GYRO_ADJUST; @@ -589,9 +603,14 @@ } pid_interval = 0; +// int Reverse = 1; switch ( conf.Model_Type ) { - case Quad_X: - case Quad_H: + case Quad_3D: +// if ( THR < conf.Reverse_Point ) { +// Reverse = -1; +// } + case Quad_X: + case Quad_VP: //Calculate Roll Pulse Width M1 += reg[ROL]; M2 -= reg[ROL]; @@ -610,8 +629,8 @@ M3 -= reg[YAW]; M4 += reg[YAW]; break; - case Delta: - case Delta_TW: + case Delta: + case Delta_TW: //Calculate Roll Pulse Width M2 += reg[ROL]; M3 -= reg[ROL]; @@ -626,58 +645,52 @@ M5 -= reg[YAW]; } break; - case AirPlane: - //Calculate Roll Pulse Width + case AirPlane: + //Calculate Roll Pulse Width M2 -= reg[ROL]; M5 -= reg[ROL]; - //Calculate Pitch Pulse Width + //Calculate Pitch Pulse Width M3 += reg[PIT]; //Calculate Yaw Pulse Width M4 -= reg[YAW]; break; } - if ( conf.Model_Type != AirPlane ) { - for ( i=0;i<4;i++ ) + int h = conf.Model_Type; + for ( int 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 ( M[i] > Thro_Hi ) M[i] = Thro_Hi; - if ( M[i] < Thro_Lo ) M[i] = Thro_Lo; // motor idle level + M[i] = conf.Stick_Ref[i] + ( ( M[i] - conf.Stick_Ref[i] ) * conf.Servo_Dir[i] ); + } + else { + if ( h == 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]; + } } } - - switch ( conf.Model_Type ) { - case Quad_X: - if ( Stick[COL] < Thro_Zero ) M1=M2=M3=M4=0; - break; - case Quad_H: - if ( Stick[COL] < Thro_Zero ) M5=0; - Servo_Reverse(1,4); - break; - case Delta: - case Delta_TW: - if ( Stick[COL] < Thro_Zero ) M1=M5=0; - Servo_Reverse(2,4); - break; - case AirPlane: - Servo_Reverse(2,5); - break; - } if ( mode ) { + h = conf.Stick_Ref[THR]; for ( i=0;i<6;i++ ) { // while ( !pwmpin[i] ); if ( conf.PWM_Mode == 1 ) - pwm[i].pulsewidth_us(conf.ESC_Low+M[i]); - else pwm[i].pulsewidth_us(M[i]); + pwm[i].pulsewidth_us(M[i]); + else pwm[i].pulsewidth_us(M[i]-conf.Stick_Ref[COL]); } } } -void Servo_Reverse(int start,int end) { - for ( int i=start-1; i<end; i++ ) { - if ( conf.Servo_Dir[i] == -1 ) - M[i] = 1500 + ( ( conf.Stick_Ref[COL] + M[i] - 1500 ) * conf.Servo_Dir[i] ) - conf.Stick_Ref[COL]; - } -} void ESC_SetUp(void) { while(1) { @@ -701,6 +714,8 @@ pwm[i].pulsewidth_us(PWM_Init[conf.Model_Type][i]); } hov_control = false; + throLimit.differential(conf.Thro_Limit_Val); + throLimit.rate(conf.Thro_Limit_Rate); Angle[ROL]=Angle[PIT]=Angle[YAW]=0; loop_cnt = 0; FlyghtTime.start();