0715 인터럽트 / I2C를 이용한 가속도센서

# PIC : Programmable Interrupt Controller

다양한 장치에서 인터럽트를 요청하면 PIC는 이런 요청들에 대해 우선순위를 매기며 현재 인터럽트 처리중인 장치와 인터럽트 요청한 장치들을 관리

 

# IRQ line

하드웨어에서 인터럽트 신호를 위한 메커니즘. CPU로부터 서비스를 원할 때 장치는 IRQ라인에 신호를 보내고 기다린다.

 

# 인터럽트 처리 과정

IRQ(인터럽트) -> 문맥 백업 -> ISR 인터럽트 처리 -> 문맥 복구

 

---

 

 

# I2C

SPI처럼 하나의 Master와 다수의 Slave 장치들로 구성되어 있음.  (Master는 클록 생성 장치)

SPI와 달리 2개의 신호선으로 구성되어 있음

1. SCL (Clock)

2. SDA (Data/Address)

(i2c는 무전기같은 통신방식, spi는 전화같은 통신방식)

 

 

# I2C통신 방법의 가속도 센서 활용

 

 

<C코드>

#include <stdio.h>
#include <wiringPi.h>

#define INTR_PIN        18
#define MPU6050_ADDR    0x68

#define REG_ACCEL_CONFIG    0x1C
#define REG_MOT_THR         0x1F
#define REG_MOT_DUR         0x20
#define REG_INT_PIN_CFG     0x37
#define REG_INT_ENABLE      0x38
#define REG_INT_STATUS      0x3A
#define REG_MOT_DETECT_CTRL 0x69
#define REG_PWR_MGNT_1      0x6B
#define REG_WHO_AM_I        0x75

int gFD;

int ScaleValue(int a)
{
    if(a >= 32768)  a = a - 65536;
    return a;
}

void GetAccValue(int *x, int *y, int *z)
{
    int h, l;
    h = wiringPiI2CReadReg8(gFD, 0x3B);
    l = wiringPiI2CReadReg8(gFD, 0x3C);
    *x = ScaleValue( (h<<8) | l );

    h = wiringPiI2CReadReg8(gFD, 0x3D);
    l = wiringPiI2CReadReg8(gFD, 0x3E);
    *y = ScaleValue( (h<<8) | l );

    h = wiringPiI2CReadReg8(gFD, 0x3F);
    l = wiringPiI2CReadReg8(gFD, 0x40);
    *z = ScaleValue( (h<<8) | l );
}

void isr_func()					
{
    int x, y, z;

    wiringPiI2CReadReg8(gFD, REG_INT_STATUS);          

    GetAccValue(&x, &y, &z);
    printf("[INTR] %d, %d, %d \r\n", x, y, z);
}

void main()
{
    int reg, sec;

    wiringPiSetupGpio() ;				                
    pinMode(INTR_PIN, INPUT) ;
    wiringPiISR(INTR_PIN, INT_EDGE_RISING, isr_func) ;	
    
    if( (gFD = wiringPiI2CSetup(MPU6050_ADDR)) == -1 )
    {
        printf("I2C Setup is failed \r\n");
        return;
    }

    // reset
    wiringPiI2CWriteReg8(gFD, REG_PWR_MGNT_1, 0x1 << 7);
    delay(100);

    // wakeup from sleep
    wiringPiI2CWriteReg8(gFD, REG_PWR_MGNT_1, 0x00);
    delay(10);                                          

    // set motion detection threshold (0 ~ 255) 가속도 센서 크기 범위 지정
    wiringPiI2CWriteReg8(gFD, REG_MOT_THR, 10);

    // HPF Configuration
    wiringPiI2CWriteReg8(gFD, REG_ACCEL_CONFIG, 0x01);

    // set motion detection duration --> 10msec
    wiringPiI2CWriteReg8(gFD, REG_MOT_DUR, 10);

    // active high, push-pull,
    wiringPiI2CWriteReg8(gFD, REG_INT_PIN_CFG, 0x00);

    // motion interrupt enable
    wiringPiI2CWriteReg8(gFD, REG_INT_ENABLE, 0x1<<6);

    sec = 0;
    while(1)
    {
        printf("sec : %d \r\n", sec++);
        delay(1000);
    }
}

메인문이 실행되다가 가속도 센서에 이벤트가 발생하면 isr_func 함수가 실행되면서 x, y, z 출력된다.

 

 

<Python 코드>

import time, smbus
import math

def ScaleValue(val):
    if val >= 32768:
        val = val - 65536
    return val

def Read_Temp():
    h = bus.read_byte_data(mpu6050_addr, 0x41)
    l = bus.read_byte_data(mpu6050_addr, 0x42)
    x = ScaleValue((h << 8) | l)
    c = x / 340 + 36.53
    return c

def Read_X_Accel():
    h = bus.read_byte_data(mpu6050_addr, 0x3B)
    l = bus.read_byte_data(mpu6050_addr, 0x3C)
    x = ScaleValue((h << 8) | l)
    return x

def Read_Y_Accel():
    h = bus.read_byte_data(mpu6050_addr, 0x3D)
    l = bus.read_byte_data(mpu6050_addr, 0x3E)
    y = ScaleValue((h << 8) | l)
    return y

def Read_Z_Accel():
    h = bus.read_byte_data(mpu6050_addr, 0x3F)
    l = bus.read_byte_data(mpu6050_addr, 0x40)
    z = ScaleValue((h << 8) | l)
    return z

bus = smbus.SMBus(1)
mpu6050_addr = 0x68

# wakeup from sleep mode
bus.write_byte_data(mpu6050_addr, 0x6B, 0)

angleX = 0; angleY =0

pp = Read_Temp(); ppx = Read_X_Accel(); ppy = Read_Y_Accel(); ppz = Read_Z_Accel()
cnt = 0
while 1:
	x = Read_X_Accel()
	y = Read_Y_Accel()
	z = Read_Z_Accel()
	c = Read_Temp()
    
    # 각도 계산
	angleX = math.atan(-y / math.sqrt(x*x + z*z))*(180/math.pi); angleX = round(angleX, 2)
	angleY = math.atan(-x / math.sqrt(y*y + z*z)) * (180/math.pi); angleY = round(angleY, 2)

	# x(n) : sensing Value , p(n) ; prediction Value
	# x(n) = a * 과거p(n-1) + b * 현재p(n) // a+b=1

	pp = 0.9 * pp + 0.1 * c  # 튀는 값 보정방법 (부드럽게 값이 변화)
	ppx = 0.9 * ppx + 0.1 * x; ppx = round(ppx,2)
	ppy = 0.9 * ppy + 0.1 * y; ppy = round(ppy,2)
	ppz = 0.9 * ppz + 0.1 * z; ppz = round(ppz,2)
	if cnt %300 == 0:
		print("각도 : ",angleX, ", ", angleY) 
		print("가속도: ",x, y, z, "    New 가속도 : ",ppx, ppy, ppz)
		print("Temp : ",round(c,2), "           New Temp : ",round(pp,2))
		print()

	cnt+=1

x, y, z축 센서를 이용해 각도 계산이 가능하다.

튀는 값들을 모두 출력하면 정확한 값이라고 판단하기 어려우므로 튀는 값을 보정해준다.

x(n) = a * 과거값 + b * 현재값 

수식을 통해서 값을 새로 계산한다. a+b=1이여야 하고 값을 바꾸면서 최적의 값으로 조정할 수 있다.

이렇게 값 보정이 좀 쌓인 다음에 출력해야 하므로 cnt가 300의 배수일때만 출력해준다