syouichi imamori
/
MulticopterQuadX
Quad X Type Multicopter
Diff: main.cpp
- Revision:
- 2:59ac9df97701
- Parent:
- 0:cca1c4e84da4
- Child:
- 3:27407c4984cf
diff -r caafe8935da6 -r 59ac9df97701 main.cpp --- a/main.cpp Tue Jul 16 06:37:28 2013 +0000 +++ b/main.cpp Fri Nov 15 20:53:36 2013 +0000 @@ -1,74 +1,139 @@ #include "mbed.h" +#include "math.h" #include "I2cPeripherals.h" -//#include "I2cLCD.h" #include "InterruptIn.h" -//#include "ITG3200.h" -//#include "L3GD20.h" #include "config.h" #include "PulseWidthCounter.h" #include "string" #include "SerialLcd.h" #include "IAP.h" -//LPC1768 Flash Memory read/write -#define MEM_SIZE 256 -#define TARGET_SECTOR 29 // use sector 29 as target sector if it is on LPC1768 -IAP iap; +#include "PID.h" +#include "SoftPWM.h" + +//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. -#ifdef LPCXpresso -DigitalOut led1(P0_22); -#define led2 led1 -#else +#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 dp25 + #define p10 dp26 + #define p13 dp16 + #define p21 dp1 + #define p22 dp2 + #define p23 dp18 + #define p24 dp24 + #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 -InterruptIn ch1(p5); -PulseWidthCounter ch[5] = { PulseWidthCounter(p6,POSITIVE), - PulseWidthCounter(p7,POSITIVE), - PulseWidthCounter(p8,POSITIVE), - PulseWidthCounter(p9,POSITIVE), - PulseWidthCounter(p10,POSITIVE) - }; -PwmOut pwm[4] = { p21,p22,p23,p24 }; +SoftPWM buzz(p26); Timer CurTime; -Timer ElapTime; +//Timer ElapTime; +Timer CycleTime; +Timer FlyghtTime; +//Ticker tick; +//Ticker tick100ms; +//Ticker mixTime; I2cPeripherals i2c(p28,p27); //sda scl -#ifdef SERIAL_LCD SerialLcd lcd(p13); -#endif config conf; -int StartTime; -int Channel = 0; -int CH[5]; -int M[6]; -int Gyro[3]; -int Accel[3]; -int Gyro_Ref[3]; -int Accel_Ref[3]; -int Stick[5]; +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 +float TotalTime = 0;; +int channel = 0; +//int intrupt_cnt = 0; +//int intrupt_cnt2 = 0; +volatile int CH[5]; +volatile int M[6]; +volatile float Gyro[3]; +volatile float Accel[3]; +volatile float Angle[3]; +volatile float Gyro_Ref[3]; +volatile float Gyro_Save[3]; +volatile int Stick[5]; +volatile int Stick_Save[3]; +//int Stick_Max[3]; float Press; char InPulseMode; //Receiver Signal Type 's':Serial, 'P':Parallel +//volatile bool tick_flag; +//volatile bool buzz_flag; +float interval; +int pid_reg[3]; +int loop_cnt; void initialize(); void FlashLED(int ); void SetUp(); -#ifdef SERIAL_LCD -void SetUpPrompt(config&,SerialLcd&); -#else void SetUpPrompt(config&,I2cPeripherals&); -#endif void PWM_Out(bool); void Get_Stick_Pos(); void CalibrateGyros(void); void CalibrateAccel(void); void Get_Gyro(); -void Get_Accel(); +bool Get_Accel(); +void Get_Angle(float); void ReadConfig(); void WriteConfig(); +void Interrupt_Check(bool&,int &,DigitalOut &); void ESC_SetUp(void); - +void Flight_SetUp(); +//void IMUfilter_Reset(void); +void LCD_printf(char *); +void LCD_cls(); +void LCD_locate(int,int); +/* +void tick_interrupt() +{ + tick_flag = true; +} +*/ void PulseCheck() { - Channel++; + channel++; } void PulseAnalysis() //Interrupt Pin5 @@ -77,10 +142,10 @@ int PulseWidth = CurTime.read_us(); CurTime.reset(); CurTime.start(); - if ( PulseWidth > 3000 ) Channel = 0; //reset pulse count + if ( PulseWidth > 3000 ) channel = 0; //reset pulse count else { if ( PulseWidth > Pulse_Min && PulseWidth < Pulse_Max ) { - switch( Channel ) { + switch( channel ) { case IR_THR: THR = PulseWidth; break; @@ -99,49 +164,51 @@ } } } - Channel++; + channel++; } int main() { - int i,j=0; + int i=0; + wait(0.5); initialize(); - wait(0.5); + Get_Stick_Pos(); - while ( Stick[COL] > 30 || conf.StartMode == 'C' ) //Shrottol Low + while ( Stick[COL] > Thro_Zero || conf.StartMode == 'C' ) //Shrottol Low { - if ( Stick[COL] > 350 || conf.StartMode == 'C' ) // Shrottle High + if ( Stick[COL] > 890 && -Stick[YAW] < Stick_Limit ) // Shrottle High + ESC_SetUp(); + if ( Stick[COL] > 890 || conf.StartMode == 'C' ) // Shrottle High { -#ifdef SERIAL_LCD - SetUpPrompt(conf,lcd); -#else + loop_cnt = 0; SetUpPrompt(conf,i2c); -#endif - for ( i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval); break; } FlashLED(3); - wait(1); + wait(0.3); Get_Stick_Pos(); } - led2 = 1; - ElapTime.start(); - + Flight_SetUp(); while (1) { - if ( Stick[COL] < 30 ) + if ( Stick[COL] < Thro_Zero ) { i = 0; - ElapTime.stop(); - while ( Stick[YAW] < -Stick_Limit && Stick[COL] < 30 ) + while ( Stick[YAW] < -Stick_Limit && Stick[COL] < Thro_Zero ) //Rudder Left { if ( i > 100 ) //wait 2 sec { + FlyghtTime.stop(); + if ( Stick[PIT] < -Stick_Limit ) { //Elevetor Down + loop_cnt = 0; + FlashLED(5); + for ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(Pulse_Min); + i2c.start(conf.LCD_Contrast); + SetUpPrompt(conf,i2c); + } CalibrateGyros(); - CalibrateAccel(); - FlashLED(6); - ElapTime.start(); + Flight_SetUp(); break; } wait(0.01); // wait 10 msec @@ -149,12 +216,6 @@ i++; } } - j++; - if (j>100) { j=0; led2 = !led2;} - ElapTime.stop(); - wait(float(conf.PWM_Interval-ElapTime.read_us()-2)/1000000); - ElapTime.reset(); - ElapTime.start(); PWM_Out(true); } @@ -162,38 +223,33 @@ void initialize() { -#ifndef SERIAL_LCD - i2c.start(ST7032_ADDR,conf.LCD_Contrast); -#endif + buzz.period_us(500); ReadConfig(); //config.inf file read -// CurTime.start(); - Channel = 0; + + i2c.start(conf.LCD_Contrast); + channel = 0; ch1.rise(&PulseCheck); //input pulse count - wait(0.1); - if ( Channel > 30 ) { + wait(0.2); + if ( channel > 50 ) { ch1.rise(&PulseAnalysis); InPulseMode = 'S'; } else InPulseMode = 'P'; - if ( conf.Gyro_Type == _ITG3200 ) - i2c.start(ITG3200_ADDR0); - else - i2c.start(L3GD20_ADDR1); - CalibrateGyros(); - i2c.start(LDXL345_ADDR0); - CalibrateGyros(); - CalibrateAccel(); - i2c.start(LPS331AP_ADDR1); + led1 = 0; + CycleTime.start(); for ( int i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval); + FlashLED(3); } void FlashLED(int cnt) { for ( int i = 0 ; i < cnt ; i++ ) { led1 = !led1; - wait(0.05); + buzz = 0.5f; + wait(0.1); led1 = !led1; - wait(0.05); + buzz = 0.0f; + wait(0.1); } } @@ -227,7 +283,8 @@ config *conf_ptr; if ( sizeof(config) > 255 ) { - i2c.printf("config size over"); + LCD_printf("config size over"); + wait(3); return; } rc = iap.blank_check( TARGET_SECTOR, TARGET_SECTOR ); @@ -237,14 +294,10 @@ conf_ptr = (config*)sector_start_adress[TARGET_SECTOR]; if ( conf_ptr->Revision == conf.Revision && conf_ptr->Struct_Size == sizeof(config) ) { for ( i=0;i<sizeof(config);i++ ) recv[i] = send[i]; -// i2c.printf("config read OK"); -// wait(1); return; } } WriteConfig(); -// i2c.printf("config write OK"); -// wait(1); #endif } @@ -260,7 +313,8 @@ char *send; int i; if ( sizeof(config) > 255 ) { - printf("config size over"); + LCD_printf("config size over"); + wait(3); return; } send = (char*)&conf; @@ -268,8 +322,10 @@ for ( i=sizeof(config);i<MEM_SIZE;i++ ) mem[i] = 0x00; iap.prepare( TARGET_SECTOR, TARGET_SECTOR ); iap.erase( TARGET_SECTOR, TARGET_SECTOR ); +//pc.printf("erase\n"); iap.prepare( TARGET_SECTOR, TARGET_SECTOR ); iap.write( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE ); +//pc.printf("write\n"); #endif } @@ -278,6 +334,10 @@ if ( InPulseMode == 'P' ) { for (int i=0;i<5;i++) CH[i] = ch[i].count; } +// Stick_Save[ROL] = Stick[ROL]; +// Stick_Save[PIT] = Stick[PIT]; +// Stick_Save[YAW] = Stick[YAW]; + Stick[ROL] = AIL - conf.Stick_Ref[ROL]; Stick[PIT] = ELE - conf.Stick_Ref[PIT]; Stick[YAW] = RUD - conf.Stick_Ref[YAW]; @@ -287,23 +347,61 @@ void Get_Gyro() { - int x,y,z; - if ( conf.Gyro_Dir[3] ==1 ) i2c.angular(&x,&y,&z); - else i2c.angular(&y,&x,&z); - Gyro[ROL] = ( x - Gyro_Ref[0] ) / 5; - Gyro[PIT] = ( y - Gyro_Ref[1] ) / 5; - Gyro[YAW] = ( z - Gyro_Ref[2] ) / 5; + float x,y,z; + bool err; + Gyro_Save[ROL] = Gyro[ROL]; + Gyro_Save[PIT] = Gyro[PIT]; + Gyro_Save[YAW] = Gyro[YAW]; + + 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] ; + + 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]; } -void Get_Accel() +bool Get_Accel() +{ + float x,y,z; + bool err; + if ( conf.Gyro_Dir[3] ==1 ) err=i2c.Acceleration(&x,&y,&z); + else err=i2c.Acceleration(&y,&x,&z); + if ( err == false ) return false; + float wx = x - conf.Accel_Ref[0]; + float wy = y - conf.Accel_Ref[1]; + float wz = z - conf.Accel_Ref[2]; + Accel[ROL] = atan(wx/sqrt( pow(wy,2)+pow(wz,2)))*180/3.14; + Accel[PIT] = atan(wy/sqrt( pow(wx,2)+pow(wz,2)))*180/3.14; + Accel[YAW] = atan(sqrt( pow(wx,2)+pow(wy,2))/wz)*180/3.14; + return true; +} + +void Get_Angle(float interval) { - int x,y,z; - if ( conf.Gyro_Dir[3] ==1 ) i2c.Acceleration(&x,&y,&z); - else i2c.Acceleration(&y,&x,&z); - Accel[ROL] = ( x - Accel_Ref[0] ); - Accel[PIT] = ( y - Accel_Ref[1] ); - Accel[YAW] = ( z - Accel_Ref[2] ); + Get_Gyro(); + Get_Accel(); + float x,y,z; + + x = ( (Gyro[ROL] + Gyro_Save[ROL]) ) * 0.5; + y = ( (Gyro[PIT] + Gyro_Save[PIT]) ) * 0.5; + z = ( (Gyro[YAW] + Gyro_Save[YAW]) ) * 0.5; + if ( Get_Accel() == true ) { + float i = 3.14 * 2 * conf.Cutoff_Freq * interval; + Angle[ROL] += -Angle[ROL] * i + Accel[ROL] * i + x * interval; + Angle[PIT] += -Angle[PIT] * i + Accel[PIT] * i + y * interval; + } + else { + Angle[ROL] += x * interval; + Angle[PIT] += y * interval; + } + Angle[YAW] += z * interval; } + void Get_Pressure() { Press = i2c.pressure(); @@ -311,36 +409,41 @@ void CalibrateGyros(void) { - int i,j,x,y,z; - int k[3]={0,0,0}; + int i; + float x,y,z; + float k[3]={0,0,0}; wait(1); + Angle[0]=Angle[1]=Angle[2]=0; for(i=0; i<16; i++) { - if ( conf.Gyro_Dir[3] ==1 ) i2c.angular(&x,&y,&z); - else i2c.angular(&y,&x,&z); + if ( conf.Gyro_Dir[3] ==1 ) i2c.angular(&x,&y,&z); + else i2c.angular(&y,&x,&z); k[0] += x; k[1] += y; k[2] += z; - wait(0.005); + wait(0.01); } - for( j=0; j<3; j++ ) Gyro_Ref[j] = k[j]/16; - FlashLED(3); + for( i=0; i<3; i++ ) Gyro_Ref[i] = k[i]/16; +// FlashLED(3); } void CalibrateAccel(void) { - int i,j,x,y,z; - int k[3]={0,0,0}; - wait(1); + int i; + float x,y,z; + float k[3]={0,0,0}; + conf.Accel_Ref[0]=conf.Accel_Ref[1]=conf.Accel_Ref[2]=0; for(i=0; i<16; i++) { - if ( conf.Gyro_Dir[3] ==1 ) i2c.Acceleration(&x,&y,&z); - else i2c.Acceleration(&y,&x,&z); + if ( conf.Gyro_Dir[3] ==1 ) i2c.Acceleration(&x,&y,&z); + else i2c.Acceleration(&y,&x,&z); k[0] += x; k[1] += y; k[2] += z; - wait(0.005); + wait(0.01); } - for( j=0; j<3; j++ ) Accel_Ref[j] = k[j]/16; - FlashLED(3); + conf.Accel_Ref[0] = k[0]/16; + conf.Accel_Ref[1] = k[1]/16; + conf.Accel_Ref[2] = k[2]/16-9.8; +// FlashLED(3); } void PWM_Out(bool mode) @@ -349,20 +452,50 @@ int i; float gain; -// wait(0.002); Get_Stick_Pos(); - Get_Gyro(); -// Get_Accel(); - M1 = M2 = M3 = M4 = Stick[COL]; - for ( i=0;i<3;i++ ) - { + interval = CycleTime.read(); + CycleTime.reset(); + if ( interval > 0.1 ) return; + TotalTime += interval; + if ( TotalTime > 0.5 ) { + led1 = !led1; + if ( ( !buzz ) && ( (float)conf.Flight_Time < FlyghtTime.read() ) ) buzz=0.5f; + else buzz=0.0; + TotalTime = 0; + } + + Get_Angle(interval); + + 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]; - reg[i] = ( Stick[i] * conf.Stick_Mix[i] ) + ( Gyro[i] * gain ); -// if ( Stick[GAIN] < 0 ) -// reg[i] += Accel[i] * conf.Accel_Gain[i]; + if ( Stick[GAIN] > 0 +// || i == YAW +// || (fabsf)(Angle[i]) > conf.Control_Exchange_Angle + ) { + reg[i] = Stick[i] * conf.Stick_Mix[i]; + reg[i] += Gyro[i] * gain * GYRO_ADJUST; + } + else { + if ( (i == YAW) || (i2c.GetAddr(ACCEL_ADDR)==0) ) { + if ( abs(Stick_Save[i]) <= abs(Stick[i]>>2) ) { + Stick_Save[i] = Stick[i]>>2; + } + else { +// pc.printf("PID RESET.%3d",i);wait(1); + Stick_Save[i] = 0; + pid[i].reset(); + Angle[i] = 0; + } + } + reg[i] = Stick[i] * conf.Stick_Mix[i]; + reg[i] += pid[i].calc(Angle[i],0,interval); +// reg[i] = pid[i].calc(Angle[i],-(float)Stick[i]*60/400,interval); + } + pid_reg[i] = reg[i]; } //Calculate Roll Pulse Width M1 += reg[ROL]; @@ -384,25 +517,69 @@ 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; // this is the motor idle level } - - if (Stick[COL] < 20 ) M1=M2=M3=M4=0; - if ( mode ) - for ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(conf.Stick_Ref[COL]+M[i]); - + + if (Stick[COL] < Thro_Zero ) M1=M2=M3=M4=0; + + if ( mode ) { + for ( i=0;i<4;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]); + } + } + } void ESC_SetUp(void) { while(1) { Get_Stick_Pos(); - for ( int i=0;i<4;i++ ) pwm[i].pulsewidth_us(Stick[COL]); - wait(0.01); + for ( int i=0;i<4;i++ ) pwm[i].pulsewidth_us(conf.Stick_Ref[COL]+Stick[COL]); + wait(0.015); } -}; +} + +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]*(float)abs(Stick[GAIN])*0.02 + ,conf.PID_Limit,conf.Integral_Limit); + Angle[ROL]=Angle[PIT]=Angle[YAW]=0; + loop_cnt = 0; + FlyghtTime.start(); + CycleTime.start(); + FlashLED(5); +} + +void LCD_locate(int clm,int row) +{ + lcd.locate(clm,row); + i2c.locate(clm,row); +} + +void LCD_cls() +{ + lcd.cls(); + i2c.cls(); +} + +void LCD_printf(char* str) +{ + lcd.printf(str); + i2c.printf(str); +} +; + +