DMC.35 · Датчики · ATmega328P
Задача
Проинтегрировать угловую скорость по времени для оценки угла рыскания.
Алгоритм
yaw += gyro_z(град/с)·dt. Демонстрирует накопление и дрейф без коррекции магнитометром.
Исходный код (C, avr-gcc)
#ifndef F_CPU
#define F_CPU 16000000UL
#endif
#include <avr/io.h>
#include <util/delay.h>
int main(void){
DDRB|=(1<<PB5);
float yaw=0; const float dt=0.01f;
volatile int16_t gz=0; // сырое гиро (LSB)
for(;;){
float w=gz/131.0f; // ±250°/с -> 131 LSB/(°/с)
yaw += w*dt; if(yaw>360)yaw-=360; if(yaw<0)yaw+=360;
if(yaw>180) PORTB|=(1<<PB5); else PORTB&=~(1<<PB5);
_delay_ms(10);
}
}
Сборка: avr-gcc -mmcu=atmega328p -DF_CPU=16000000UL -Os файл.c -o файл.elf
Пояснения
Чистый гиро-интеграл уплывает — на практике корректируется компасом/акселерометром (слияние).