Комплементарный фильтр — проект для дрона на ATmega328P

DMC.32 · Датчики · ATmega328P

Задача

Объединить интеграл гироскопа и угол из акселерометра в устойчивую оценку крена.

Алгоритм

angle = k·(angle + gyro·dt) + (1-k)·acc_angle. k близко к 1 (напр. 0.98).

Исходный код (C, avr-gcc)

#ifndef F_CPU
#define F_CPU 16000000UL
#endif

#include <avr/io.h>
#include <util/delay.h>
#include <math.h>
int main(void){
  DDRB|=(1<<PB5);
  float angle=0; const float k=0.98f, dt=0.01f;
  volatile int16_t ay=0, az=16384, gx=0;   // условные сырые данные
  for(;;){
    float acc = atan2((float)ay,(float)az)*180.0f/3.14159f;
    float g = gx/131.0f;                    // град/с при ±250°/с
    angle = k*(angle + g*dt) + (1.0f-k)*acc;
    if(angle>10||angle<-10) PORTB|=(1<<PB5); else PORTB&=~(1<<PB5);
    _delay_ms(10);
  }
}

Сборка: avr-gcc -mmcu=atmega328p -DF_CPU=16000000UL -Os файл.c -o файл.elf

Пояснения

Дёшев и эффективен для стабилизации. acc_angle=atan2(ay,az). Значения здесь условные (демо расчёта).

Смотрите также

Слияние датчиков