main.cpp

00001 /*  PWM Output
00002         type    |   M1      |   M2      |   M3      |   M4      |   M5      |   M6
00003     ------------+-----------+-----------+-----------+-----------+---------------------
00004     Quad-X      |FL Thr ESC |FR Thr ESC |BL Thr ESC |BR Thr ESC |   -       |   -  
00005     Quad-H      |FL VP Ser  |FR VP Ser  |BL VP Ser  |BR VP Ser  |Thr ESC    |   -
00006     Quad-3D     |FL Thr ESC |FR Thr ESC |BL Thr ESC |BR Thr ESC |   -       |   -  
00007     Delta       |FL Tro ESC |L Alv Ser  |R Alv Ser  |Rud Ser    |   -       |   -     
00008     Delta-TW    |FL Tro ESC |L Alv Ser  |R Alv Ser  |Rud Ser    |FR Thr ESC |   - 
00009     Airplane    |Thr ESC    |Ail Ser    |Ele Ser    |Rud Ser    |Ail Ser    |   -
00010     
00011     F:Front,  B:Back,  L:Left,  R:Right
00012     Ele:Elevator, Rud:Rudder, Ail:Aileron, Thr:Throttle, Alv:Alevon
00013     ESC:for ESC, Ser:for Servo, VP:Variable Pitch
00014 */
00015 
00016 #include "mbed.h"
00017 #include "math.h"
00018 #include "I2cPeripherals.h"
00019 #include "InterruptIn.h"
00020 #include "config.h"
00021 #include "PulseWidthCounter.h"
00022 #include "string"
00023 #include "SerialLcd.h"
00024 
00025 //#if defined(TARGET_NUCLEO_FXXXXX)
00026 //    #include "eeprom_flash.h"
00027 //#endif
00028 //#include "PID.h"
00029 //#include "SoftPWM.h"
00030 #include "Limiter.h"
00031 #include  "IAP.h"
00032 
00033 #define DEBUG
00034 //Serial pc(USBTX, USBRX); 
00035 //Serial pc(PA_9,PA_10); 
00036 
00037 #if defined(TARGET_LPC1768)
00038     DigitalInOut pwmpin[] = { p21,p22,p23,p24 };
00039 //    #ifdef LPCXpresso
00040         #define LED1 P0_22
00041 //    #endif
00042     DigitalOut led1(LED1);
00043 //    DigitalOut led2(LED2);
00044     InterruptIn ch1(p5);
00045     PulseWidthCounter ch[6] = { PulseWidthCounter(p6),
00046                                 PulseWidthCounter(p7),
00047                                 PulseWidthCounter(p8),
00048                                 PulseWidthCounter(p9),
00049                                 PulseWidthCounter(p10),
00050                                 PulseWidthCounter(p11) };
00051     PwmOut pwm[6] = { p21,p22,p23,p24,p25,p26 };
00052 //    SoftPWM buzz(p20);
00053     I2cPeripherals i2c(p28,p27); //sda scl
00054     SerialLcd lcd(p13,p14);
00055     #define PwmNum          6
00056     #define MEM_SIZE        256
00057     #define TARGET_SECTOR   29     //  use sector 29 as target sector if it is on LPC1768
00058     IAP iap;
00059 #elif defined(TARGET_STM32F1)
00060 //#define NAZE32BORD
00061   #ifdef NAZE32BORD
00062     DigitalOut led1(PC_13);
00063     InterruptIn ch1(PA_0);
00064     PulseWidthCounter ch[6] = { PA_8,PA_11,PB_6,PB_7,PB_8,PB_9};
00065     PwmOut pwm[6] = { PA_0,PA_1,PA_2,PA_3,PA_6,PA_7 };
00066 //    SoftPWM buzz(PA_2);
00067 //    I2cPeripherals i2c(I2C_SDA,I2C_SCL); //sda scl
00068     I2cPeripherals i2c(PB_9,PB_8); //sda scl
00069     SerialLcd lcd(PA_2);
00070     #define PwmNum          6
00071     #define MEM_SIZE        256
00072     #define STM32_EEPROM                 //24AAXX/24LCXX/24FCXX EEPROM
00073   #else
00074     DigitalOut led1(PC_13);
00075     InterruptIn ch1(PA_0);
00076     PulseWidthCounter ch[6] = { PA_11,PA_12,PA_15,PB_3,PB_4,PB_5};
00077     PwmOut pwm[6] = { PA_6,PA_7,PB_0,PB_1,PB_10,PB_11 };
00078 //    SoftPWM buzz(PA_2);
00079 //    I2cPeripherals i2c(I2C_SDA,I2C_SCL); //sda scl
00080     I2cPeripherals i2c(PB_9,PB_8); //sda scl
00081     SerialLcd lcd(PA_2);
00082     #define PwmNum          6
00083     #define MEM_SIZE        256
00084     #define STM32_EEPROM                 //24AAXX/24LCXX/24FCXX EEPROM
00085   #endif
00086 #elif defined(TARGET_STM32F3)
00087     DigitalOut led1(PB_3);
00088     InterruptIn ch1(PA_0);
00089     PulseWidthCounter ch[6] = { PA_0,PA_1,PB_11,PB_10,PB_4,PB_5};
00090     PwmOut pwm[6] = { PA_6,PA_7,PA_11,PA_12,PB_8,PB_9 };
00091 //    SoftPWM buzz(PA_2);
00092 //    I2cPeripherals i2c(I2C_SDA,I2C_SCL); //sda scl
00093     I2cPeripherals i2c(PB_7,PB_6); //sda scl
00094     SerialLcd lcd(PA_9);
00095     #define PwmNum          6
00096     #define MEM_SIZE        256
00097     #define STM32_EEPROM                 //24AAXX/24LCXX/24FCXX EEPROM
00098 #elif defined(TARGET_NUCLEO_F4)
00099     DigitalOut led1(PA_5);
00100     InterruptIn ch1(PC_2);
00101 //    PulseWidthCounter ch[6] = { PA_0,PA_1,PA_4,PB_0,PC_1,PC_0 };
00102     PulseWidthCounter ch[6] = { A0,A1,A2,A3,A4,A5 };
00103     PwmOut pwm[6] = { D3,D5,D6,D9,D11,D12 };
00104 //    SoftPWM buzz(PB_13);
00105 //    I2cPeripherals i2c(I2C_SDA,I2C_SCL); //sda scl
00106     I2cPeripherals i2c(PB_9,PB_8); //sda scl
00107     SerialLcd lcd(PA_11);
00108     #define PwmNum          6
00109     #define MEM_SIZE        256
00110     #define STM32_EEPROM                 //24AAXX/24LCXX/24FCXX EEPROM
00111 #elif defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501)
00112     DigitalInOut pwmpin[] = { P0_14,P0_2,P0_23,P0_17 };
00113     DigitalOut led1(P0_21);
00114 //    DigitalOut led2(P0_21);
00115     InterruptIn ch1(P0_9);
00116     PulseWidthCounter ch[5] = { P0_8,P0_10,P0_7,P0_22,P1_15 };
00117     PwmOut pwm[4] = { P0_14,P0_2,P0_23,P0_17 };
00118 //    SoftPWM buzz(P1_19);
00119     I2cPeripherals i2c(P0_5,P0_4); //sda scl
00120     SerialLcd lcd(P0_19,P0_18);
00121     #define PwmNum          4
00122     #define MEM_SIZE        256
00123     #define TARGET_EEPROM_ADDRESS   64
00124     #define INTERNAL_EEPROM
00125     IAP iap;
00126 #elif defined(TARGET_LPC1114)   // LPC1114
00127 //    DigitalInOut pwmpin[] = { dp1,dp2,dp18,dp24 };
00128     DigitalOut led1(dp28);
00129     InterruptIn ch1(dp4);
00130 //    PulseWidthCounter ch[5] = { dp9,dp10,dp11,dp13,dp26 };
00131     PwmOut pwm[4] = { dp1,dp2,dp18,dp24 };
00132 //    SoftPWM buzz(dp25);
00133     I2cPeripherals i2c(dp5,dp27); //sda scl
00134     SerialLcd lcd(dp16,dp15);
00135     #define PwmNum          4
00136     #define MEM_SIZE        256
00137     #define EXTERNAL_EEPROM
00138 #elif defined(TARGET_TEENSY3_1)   // Teensy3.1
00139     DigitalOut led1(D13);
00140     InterruptIn ch1(D2);
00141     PwmOut pwm[6] = { D3,D4,D5,D6,d20,D21 };
00142     I2cPeripherals i2c(D18,D19); //sda scl
00143     SerialLcd lcd(D3,D4);   //TX,RX
00144     #define PwmNum          6
00145     #define MEM_SIZE        256
00146     #define EXTERNAL_EEPROM
00147 #endif
00148 
00149 Timer CurTime;
00150 //Timer ElapTime;
00151 Timer CycleTime;
00152 //Timer FlyghtTime;
00153 config conf;
00154 //PID pid[4];
00155 //Limiter throLimit = 100;
00156 Limiter gyroLimit[3] = {0.9,0.9,0.9};
00157 Limiter accLimit[3] = {0.95,0.95,0.95};
00158 //Limiter pwmLimit[4] = {50,50,50,50};
00159 //PID height;
00160 float TotalTime = 0;;
00161 int channel = 0;
00162 int Signal[9] = { _THR,_AIL,_ELE,_RUD,_GYRO,_AUX1,_AUX2,_AUX3,_AUX4 };
00163 volatile int CH[9];
00164 volatile int M[6];
00165 volatile float Gyro[3];
00166 volatile float Accel[3]= {0,0,0};
00167 volatile float Accel_Angle[3];
00168 volatile float Accel_Save[3]= {0,0,0};
00169 volatile float Angle[3];
00170 volatile float Gyro_Ref[3];
00171 volatile float Gyro_Save[3];
00172 volatile int Stick[6];
00173 volatile int Stick_Save[6];
00174 int PWM_Init[6][6] = {  1080,1080,1080,1080,1080,1080,      //Quad_X
00175                         1500,1500,1500,1500,1080,1080,      //Quad_VP
00176                         1500,1500,1500,1500,1080,1080,      //Quad_3D
00177                         1080,1500,1500,1500,1500,1080,      //Delta
00178                         1080,1500,1500,1500,1080,1500,      //Delta_TW
00179                         1080,1500,1500,1500,1500,1080      //AirPlane
00180                         };
00181 char Servo_idx[6][6] = { 0,0,0,0,0,0,      //Quad_X
00182                          1,1,1,1,0,0,      //Quad_VP
00183                          0,0,0,0,0,0,      //Quad_3D
00184                          0,1,1,1,1,1,      //Delta
00185                          0,1,1,1,0,1,      //Delta_TW
00186                          0,1,1,1,1,1      //AirPlane
00187                         };
00188 //int Stick_Max[3];
00189 float Press;
00190 float Base_Press;
00191 char InPulseMode;               //Receiver Signal Type    's':Serial, 'P':Parallel
00192 //volatile bool tick_flag;
00193 //volatile bool buzz_flag;
00194 volatile float interval;
00195 float pid_interval;
00196 //int pid_reg[4];
00197 int loop_cnt;
00198 float target_height;
00199 float cuurent_height;
00200 float base_Throttl;
00201 int Throttl;
00202 bool hov_control;
00203 float Rdata;
00204 
00205 void initialize();
00206 void FlashLED(int ,float tm=0.1);
00207 void SetUp();
00208 void SetUpPrompt(config&,I2cPeripherals&);
00209 void PWM_Out(bool);
00210 void Get_Stick_Pos();
00211 void CalibrateGyros(void);
00212 void CalibrateAccel(void);
00213 void Get_Gyro(float);
00214 bool Get_Accel(float);
00215 void Get_Angle(float);
00216 void ReadConfig();
00217 void WriteConfig();
00218 void Interrupt_Check(bool&,int &,DigitalOut &);
00219 void ESC_SetUp(void);
00220 void Flight_SetUp();
00221 //void IMUfilter_Reset(void);
00222 void LCD_printf(char *);
00223 void LCD_cls();
00224 void LCD_locate(int,int);
00225 void wait(float);
00226 
00227 void PulseCheck()               //cppm信号のチェック
00228 {
00229     channel++;
00230 }
00231 
00232 void PulseAnalysis()            //cppm信号の解析
00233 {
00234     CurTime.stop();
00235     int PulseWidth =  CurTime.read_us();
00236     CurTime.reset();
00237     CurTime.start();
00238     if ( PulseWidth > 3000 ) channel = 0;      //reset pulse count
00239     else {
00240         if ( PulseWidth > Pulse_Min && PulseWidth < Pulse_Max && channel < 10 ) {
00241             CH[Signal[channel-1]] = PulseWidth;
00242       }
00243     }
00244     channel++;
00245 }
00246 
00247 int main()
00248 {
00249     int i=0;
00250     
00251     wait(1.5);
00252     initialize();
00253 
00254     Get_Stick_Pos();
00255 #ifdef DEBUG
00256             char str[18];
00257             while ( THR > 1800 ) {
00258                 LCD_locate(0,0);
00259                 sprintf(str,"TR=%4d,AL=%4d",THR,AIL);
00260                 LCD_printf(str);
00261                 LCD_locate(0,1);
00262                 sprintf(str,"EL=%4d,RD=%4d",ELE,RUD);
00263                 LCD_printf(str);
00264                 Get_Stick_Pos();
00265             }
00266 #endif
00267     while (  Stick[COL] > Thro_Zero || conf.StartMode == 'C'
00268              || ( conf.Model_Type == Quad_3D && Stick[GAIN] < 0 ) )          //Shrottol Low
00269     {
00270 //        if ( Stick[COL] > 890 && -Stick[YAW] < Stick_Limit )              // Shrottle High
00271 //            ESC_SetUp();
00272         if ( Stick[COL] > 890 || conf.StartMode == 'C' )              // Shrottle High
00273         {
00274             loop_cnt = 0;
00275             SetUpPrompt(conf,i2c);
00276             break;
00277         }
00278         FlashLED(2);
00279         wait(0.5);
00280         Get_Stick_Pos();
00281     }
00282     Flight_SetUp();
00283     while (1)
00284     {
00285         if ( Stick[COL] < Thro_Zero )
00286         {
00287             i = 0;
00288             while ( Stick[YAW] < -Stick_Limit && Stick[COL] < Thro_Zero )      //Rudder Left
00289             {
00290                 if ( i > 100 )                          //wait 2 sec
00291                 {
00292 //                    FlyghtTime.stop();
00293                     if ( Stick[PIT] < -Stick_Limit )    {       //Elevetor Down
00294                         loop_cnt = 0;
00295                         FlashLED(5);
00296 
00297                         for ( int x=0; x<PwmNum; x++ ) {
00298                             pwm[x].pulsewidth_us(PWM_Init[conf.Model_Type][x]);
00299                         }
00300                         i2c.start(conf.LCD_Contrast);
00301                         SetUpPrompt(conf,i2c);
00302                     }                   
00303                     CalibrateGyros();
00304                     Flight_SetUp();
00305                     break;
00306                 }
00307                 wait(0.01);                           // wait 10 msec
00308                 Get_Stick_Pos();
00309                 i++;
00310             }
00311         }
00312         PWM_Out(true);
00313     }
00314 
00315 }
00316 
00317 void  initialize()
00318 {
00319     i2c.start(conf.LCD_Contrast);
00320     for ( int i=0;i<PwmNum;i++ ) pwm[i].pulsewidth_us(0);
00321     ReadConfig();               //config.inf file read
00322 
00323     channel = 0;
00324 //#if defined(TARGET_LPC1114)   // LPC1114
00325 
00326     ch1.rise(&PulseCheck);      //input pulse count
00327     wait(0.2);
00328     if ( channel > 50 )    {
00329         FlashLED(50,0.02);
00330         ch1.rise(&PulseAnalysis);
00331         InPulseMode = 'S';    }
00332     else InPulseMode = 'P';
00333 //#endif
00334     led1 = 0;
00335     CycleTime.start();
00336 //    throLimit.differential(conf.Thro_Limit_Val);
00337 //    throLimit.rate(conf.Thro_Limit_Rate);
00338 //    Base_Press = (float)i2c.pressure() / 4096;
00339     FlashLED(10,0.05);
00340 }
00341 
00342 void FlashLED(int cnt,float tm)
00343 {
00344     for ( int i = 0 ; i < cnt ; i++ ) {
00345         led1 = !led1;
00346 //        buzz = 0.5f;
00347         wait(tm);
00348         led1 = !led1;
00349 //        buzz = 0.0f;
00350         wait(tm);
00351     }
00352 }
00353 
00354 void ReadConfig()
00355 {
00356 #ifdef LocalFileOut
00357     LocalFileSystem local("local");               // Create the local filesystem under the name "local"
00358     FILE *fp = fopen("/local/setup.inf", "rb");  // Open "out.txt" on the local file system for writing
00359     if ( fp != NULL )  {
00360         float rev = conf.Revision;
00361         int len = fread(&conf,1,sizeof(config),fp);
00362         switch ( len ) {
00363             case  sizeof(config):    // File size ok
00364                 if ( rev == conf.Revision ) break;
00365             default:
00366                 fclose(fp);
00367                 config init;
00368                 conf = init;
00369                 fp = fopen("/local/setup.inf", "wb");
00370                 fwrite(&conf,1,sizeof(config),fp);
00371         }
00372         fclose(fp);
00373     } else  {
00374         WriteConfig();
00375         wait(2);
00376     }
00377 #else
00378     char *send;
00379     char *recv;
00380     int i;
00381     char buf[MEM_SIZE];
00382     config *conf_ptr;
00383     
00384     if ( sizeof(config) > MEM_SIZE ) {
00385 //        pc.printf("config size over");
00386         wait(3);
00387         return;
00388     }
00389 //#if defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501)
00390 #if defined(INTERNAL_EEPROM) || defined(EXTERNAL_EEPROM) || defined(STM32_EEPROM)
00391     #if defined(INTERNAL_EEPROM)
00392     iap.read_eeprom( (char*)TARGET_EEPROM_ADDRESS, buf, MEM_SIZE );
00393     #elif defined(EXTERNAL_EEPROM) 
00394     //External Flash Memory Wreite
00395     short pos = 0;
00396     if ( i2c.read_EEPROM(pos,buf,MEM_SIZE) != 0 )   {
00397         while(1)   {
00398             FlashLED(3);
00399             wait(0.5);
00400         }
00401     }
00402     #else       //STM32 emulate eeprom
00403 //    EEPROM_Read(0,buf,MEM_SIZE)
00404     readEEPROM(0,(uint32_t*)buf,sizeof(config));
00405     #endif
00406     send = buf;
00407     conf_ptr = (config*)buf;
00408 //pc.printf("rev=%f",conf_ptr->Revision);
00409     recv = (char*)&conf;
00410 //    conf_ptr = (config*)buf;
00411     if ( conf_ptr->Revision == conf.Revision && conf_ptr->Struct_Size == sizeof(config) ) {
00412         for ( i=0;i<sizeof(config);i++ ) recv[i] = send[i];
00413         return;
00414     }
00415     
00416 #else
00417     int rc = iap.blank_check( TARGET_SECTOR, TARGET_SECTOR );
00418     if ( rc == SECTOR_NOT_BLANK ) {
00419         send = sector_start_adress[TARGET_SECTOR];
00420         recv = (char*)&conf;
00421         conf_ptr = (config*)sector_start_adress[TARGET_SECTOR];
00422         if ( conf_ptr->Revision == conf.Revision && conf_ptr->Struct_Size == sizeof(config) ) {
00423             for ( i=0;i<sizeof(config);i++ ) recv[i] = send[i];
00424             return;
00425         }
00426     }
00427 #endif
00428     WriteConfig();
00429 #endif
00430 }
00431 
00432 void WriteConfig()
00433 {
00434 #ifdef LocalFileOut
00435     LocalFileSystem local("local");               // Create the local filesystem under the name "local"
00436     FILE *fp = fopen("/local/setup.inf", "wb");
00437     fwrite(&conf,1,sizeof(config),fp);
00438     fclose(fp);
00439 #else
00440     char mem[MEM_SIZE]= " ";;
00441     char *send;
00442     int i;
00443     if ( sizeof(config) > MEM_SIZE ) {
00444 //        pc.printf("config size over");
00445         wait(3);
00446         return;
00447     }
00448     send = (char*)&conf;
00449     for ( i=0;i<sizeof(config);i++ ) mem[i] = send[i];
00450     for ( i=sizeof(config);i<MEM_SIZE;i++ ) mem[i] = 0x00;
00451 //#if defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501)
00452     #if defined(INTERNAL_EEPROM)
00453         iap.write_eeprom( mem, (char*)TARGET_EEPROM_ADDRESS, MEM_SIZE );
00454     #elif defined(EXTERNAL_EEPROM)
00455 //External Flash Memory Wreite
00456         short pos = 0;
00457         i2c.write_EEPROM( pos,mem,MEM_SIZE) ;
00458     #elif defined(STM32_EEPROM)
00459 //    EEPROM_Write(0,buf,MEM_SIZE);
00460         enableEEPROMWriting();
00461         writeEEPROM(0, (uint32_t *)mem,sizeof(config));
00462         disableEEPROMWriting();
00463 //    pc.printf("rev=%f,rev=%d",((config*)mem)->Revision,readEEPROMWord(0));
00464     #else
00465         iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
00466         iap.erase( TARGET_SECTOR, TARGET_SECTOR );
00467         iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
00468         iap.write( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE );
00469     #endif
00470         for ( i=0; i<4; i++ )   {
00471             FlashLED(10,0.03);
00472             wait(0.5);
00473         }
00474 #endif
00475 }
00476 
00477 void Get_Stick_Pos(void)
00478 {
00479 #ifndef TARGET_LPC1114   // LPC1114
00480     if ( InPulseMode == 'P' ) {
00481         for (int i=0;i<5;i++) CH[i] = ch[i].count;
00482     }
00483 #endif
00484 //    Stick_Save[ROL] = Stick[ROL];
00485 //    Stick_Save[PIT] = Stick[PIT];
00486 //    Stick_Save[YAW] = Stick[YAW];
00487 //    Stick_Save[COL] = Stick[COL];
00488     
00489     Stick[ROL] = AIL - conf.Stick_Ref[ROL];
00490     Stick[PIT] = ELE - conf.Stick_Ref[PIT];
00491     Stick[YAW] = RUD - conf.Stick_Ref[YAW];
00492     Stick[COL] = THR - conf.Stick_Ref[COL];
00493     Stick[GAIN] = ( AUX - conf.Stick_Ref[GAIN] ) / 4;
00494     Stick[AUX2] = AX2 - conf.Stick_Ref[COL];
00495 }
00496 
00497 void Get_Gyro(float interval)
00498 {
00499     float x,y,z;
00500     bool err;
00501     Gyro_Save[ROL] = Gyro[ROL];
00502     Gyro_Save[PIT] = Gyro[PIT];
00503     Gyro_Save[YAW] = Gyro[YAW];
00504     
00505     if ( conf.Gyro_Dir[3] ==1 ) err=i2c.angular(&x,&y,&z);
00506     else err=i2c.angular(&y,&x,&z);
00507     if ( err == false ) return;
00508     Gyro[ROL] = gyroLimit[0].calc(x) - Gyro_Ref[0] ;
00509     Gyro[PIT] = gyroLimit[1].calc(y) - Gyro_Ref[1] ;
00510     Gyro[YAW] = gyroLimit[2].calc(z) - Gyro_Ref[2] ;
00511 //pc.printf("%6.1f,%6.1f\r\n",x,Gyro[ROL]);
00512 
00513 }
00514 
00515 bool Get_Accel(float interval)
00516 {
00517     float x,y,z;
00518     bool err;   
00519 //    Accel_Save[ROL] = Accel_Angle[ROL];
00520 //    Accel_Save[PIT] = Accel_Angle[PIT];
00521 //    Accel_Save[YAW] = Accel_Angle[YAW];    
00522     if ( conf.Gyro_Dir[3] ==1 ) err=i2c.Acceleration(&x,&y,&z);
00523     else err=i2c.Acceleration(&y,&x,&z);
00524     if ( err == false ) return false;
00525 //pc.printf("%6.4f,%6.4f,%6.4f\r\n",x,y,z);
00526 
00527     x = accLimit[0].calc(x) - conf.Accel_Ref[0];
00528     y = accLimit[1].calc(y) - conf.Accel_Ref[1];
00529     z = accLimit[2].calc(z) - conf.Accel_Ref[2];
00530 //pc.printf("%6.4f,%6.4f,%6.4f\r\n",Accel[ROL],Accel[PIT],Accel[YAW]);
00531     Accel[ROL] = atan(x/sqrtf( powf(y,2)+powf(z,2)))*180/3.14f;
00532     Accel[PIT] = atan(y/sqrtf( powf(x,2)+powf(z,2)))*180/3.14f;
00533     Accel[YAW] = atan(sqrtf( powf(x,2)+powf(y,2))/z)*180/3.14f;      
00534    return true;  
00535 }
00536 
00537 void Get_Angle(float interval)
00538 {
00539     float x,y,z;
00540 //    Get_Accel(interval);
00541     Get_Gyro(interval);
00542 
00543     x = ( Gyro[ROL] + Gyro_Save[ROL] ) * 0.5f;
00544     y = ( Gyro[PIT] + Gyro_Save[PIT] ) * 0.5f;
00545     z = ( Gyro[YAW] + Gyro_Save[YAW] ) * 0.5f;
00546     if ( Get_Accel(interval) == true )  {
00547         float i =  3.14 * 2 * conf.Cutoff_Freq * interval;
00548         Angle[ROL] += -Angle[ROL] * i + Accel[ROL] * i + x * interval;
00549         Angle[PIT] += -Angle[PIT] * i + Accel[PIT] * i + y * interval;
00550     }
00551     else    {  
00552         Angle[ROL] += x * interval;
00553         Angle[PIT] += y * interval;
00554     }
00555     Angle[YAW] += z * interval;
00556 //pc.printf("%6.1f,%6.1f,%6.3f\r\n",Angle[ROL],Gyro[ROL],Accel[ROL]);
00557 }
00558 
00559 void Get_Pressure()
00560 {
00561     float P = (float)i2c.pressure()/4096;
00562 //    Press = 153.8 * ( T + 273.2 ) * ( 1.0 - ( P / Base_Press ) ^ 0.1902f );
00563     Press = 8.43f * ( P - Base_Press );
00564 } 
00565 
00566 void CalibrateGyros(void)
00567 {
00568     int i;
00569     float x,y,z;
00570     float k[3]={0,0,0};
00571     wait(1);
00572     Angle[0]=Angle[1]=Angle[2]=0;
00573     for(i=0; i<16; i++) {
00574         if ( conf.Gyro_Dir[3] ==1 ) i2c.angular(&x,&y,&z);
00575         else i2c.angular(&y,&x,&z);
00576         k[0] += x;
00577         k[1] += y;
00578         k[2] += z;
00579         wait(0.01);
00580     }
00581     for( i=0; i<3; i++ ) Gyro_Ref[i] = k[i]/16;
00582 //    FlashLED(3);
00583 }
00584 
00585 void CalibrateAccel(void)
00586 {
00587     int i;
00588     float x,y,z;
00589     float k[3]={0,0,0};
00590     conf.Accel_Ref[0]=conf.Accel_Ref[1]=conf.Accel_Ref[2]=0;
00591     for(i=0; i<16; i++) {
00592         if ( conf.Gyro_Dir[3] ==1 ) i2c.Acceleration(&x,&y,&z);
00593         else i2c.Acceleration(&y,&x,&z);
00594         k[0] += x;
00595         k[1] += y;
00596         k[2] += z;
00597         wait(0.01);
00598     }
00599     conf.Accel_Ref[0] = k[0]/16;       
00600     conf.Accel_Ref[1] = k[1]/16;
00601     conf.Accel_Ref[2] = k[2]/16-1;  
00602 //    conf.Accel_Ref[2] = k[2]/16;  
00603 //    FlashLED(3);
00604 }
00605 
00606 void PWM_Out(bool mode)
00607 {
00608     int reg[3];
00609     int i,j,k;
00610     float gain;
00611 //    float cur_height;
00612     
00613     interval = CycleTime.read();
00614     CycleTime.reset();
00615     if ( interval > 0.005f && mode ) return;
00616     TotalTime += interval;
00617     if ( TotalTime > 0.5f )    {
00618         led1 = !led1; 
00619  //       if ( ( !buzz ) && ( (float)conf.Flight_Time < FlyghtTime.read() ) ) buzz=0.5;
00620  //       else buzz=0.0;
00621         TotalTime = 0;
00622     }
00623     
00624     Get_Angle(interval);
00625     pid_interval += interval;
00626     if ( (pid_interval < (float)conf.PWM_Interval/1000000) && (Stick[GAIN] < 0) ) return;
00627     pid_interval = 0;
00628      
00629     Get_Stick_Pos();
00630     switch ( conf.Model_Type )   {
00631         case Quad_X:
00632             M1 = M2 = M3 = M4 = THR + conf.Throttl_Trim;
00633             break;
00634         case Quad_VP:
00635             M1 = M2 = M3 = M4 = Stick[AUX2] + conf.Stick_Ref[COL];
00636             M5 = THR + conf.Throttl_Trim;
00637             break;
00638         case Quad_3D:
00639             j = THR + conf.Throttl_Trim;
00640             if ( Stick[GAIN] < 0 )  {
00641                 k = THR - conf.Reverse_Point;
00642                 if ( abs(k) < THR_MIDRANGE )   { 
00643                     if ( k > 0 ) j = conf.Reverse_Point + THR_MIDRANGE;
00644                     else j = conf.Reverse_Point - THR_MIDRANGE;
00645                 }
00646             }
00647 //           i = (int)throLimit.calc((float)THR);
00648  //           if ( abs(i-conf.Reverse_Point) > 100 ) i = THR;
00649             M1 = M2 = M3 = M4 = j;
00650             break;
00651         case Delta:
00652         case Delta_TW:
00653         case AirPlane:
00654             M1 = THR + conf.Throttl_Trim;
00655             M2 = conf.Stick_Ref[ROL];
00656             M3 = conf.Stick_Ref[PIT];
00657             M4 = conf.Stick_Ref[YAW];
00658             if ( conf.Model_Type == AirPlane )
00659                 M5 = conf.Stick_Ref[ROL];
00660             else M5 = M1;
00661             break;
00662     }
00663           
00664     for ( i=0;i<3;i++ ) {
00665             
00666 //      Stick Angle Mixing
00667         if ( conf.Gyro_Gain_Setting == 1 ) gain = conf.Gyro_Gain[i] * conf.Gyro_Dir[i];
00668         else gain = ( (float)fabsf(Stick[GAIN])/100 + conf.Gyro_Gain[i+3] ) * conf.Gyro_Dir[i];
00669         if ( Stick[i] > 0 ) gain -= gain * ( fabsf(Stick[i]) / 400 ) * conf.Active_Gyro_Gain;
00670         if ( Stick[GAIN] > 0 
00671             || i == YAW
00672             || conf.Model_Type > 0
00673             || i2c.GetAddr(ACCEL_ADDR) == 0
00674             )  {
00675             reg[i] = Stick[i] * conf.Stick_Mix[i];
00676             reg[i] += Gyro[i] * gain * GYRO_ADJUST;
00677         }
00678         else   {
00679             reg[i] = ( Angle[i]*conf.Gyro_Dir[i]*400/50 + (float)Stick[i] ) * conf.Stick_Mix[i];
00680             reg[i] += Gyro[i] * gain * GYRO_ADJUST;
00681         }
00682         
00683 //        pid_reg[i] = reg[i];
00684     }
00685     pid_interval = 0;
00686     
00687 //    int Reverse = 1;
00688     switch ( conf.Model_Type )  {
00689       case Quad_3D:
00690 //        if ( THR < conf.Reverse_Point )  { 
00691 //            Reverse = -1;
00692 //        }
00693       case Quad_X:
00694       case Quad_VP:
00695        //Calculate Roll Pulse Width
00696         M1 += reg[ROL];
00697         M2 -= reg[ROL];
00698         M3 -= reg[ROL];
00699         M4 += reg[ROL];
00700 
00701         //Calculate Pitch Pulse Width
00702         M1 += reg[PIT];
00703         M2 += reg[PIT];
00704         M3 -= reg[PIT];
00705         M4 -= reg[PIT];
00706 
00707         //Calculate Yaw Pulse Width
00708         M1 -= reg[YAW];
00709         M2 += reg[YAW];
00710         M3 -= reg[YAW];
00711         M4 += reg[YAW];
00712         break;
00713       case Delta:
00714       case Delta_TW:    
00715         //Calculate Roll Pulse Width
00716         M2 += reg[ROL];
00717         M3 -= reg[ROL];
00718         //Calculate Pitch Pulse Width
00719         M2 += reg[PIT];
00720         M3 += reg[PIT];
00721         //Calculate Yaw Pulse Width
00722         M4 -= reg[YAW];
00723         //Calculate Yaw Pulse Width
00724         if ( conf.Model_Type == Delta_TW ) {
00725             M1 += reg[YAW];
00726             M5 -= reg[YAW];
00727         }
00728         break;
00729       case AirPlane:
00730         //Calculate Roll Pulse Width
00731         M2 -= reg[ROL];
00732         M5 -= reg[ROL];
00733         //Calculate Pitch Pulse Width
00734         M3 += reg[PIT];
00735         //Calculate Yaw Pulse Width
00736         M4 -= reg[YAW];
00737         break;
00738     }
00739     j = conf.Model_Type;
00740     for ( i=0; i<PwmNum; i++ ) {
00741         if ( M[i] > Pulse_Max )   M[i] = Pulse_Max; 
00742         if ( M[i] < Pulse_Min )   M[i] = Pulse_Min;
00743         if ( Servo_idx[j][i] == 1 )
00744         {
00745             M[i] = conf.Stick_Ref[i] + ( ( M[i] - conf.Stick_Ref[i] ) * conf.Servo_Dir[i] );
00746         }
00747         else    {
00748             if ( j == Quad_3D ) {
00749                 if ( Stick[GAIN] > 0 )  {
00750                     if ( THR < conf.Reverse_Point ) 
00751                         M[i] = conf.Reverse_Point;
00752                 }
00753             }
00754             else    {
00755                 if ( Stick[COL] < Thro_Zero ) M[i] = conf.Stick_Ref[COL];
00756             }
00757         }
00758     }
00759     if ( mode ) {
00760 //        h = conf.Stick_Ref[THR];
00761         for ( i=0;i<PwmNum;i++ ) {
00762 //           while ( !pwmpin[i] );
00763             if ( conf.PWM_Mode == 1 )
00764                 pwm[i].pulsewidth_us(M[i]);
00765             else pwm[i].pulsewidth_us(M[i]-conf.Stick_Ref[COL]);
00766         }
00767     }
00768         
00769 }
00770 
00771         
00772 void ESC_SetUp(void)    {
00773     while(1)    {
00774         Get_Stick_Pos();
00775         for ( int i=0;i<PwmNum;i++ ) pwm[i].pulsewidth_us(conf.Stick_Ref[COL]+Stick[COL]);
00776         wait(0.015);
00777     }
00778 }
00779 
00780 void Flight_SetUp(void)
00781 {
00782     int i;
00783     for ( i=0;i<PwmNum;i++ ) pwm[i].pulsewidth_us(0);
00784     for ( i=0;i<PwmNum;i++ ) pwm[i].period_us(conf.PWM_Interval);
00785     for ( i=0; i<PwmNum; i++ ) {
00786         pwm[i].pulsewidth_us(PWM_Init[conf.Model_Type][i]);
00787     }
00788     hov_control = false;
00789 //    throLimit.differential(conf.Thro_Limit_Val);
00790 //    throLimit.rate(conf.Thro_Limit_Rate);
00791     Angle[ROL]=Angle[PIT]=Angle[YAW]=0;
00792     loop_cnt = 0;
00793 //    FlyghtTime.start();
00794     CycleTime.start();
00795     pid_interval = 0;
00796     Stick_Save[COL] = Stick[COL];
00797     FlashLED(5);
00798 }
00799 
00800 void LCD_locate(int clm,int row)
00801 {
00802     lcd.locate(clm,row);
00803     i2c.locate(clm,row);
00804 }
00805 
00806 void LCD_cls()
00807 {
00808     lcd.cls();
00809     i2c.cls();
00810 }
00811 
00812 void LCD_printf(char* str)
00813 {
00814     lcd.write(str);
00815     i2c.write_lcd(str);
00816 }
00817 
00818 void wait(float tm)
00819 {
00820     wait_us(tm*1000000);
00821 }
00822 ;
00823 
00824 
00825 
00826 
00827 
00828 
00829 
00830 
00831 
00832 
00833 
00834 
00835 
00836 
00837