Login▼/회원가입BASIC4MCU | 센서 | 자이로센서 | 코드비젼 통신테스트 소스코드 (수정) - avrstudio
페이지 정보
작성자 키트 작성일2017-08-21 16:31 조회1,709회 댓글0건첨부파일
본문
먼저 소스에서 타이머 주기를 16.3ms로 고치고
TWI 주파수를 400KHz로 바꾸는등 일부 수정했습니다.
// MCU BASIC: https://www.basic4mcu.com// DateTime : 2018-03-16 오후 6:16:38// by Ok-Hyun Park//// mahony filter//#include <mega128.h>#include <delay.h>#include <math.h>#include <stdlib.h>#include <stdio.h>#include <string.h>//#define TWINT 0x80#define TWEA 0x40#define TWSTA 0x20#define TWSTO 0x10#define TWWC 0x08#define TWEN 0x04#define TWIE 0x01// TW_MT_xxx-master transmitter// TW_MR_xxx-master receiver// TW_ST_xxx-slave transmitter// TW_SR_xxx-slave receiver//#define TW_START 0x08#define TW_REP_START 0x10#define TW_MT_SLA_ACK 0x18#define TW_MT_SLA_NACK 0x20#define TW_MT_DATA_ACK 0x28#define TW_MT_DATA_NACK 0x30#define TW_MT_ARB_LOST 0x38#define TW_MR_ARB_LOST 0x38#define TW_MR_SLA_ACK 0x40#define TW_MR_SLA_NACK 0x48#define TW_MR_DATA_ACK 0x50#define TW_MR_DATA_NACK 0x58#define TW_ST_SLA_ACK 0xA8#define TW_ST_ARB_LOST_SLA_ACK 0xB0#define TW_ST_DATA_ACK 0xB8#define TW_ST_DATA_NACK 0xC0#define TW_ST_LAST_DATA 0xC8#define TW_SR_SLA_ACK 0x60#define TW_SR_ARB_LOST_SLA_ACK 0x68#define TW_SR_GCALL_ACK 0x70#define TW_SR_ARB_LOST_GCALL_ACK 0x78#define TW_SR_DATA_ACK 0x80#define TW_SR_DATA_NACK 0x88#define TW_SR_GCALL_DATA_ACK 0x90#define TW_SR_GCALL_DATA_NACK 0x98#define TW_SR_STOP 0xA0#define TW_NO_INFO 0xF8#define TW_BUS_ERROR 0x00//#define MPU6050_RA_XG_OFFS_TC 0x00 //[7]PWR_MODE,[6: 1]XG_OFFS_TC,[0]OTP_BNK_VLD#define MPU6050_RA_YG_OFFS_TC 0x01 //[7]PWR_MODE,[6: 1]YG_OFFS_TC,[0]OTP_BNK_VLD#define MPU6050_RA_ZG_OFFS_TC 0x02 //[7]PWR_MODE,[6: 1]ZG_OFFS_TC,[0]OTP_BNK_VLD#define MPU6050_RA_X_FINE_GAIN 0x03 //[7: 0]X_FINE_GAIN#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7: 0]Y_FINE_GAIN#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7: 0]Z_FINE_GAIN#define MPU6050_RA_XA_OFFS_H 0x06 //[15: 0]XA_OFFS#define MPU6050_RA_XA_OFFS_L_TC 0x07#define MPU6050_RA_YA_OFFS_H 0x08 //[15: 0]YA_OFFS#define MPU6050_RA_YA_OFFS_L_TC 0x09#define MPU6050_RA_ZA_OFFS_H 0x0A //[15: 0]ZA_OFFS#define MPU6050_RA_ZA_OFFS_L_TC 0x0B#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15: 0]XG_OFFS_USR#define MPU6050_RA_XG_OFFS_USRL 0x14#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15: 0]YG_OFFS_USR#define MPU6050_RA_YG_OFFS_USRL 0x16#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15: 0]ZG_OFFS_USR#define MPU6050_RA_ZG_OFFS_USRL 0x18#define MPU6050_RA_SMPLRT_DIV 0x19#define MPU6050_RA_CONFIG 0x1A#define MPU6050_RA_GYRO_CONFIG 0x1B#define MPU6050_RA_ACCEL_CONFIG 0x1C#define MPU6050_RA_FF_THR 0x1D#define MPU6050_RA_FF_DUR 0x1E#define MPU6050_RA_MOT_THR 0x1F#define MPU6050_RA_MOT_DUR 0x20#define MPU6050_RA_ZRMOT_THR 0x21#define MPU6050_RA_ZRMOT_DUR 0x22#define MPU6050_RA_FIFO_EN 0x23#define MPU6050_RA_I2C_MST_CTRL 0x24#define MPU6050_RA_I2C_SLV0_ADDR 0x25#define MPU6050_RA_I2C_SLV0_REG 0x26#define MPU6050_RA_I2C_SLV0_CTRL 0x27#define MPU6050_RA_I2C_SLV1_ADDR 0x28#define MPU6050_RA_I2C_SLV1_REG 0x29#define MPU6050_RA_I2C_SLV1_CTRL 0x2A#define MPU6050_RA_I2C_SLV2_ADDR 0x2B#define MPU6050_RA_I2C_SLV2_REG 0x2C#define MPU6050_RA_I2C_SLV2_CTRL 0x2D#define MPU6050_RA_I2C_SLV3_ADDR 0x2E#define MPU6050_RA_I2C_SLV3_REG 0x2F#define MPU6050_RA_I2C_SLV3_CTRL 0x30#define MPU6050_RA_I2C_SLV4_ADDR 0x31#define MPU6050_RA_I2C_SLV4_REG 0x32#define MPU6050_RA_I2C_SLV4_DO 0x33#define MPU6050_RA_I2C_SLV4_CTRL 0x34#define MPU6050_RA_I2C_SLV4_DI 0x35#define MPU6050_RA_I2C_MST_STATUS 0x36#define MPU6050_RA_INT_PIN_CFG 0x37#define MPU6050_RA_INT_ENABLE 0x38#define MPU6050_RA_DMP_INT_STATUS 0x39#define MPU6050_RA_INT_STATUS 0x3A#define MPU6050_RA_ACCEL_XOUT_H 0x3B#define MPU6050_RA_ACCEL_XOUT_L 0x3C#define MPU6050_RA_ACCEL_YOUT_H 0x3D#define MPU6050_RA_ACCEL_YOUT_L 0x3E#define MPU6050_RA_ACCEL_ZOUT_H 0x3F#define MPU6050_RA_ACCEL_ZOUT_L 0x40#define MPU6050_RA_TEMP_OUT_H 0x41#define MPU6050_RA_TEMP_OUT_L 0x42#define MPU6050_RA_GYRO_XOUT_H 0x43#define MPU6050_RA_GYRO_XOUT_L 0x44#define MPU6050_RA_GYRO_YOUT_H 0x45#define MPU6050_RA_GYRO_YOUT_L 0x46#define MPU6050_RA_GYRO_ZOUT_H 0x47#define MPU6050_RA_GYRO_ZOUT_L 0x48#define MPU6050_RA_EXT_SENS_DATA_00 0x49#define MPU6050_RA_EXT_SENS_DATA_01 0x4A#define MPU6050_RA_EXT_SENS_DATA_02 0x4B#define MPU6050_RA_EXT_SENS_DATA_03 0x4C#define MPU6050_RA_EXT_SENS_DATA_04 0x4D#define MPU6050_RA_EXT_SENS_DATA_05 0x4E#define MPU6050_RA_EXT_SENS_DATA_06 0x4F#define MPU6050_RA_EXT_SENS_DATA_07 0x50#define MPU6050_RA_EXT_SENS_DATA_08 0x51#define MPU6050_RA_EXT_SENS_DATA_09 0x52#define MPU6050_RA_EXT_SENS_DATA_10 0x53#define MPU6050_RA_EXT_SENS_DATA_11 0x54#define MPU6050_RA_EXT_SENS_DATA_12 0x55#define MPU6050_RA_EXT_SENS_DATA_13 0x56#define MPU6050_RA_EXT_SENS_DATA_14 0x57#define MPU6050_RA_EXT_SENS_DATA_15 0x58#define MPU6050_RA_EXT_SENS_DATA_16 0x59#define MPU6050_RA_EXT_SENS_DATA_17 0x5A#define MPU6050_RA_EXT_SENS_DATA_18 0x5B#define MPU6050_RA_EXT_SENS_DATA_19 0x5C#define MPU6050_RA_EXT_SENS_DATA_20 0x5D#define MPU6050_RA_EXT_SENS_DATA_21 0x5E#define MPU6050_RA_EXT_SENS_DATA_22 0x5F#define MPU6050_RA_EXT_SENS_DATA_23 0x60#define MPU6050_RA_MOT_DETECT_STATUS 0x61#define MPU6050_RA_I2C_SLV0_DO 0x63#define MPU6050_RA_I2C_SLV1_DO 0x64#define MPU6050_RA_I2C_SLV2_DO 0x65#define MPU6050_RA_I2C_SLV3_DO 0x66#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67#define MPU6050_RA_SIGNAL_PATH_RESET 0x68#define MPU6050_RA_MOT_DETECT_CTRL 0x69#define MPU6050_RA_USER_CTRL 0x6A#define MPU6050_RA_PWR_MGMT_1 0x6B#define MPU6050_RA_PWR_MGMT_2 0x6C#define MPU6050_RA_BANK_SEL 0x6D#define MPU6050_RA_MEM_START_ADDR 0x6E#define MPU6050_RA_MEM_R_W 0x6F#define MPU6050_RA_DMP_CFG_1 0x70#define MPU6050_RA_DMP_CFG_2 0x71#define MPU6050_RA_FIFO_COUNTH 0x72#define MPU6050_RA_FIFO_COUNTL 0x73#define MPU6050_RA_FIFO_R_W 0x74#define MPU6050_RA_WHO_AM_I 0x75#define MPU6050_TC_PWR_MODE_BIT 7#define MPU6050_TC_OFFSET_BIT 6#define MPU6050_TC_OFFSET_LENGTH 6#define MPU6050_TC_OTP_BNK_VLD_BIT 0#define MPU6050_VDDIO_LEVEL_VLOGIC 0#define MPU6050_VDDIO_LEVEL_VDD 1#define MPU6050_CFG_EXT_SYNC_SET_BIT 5#define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3#define MPU6050_CFG_DLPF_CFG_BIT 2#define MPU6050_CFG_DLPF_CFG_LENGTH 3#define MPU6050_EXT_SYNC_DISABLED 0x0#define MPU6050_EXT_SYNC_TEMP_OUT_L 0x1#define MPU6050_EXT_SYNC_GYRO_XOUT_L 0x2#define MPU6050_EXT_SYNC_GYRO_YOUT_L 0x3#define MPU6050_EXT_SYNC_GYRO_ZOUT_L 0x4#define MPU6050_EXT_SYNC_ACCEL_XOUT_L 0x5#define MPU6050_EXT_SYNC_ACCEL_YOUT_L 0x6#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L 0x7#define MPU6050_DLPF_BW_256 0x00#define MPU6050_DLPF_BW_188 0x01#define MPU6050_DLPF_BW_98 0x02#define MPU6050_DLPF_BW_42 0x03#define MPU6050_DLPF_BW_20 0x04#define MPU6050_DLPF_BW_10 0x05#define MPU6050_DLPF_BW_5 0x06#define MPU6050_GCONFIG_FS_SEL_BIT 4#define MPU6050_GCONFIG_FS_SEL_LENGTH 2#define MPU6050_GYRO_FS_250 0x00#define MPU6050_GYRO_FS_500 0x01#define MPU6050_GYRO_FS_1000 0x02#define MPU6050_GYRO_FS_2000 0x03#define MPU6050_ACONFIG_XA_ST_BIT 7#define MPU6050_ACONFIG_YA_ST_BIT 6#define MPU6050_ACONFIG_ZA_ST_BIT 5#define MPU6050_ACONFIG_AFS_SEL_BIT 4#define MPU6050_ACONFIG_AFS_SEL_LENGTH 2#define MPU6050_ACONFIG_ACCEL_HPF_BIT 2#define MPU6050_ACONFIG_ACCEL_HPF_LENGTH 3#define MPU6050_DHPF_RESET 0x00#define MPU6050_DHPF_5 0x01#define MPU6050_DHPF_2P5 0x02#define MPU6050_DHPF_1P25 0x03#define MPU6050_DHPF_0P63 0x04#define MPU6050_DHPF_HOLD 0x07#define MPU6050_TEMP_FIFO_EN_BIT 7#define MPU6050_XG_FIFO_EN_BIT 6#define MPU6050_YG_FIFO_EN_BIT 5#define MPU6050_ZG_FIFO_EN_BIT 4#define MPU6050_ACCEL_FIFO_EN_BIT 3#define MPU6050_SLV2_FIFO_EN_BIT 2#define MPU6050_SLV1_FIFO_EN_BIT 1#define MPU6050_SLV0_FIFO_EN_BIT 0#define MPU6050_MULT_MST_EN_BIT 7#define MPU6050_WAIT_FOR_ES_BIT 6#define MPU6050_SLV_3_FIFO_EN_BIT 5#define MPU6050_I2C_MST_P_NSR_BIT 4#define MPU6050_I2C_MST_CLK_BIT 3#define MPU6050_I2C_MST_CLK_LENGTH 4#define MPU6050_CLOCK_DIV_348 0x0#define MPU6050_CLOCK_DIV_333 0x1#define MPU6050_CLOCK_DIV_320 0x2#define MPU6050_CLOCK_DIV_308 0x3#define MPU6050_CLOCK_DIV_296 0x4#define MPU6050_CLOCK_DIV_286 0x5#define MPU6050_CLOCK_DIV_276 0x6#define MPU6050_CLOCK_DIV_267 0x7#define MPU6050_CLOCK_DIV_258 0x8#define MPU6050_CLOCK_DIV_500 0x9#define MPU6050_CLOCK_DIV_471 0xA#define MPU6050_CLOCK_DIV_444 0xB#define MPU6050_CLOCK_DIV_421 0xC#define MPU6050_CLOCK_DIV_400 0xD#define MPU6050_CLOCK_DIV_381 0xE#define MPU6050_CLOCK_DIV_364 0xF#define MPU6050_I2C_SLV_RW_BIT 7#define MPU6050_I2C_SLV_ADDR_BIT 6#define MPU6050_I2C_SLV_ADDR_LENGTH 7#define MPU6050_I2C_SLV_EN_BIT 7#define MPU6050_I2C_SLV_BYTE_SW_BIT 6#define MPU6050_I2C_SLV_REG_DIS_BIT 5#define MPU6050_I2C_SLV_GRP_BIT 4#define MPU6050_I2C_SLV_LEN_BIT 3#define MPU6050_I2C_SLV_LEN_LENGTH 4#define MPU6050_I2C_SLV4_RW_BIT 7#define MPU6050_I2C_SLV4_ADDR_BIT 6#define MPU6050_I2C_SLV4_ADDR_LENGTH 7#define MPU6050_I2C_SLV4_EN_BIT 7#define MPU6050_I2C_SLV4_INT_EN_BIT 6#define MPU6050_I2C_SLV4_REG_DIS_BIT 5#define MPU6050_I2C_SLV4_MST_DLY_BIT 4#define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5#define MPU6050_MST_PASS_THROUGH_BIT 7#define MPU6050_MST_I2C_SLV4_DONE_BIT 6#define MPU6050_MST_I2C_LOST_ARB_BIT 5#define MPU6050_MST_I2C_SLV4_NACK_BIT 4#define MPU6050_MST_I2C_SLV3_NACK_BIT 3#define MPU6050_MST_I2C_SLV2_NACK_BIT 2#define MPU6050_MST_I2C_SLV1_NACK_BIT 1#define MPU6050_MST_I2C_SLV0_NACK_BIT 0#define MPU6050_INTCFG_INT_LEVEL_BIT 7#define MPU6050_INTCFG_INT_OPEN_BIT 6#define MPU6050_INTCFG_LATCH_INT_EN_BIT 5#define MPU6050_INTCFG_INT_RD_CLEAR_BIT 4#define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT 3#define MPU6050_INTCFG_FSYNC_INT_EN_BIT 2#define MPU6050_INTCFG_I2C_BYPASS_EN_BIT 1#define MPU6050_INTCFG_CLKOUT_EN_BIT 0#define MPU6050_INTMODE_ACTIVEHIGH 0x00#define MPU6050_INTMODE_ACTIVELOW 0x01#define MPU6050_INTDRV_PUSHPULL 0x00#define MPU6050_INTDRV_OPENDRAIN 0x01#define MPU6050_INTLATCH_50USPULSE 0x00#define MPU6050_INTLATCH_WAITCLEAR 0x01#define MPU6050_INTCLEAR_STATUSREAD 0x00#define MPU6050_INTCLEAR_ANYREAD 0x01#define MPU6050_INTERRUPT_FF_BIT 7#define MPU6050_INTERRUPT_MOT_BIT 6#define MPU6050_INTERRUPT_ZMOT_BIT 5#define MPU6050_INTERRUPT_FIFO_OFLOW_BIT 4#define MPU6050_INTERRUPT_I2C_MST_INT_BIT 3#define MPU6050_INTERRUPT_PLL_RDY_INT_BIT 2#define MPU6050_INTERRUPT_DMP_INT_BIT 1#define MPU6050_INTERRUPT_DATA_RDY_BIT 0// TODO: figure out what these actually do// UMPL source code is not very obivous#define MPU6050_DMPINT_5_BIT 5#define MPU6050_DMPINT_4_BIT 4#define MPU6050_DMPINT_3_BIT 3#define MPU6050_DMPINT_2_BIT 2#define MPU6050_DMPINT_1_BIT 1#define MPU6050_DMPINT_0_BIT 0#define MPU6050_MOTION_MOT_XNEG_BIT 7#define MPU6050_MOTION_MOT_XPOS_BIT 6#define MPU6050_MOTION_MOT_YNEG_BIT 5#define MPU6050_MOTION_MOT_YPOS_BIT 4#define MPU6050_MOTION_MOT_ZNEG_BIT 3#define MPU6050_MOTION_MOT_ZPOS_BIT 2#define MPU6050_MOTION_MOT_ZRMOT_BIT 0#define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT 7#define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT 4#define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT 3#define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT 2#define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT 1#define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT 0#define MPU6050_PATHRESET_GYRO_RESET_BIT 2#define MPU6050_PATHRESET_ACCEL_RESET_BIT 1#define MPU6050_PATHRESET_TEMP_RESET_BIT 0#define MPU6050_DETECT_ACCEL_ON_DELAY_BIT 5#define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH 2#define MPU6050_DETECT_FF_COUNT_BIT 3#define MPU6050_DETECT_FF_COUNT_LENGTH 2#define MPU6050_DETECT_MOT_COUNT_BIT 1#define MPU6050_DETECT_MOT_COUNT_LENGTH 2#define MPU6050_DETECT_DECREMENT_RESET 0x0#define MPU6050_DETECT_DECREMENT_1 0x1#define MPU6050_DETECT_DECREMENT_2 0x2#define MPU6050_DETECT_DECREMENT_4 0x3#define MPU6050_USERCTRL_DMP_EN_BIT 7#define MPU6050_USERCTRL_FIFO_EN_BIT 6#define MPU6050_USERCTRL_I2C_MST_EN_BIT 5#define MPU6050_USERCTRL_I2C_IF_DIS_BIT 4#define MPU6050_USERCTRL_DMP_RESET_BIT 3#define MPU6050_USERCTRL_FIFO_RESET_BIT 2#define MPU6050_USERCTRL_I2C_MST_RESET_BIT 1#define MPU6050_USERCTRL_SIG_COND_RESET_BIT 0#define MPU6050_PWR1_DEVICE_RESET_BIT 7#define MPU6050_PWR1_SLEEP_BIT 6#define MPU6050_PWR1_CYCLE_BIT 5#define MPU6050_PWR1_TEMP_DIS_BIT 3#define MPU6050_PWR1_CLKSEL_BIT 2#define MPU6050_PWR1_CLKSEL_LENGTH 3#define MPU6050_CLOCK_INTERNAL 0x00#define MPU6050_CLOCK_PLL_XGYRO 0x01#define MPU6050_CLOCK_PLL_YGYRO 0x02#define MPU6050_CLOCK_PLL_ZGYRO 0x03#define MPU6050_CLOCK_PLL_EXT32K 0x04#define MPU6050_CLOCK_PLL_EXT19M 0x05#define MPU6050_CLOCK_KEEP_RESET 0x07#define MPU6050_PWR2_LP_WAKE_CTRL_BIT 7#define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH 2#define MPU6050_PWR2_STBY_XA_BIT 5#define MPU6050_PWR2_STBY_YA_BIT 4#define MPU6050_PWR2_STBY_ZA_BIT 3#define MPU6050_PWR2_STBY_XG_BIT 2#define MPU6050_PWR2_STBY_YG_BIT 1#define MPU6050_PWR2_STBY_ZG_BIT 0#define MPU6050_WAKE_FREQ_1P25 0x0#define MPU6050_WAKE_FREQ_2P5 0x1#define MPU6050_WAKE_FREQ_5 0x2#define MPU6050_WAKE_FREQ_10 0x3#define MPU6050_BANKSEL_PRFTCH_EN_BIT 6#define MPU6050_BANKSEL_CFG_USER_BANK_BIT 5#define MPU6050_BANKSEL_MEM_SEL_BIT 4#define MPU6050_BANKSEL_MEM_SEL_LENGTH 5#define MPU6050_WHO_AM_I_BIT 6#define MPU6050_WHO_AM_I_LENGTH 6#define MPU6050_DMP_MEMORY_BANKS 8#define MPU6050_DMP_MEMORY_BANK_SIZE 256#define MPU6050_DMP_MEMORY_CHUNK_SIZE 16//unsigned char buffer[14];//#define MPU6050_RD_ADD 0xD1#define MPU6050_WR_ADD 0xD0//unsigned char twst,i2c_timer=0;//unsigned char iic_start(unsigned char address){TWCR=TWINT|TWSTA|TWEN; // send START conditioni2c_timer=255; while(!(TWCR&TWINT)&&i2c_timer--); if(i2c_timer==0)return 1;twst=TWSR&0xF8; if((twst!=TW_START)&&(twst!=TW_REP_START))return 1;//TWDR=address; TWCR=TWINT|TWEN; // send device addressi2c_timer=255; while(!(TWCR&TWINT)&&i2c_timer--); if(i2c_timer==0)return 1;twst=TWSR&0xF8; if((twst!=TW_MT_SLA_ACK)&&(twst!=TW_MR_SLA_ACK))return 1;//return 0;}//void iic_stop(void){TWCR=TWINT|TWEN|TWSTO; /*send stop condition*/i2c_timer=255; while((TWCR&TWSTO)&&i2c_timer--);}//unsigned char iic_write(unsigned char data){// send data to the previously addressed deviceTWDR=data; TWCR=TWINT|TWEN;i2c_timer=255; while(!(TWCR&TWINT)&&i2c_timer--);if(i2c_timer==0)return 1;twst=TWSR&0xF8;if(twst!=TW_MT_DATA_ACK)return 1;return 0;}//unsigned char i2c_readAck(void){TWCR=TWINT|TWEN|TWEA;i2c_timer=255; while(!(TWCR&TWINT)&&i2c_timer--);if(i2c_timer==0)return 0;return TWDR;}//unsigned char i2c_readNak(void){TWCR=TWINT|TWEN;i2c_timer=255; while(!(TWCR&TWINT)&&i2c_timer--);if(i2c_timer==0)return 0;return TWDR;}//char mpu6050_readBytes(unsigned char regAddr,unsigned char length,unsigned char*data){unsigned char i=0,count=0;if(length>0){iic_start(MPU6050_WR_ADD);iic_write(regAddr);delay_us(10);iic_start(MPU6050_RD_ADD);for(i=0; i<length; i++){count++;if(i==(length-1))data[i]=i2c_readNak();else data[i]=i2c_readAck();}iic_stop();}return count;}//void mpu6050_writeByte(unsigned char regAddr,unsigned char data){iic_start(MPU6050_WR_ADD);iic_write(regAddr); //regiic_write(data);iic_stop();}//char mpu6050_readBit(unsigned char regAddr,unsigned char bitNum,unsigned char*data){unsigned char b;unsigned char count=mpu6050_readBytes(regAddr,1,&b);*data=b&(1<<bitNum);return count;}//void mpu6050_writeBits(unsigned char regAddr,unsigned char bitStart,unsigned char length,unsigned char data){unsigned char mask,b;if(mpu6050_readBytes(regAddr,1,&b)!=0){ //get current datamask=((1<<length)-1)<<(bitStart-length+1);data<<=(bitStart-length+1); // shift data into correct positiondata&=mask; // zero all non-important bits in datab&=~(mask); // zero all important bits in existing byteb|=data; // combine data with existing bytempu6050_writeByte(regAddr,b);}}//void mpu6050_writeBit(unsigned char regAddr,unsigned char bitNum,unsigned char data){unsigned char b;mpu6050_readBytes(regAddr,1,&b);b=(data!=0)?(b|(1<<bitNum)):(b&~(1<<bitNum));mpu6050_writeByte(regAddr,b);}//void mpu6050_setSleepDisabled(){mpu6050_writeBit(MPU6050_RA_PWR_MGMT_1,MPU6050_PWR1_SLEEP_BIT,0);}//void mpu6050_init(){delay_ms(100); //allow mpu6050 chip clocks to start upmpu6050_setSleepDisabled(); //set sleep disableddelay_ms(10); //wake up delay needed sleep disabled//set clock source// it is highly recommended that the device be configured to use one of the gyroscopes(or an external clock source)// as the clock reference for improved stabilitympu6050_writeBits(MPU6050_RA_PWR_MGMT_1,MPU6050_PWR1_CLKSEL_BIT,MPU6050_PWR1_CLKSEL_LENGTH,MPU6050_CLOCK_PLL_XGYRO);//set DLPF bandwidth to 42Hzmpu6050_writeBits(MPU6050_RA_CONFIG,MPU6050_CFG_DLPF_CFG_BIT,MPU6050_CFG_DLPF_CFG_LENGTH,MPU6050_DLPF_BW_42);//set sampe ratempu6050_writeByte(MPU6050_RA_SMPLRT_DIV,4); //1khz/(1+4)=200Hz//set gyro rangempu6050_writeBits(MPU6050_RA_GYRO_CONFIG,MPU6050_GCONFIG_FS_SEL_BIT,MPU6050_GCONFIG_FS_SEL_LENGTH,3); //MPU6050_GYRO_FS=3//set accel rangempu6050_writeBits(MPU6050_RA_ACCEL_CONFIG,MPU6050_ACONFIG_AFS_SEL_BIT,MPU6050_ACONFIG_AFS_SEL_LENGTH,0); //MPU6050_ACCEL_FS_2=0}//float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f,integralFBx=0.0f,integralFBy=0.0f,integralFBz=0.0f;float qw=1.0f,qx=0.0f,qy=0.0f,qz=0.0f,roll=0.0f,pitch=0.0f,yaw=0.0f;bit flag=0;// Mahony update function(for 6DOF)void mpu6050_mahonyUpdate(float gx,float gy,float gz,float ax,float ay,float az){float norm,halfvx,halfvy,halfvz,halfex,halfey,halfez,qa,qb,qc;// Compute feedback only if accelerometer measurement valid(avoids NaN in accelerometer normalisation)if(!((ax==0.0f)&&(ay==0.0f)&&(az==0.0f))){// Normalise accelerometer measurementnorm=sqrt(ax*ax+ay*ay+az*az);ax/=norm; ay/=norm; az/=norm;// Estimated direction of gravity and vector perpendicular to magnetic fluxhalfvx=q1*q3-q0*q2; halfvy=q0*q1+q2*q3; halfvz=q0*q0-0.5f+q3*q3;// Error is sum of cross product between estimated and measured direction of gravityhalfex=(ay*halfvz-az*halfvy); halfey=(az*halfvx-ax*halfvz); halfez=(ax*halfvy-ay*halfvx);// Compute and apply integral feedback if enabledintegralFBx+=halfex*0.0032786885245901639344262295081967; // integral error scaled by Ki //mpu6050_mahonysampleFreq=61.0fintegralFBy+=halfey*0.0032786885245901639344262295081967;integralFBz+=halfez*0.0032786885245901639344262295081967;gx+=integralFBx; gy+=integralFBy; gz+=integralFBz; // apply integral feedback// Apply proportional feedbackgx+=halfex; gy+=halfey; gz+=halfez; // mpu6050_mahonytwoKpDef=(2.0f*0.5f)}// Integrate rate of change of quaterniongx*=0.0081967213114754098360655737704918; // pre-multiply common factors //mpu6050_mahonysampleFreq=61.0fgy*=0.0081967213114754098360655737704918;gz*=0.0081967213114754098360655737704918;qa=q0; qb=q1; qc=q2;q0+=(-qb*gx-qc*gy-q3*gz); q1+=(qa*gx+qc*gz-q3*gy); q2+=(qa*gy-qb*gz+q3*gx); q3+=(qa*gz+qb*gy-qc*gx);// Normalise quaternionnorm=sqrt(q0*q0+q1*q1+q2*q2+q3*q3);q0/=norm; q1/=norm; q2/=norm; q3/=norm;//qw=q0; qx=q1; qy=q2; qz=q3; /**get quaternion*/yaw=atan2(2*q1*q2-2*q0*q3,2*q0*q0+2*q1*q1-1); // mpu6050_getRollPitchYawpitch=-asin(2*q1*q3+2*q0*q2);roll=atan2(2*q2*q3-2*q0*q1,2*q0*q0+2*q3*q3-1);flag=1;}//void mpu6050_updateQuaternion(){int ax=0,ay=0,az=0,gx=0,gy=0,gz=0;float axg=0,ayg=0,azg=0,gxrs=0,gyrs=0,gzrs=0;//get raw datawhile(1){mpu6050_readBit(MPU6050_RA_INT_STATUS,MPU6050_INTERRUPT_DATA_RDY_BIT,(unsigned char*)buffer);if(buffer[0])break;delay_us(10);}mpu6050_readBytes(MPU6050_RA_ACCEL_XOUT_H,14,(unsigned char*)buffer);ax=(((int)buffer[0])<<8)|buffer[1];ay=(((int)buffer[2])<<8)|buffer[3];az=(((int)buffer[4])<<8)|buffer[5];gx=(((int)buffer[8])<<8)|buffer[9];gy=(((int)buffer[10])<<8)|buffer[11];gz=(((int)buffer[12])<<8)|buffer[13];axg=(float)ax*0.00006103515625; // MPU6050_AXOFFSET=0,MPU6050_AXGAINayg=(float)ay*0.00006103515625; // MPU6050_AYOFFSET=0,MPU6050_AYGAINazg=(float)az*0.00006103515625; // MPU6050_AZOFFSET=0,MPU6050_AZGAIN;gxrs=(float)(gx-(-42))*0.001064225; //degree to radians // MPU6050_GXOFFSET=-42,MPU6050_GXGAIN=16.4gyrs=(float)(gy-( 9))*0.001064225; //degree to radians // MPU6050_GYOFFSET=9,MPU6050_GYGAIN=16.4gzrs=(float)(gz-(-29))*0.001064225; //degree to radians // MPU6050_GZOFFSET=-29,MPU6050_GZGAIN=16.4//compute datampu6050_mahonyUpdate(gxrs,gyrs,gzrs,axg,ayg,azg);}//interrupt[TIM0_OVF]void timer0_ovf_isr(void){ mpu6050_updateQuaternion(); }//void TX0_CH(char ch){ while(!(UCSR0A&0x20)); UDR0=ch; } // 송신 1바이트void TX0_STR(char*str){ while(*str)TX0_CH(*str++); } // 송신 문자열//void main(void){char str[100];//UCSR0B=8; UBRR0H=0; UBRR0L=8; //115200 @16MHzTCCR0=7; TIMSK=1; //16000000/1024/256=61Hz=16.384msTWBR=18; TWSR=0; //400 kHz i2c_init();mpu6050_init();delay_ms(50);SREG=0x80;for(;;){while(!flag); flag=0;//sprintf(str,"w=%7.3f x=%7.3f y=%7.3f z=%7.3f r=%7.3f p=%7.3f yw=%7.3f\r\n",qw,qx,qy,qz,roll,pitch,yaw); TX0_STR(str);}}// 추가 질문은 MCU BASIC 사이트에서 하세요.^^
댓글 0
조회수 1,709등록된 댓글이 없습니다.