센서 > 코드비젼 통신테스트 소스코드 (수정) - avrstudio

인기검색어 > 아두이노 센서 ATMEGA128

최신글 질문게시판 동영상강좌 가입하기

▼ BASIC4MCU 후원업체 신제품 정보 ▼

▲ BASIC4MCU 후원업체 신제품 정보 ▲

BASIC4MCU > 센서

자이로센서 | 코드비젼 통신테스트 소스코드 (수정) - avrstudio

페이지 정보

작성자 키트 작성일17-08-21 16:31 조회1,749회 댓글0건

첨부파일

본문

1949137565_GmMuHjkS_20131229-1.PNG3Ftype3Dw7401949137565_BmIUj7Xl_20131229-3.jpg3Ftype3Dw740
 
1949137565_XxnYhUDq_20131229-2.jpg3Ftype3Dw740

 

먼저 소스에서 타이머 주기를 16.3ms로 고치고

TWI 주파수를 400KHz로 바꾸는등 일부 수정했습니다.

 

 

// 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 condition
  i2c_timer=255while(!(TWCR&TWINT)&&i2c_timer--); if(i2c_timer==0)return 1;
  twst=TWSR&0xF8if((twst!=TW_START)&&(twst!=TW_REP_START))return 1;
  //
  TWDR=addressTWCR=TWINT|TWEN// send device address
  i2c_timer=255while(!(TWCR&TWINT)&&i2c_timer--); if(i2c_timer==0)return 1;
  twst=TWSR&0xF8if((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=255while((TWCR&TWSTO)&&i2c_timer--);
}
//
unsigned char iic_write(unsigned char data){
  // send data to the previously addressed device
  TWDR=dataTWCR=TWINT|TWEN;
  i2c_timer=255while(!(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=255while(!(TWCR&TWINT)&&i2c_timer--);
  if(i2c_timer==0)return 0;
  return TWDR;
}
//
unsigned char i2c_readNak(void){
  TWCR=TWINT|TWEN;
  i2c_timer=255while(!(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=0i<lengthi++){
      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); //reg
  iic_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 data
    mask=((1<<length)-1)<<(bitStart-length+1);
    data<<=(bitStart-length+1); // shift data into correct position
    data&=mask;                 // zero all non-important bits in data
    b&=~(mask);                 // zero all important bits in existing byte
    b|=data;                    // combine data with existing byte
    mpu6050_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 up
  mpu6050_setSleepDisabled(); //set sleep disabled
  delay_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 stability
  mpu6050_writeBits(MPU6050_RA_PWR_MGMT_1,MPU6050_PWR1_CLKSEL_BIT,MPU6050_PWR1_CLKSEL_LENGTH,MPU6050_CLOCK_PLL_XGYRO);
  //set DLPF bandwidth to 42Hz
  mpu6050_writeBits(MPU6050_RA_CONFIG,MPU6050_CFG_DLPF_CFG_BIT,MPU6050_CFG_DLPF_CFG_LENGTH,MPU6050_DLPF_BW_42);
  //set sampe rate
  mpu6050_writeByte(MPU6050_RA_SMPLRT_DIV,4); //1khz/(1+4)=200Hz
  //set gyro range
  mpu6050_writeBits(MPU6050_RA_GYRO_CONFIG,MPU6050_GCONFIG_FS_SEL_BIT,MPU6050_GCONFIG_FS_SEL_LENGTH,3); //MPU6050_GYRO_FS=3
  //set accel range
  mpu6050_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 measurement
    norm=sqrt(ax*ax+ay*ay+az*az);
    ax/=normay/=normaz/=norm;
    // Estimated direction of gravity and vector perpendicular to magnetic flux
    halfvx=q1*q3-q0*q2halfvy=q0*q1+q2*q3halfvz=q0*q0-0.5f+q3*q3;
    // Error is sum of cross product between estimated and measured direction of gravity
    halfex=(ay*halfvz-az*halfvy); halfey=(az*halfvx-ax*halfvz); halfez=(ax*halfvy-ay*halfvx);
    // Compute and apply integral feedback if enabled
    integralFBx+=halfex*0.0032786885245901639344262295081967// integral error scaled by Ki //mpu6050_mahonysampleFreq=61.0f
    integralFBy+=halfey*0.0032786885245901639344262295081967;
    integralFBz+=halfez*0.0032786885245901639344262295081967;
    gx+=integralFBxgy+=integralFBygz+=integralFBz// apply integral feedback
    // Apply proportional feedback
    gx+=halfexgy+=halfeygz+=halfez// mpu6050_mahonytwoKpDef=(2.0f*0.5f)
  }
  // Integrate rate of change of quaternion
  gx*=0.0081967213114754098360655737704918// pre-multiply common factors //mpu6050_mahonysampleFreq=61.0f
  gy*=0.0081967213114754098360655737704918;
  gz*=0.0081967213114754098360655737704918;
  qa=q0qb=q1qc=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 quaternion
  norm=sqrt(q0*q0+q1*q1+q2*q2+q3*q3);
  q0/=normq1/=normq2/=normq3/=norm;
  //
  qw=q0qx=q1qy=q2qz=q3/**get quaternion*/
  yaw=atan2(2*q1*q2-2*q0*q3,2*q0*q0+2*q1*q1-1); // mpu6050_getRollPitchYaw
  pitch=-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 data
  while(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_AXGAIN
  ayg=(float)ay*0.00006103515625// MPU6050_AYOFFSET=0,MPU6050_AYGAIN
  azg=(float)az*0.00006103515625// MPU6050_AZOFFSET=0,MPU6050_AZGAIN;
  gxrs=(float)(gx-(-42))*0.001064225//degree to radians // MPU6050_GXOFFSET=-42,MPU6050_GXGAIN=16.4
  gyrs=(float)(gy-(  9))*0.001064225//degree to radians // MPU6050_GYOFFSET=9,MPU6050_GYGAIN=16.4
  gzrs=(float)(gz-(-29))*0.001064225//degree to radians // MPU6050_GZOFFSET=-29,MPU6050_GZGAIN=16.4
  //compute data
  mpu6050_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=8UBRR0H=0UBRR0L=8//115200 @16MHz
  TCCR0=7TIMSK=1//16000000/1024/256=61Hz=16.384ms
  TWBR=18TWSR=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,749

등록된 댓글이 없습니다.

게시물 검색

센서 목록

게시물 검색


Privacy Policy
MCU BASIC ⓒ 2017
PC버전