syouichi imamori
/
MulticopterQuadX
Quad X Type Multicopter
Diff: main.cpp
- Revision:
- 0:cca1c4e84da4
- Child:
- 2:59ac9df97701
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Jul 11 19:18:44 2013 +0000 @@ -0,0 +1,408 @@ +#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); + } +}; + + + + +