syouichi imamori
/
MulticopterQuadX
Quad X Type Multicopter
Diff: main.cpp
- Revision:
- 4:4060309b9cc0
- Parent:
- 3:27407c4984cf
- Child:
- 5:7b02775787a9
--- a/main.cpp Thu Feb 13 16:07:07 2014 +0000 +++ b/main.cpp Tue Oct 14 08:15:03 2014 +0000 @@ -1,3 +1,17 @@ +/* PWM Output + type | M1 | M2 | M3 | M4 | M5 | M6 + ------------+-----------+-----------+-----------+-----------+--------------------- + 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 | - + Airplane |Thr ESC |Ail Ser |Ele Ser |Rud Ser |Ail Ser | - + + Ele:Elevator, Rud:Rudder, Ail:Aileron, Thr:Throttle, Ale:Alevon + ESC:for ESC, Ser:for Servo, VP:Variable Pitch + F:Front, B:Back, L:Light, R:Right +*/ + #include "mbed.h" #include "math.h" #include "I2cPeripherals.h" @@ -6,107 +20,127 @@ #include "PulseWidthCounter.h" #include "string" #include "SerialLcd.h" -#include "IAP.h" -#include "PID.h" +//#include "PID.h" #include "SoftPWM.h" +#include "PulseOut.h" +#include "Limiter.h" +#ifndef TARGET_NUCLEO_F401RE + #include "IAP.h" +#endif //Serial pc(USBTX, USBRX); -//Gravity at Earth's surface in m/s/s -#define g0 9.812865328 -//Convert from radians to degrees. -#define toDegrees(x) (x * 57.2957795) -//Convert from degrees to radians. -#define toRadians(x) (x * 0.01745329252) -//ITG-3200 sensitivity is 14.375 LSB/(degrees/sec). -#define GYROSCOPE_GAIN (1 / 14.375) -//Full scale resolution on the ADXL345 is 4mg/LSB. -#define ACCELEROMETER_GAIN (0.004 * g0) -//Updating filter at 40Hz. -#define FILTER_RATE 0.05 -//At rest the gyroscope is centred around 0 and goes between about -//-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly -//5/15 = 0.3 degrees/sec. +#if defined(TARGET_LPC1768) + DigitalInOut pwmpin[] = { p21,p22,p23,p24 }; +// #ifdef LPCXpresso +// #define LED1 P0_22 +// #endif + DigitalOut led1(LED1); +// DigitalOut led2(LED2); + InterruptIn ch1(p5); + PulseWidthCounter ch[6] = { p6,p7,p8,p9,p10,p11 }; + PwmOut pwm[6] = { p21,p22,p23,p24,p25,p26 }; +// SoftPWM pwm[6] = { p21,p22,p23,p24,p25,p26 }; + SoftPWM buzz(p20); + I2cPeripherals i2c(p28,p27); //sda scl + SerialLcd lcd(p13,p14); + #define MEM_SIZE 256 + #define TARGET_SECTOR 29 // use sector 29 as target sector if it is on LPC1768 + IAP iap; +#elif defined(TARGET_NUCLEO_F401RE) + DigitalOut led1(LED1); + InterruptIn ch1(PC_2); +// PulseWidthCounter ch[6] = { PA_0,PA_1,PA_4,PB_0,PC_1,PC_0 }; + PulseWidthCounter ch[6] = { A0,A1,A2,A3,A4,A5 }; + PwmOut pwm[6] = { D8,D9,D10,D11,D12,D14 }; +// PwmOut pwm[6] = { D2,D3,D4,D5,D6,D7 }; +// SoftPWM pwm[6] = { PB_3,PB_4,PB_10,PC_6,PB_6,PA_7 }; + SoftPWM buzz(PB_13); +// I2cPeripherals i2c(I2C_SDA,I2C_SCL); //sda scl + I2cPeripherals i2c(D5,D7); //sda scl + SerialLcd lcd(PA_11,PA_12); + #define MEM_SIZE 256 + #define EXTERNAL_EEPROM //24AAXX/24LCXX/24FCXX EEPROM +#elif defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501) + DigitalInOut pwmpin[] = { P0_14,P0_2,P0_23,P0_17 }; + DigitalOut led1(P0_21); +// DigitalOut led2(P0_21); + InterruptIn ch1(P0_9); + PulseWidthCounter ch[5] = { P0_8,P0_10,P0_7,P0_22,P1_15 }; +// SoftPWM pwm[4] = { P0_14,P0_2,P0_23,P0_17 }; + PulseOut pwm[6] = { P0_14,P0_2,P0_23,P0_17,p0_20,p015 }; + Ticker Tpwm; + #define SOFTPWM + SoftPWM buzz(P1_19); + I2cPeripherals i2c(P0_5,P0_4); //sda scl + SerialLcd lcd(P0_19,P0_18); + #define MEM_SIZE 256 + #define TARGET_EEPROM_ADDRESS 64 +// #define EXTERNAL_EEPROM + #define INTERNAL_EEPROM + IAP iap; +#elif defined(TARGET_LPC1114) // LPC1114 + DigitalInOut pwmpin[] = { dp1,dp2,dp18,dp24 }; + DigitalOut led1(dp28); + InterruptIn ch1(dp4); + PulseWidthCounter ch[5] = { dp9,dp10,dp11,dp13,dp26 }; +// SoftPWM pwm[4] = { dp1,dp2,dp18,dp24 }; + PulseOut pwm[6] = { dp1,dp2,dp18,dp24,dp17,dp6 }; + Ticker Tpwm; + #define SOFTPWM + SoftPWM buzz(dp25); + I2cPeripherals i2c(dp5,dp27); //sda scl + SerialLcd lcd(dp16,dp15); + #define MEM_SIZE 256 + #define EXTERNAL_EEPROM +#endif -#ifdef TARGET_LPC1114 // LPC1114 -// PulseOut pwm[4] = { dp1,dp2,dp18,dp24 ); - #define LED1 dp28 - #define p5 dp4 - #define p6 dp9 - #define p7 dp10 - #define p8 dp11 - #define p9 dp13 - #define p10 dp26 - #define p13 dp16 - #define p21 dp1 - #define p22 dp2 - #define p23 dp18 - #define p24 dp24 - #define p26 dp25 - #define p27 dp27 - #define p28 dp5 -#else //LPC1768 - #ifdef LPCXpresso - #define LED1 P0_22 - #endif -#endif -DigitalInOut pwmpin[] = { p21,p22,p23,p24 }; -DigitalOut led1(LED1); -DigitalOut led2(LED2); -InterruptIn ch1(p5); -PulseWidthCounter ch[5] = { p6,p7,p8,p9,p10 }; -#if defined(SOFT_PWM) || defined(TARGET_LPC1114) -SoftPWM pwm[4] = { p21,p22,p23,p24 }; -#else -PwmOut pwm[4] = { p21,p22,p23,p24 }; -#endif -SoftPWM buzz(p26); Timer CurTime; //Timer ElapTime; Timer CycleTime; Timer FlyghtTime; -//Ticker tick; -//Ticker tick100ms; -//Ticker mixTime; -I2cPeripherals i2c(p28,p27); //sda scl -SerialLcd lcd(p13); config conf; -PID pid[3]; -#ifdef TARGET_LPC1114 -//LPC1114 Flash Memory read/write - #define MEM_SIZE 256 - #define TARGET_SECTOR 7 // use sector 29 as target sector if it is on LPC1768 -#else -//LPC1768 Flash Memory read/write - #define MEM_SIZE 256 - #define TARGET_SECTOR 29 // use sector 29 as target sector if it is on LPC1768 -#endif -#ifndef LocalFileOut -IAP iap; -#endif +//PID pid[4]; +Limiter gyroLimit[3] = {300,300,300}; +Limiter accLimit[3] = {0.5,0.5,0.5}; +Limiter pwmLimit[4] = {50,50,50,50}; +//PID height; float TotalTime = 0;; int channel = 0; -//int intrupt_cnt = 0; -//int intrupt_cnt2 = 0; -int volatile CH[5]; +int Signal[9] = { _THR,_AIL,_ELE,_RUD,_GYRO,_AUX1,_AUX2,_AUX3,_AUX4 }; +volatile int CH[9]; volatile int M[6]; volatile float Gyro[3]; -volatile float Accel[3]; -volatile float Accel_Save[3]; +volatile float Accel[3]= {0,0,0}; +volatile float Accel_Angle[3]; +volatile float Accel_Save[3]= {0,0,0}; volatile float Angle[3]; volatile float Gyro_Ref[3]; volatile float Gyro_Save[3]; -volatile int Stick[5]; -volatile int Stick_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 + 1080,1500,1500,1500,1500,1080, //Delta + 1080,1500,1500,1500,1080,1500, //Delta_TW + 1080,1500,1500,1500,1500,1080 //AirPlane + }; //int Stick_Max[3]; float Press; +float Base_Press; char InPulseMode; //Receiver Signal Type 's':Serial, 'P':Parallel //volatile bool tick_flag; //volatile bool buzz_flag; -float interval; +volatile float interval; float pid_interval; -int pid_reg[3]; +//int pid_reg[4]; int loop_cnt; +float target_height; +float cuurent_height; +float base_throttol; +int throttol; +bool hov_control; +float Rdata; void initialize(); void FlashLED(int ); @@ -128,18 +162,19 @@ void LCD_printf(char *); void LCD_cls(); void LCD_locate(int,int); -/* -void tick_interrupt() +void Servo_Reverse(int,int); +#ifdef SOFTPWM +void Tpwm_interrupt() { - tick_flag = true; + for ( int i=0; i<4; i++ ) pwm[i].start(); } -*/ -void PulseCheck() +#endif +void PulseCheck() //cppm信号のチェック { channel++; } -void PulseAnalysis() //Interrupt Pin5 +void PulseAnalysis() //cppm信号の解析 { CurTime.stop(); int PulseWidth = CurTime.read_us(); @@ -147,25 +182,9 @@ CurTime.start(); if ( PulseWidth > 3000 ) channel = 0; //reset pulse count else { - if ( PulseWidth > Pulse_Min && PulseWidth < Pulse_Max ) { - switch( channel ) { - case IR_THR: - THR = PulseWidth; - break; - case IR_AIL: - AIL = PulseWidth; - break; - case IR_ELE: - ELE = PulseWidth; - break; - case IR_RUD: - RUD = PulseWidth; - break; - case IR_AUX: - AUX = PulseWidth; - break; - } - } + if ( PulseWidth > Pulse_Min && PulseWidth < Pulse_Max && channel < 10 ) { + CH[Signal[channel-1]] = PulseWidth; + } } channel++; } @@ -180,8 +199,8 @@ Get_Stick_Pos(); while ( Stick[COL] > Thro_Zero || conf.StartMode == 'C' ) //Shrottol Low { - if ( Stick[COL] > 890 && -Stick[YAW] < Stick_Limit ) // Shrottle High - ESC_SetUp(); +// if ( Stick[COL] > 890 && -Stick[YAW] < Stick_Limit ) // Shrottle High +// ESC_SetUp(); if ( Stick[COL] > 890 || conf.StartMode == 'C' ) // Shrottle High { loop_cnt = 0; @@ -206,7 +225,10 @@ if ( Stick[PIT] < -Stick_Limit ) { //Elevetor Down loop_cnt = 0; FlashLED(5); - for ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(Pulse_Min); + + for ( int x=0; x<6; x++ ) { + pwm[x].pulsewidth_us(PWM_Init[conf.Model_Type][x]); + } i2c.start(conf.LCD_Contrast); SetUpPrompt(conf,i2c); } @@ -227,9 +249,10 @@ void initialize() { buzz.period_us(400); + i2c.start(conf.LCD_Contrast); + for ( int i=0;i<6;i++ ) pwm[i].pulsewidth_us(0); ReadConfig(); //config.inf file read - i2c.start(conf.LCD_Contrast); channel = 0; ch1.rise(&PulseCheck); //input pulse count wait(0.2); @@ -240,7 +263,10 @@ else InPulseMode = 'P'; led1 = 0; CycleTime.start(); - for ( int i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval); +#ifdef SOFTPWM + Tpwm.attach_us(&Tpwm_interrupt,conf.PWM_Interval); +#endif + Base_Press = (float)i2c.pressure() / 4096; FlashLED(3); } @@ -282,15 +308,40 @@ #else char *send; char *recv; - int i,rc; + int i; config *conf_ptr; - if ( sizeof(config) > 255 ) { + if ( sizeof(config) > MEM_SIZE ) { LCD_printf("config size over"); wait(3); return; } - rc = iap.blank_check( TARGET_SECTOR, TARGET_SECTOR ); +//#if defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501) +#if defined(INTERNAL_EEPROM) || defined(EXTERNAL_EEPROM) + char buf[MEM_SIZE]; + #if defined(INTERNAL_EEPROM) + iap.read_eeprom( (char*)TARGET_EEPROM_ADDRESS, buf, MEM_SIZE ); + #else + //External Flash Memory Wreite + short pos = 0; + if ( i2c.read_EEPROM(pos,buf,MEM_SIZE) != 0 ) { + while(1) { + FlashLED(3); + wait(0.5); +return; + } + } + #endif + send = buf; + recv = (char*)&conf; + conf_ptr = (config*)buf; + if ( conf_ptr->Revision == conf.Revision && conf_ptr->Struct_Size == sizeof(config) ) { + for ( i=0;i<sizeof(config);i++ ) recv[i] = send[i]; + return; + } + +#else + int rc = iap.blank_check( TARGET_SECTOR, TARGET_SECTOR ); if ( rc == SECTOR_NOT_BLANK ) { send = sector_start_adress[TARGET_SECTOR]; recv = (char*)&conf; @@ -300,6 +351,7 @@ return; } } +#endif WriteConfig(); #endif } @@ -315,25 +367,27 @@ char mem[MEM_SIZE]; char *send; int i; -//pc.printf("start\n\r"); - if ( sizeof(config) > 255 ) { + if ( sizeof(config) > MEM_SIZE ) { LCD_printf("config size over"); wait(3); return; } -//pc.printf("iap start\n\r"); send = (char*)&conf; for ( i=0;i<sizeof(config);i++ ) mem[i] = send[i]; for ( i=sizeof(config);i<MEM_SIZE;i++ ) mem[i] = 0x00; -//pc.printf("prepare start\n\r"); - int rc = iap.prepare( TARGET_SECTOR, TARGET_SECTOR ); -//pc.printf("prepare1(%5d)\n\r",rc); - rc = iap.erase( TARGET_SECTOR, TARGET_SECTOR ); -//pc.printf("erase(%5d)\n\r",rc); - rc = iap.prepare( TARGET_SECTOR, TARGET_SECTOR ); -//pc.printf("prepare2(%5d)\n\r",rc); - rc = iap.write( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE ); -//pc.printf("write(%5d)\n\r",rc); +//#if defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501) +#if defined(INTERNAL_EEPROM) + iap.write_eeprom( mem, (char*)TARGET_EEPROM_ADDRESS, MEM_SIZE ); +#elif defined(EXTERNAL_EEPROM) +//External Flash Memory Wreite + short pos = 0; + i2c.write_EEPROM( pos,mem,MEM_SIZE) ; +#else + iap.prepare( TARGET_SECTOR, TARGET_SECTOR ); + iap.erase( TARGET_SECTOR, TARGET_SECTOR ); + iap.prepare( TARGET_SECTOR, TARGET_SECTOR ); + iap.write( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE ); +#endif #endif } @@ -345,12 +399,14 @@ // Stick_Save[ROL] = Stick[ROL]; // Stick_Save[PIT] = Stick[PIT]; // Stick_Save[YAW] = Stick[YAW]; +// Stick_Save[COL] = Stick[COL]; Stick[ROL] = AIL - conf.Stick_Ref[ROL]; Stick[PIT] = ELE - conf.Stick_Ref[PIT]; Stick[YAW] = RUD - conf.Stick_Ref[YAW]; Stick[COL] = THR - conf.Stick_Ref[COL]; Stick[GAIN] = ( AUX - conf.Stick_Ref[GAIN] ) / 4; + Stick[AUX2] = AX2 - conf.Stick_Ref[COL]; } void Get_Gyro(float interval) @@ -364,49 +420,37 @@ if ( conf.Gyro_Dir[3] ==1 ) err=i2c.angular(&x,&y,&z); else err=i2c.angular(&y,&x,&z); if ( err == false ) return; - Gyro[ROL] = x - Gyro_Ref[0] ; - Gyro[PIT] = y - Gyro_Ref[1] ; - Gyro[YAW] = z - Gyro_Ref[2] ; -/* - x -= Gyro_Ref[0]; - y -= Gyro_Ref[1]; - z -= Gyro_Ref[2]; - float i = 3.14 * 2 * 10 * interval; LPF 10Hz - Gyro[ROL] += -Gyro[ROL] * i + x * i; - Gyro[PIT] += -Gyro[PIT] * i + y * i; - Gyro[YAW] += -Gyro[YAW] * i + z * i; -*/ - if ( fabs(Gyro[ROL]) > 300.0 ) Gyro[ROL] = Gyro_Save[ROL]; - if ( fabs(Gyro[PIT]) > 300.0 ) Gyro[PIT] = Gyro_Save[PIT]; - if ( fabs(Gyro[YAW]) > 300.0 ) Gyro[YAW] = Gyro_Save[YAW]; + Gyro[ROL] = gyroLimit[0].calc(x) - Gyro_Ref[0] ; + Gyro[PIT] = gyroLimit[1].calc(y) - Gyro_Ref[1] ; + Gyro[YAW] = gyroLimit[2].calc(z) - Gyro_Ref[2] ; +//pc.printf("%6.1f,%6.1f\r\n",x,Gyro[ROL]); + } bool Get_Accel(float interval) { float x,y,z; bool err; - Accel_Save[ROL] = Accel[ROL]; - Accel_Save[PIT] = Accel[PIT]; - Accel_Save[YAW] = Accel[YAW]; +// Accel_Save[ROL] = Accel_Angle[ROL]; +// Accel_Save[PIT] = Accel_Angle[PIT]; +// Accel_Save[YAW] = Accel_Angle[YAW]; if ( conf.Gyro_Dir[3] ==1 ) err=i2c.Acceleration(&x,&y,&z); else err=i2c.Acceleration(&y,&x,&z); if ( err == false ) return false; -// Accel[ROL] = x - conf.Accel_Ref[0]; -// Accel[PIT] = y - conf.Accel_Ref[1]; -// Accel[YAW] = z - conf.Accel_Ref[2]; +//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]; -// float i = 3.14 * 2 * 10 * interval; //LPF 10Hz -// Accel[ROL] += -Accel[ROL] * i + x * i; -// Accel[PIT] += -Accel[PIT] * i + y * i; -// Accel[YAW] += -Accel[YAW] * i + z * i; - Accel[ROL] = atan(x/sqrt( pow(y,2)+pow(z,2)))*180/3.14; - Accel[PIT] = atan(y/sqrt( pow(x,2)+pow(z,2)))*180/3.14; -// Accel[YAW] = atan(sqrt( pow(x,2)+pow(y,2))/z)*180/3.14; - return true; + Accel[ROL] = accLimit[0].calc(x); + Accel[PIT] = accLimit[1].calc(y); + Accel[YAW] = accLimit[2].calc(z); +//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; + Accel[YAW] = atan(sqrtf( powf(x,2)+powf(y,2))/z)*180/3.14f; + return true; } void Get_Angle(float interval) @@ -415,9 +459,9 @@ Get_Accel(interval); Get_Gyro(interval); - x = ( (Gyro[ROL] + Gyro_Save[ROL]) ) * 0.5; - y = ( (Gyro[PIT] + Gyro_Save[PIT]) ) * 0.5; - z = ( (Gyro[YAW] + Gyro_Save[YAW]) ) * 0.5; + x = ( Gyro[ROL] + Gyro_Save[ROL] ) * 0.5f; + y = ( Gyro[PIT] + Gyro_Save[PIT] ) * 0.5f; + z = ( Gyro[YAW] + Gyro_Save[YAW] ) * 0.5f; if ( Get_Accel(interval) == true ) { float i = 3.14 * 2 * conf.Cutoff_Freq * interval; Angle[ROL] += -Angle[ROL] * i + Accel[ROL] * i + x * interval; @@ -428,12 +472,15 @@ Angle[PIT] += y * interval; } Angle[YAW] += z * interval; +//pc.printf("%6.1f,%6.1f,%6.3f\r\n",Angle[ROL],Gyro[ROL],Accel[ROL]); } void Get_Pressure() { - Press = i2c.pressure(); -} + float P = (float)i2c.pressure()/4096; +// Press = 153.8 * ( T + 273.2 ) * ( 1.0 - ( P / Base_Press ) ^ 0.1902f ); + Press = 8.43f * ( P - Base_Press ); +} void CalibrateGyros(void) { @@ -471,109 +518,167 @@ conf.Accel_Ref[0] = k[0]/16; conf.Accel_Ref[1] = k[1]/16; conf.Accel_Ref[2] = k[2]/16-1; +// conf.Accel_Ref[2] = k[2]/16; // FlashLED(3); } void PWM_Out(bool mode) { - int reg[3]; + int reg[3]; int i; float gain; +// float cur_height; interval = CycleTime.read(); CycleTime.reset(); - if ( interval > 0.2 ) return; + if ( interval > 0.2f ) return; TotalTime += interval; - if ( TotalTime > 0.5 ) { + if ( TotalTime > 0.5f ) { led1 = !led1; - if ( ( !buzz ) && ( (float)conf.Flight_Time < FlyghtTime.read() ) ) buzz=0.5f; + if ( ( !buzz ) && ( (float)conf.Flight_Time < FlyghtTime.read() ) ) buzz=0.5; else buzz=0.0; TotalTime = 0; } Get_Angle(interval); pid_interval += interval; - if ( pid_interval < conf.PID_Interval && Stick[GAIN] < 0 ) return; + if ( (pid_interval < (float)conf.PWM_Interval/1000000) && (Stick[GAIN] < 0) ) return; + pid_interval = 0; + Get_Stick_Pos(); - M1 = M2 = M3 = M4 = Stick[COL]; - + switch ( conf.Model_Type ) { + case Quad_X: + M1 = M2 = M3 = M4 = Stick[COL]; + break; + case Quad_H: + M1 = M2 = M3 = M4 = Stick[AUX2]; + M5 = Stick[COL]; + 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]; + if ( conf.Model_Type == AirPlane ) + M5 = conf.Stick_Ref[ROL] - conf.Stick_Ref[COL]; + else M5 = Stick[COL]; + break; + } + for ( i=0;i<3;i++ ) { // 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; if ( Stick[GAIN] > 0 - || i == YAW + || i == YAW + || conf.Model_Type > 0 ) { reg[i] = Stick[i] * conf.Stick_Mix[i]; reg[i] += Gyro[i] * gain * GYRO_ADJUST; } else { -// reg[i] = Stick[i] * conf.Stick_Mix[i]; -// reg[i] += pid[i].calc(Angle[i],0,pid_interval) * conf.Gyro_Dir[i]; - float x = -(float)Stick[i]*45/400; - reg[i] = pid[i].calc(Angle[i],x*conf.Gyro_Dir[i],pid_interval) * conf.Gyro_Dir[i]; - if ( (i == YAW) || (i2c.GetAddr(ACCEL_ADDR)==0) ) { - if ( ( fabsf(Angle[i]) < fabsf(x) ) && ( fabsf(Angle[i]) > fabsf(x*0.7) ) ) { Angle[i] = 0; pid[i].reset(); } - if ( abs(Stick_Save[i]) <= abs(Stick[i]) ) { - Stick_Save[i] = Stick[i]; - } - else { - if ( (abs(Stick_Save[i]) - abs(Stick[i])) > 10 ) { - Stick_Save[i] = 0; - pid[i].reset(); - Angle[i] = 0; - } - } - } -// else { -// if ( fabsf(Accel[i]) < 0.001 ) { -// pid[i].reset(); -// Angle[i] = 0; -// } -// } + reg[i] = ( Angle[i]*conf.Gyro_Dir[i]*400/50 + (float)Stick[i] ) * conf.Stick_Mix[i]; + reg[i] += Gyro[i] * gain * GYRO_ADJUST; } - pid_reg[i] = reg[i]; + +// pid_reg[i] = reg[i]; } pid_interval = 0; - //Calculate Roll Pulse Width - M1 += reg[ROL]; - M2 -= reg[ROL]; - M3 -= reg[ROL]; - M4 += reg[ROL]; + + switch ( conf.Model_Type ) { + case Quad_X: + case Quad_H: + //Calculate Roll Pulse Width + M1 += reg[ROL]; + M2 -= reg[ROL]; + M3 -= reg[ROL]; + M4 += reg[ROL]; - //Calculate Pitch Pulse Width - M1 += reg[PIT]; - M2 += reg[PIT]; - M3 -= reg[PIT]; - M4 -= reg[PIT]; + //Calculate Pitch Pulse Width + M1 += reg[PIT]; + M2 += reg[PIT]; + M3 -= reg[PIT]; + M4 -= reg[PIT]; - //Calculate Yaw Pulse Width - M1 -= reg[YAW]; - M2 += reg[YAW]; - M3 -= reg[YAW]; - M4 += reg[YAW]; - - for ( i=0;i<4;i++ ) - { -// if ( Stick[COL] > 150 && M[i] < 150 ) M[i] = 150; - if ( M[i] > Thro_Hi ) M[i] = Thro_Hi; - if ( M[i] < Thro_Lo ) M[i] = Thro_Lo; // motor idle level + //Calculate Yaw Pulse Width + M1 -= reg[YAW]; + M2 += reg[YAW]; + M3 -= reg[YAW]; + M4 += reg[YAW]; + break; + case Delta: + case Delta_TW: + //Calculate Roll Pulse Width + M2 += reg[ROL]; + M3 -= reg[ROL]; + //Calculate Pitch Pulse Width + M2 += reg[PIT]; + M3 += reg[PIT]; + //Calculate Yaw Pulse Width + M4 -= reg[YAW]; + //Calculate Yaw Pulse Width + if ( conf.Model_Type == Delta_TW ) { + M1 += reg[YAW]; + M5 -= reg[YAW]; + } + break; + case AirPlane: + //Calculate Roll Pulse Width + M2 -= reg[ROL]; + M5 -= reg[ROL]; + //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++ ) + { + if ( M[i] > Thro_Hi ) M[i] = Thro_Hi; + if ( M[i] < Thro_Lo ) M[i] = Thro_Lo; // motor idle level + } } - if (Stick[COL] < Thro_Zero ) M1=M2=M3=M4=0; - + 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 ) { - for ( i=0;i<4;i++ ) { - while ( !pwmpin[i] ); - if ( conf.PWM_Mode == 1 ) + 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]); + else pwm[i].pulsewidth_us(M[i]); } } } +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) { Get_Stick_Pos(); @@ -585,16 +690,23 @@ void Flight_SetUp(void) { int i; - for ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(0); - for ( i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval); - for ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(Pulse_Min); - for ( i=0;i<4;i++ ) pid[i].init(conf.kp[i],conf.ki[i],conf.kd[i] - ,conf.PID_Limit,conf.Differential_Limit); +#ifdef SOFTPWM + Tpwm.detach(); + Tpwm.attach_us(&Tpwm_interrupt,conf.PWM_Interval); +#else + for ( i=0;i<6;i++ ) pwm[i].pulsewidth_us(0); + for ( i=0;i<6;i++ ) pwm[i].period_us(conf.PWM_Interval); +#endif + for ( i=0; i<6; i++ ) { + pwm[i].pulsewidth_us(PWM_Init[conf.Model_Type][i]); + } + hov_control = false; Angle[ROL]=Angle[PIT]=Angle[YAW]=0; loop_cnt = 0; FlyghtTime.start(); CycleTime.start(); pid_interval = 0; + Stick_Save[COL] = Stick[COL]; FlashLED(5); } @@ -625,3 +737,9 @@ + + + + + +