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

TODAY77 TOTAL2,271,544
사이트 이용안내
Login▼/회원가입
최신글보기 질문게시판 기술자료 동영상강좌

아두이노 센서 ATMEGA128 PWM LED 초음파 AVR 블루투스 LCD UART 모터 적외선


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

페이지 정보

작성자 키트 작성일2017-08-21 16:31 조회1,754회 댓글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,754

등록된 댓글이 없습니다.

센서HOME > 센서 > 자이로센서 목록

게시물 검색

2022년 1월 2월 3월 4월 5월 6월 7월 8월 9월 10월 11월 12월
2021년 1월 2월 3월 4월 5월 6월 7월 8월 9월 10월 11월 12월
2020년 1월 2월 3월 4월 5월 6월 7월 8월 9월 10월 11월 12월
2019년 1월 2월 3월 4월 5월 6월 7월 8월 9월 10월 11월 12월
2018년 1월 2월 3월 4월 5월 6월 7월 8월 9월 10월 11월 12월
Privacy Policy
MCU BASIC ⓒ 2020
모바일버전으로보기