[STM32] LL 드라이버 - I2C DMA로 작성한 MPU6050 상보필터

I2C.c

0.00MB

I2C.h

0.00MB

MPU6050.c

0.00MB

MPU6050.h

0.00MB

MPU6050REG.h

0.01MB

LL드라이버를 기반으로 MPU6050과 I2C 통신을 하는 코드를 작성했다.

 

초기화하는 코드와 Offset을 구하는 코드에는 DMA를 적용하지 않았고 상보 필터를 사용하여 각도를 계산하는 과정에 DMA를 사용했다.

 

코드에 나오는 구조체들은 전부 헤더들에 정의되어 있으므로 코드에서는 따로 설명하지 않는다.

 

I2C 코드 생성 (0)

Parameter Settings

NVIC Settings

DMA Settings

I2C는 Fast Mode를 사용할 것이다.

 

인터럽트는 event  interrupt를 사용하여 SB, ADDR 인터럽트를 처리할 것이다.

 

DMA는 Circular 모드를 사용하여 MPU6050에서 데이터를 읽고 자동으로 DMA_SxNDTR 레지스터를 세팅할 것이다.

 

DMA_SxNDTR 레지스터는 DMA가 전송해야 할 데이터의 양을 나타내는 레지스터이며 Circular 모드일 때 값이 0이 되면 자동으로 초기화한 값으로 세팅된다.

 

 

TIM11 코드 생성 (1)

Parameter Settings

NVIC Settings

692p

APB2 timer clock (84Mhz)

1ms를 구하기 위해 TIM11을 사용했다. Prescaler값에 -1이 붙어있는 이유는 TIMx_PSC레지스터를 보면 PSC[15:0] +1의 값을 나눠주기 때문에 실제 나누고자 하는 값에 -1을 해줘야 한다.

 

TIM11는 STM32F407 MCU에서 APB2 bus와 연결되어 있으므로 84Mhz가 기본 클럭이다. 따라서 84로 나눠 1us를 만들고 Counter Period를 1000으로 하여 1ms를 만들었다.

 

 

코드 - main.c (2)

 

/* USER CODE BEGIN Includes */
#include <stdio.h>
#include "MPU6050.h"
#include "I2C.h"
/* USER CODE END Includes */

 

printf를 사용하기 위한 stdio.h와 MPU6050.h, I2C.h를 인클루드 한다.

 

/* USER CODE BEGIN 0 */
MPU6050 mpu6050;
I2C_DMA_struct DMA;
 
uint8_t i2c_dma_buf[14];
 
uint8_t condition_1ms=0;
/* USER CODE END 0 */

MPU6050 구조체와 I2C_DMA_struct 구조체를 사용할 것이고 i2c_dma_buf로 DMA가 데이터를 저장할 것이다. 두 개의 구조체는 stm32f4xx_it.c 파일에서도 extern으로 사용할 것이므로 글로벌 변수로 선언한다.

 

condition_1ms는 1ms가 되면 1로 SET 되어 1ms가 지났다는 것을 알려줄 것이다.

 

/* USER CODE BEGIN 2 */
 
  uint8_t data[14]={0,};
  uint16_t counter=0;
 
  ANGLE angle;
 
  LL_mDelay(500);
 
  LL_TIM_EnableIT_UPDATE(TIM11);
  LL_TIM_EnableCounter(TIM11);
 
  I2C_Receive_DMA_init(&(mpu6050.I2C),
                                  &DMA,
                                  I2C1,
                          DMA1,
                          LL_DMA_STREAM_0,
                          (uint32_t)i2c_dma_buf,
                          LL_I2C_DMA_GetRegAddr(I2C1),
                          14);
 
  printf("start\r\n");
 
  init_MPU6050(&mpu6050,I2C1);
  MPU_Gyrocali(&mpu6050,&angle);
  MPU_Angcali(&mpu6050,&angle);
 
  data[0]=MPU6050_RA_ACCEL_XOUT_H;
  I2C_Transmit(&(mpu6050.I2C),mpu6050.gyro_address,data,1);
  I2C_Receive_DMA(&(mpu6050.I2C),&DMA, mpu6050.gyro_address);
 
  /* USER CODE END 2 */

 

