ICM-20xxx 센서 필터 적용

 

 

 

 

쿼드콥터가 자꾸 한 쪽으로 과도하게 기울거나 진동이 너무 심해서 제어가 안된다면

필터 오동작을 의심해봐야 한다.

 

필터 없이 ICM-20608 센서에서 값을 1khz 단위로 읽어와 필터를 읽어와서

오일러 각에 상보필터를 적용하고 자이로에 칼만필터를 적용하는 코드를

작성한게 원인이었다.

 

ICM-20608 센서는 자체적으로 필터 기능을 내장하고 있어 설정을 통해 기능을 사용할 수 있는데

 

관련한 센서의 레지스터 비트는 FCHOICE_B, DLPF_CFG 이고 온도 센서도 켜야 제대로 동작하는 거 같다.

void init_mpu () {
  int delay_time = 100;
  HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET);
  HAL_Delay(delay_time);

  uint8_t tx_val = 0x00;
  uint8_t rx_val;

  read_mpu(0x75, &rx_val, 1);

  tx_val = 0x80;
  write_mpu(0x6B, &tx_val);
  HAL_Delay(delay_time);

  tx_val = 0x01;
  write_mpu(0x6B, &tx_val);
  HAL_Delay(delay_time);

  tx_val = 0x00;
  write_mpu(0x19, &tx_val);
  HAL_Delay(delay_time);

  tx_val = 0x05;
  write_mpu(0x1a, &tx_val);
  HAL_Delay(delay_time);

  tx_val = 0x03;
  write_mpu(0x1d, &tx_val);
  HAL_Delay(delay_time);

  tx_val = 0x10; // +- 1000dps
  write_mpu(0x6A, &tx_val);
  HAL_Delay(delay_time);

  tx_val = 0x08; // +- 4g
  write_mpu(0x1B, &tx_val);
  HAL_Delay(delay_time);

  write_mpu(0x1C, &tx_val);
  HAL_Delay(delay_time);
}

 

이 증상 외에도 자작 펌웨어가 잘 동작하지 않을 때 m-hive 님의 github을 참고하면 정말 좋을 거 같다.

https://github.com/ChrisWonyeobPark/M-HIVE-STM32_drone_programming_course-MH-FC-FW1.0

GitHub - ChrisWonyeobPark/M-HIVE-STM32_drone_programming_course-MH-FC-FW1.0: Source code for M-HIVE STM32 drone programming from

 

혹시 모르니

ICM-20608-Register-Map.pdf

0.49MB