syouichi imamori
/
MulticopterQuadX
Quad X Type Multicopter
main.cpp
- Committer:
- komaida424
- Date:
- 2013-07-11
- Revision:
- 0:cca1c4e84da4
- Child:
- 2:59ac9df97701
File content as of revision 0:cca1c4e84da4:
#include "mbed.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; #ifdef LPCXpresso DigitalOut led1(P0_22); #define led2 led1 #else DigitalOut led1(LED1); DigitalOut led2(LED2); #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 }; Timer CurTime; Timer ElapTime; 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]; float Press; char InPulseMode; //Receiver Signal Type 's':Serial, 'P':Parallel 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(); void ReadConfig(); void WriteConfig(); void ESC_SetUp(void); 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,j=0; initialize(); wait(0.5); Get_Stick_Pos(); while ( Stick[COL] > 30 || conf.StartMode == 'C' ) //Shrottol Low { if ( Stick[COL] > 350 || conf.StartMode == 'C' ) // Shrottle High { #ifdef SERIAL_LCD SetUpPrompt(conf,lcd); #else SetUpPrompt(conf,i2c); #endif for ( i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval); break; } FlashLED(3); wait(1); Get_Stick_Pos(); } led2 = 1; ElapTime.start(); while (1) { if ( Stick[COL] < 30 ) { i = 0; ElapTime.stop(); while ( Stick[YAW] < -Stick_Limit && Stick[COL] < 30 ) { if ( i > 100 ) //wait 2 sec { CalibrateGyros(); CalibrateAccel(); FlashLED(6); ElapTime.start(); break; } wait(0.01); // wait 10 msec Get_Stick_Pos(); 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); } } void initialize() { #ifndef SERIAL_LCD i2c.start(ST7032_ADDR,conf.LCD_Contrast); #endif ReadConfig(); //config.inf file read // CurTime.start(); Channel = 0; ch1.rise(&PulseCheck); //input pulse count wait(0.1); if ( Channel > 30 ) { 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); for ( int i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval); } void FlashLED(int cnt) { for ( int i = 0 ; i < cnt ; i++ ) { led1 = !led1; wait(0.05); led1 = !led1; wait(0.05); } } 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 ) { i2c.printf("config size over"); 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]; // i2c.printf("config read OK"); // wait(1); return; } } WriteConfig(); // i2c.printf("config write OK"); // wait(1); #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; if ( sizeof(config) > 255 ) { printf("config size over"); return; } 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; 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 } void Get_Stick_Pos(void) { if ( InPulseMode == 'P' ) { for (int i=0;i<5;i++) CH[i] = ch[i].count; } 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() { 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; } void Get_Accel() { 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] ); } void Get_Pressure() { Press = i2c.pressure(); } void CalibrateGyros(void) { int i,j,x,y,z; int k[3]={0,0,0}; wait(1); 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.005); } for( j=0; j<3; j++ ) Gyro_Ref[j] = k[j]/16; FlashLED(3); } void CalibrateAccel(void) { int i,j,x,y,z; int k[3]={0,0,0}; wait(1); 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.005); } for( j=0; j<3; j++ ) Accel_Ref[j] = k[j]/16; FlashLED(3); } void PWM_Out(bool mode) { int reg[3]; 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++ ) { // 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]; } //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 ( 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]); } 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); } };