syouichi imamori
/
MulticopterQuadX
Quad X Type Multicopter
main.cpp
- Committer:
- komaida424
- Date:
- 2014-02-13
- Revision:
- 3:27407c4984cf
- Parent:
- 2:59ac9df97701
- Child:
- 4:4060309b9cc0
File content as of revision 3:27407c4984cf:
#include "mbed.h" #include "math.h" #include "I2cPeripherals.h" #include "InterruptIn.h" #include "config.h" #include "PulseWidthCounter.h" #include "string" #include "SerialLcd.h" #include "IAP.h" #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 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 float TotalTime = 0;; int channel = 0; //int intrupt_cnt = 0; //int intrupt_cnt2 = 0; int volatile CH[5]; volatile int M[6]; volatile float Gyro[3]; volatile float Accel[3]; volatile float Accel_Save[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; float pid_interval; int pid_reg[3]; int loop_cnt; void initialize(); void FlashLED(int ); void SetUp(); void SetUpPrompt(config&,I2cPeripherals&); void PWM_Out(bool); void Get_Stick_Pos(); void CalibrateGyros(void); void CalibrateAccel(void); void Get_Gyro(float); bool Get_Accel(float); 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++; } void PulseAnalysis() //Interrupt Pin5 { CurTime.stop(); int PulseWidth = CurTime.read_us(); CurTime.reset(); 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; } } } channel++; } int main() { int i=0; wait(1.5); initialize(); 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 || conf.StartMode == 'C' ) // Shrottle High { loop_cnt = 0; SetUpPrompt(conf,i2c); break; } FlashLED(3); wait(0.3); Get_Stick_Pos(); } Flight_SetUp(); while (1) { if ( Stick[COL] < Thro_Zero ) { i = 0; 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(); Flight_SetUp(); break; } wait(0.01); // wait 10 msec Get_Stick_Pos(); i++; } } PWM_Out(true); } } void initialize() { buzz.period_us(400); ReadConfig(); //config.inf file read i2c.start(conf.LCD_Contrast); channel = 0; ch1.rise(&PulseCheck); //input pulse count wait(0.2); if ( channel > 50 ) { ch1.rise(&PulseAnalysis); InPulseMode = 'S'; } else InPulseMode = 'P'; 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; buzz = 0.5f; wait(0.1); led1 = !led1; buzz = 0.0f; wait(0.1); } } void ReadConfig() { #ifdef LocalFileOut LocalFileSystem local("local"); // Create the local filesystem under the name "local" FILE *fp = fopen("/local/setup.inf", "rb"); // Open "out.txt" on the local file system for writing if ( fp != NULL ) { float rev = conf.Revision; int len = fread(&conf,1,sizeof(config),fp); switch ( len ) { case sizeof(config): // File size ok if ( rev == conf.Revision ) break; default: fclose(fp); config init; conf = init; fp = fopen("/local/setup.inf", "wb"); fwrite(&conf,1,sizeof(config),fp); } fclose(fp); } else { WriteConfig(); wait(2); } #else char *send; char *recv; int i,rc; config *conf_ptr; if ( sizeof(config) > 255 ) { LCD_printf("config size over"); wait(3); return; } rc = iap.blank_check( TARGET_SECTOR, TARGET_SECTOR ); if ( rc == SECTOR_NOT_BLANK ) { send = sector_start_adress[TARGET_SECTOR]; recv = (char*)&conf; 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]; return; } } WriteConfig(); #endif } void WriteConfig() { #ifdef LocalFileOut LocalFileSystem local("local"); // Create the local filesystem under the name "local" FILE *fp = fopen("/local/setup.inf", "wb"); fwrite(&conf,1,sizeof(config),fp); fclose(fp); #else char mem[MEM_SIZE]; char *send; int i; //pc.printf("start\n\r"); if ( sizeof(config) > 255 ) { 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); #endif } void Get_Stick_Pos(void) { 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]; Stick[COL] = THR - conf.Stick_Ref[COL]; Stick[GAIN] = ( AUX - conf.Stick_Ref[GAIN] ) / 4; } void Get_Gyro(float interval) { 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] ; /* 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]; } 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]; 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]; 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; } void Get_Angle(float interval) { float x,y,z; 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; if ( Get_Accel(interval) == 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(); } void CalibrateGyros(void) { 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); k[0] += x; k[1] += y; k[2] += z; wait(0.01); } for( i=0; i<3; i++ ) Gyro_Ref[i] = k[i]/16; // FlashLED(3); } void CalibrateAccel(void) { 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); k[0] += x; k[1] += y; k[2] += z; wait(0.01); } conf.Accel_Ref[0] = k[0]/16; conf.Accel_Ref[1] = k[1]/16; conf.Accel_Ref[2] = k[2]/16-1; // FlashLED(3); } void PWM_Out(bool mode) { int reg[3]; int i; float gain; interval = CycleTime.read(); CycleTime.reset(); if ( interval > 0.2 ) 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); pid_interval += interval; if ( pid_interval < conf.PID_Interval && Stick[GAIN] < 0 ) return; Get_Stick_Pos(); M1 = M2 = M3 = M4 = Stick[COL]; 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 ( Stick[GAIN] > 0 || i == YAW ) { 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; // } // } } pid_reg[i] = reg[i]; } pid_interval = 0; //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 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 } 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(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] ,conf.PID_Limit,conf.Differential_Limit); Angle[ROL]=Angle[PIT]=Angle[YAW]=0; loop_cnt = 0; FlyghtTime.start(); CycleTime.start(); pid_interval = 0; 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); } ;