syouichi imamori
/
MulticopterQuadX
Quad X Type Multicopter
main.cpp
- Committer:
- komaida424
- Date:
- 2015-02-24
- Revision:
- 6:a50e6d3924f1
- Parent:
- 5:7b02775787a9
- Child:
- 7:16bf0085d914
File content as of revision 6:a50e6d3924f1:
/* PWM Output type | M1 | M2 | M3 | M4 | M5 | M6 ------------+-----------+-----------+-----------+-----------+--------------------- Quad-X |FL Thr ESC |FR Thr ESC |BL Thr ESC |BR Thr ESC | - | - Quad-H |FL VP Ser |FR VP Ser |BL VP Ser |BR VP Ser |Thr ESC | - Quad-3D |FL Thr ESC |FR Thr ESC |BL Thr ESC |BR Thr ESC | - | - Delta |FL Tro ESC |L Alv Ser |R Alv Ser |Rud Ser | - | - Delta-TW |FL Tro ESC |L Alv Ser |R Alv Ser |Rud Ser |FR Thr ESC | - Airplane |Thr ESC |Ail Ser |Ele Ser |Rud Ser |Ail Ser | - F:Front, B:Back, L:Left, R:Right Ele:Elevator, Rud:Rudder, Ail:Aileron, Thr:Throttle, Alv:Alevon ESC:for ESC, Ser:for Servo, VP:Variable Pitch */ #include "mbed.h" #include "math.h" #include "I2cPeripherals.h" #include "InterruptIn.h" #include "config.h" #include "PulseWidthCounter.h" #include "string" #include "SerialLcd.h" //#include "PID.h" #include "SoftPWM.h" #include "PulseOut.h" #include "Limiter.h" #ifndef TARGET_NUCLEO_F401RE #include "IAP.h" #endif //Serial pc(USBTX, USBRX); #if defined(TARGET_LPC1768) DigitalInOut pwmpin[] = { p21,p22,p23,p24 }; // #ifdef LPCXpresso #define LED1 P0_22 // #endif DigitalOut led1(LED1); // DigitalOut led2(LED2); InterruptIn ch1(p5); PulseWidthCounter ch[6] = { p6,p7,p8,p9,p10,p11 }; PwmOut pwm[6] = { p21,p22,p23,p24,p25,p26 }; // SoftPWM pwm[6] = { p21,p22,p23,p24,p25,p26 }; SoftPWM buzz(p20); I2cPeripherals i2c(p28,p27); //sda scl SerialLcd lcd(p13,p14); #define MEM_SIZE 256 #define TARGET_SECTOR 29 // use sector 29 as target sector if it is on LPC1768 IAP iap; #elif defined(TARGET_NUCLEO_F401RE) DigitalOut led1(LED1); InterruptIn ch1(PC_2); // PulseWidthCounter ch[6] = { PA_0,PA_1,PA_4,PB_0,PC_1,PC_0 }; PulseWidthCounter ch[6] = { A0,A1,A2,A3,A4,A5 }; PwmOut pwm[6] = { D8,D9,D10,D11,D12,D14 }; // PwmOut pwm[6] = { D2,D3,D4,D5,D6,D7 }; // SoftPWM pwm[6] = { PB_3,PB_4,PB_10,PC_6,PB_6,PA_7 }; SoftPWM buzz(PB_13); // I2cPeripherals i2c(I2C_SDA,I2C_SCL); //sda scl I2cPeripherals i2c(D5,D7); //sda scl SerialLcd lcd(PA_11,PA_12); #define MEM_SIZE 256 #define EXTERNAL_EEPROM //24AAXX/24LCXX/24FCXX EEPROM #elif defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501) DigitalInOut pwmpin[] = { P0_14,P0_2,P0_23,P0_17 }; DigitalOut led1(P0_21); // DigitalOut led2(P0_21); InterruptIn ch1(P0_9); PulseWidthCounter ch[5] = { P0_8,P0_10,P0_7,P0_22,P1_15 }; // SoftPWM pwm[4] = { P0_14,P0_2,P0_23,P0_17 }; PulseOut pwm[6] = { P0_14,P0_2,P0_23,P0_17,p0_20,p015 }; Ticker Tpwm; #define SOFTPWM SoftPWM buzz(P1_19); I2cPeripherals i2c(P0_5,P0_4); //sda scl SerialLcd lcd(P0_19,P0_18); #define MEM_SIZE 256 #define TARGET_EEPROM_ADDRESS 64 // #define EXTERNAL_EEPROM #define INTERNAL_EEPROM IAP iap; #elif defined(TARGET_LPC1114) // LPC1114 DigitalInOut pwmpin[] = { dp1,dp2,dp18,dp24 }; DigitalOut led1(dp28); InterruptIn ch1(dp4); PulseWidthCounter ch[5] = { dp9,dp10,dp11,dp13,dp26 }; // SoftPWM pwm[4] = { dp1,dp2,dp18,dp24 }; PulseOut pwm[6] = { dp1,dp2,dp18,dp24,dp17,dp6 }; Ticker Tpwm; #define SOFTPWM SoftPWM buzz(dp25); I2cPeripherals i2c(dp5,dp27); //sda scl SerialLcd lcd(dp16,dp15); #define MEM_SIZE 256 #define EXTERNAL_EEPROM #endif Timer CurTime; //Timer ElapTime; Timer CycleTime; Timer FlyghtTime; config conf; //PID pid[4]; Limiter throLimit = 100; Limiter gyroLimit[3] = {300,300,300}; Limiter accLimit[3] = {0.5,0.5,0.5}; Limiter pwmLimit[4] = {50,50,50,50}; //PID height; float TotalTime = 0;; int channel = 0; int Signal[9] = { _THR,_AIL,_ELE,_RUD,_GYRO,_AUX1,_AUX2,_AUX3,_AUX4 }; volatile int CH[9]; volatile int M[6]; volatile float Gyro[3]; volatile float Accel[3]= {0,0,0}; volatile float Accel_Angle[3]; volatile float Accel_Save[3]= {0,0,0}; volatile float Angle[3]; volatile float Gyro_Ref[3]; volatile float Gyro_Save[3]; volatile int Stick[6]; volatile int Stick_Save[6]; int PWM_Init[6][6] = { 1080,1080,1080,1080,1080,1080, //Quad_X 1500,1500,1500,1500,1080,1080, //Quad_VP 1500,1500,1500,1500,1080,1080, //Quad_3D 1080,1500,1500,1500,1500,1080, //Delta 1080,1500,1500,1500,1080,1500, //Delta_TW 1080,1500,1500,1500,1500,1080 //AirPlane }; char Servo_idx[6][6] = { 0,0,0,0,0,0, //Quad_X 1,1,1,1,0,0, //Quad_VP 0,0,0,0,0,0, //Quad_3D 0,1,1,1,1,1, //Delta 0,1,1,1,0,1, //Delta_TW 0,1,1,1,1,1 //AirPlane }; //int Stick_Max[3]; float Press; float Base_Press; char InPulseMode; //Receiver Signal Type 's':Serial, 'P':Parallel //volatile bool tick_flag; //volatile bool buzz_flag; volatile float interval; float pid_interval; //int pid_reg[4]; int loop_cnt; float target_height; float cuurent_height; float base_Throttl; int Throttl; bool hov_control; float Rdata; 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); #ifdef SOFTPWM void Tpwm_interrupt() { for ( int i=0; i<4; i++ ) pwm[i].start(); } #endif void PulseCheck() //cppm信号のチェック { channel++; } void PulseAnalysis() //cppm信号の解析 { 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 && channel < 10 ) { CH[Signal[channel-1]] = PulseWidth; } } channel++; } int main() { int i=0; wait(1.5); initialize(); Get_Stick_Pos(); while ( Stick[COL] > Thro_Zero || conf.StartMode == 'C' || ( conf.Model_Type == Quad_3D && Stick[GAIN] < 0 ) ) //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(2); wait(0.5); 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 ( int x=0; x<6; x++ ) { pwm[x].pulsewidth_us(PWM_Init[conf.Model_Type][x]); } 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); i2c.start(conf.LCD_Contrast); for ( int i=0;i<6;i++ ) pwm[i].pulsewidth_us(0); ReadConfig(); //config.inf file read 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(); throLimit.differential(conf.Thro_Limit_Val); throLimit.rate(conf.Thro_Limit_Rate); #ifdef SOFTPWM Tpwm.attach_us(&Tpwm_interrupt,conf.PWM_Interval); #endif Base_Press = (float)i2c.pressure() / 4096; 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; config *conf_ptr; if ( sizeof(config) > MEM_SIZE ) { LCD_printf("config size over"); wait(3); return; } //#if defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501) #if defined(INTERNAL_EEPROM) || defined(EXTERNAL_EEPROM) char buf[MEM_SIZE]; #if defined(INTERNAL_EEPROM) iap.read_eeprom( (char*)TARGET_EEPROM_ADDRESS, buf, MEM_SIZE ); #else //External Flash Memory Wreite short pos = 0; if ( i2c.read_EEPROM(pos,buf,MEM_SIZE) != 0 ) { while(1) { FlashLED(3); wait(0.5); return; } } #endif send = buf; recv = (char*)&conf; conf_ptr = (config*)buf; if ( conf_ptr->Revision == conf.Revision && conf_ptr->Struct_Size == sizeof(config) ) { for ( i=0;i<sizeof(config);i++ ) recv[i] = send[i]; return; } #else int 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; } } #endif 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; if ( sizeof(config) > MEM_SIZE ) { LCD_printf("config size over"); wait(3); 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; //#if defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501) #if defined(INTERNAL_EEPROM) iap.write_eeprom( mem, (char*)TARGET_EEPROM_ADDRESS, MEM_SIZE ); #elif defined(EXTERNAL_EEPROM) //External Flash Memory Wreite short pos = 0; i2c.write_EEPROM( pos,mem,MEM_SIZE) ; #else 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 #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_Save[COL] = Stick[COL]; 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; Stick[AUX2] = AX2 - conf.Stick_Ref[COL]; } 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] = gyroLimit[0].calc(x) - Gyro_Ref[0] ; Gyro[PIT] = gyroLimit[1].calc(y) - Gyro_Ref[1] ; Gyro[YAW] = gyroLimit[2].calc(z) - Gyro_Ref[2] ; //pc.printf("%6.1f,%6.1f\r\n",x,Gyro[ROL]); } bool Get_Accel(float interval) { float x,y,z; bool err; // Accel_Save[ROL] = Accel_Angle[ROL]; // Accel_Save[PIT] = Accel_Angle[PIT]; // Accel_Save[YAW] = Accel_Angle[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; //pc.printf("%6.4f,%6.4f,%6.4f\r\n",x,y,z); x = accLimit[0].calc(x) - conf.Accel_Ref[0]; y = accLimit[1].calc(y) - conf.Accel_Ref[1]; z = accLimit[2].calc(z) - conf.Accel_Ref[2]; //pc.printf("%6.4f,%6.4f,%6.4f\r\n",Accel[ROL],Accel[PIT],Accel[YAW]); Accel[ROL] = atan(x/sqrtf( powf(y,2)+powf(z,2)))*180/3.14f; Accel[PIT] = atan(y/sqrtf( powf(x,2)+powf(z,2)))*180/3.14f; Accel[YAW] = atan(sqrtf( powf(x,2)+powf(y,2))/z)*180/3.14f; return true; } void Get_Angle(float interval) { float x,y,z; // Get_Accel(interval); Get_Gyro(interval); x = ( Gyro[ROL] + Gyro_Save[ROL] ) * 0.5f; y = ( Gyro[PIT] + Gyro_Save[PIT] ) * 0.5f; z = ( Gyro[YAW] + Gyro_Save[YAW] ) * 0.5f; 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; //pc.printf("%6.1f,%6.1f,%6.3f\r\n",Angle[ROL],Gyro[ROL],Accel[ROL]); } void Get_Pressure() { float P = (float)i2c.pressure()/4096; // Press = 153.8 * ( T + 273.2 ) * ( 1.0 - ( P / Base_Press ) ^ 0.1902f ); Press = 8.43f * ( P - Base_Press ); } 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; // conf.Accel_Ref[2] = k[2]/16; // FlashLED(3); } void PWM_Out(bool mode) { int reg[3]; int i; float gain; // float cur_height; interval = CycleTime.read(); CycleTime.reset(); if ( interval > 0.005f && mode ) return; TotalTime += interval; if ( TotalTime > 0.5f ) { led1 = !led1; if ( ( !buzz ) && ( (float)conf.Flight_Time < FlyghtTime.read() ) ) buzz=0.5; else buzz=0.0; TotalTime = 0; } Get_Angle(interval); pid_interval += interval; if ( (pid_interval < (float)conf.PWM_Interval/1000000) && (Stick[GAIN] < 0) ) return; pid_interval = 0; Get_Stick_Pos(); switch ( conf.Model_Type ) { case Quad_X: M1 = M2 = M3 = M4 = THR + conf.Throttl_Trim; break; case Quad_VP: M1 = M2 = M3 = M4 = Stick[AUX2] + conf.Stick_Ref[COL]; M5 = THR + conf.Throttl_Trim; break; case Quad_3D: i = (int)throLimit.calc((float)THR); if ( abs(i-conf.Reverse_Point) > 100 ) i = THR; M1 = M2 = M3 = M4 = i + conf.Throttl_Trim; break; case Delta: case Delta_TW: case AirPlane: M1 = THR + conf.Throttl_Trim; M2 = conf.Stick_Ref[ROL]; M3 = conf.Stick_Ref[PIT]; M4 = conf.Stick_Ref[YAW]; if ( conf.Model_Type == AirPlane ) M5 = conf.Stick_Ref[ROL]; else M5 = M1; break; } 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)fabsf(Stick[GAIN])/100 + conf.Gyro_Gain[i+3] ) * conf.Gyro_Dir[i]; if ( Stick[i] > 0 ) gain -= gain * ( fabsf(Stick[i]) / 400 ) * conf.Active_Gyro_Gain; if ( Stick[GAIN] > 0 || i == YAW || conf.Model_Type > 0 || i2c.GetAddr(ACCEL_ADDR) == 0 ) { reg[i] = Stick[i] * conf.Stick_Mix[i]; reg[i] += Gyro[i] * gain * GYRO_ADJUST; } else { reg[i] = ( Angle[i]*conf.Gyro_Dir[i]*400/50 + (float)Stick[i] ) * conf.Stick_Mix[i]; reg[i] += Gyro[i] * gain * GYRO_ADJUST; } // pid_reg[i] = reg[i]; } pid_interval = 0; // int Reverse = 1; switch ( conf.Model_Type ) { case Quad_3D: // if ( THR < conf.Reverse_Point ) { // Reverse = -1; // } case Quad_X: case Quad_VP: //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]; break; case Delta: case Delta_TW: //Calculate Roll Pulse Width M2 += reg[ROL]; M3 -= reg[ROL]; //Calculate Pitch Pulse Width M2 += reg[PIT]; M3 += reg[PIT]; //Calculate Yaw Pulse Width M4 -= reg[YAW]; //Calculate Yaw Pulse Width if ( conf.Model_Type == Delta_TW ) { M1 += reg[YAW]; M5 -= reg[YAW]; } break; case AirPlane: //Calculate Roll Pulse Width M2 -= reg[ROL]; M5 -= reg[ROL]; //Calculate Pitch Pulse Width M3 += reg[PIT]; //Calculate Yaw Pulse Width M4 -= reg[YAW]; break; } int h = conf.Model_Type; for ( int i=0; i<6; i++ ) { if ( M[i] > Pulse_Max ) M[i] = Pulse_Max; if ( M[i] < Pulse_Min ) M[i] = Pulse_Min; if ( Servo_idx[h][i] == 1 ) { M[i] = conf.Stick_Ref[i] + ( ( M[i] - conf.Stick_Ref[i] ) * conf.Servo_Dir[i] ); } else { if ( h == Quad_3D ) { if ( Stick[GAIN] > 0 ) { if ( THR < conf.Reverse_Point ) M[i] = conf.Reverse_Point; } else { if ( abs(THR - conf.Reverse_Point ) < 10 ) M[i] = conf.Reverse_Point; } } else { // if ( Stick[COL] < Thro_Zero ) M[i] = conf.Stick_Ref[COL]; } } } if ( mode ) { h = conf.Stick_Ref[THR]; for ( i=0;i<6;i++ ) { // while ( !pwmpin[i] ); if ( conf.PWM_Mode == 1 ) pwm[i].pulsewidth_us(M[i]); else pwm[i].pulsewidth_us(M[i]-conf.Stick_Ref[COL]); } } } 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; #ifdef SOFTPWM Tpwm.detach(); Tpwm.attach_us(&Tpwm_interrupt,conf.PWM_Interval); #else for ( i=0;i<6;i++ ) pwm[i].pulsewidth_us(0); for ( i=0;i<6;i++ ) pwm[i].period_us(conf.PWM_Interval); #endif for ( i=0; i<6; i++ ) { pwm[i].pulsewidth_us(PWM_Init[conf.Model_Type][i]); } hov_control = false; throLimit.differential(conf.Thro_Limit_Val); throLimit.rate(conf.Thro_Limit_Rate); Angle[ROL]=Angle[PIT]=Angle[YAW]=0; loop_cnt = 0; FlyghtTime.start(); CycleTime.start(); pid_interval = 0; Stick_Save[COL] = Stick[COL]; 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); } ;