data 배열의 개수는 DMA가 아닌 방식으로 데이터를 읽는 테스트를 할 때 사용하려고 14개를 선언했고  DMA를 사용할 경우 1 이상의 값만 선언되어있으면 된다.

 

ANGLE 구조체에 각도와 각속도, Offset에 관한 정보를 저장한다.

 

TIM11의 UPDATE 인터럽트를 활성화시킨 후 타이머를 작동시킨다.

 

DMA를 사용할 때 I2C_Receive_DMA_init()함수를 사용하여 I2C, DMA에 관한 기본정보를 초기화한다. DMA를 사용하지 않을 경우 사용하지 않는다.

 

init_MPU6050에서 MPU6050 구조체를 초기화한다.

 

MPU_Gyrocali() 함수는 각속도 데이터의 Offset을 계산하는데 DMA는 사용하지 않는다.

 

MPU_Angcali() 함수는 각도의 Offset을 계산한다. 실제로 Offset이 없으면 평지라고 생각한 곳에서의 각도가 0도가 아닌 5도 정도의 차이가 났다.

 

마지막 3줄은 MPU6050에 레지스터 값을 보낸 후 그 값을 DMA로 읽는 코드이다.

 

데이터를 보내는 코드는 DMA로 작성할 필요가 없어 보여 기존의 I2C_Transmit() 함수를 사용했다.

 

/* USER CODE END 2 */
 
  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
      if(condition_1ms && DMA.i2c_receive_dma){
          float accdegx,accdegy,acctotvec;
          float pitch,roll;
          int16_t temp[3];
          int16_t tempmpuaccx,tempmpuaccy,tempmpuaccz;
 
 
          tempmpuaccx=(int16_t)(i2c_dma_buf[0]<<8 | i2c_dma_buf[1]);
          tempmpuaccy=(int16_t)(i2c_dma_buf[2]<<8 | i2c_dma_buf[3]);
          tempmpuaccz=(int16_t)(i2c_dma_buf[4]<<8 | i2c_dma_buf[5]);
          temp[0]=(int16_t)(i2c_dma_buf[8]<<8 | i2c_dma_buf[9]);
          temp[1]=(int16_t)(i2c_dma_buf[10]<<8 | i2c_dma_buf[11]);
          temp[2]=(int16_t)(i2c_dma_buf[12]<<8 | i2c_dma_buf[13]);
 
          angle.getmpuaccx=tempmpuaccx;
          angle.getmpuaccy=tempmpuaccy;
          angle.getmpuaccz=tempmpuaccz;
          angle.f_gyx=((float)(temp[0]-angle.gyro_offset[0]))/65.5;
          angle.f_gyy=((float)(temp[1]-angle.gyro_offset[1]))/65.5;
          angle.f_gyz=((float)(temp[2]-angle.gyro_offset[2]))/65.5;
 
 
          acctotvec=sqrtf((float)(angle.getmpuaccx*angle.getmpuaccx/100
                  +angle.getmpuaccy*angle.getmpuaccy/100
                  +angle.getmpuaccz*angle.getmpuaccz/100))*10;
          accdegx=asinf((float)angle.getmpuaccx/acctotvec)*(57.29577951);
          accdegy=asinf((float)angle.getmpuaccy/acctotvec)*(57.29577951);
 
          angle.pitch=(alpha)*(angle.pitch-(angle.f_gyy)*dt)+(1-alpha)*(accdegx);
          angle.roll=(alpha)*(angle.roll+(angle.f_gyx)*dt)+(1-alpha)*(accdegy);
 
          pitch=angle.pitch-angle.pitch_offset;
          roll=angle.roll-angle.roll_offset;
          counter++;
          if(counter>200){
              printf("pitch:%.2f\r\nroll:%.2f\n\n\r",pitch,roll);
              counter=0;
          }
 
          data[0]=MPU6050_RA_ACCEL_XOUT_H;
          I2C_Transmit(&mpu6050.I2C,mpu6050.gyro_address,data,1);
          I2C_Receive_DMA(&mpu6050.I2C,&DMA,mpu6050.gyro_address);
          condition_1ms=0;
      }
 
    /* USER CODE END WHILE */
 
    /* USER CODE BEGIN 3 */
 }

 

