【正文】 (RAW_GYRO_X GYRO_BIAS_Z)*dt。}//****************//FAST PWM MODE//****************voidpwm_init(void){//***************8bit T/C2 setting*******************/ TCCR2A = (1COM2A1) | (1COM2B1) | (1WGM21) | (1WGM20)。 //FAST PWM TCCR2B = (1CS22) | (1CS21)。 //256 prescaleing//***************16bit T/C1 setting*******************/ TCCR1A = (1COM1A1) | (1COM1B1) | (1WGM10) 。 //Fast PWM TOP:256Updata:BOTTOM TCCR1B = (1WGM12) | (1CS11) 。 //256 prescaleing}voidpwm_out(uint16_t a){static uint16_t chan3。if(a!=chan3){OCR2A = (a 1024)*+63。} chan3 = a。}ISR(PCINT2_vect){static uint8_t pindlast。uint8_tchan,pindnow。uint16_t time。static uint16_t last[4],now[4]。time = micros()。pindnow = PIND。chan = pindlast^pindnow。 //signal changed at which channel //if(chanamp。(12)){ now[0] = time。 if(pindnowamp。(12))last[0] = now[0]。else{ppmInValue[0] = now[0] last[0]。} }if(chanamp。(15)){now[1] = time。 if(pindnowamp。(15))last[1] = now[1]。else{ppmInValue[1] = now[1] last[1]。} }if(chanamp。(16)){now[2] = time。if(pindnowamp。(16))last[2] = now[2]。else{ppmInValue[2] = now[2] last[2]。} }if(chanamp。(17)){now[3] = time。if(pindnowamp。(17))last[3] = now[3]。else{ppmInValue[3] = now[3] last[3]。} }pindlast=pindnow。 test1=10。}//****************//IIC mon//****************void i2c_init(void) { TWSR = 0。 // TWPS0 TWPS1 is R/ prescaler =prescaler = 1 TWBR = 0b1100。 // set the I2C clock rate to 400kHz}void i2c_start(void){ TWCR = (1TWINT)|(1TWSTA)|(1TWEN)。 // enable twi module, no interruptwhile (!(TWCR amp。 (1TWINT)))。 //wait for TWINT Flag }void i2c_SLA_W(void){ TWDR = SLA_ADDR。 //SLA+W TWCR = (1TWINT) | (1TWEN)。while (!(TWCR amp。 (1TWINT)))。}void i2c_SLA_R(void){ TWDR = SLA_ADDR+1。 TWCR = (1TWINT) | (1TWEN)。while (!(TWCR amp。 (1TWINT)))。 }void i2c_data_W(uint8_t data){ TWDR = data。 TWCR = (1TWINT) | (1TWEN)。while (!(TWCR amp。 (1TWINT)))。 }void i2c_data_R(uint8_t data){data = TWDR。 TWCR = (1TWINT) | (1TWEN)。while (!(TWCR amp。 (1TWINT)))。 }void i2c_rep_start(uint8_t address) { TWCR = (1TWINT) | (1TWSTA) | (1TWEN) 。 // send REPEAT START conditionwhile (!(TWCR amp。 (1TWINT)))。 // Wait for TWINT Flag set. This indicates that the SLA+W has been transmitted, and ACK/NACK has been received. TWDR = address。 // send device address TWCR = (1TWINT) | (1TWEN)。while (!(TWCR amp。 (1TWINT)))。 // Wait for TWINT Flag set. This indicates that the SLA+W has been transmitted, and ACK/NACK has been received.}void i2c_stop(void) { TWCR = (1 TWINT) | (1 TWEN) | (1 TWSTO)。 // while(TWCR amp。 (1TWSTO))。 // can produce a blocking state with some WMP clones}//****************//MPU6050 initiate//****************void mpu6050_init(void){ i2c_rep_start(SLA_ADDR)。 //start i2c_data_W(PWR_MGMT_1)。 //send RA i2c_data_W(0x03)。 //PWR_MGMT_1 SLEEP 0。 CYCLE 0。 TEMP_DIS 0。 CLKSEL 3 (PLL with Z Gyro reference) i2c_rep_start(SLA_ADDR)。 //restart i2c_data_W(SMPLRT_DIV)。 //send RA i2c_data_W(0x00)。 //SAMPLE RATE DIVIDER 8K i2c_rep_start(SLA_ADDR)。 i2c_data_W(CONFIG)。 //send RA i2c_data_W(0x00)。 //CONFIG EXT_SYNC_SET 0 (disable input pin for data sync) 。 default DLPF_CFG = 0 = ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) i2c_rep_start(SLA_ADDR)。 i2c_data_W(GYRO_CONFIG)。 //send RA i2c_data_W(0x10)。 //GYRO CONFIGno selftest,full scale range:+/1000deg/s i2c_rep_start(SLA_ADDR)。 i2c_data_W(ACCEL_CONFIG)。 //send RA i2c_data_W(0x10)。 //ACCELE CONFIG+/8g,no HPF TWCR = (1TWINT)|(1TWSTO)|(1TWEN)。 //Transmit STOP conditionwhile ((TWCR amp。 (1TWINT)))。 // TWINT GO TO INITIAL VALUE 0}//****************//MPU6050 get6rawADC//****************voidget_rawADC(uint8_t reg){intrawADC[6]。 //intrawADC cause that we get a in a float RAW_ACC_X when 1G TWCR = (1TWINT)|(1TWSTA)|(1TWEN)。 //Send START conditionwhile (!(TWCR amp。 (1TWINT)))。 i2c_SLA_W()。 //send SLA+W i2c_data_W(reg)。 // TWCR = (1TWINT)|(1TWSTA)|(1TWEN)。 //Send START conditionwhile (!(TWCR amp。 (1TWINT)))。 i2c_SLA_R()。for(uint8_t i = 0。i5。i++){ TWCR = (1TWINT) | (1TWEA) | (1TWEN)。 //Data byte will be received and ACK will be returnedwhile (!(TWCR amp。 (1TWINT)))。if(TWSR!=0X50){while(1)。}rawADC[i]=TWDR。 } TWCR = (1TWINT) | (1TWEN)。 //Data byte will be received and NACK will be returnedwhile (!(TWCR amp。 (1TWINT)))。if(TWSR!=0X58){while(1)。}rawADC[5]=TWDR。 TWCR = (1TWINT) | (1TWSTO) | (1TWEN)。 //Transmit STOP conditionwhile ((TWCR amp。 (1TWINT)))。 // TWINT GO TO INITIAL VALUE 0if(reg==0x3B){ RAW_ACC_X = ((rawADC[0]8)|(rawADC[1]))。 RAW_ACC_Y = ((rawADC[2]8)|(rawADC[3]))。 RAW_ACC_Z = ((rawADC[4]8)|(rawADC[5]))。}else{ RAW_GYRO_X = ((rawADC[0]8)|(rawADC[1]))。 RAW_GYRO_Y = ((rawADC[2]8)|(rawADC[3]))。 RAW_GYRO_Z = ((rawADC[4]8)|(rawADC[5]))。 }}//**********************//MPU6050 internal address//**********************define SMPLRT_DIV 0x19define CONFIG 0x1A define GYRO_CONFIG 0x1