while문 내부에서 DMA가 값을 전부 전송하고 1ms가 지났을 때 조건문을 만족하여 각도를 계산하는 코드이다.

 

counter로 일부분의 데이터만 보여주게 했으며 그렇지 않을 경우 터미널 프로그램에 너무 많은 값들이 떠서 제대로 결과를 보기 힘들다.

 

 

코드 - stm32f4xx_it.c (3)

 

/* USER CODE BEGIN Includes */
#include "I2C.h"
#include "MPU6050.h"
/* USER CODE END Includes */
 
/* USER CODE BEGIN EV */
extern MPU6050 mpu6050;
extern I2C_DMA_struct DMA;
 
extern uint8_t condition_1ms;
/* USER CODE END EV */

 

main.c의 mpu6050, DMA 변수들을 extern으로 선언하여 stm32f4xx_it.c 파일에서도 사용할 수 있게 한다.

 

/**
  * @brief This function handles DMA1 stream0 global interrupt.
  */
void DMA1_Stream0_IRQHandler(void)
{
  /* USER CODE BEGIN DMA1_Stream0_IRQn 0 */
    DMA_Stream_irq(&(mpu6050.I2C),&DMA);
  /* USER CODE END DMA1_Stream0_IRQn 0 */
  
  /* USER CODE BEGIN DMA1_Stream0_IRQn 1 */
 
  /* USER CODE END DMA1_Stream0_IRQn 1 */
}
 
/**
  * @brief This function handles TIM1 trigger and commutation interrupts and TIM11 global interrupt.
  */
void TIM1_TRG_COM_TIM11_IRQHandler(void)
{
  /* USER CODE BEGIN TIM1_TRG_COM_TIM11_IRQn 0 */
    if(LL_TIM_IsActiveFlag_UPDATE(TIM11)){
        LL_TIM_ClearFlag_UPDATE(TIM11);
        condition_1ms=1;
    }
  /* USER CODE END TIM1_TRG_COM_TIM11_IRQn 0 */
  
  /* USER CODE BEGIN TIM1_TRG_COM_TIM11_IRQn 1 */
 
  /* USER CODE END TIM1_TRG_COM_TIM11_IRQn 1 */
}
 
/**
  * @brief This function handles I2C1 event interrupt.
  */
void I2C1_EV_IRQHandler(void)
{
  /* USER CODE BEGIN I2C1_EV_IRQn 0 */
    I2C_DMA_irq(&(mpu6050.I2C),&DMA);
 
  /* USER CODE END I2C1_EV_IRQn 0 */
  
  /* USER CODE BEGIN I2C1_EV_IRQn 1 */
 
  /* USER CODE END I2C1_EV_IRQn 1 */
}

 

3개의 인터럽트 서비스 루틴 중 DMA_Stream_irq(), I2C_DMA_irq()는 따로 만들었으며 내부는 I2C 인터럽트와 DMA인터럽트에 관한 코드이다.

 

TIM11의 경우 UPDATE인터럽트가 발생하면 condition_1ms를 1로 바꿔 1ms가 지났다는 것을 표시한다.

 

결과 - (4)

 

처음 동작시키면 Offset을 구하며 값들을 표시한다.

 

그 후에 pitch값들과 roll값들을 터미널 프로그램에 출력한다.

 

 

 

-

DMA를 사용하여 MPU6050 상보 필터 코드를 작성해 봤다.

 

코드를 좀 더 정리하여 실제 헥사콥터에 적용할 생각이다.

 

또한 아직 alpha값이 완벽하지 않은 것 같아 계속 실험해보면서 값을 조정해야 할 것 같다.

 

 

추가

ddtxrx.tistory.com/entry/Quaternion-Open-source-AHRS-%EC%95%8C%EA%B3%A0%EB%A6%AC%EC%A6%98?category=925643

Quaternion Open source AHRS 알고리즘

 

상보필터말고 쿼터니언을 사용한 다른 알고리즘이다. 실제 적용해보면 좋을 것 같다.