Seg... pinne. (10 / 14 steg)
Steg 10: Ett mycket smickrande filter.
vinkel = A * (vinkel + ränta * DT) + (1 - A) * (float) accel_raw * A_GAIN;
Okej, så det är en lång linje. Du kan också göra det med en op-förstärkare eller två.
Detta filter gör exakt vad som är nödvändigt i det här scenariot: det gynnar gyroskopet läsande för kort tid varaktigheter och accelerometer genomsnittet läsning för lång tid varaktigheter. Låt oss bryta ner det:
A är den faktor som bestämmer den cutoff tid för lita på gyro och filtrering i accelerometern. Det är alltid mellan 0 och 1, vanligtvis nära 1. I det här fallet definieras A för att vara 0.962. Detta innebär att på varje gång steg, 96,2% av nya vinkel mätning kommer från gamla vinkel mätning plus integrerad gyro mätning. De återstående 3,8% kommer från accelerometern. Detta genomsnitt långsamt i accelerometern över många tid steg.
DT är tid i sekunder mellan program loopar, tidssteg. Här det definieras för att vara 0.020 som delay(20) i slutet av slingan. Koden i en slinga sig ta mycket mindre än 20ms, så förseningen dominerar tidssteg.
ränta är gyro läser, omvandlas till grader per sekund.
accel_raw * A_GAIN är den accelerometer behandlingen omvandlas till grader. Det är mycket viktigt att dessa två är i samma enhet bas innan du lägger ihop. (Du kan inte lägga till äpplen med apelsiner.)
Tidskonstant av filtret är den tidpunkt då den gyroskop behandlingen börjar filtreras kraftigt och accelerometer läsningen börjar beräknas i tungt. Det är faktiskt en kontinuerlig process, men tidskonstanten är ett enda mått på där balansen börjar att flytta. Tidskonstanten är:
Tau = DT*(A)/(1-A) = 0.5s
Så för detta filter är gyrot betrodda för om 0,5 sekunder och accelerometer start i genomsnitt i betydligt efter det. Detta värde kan finjusteras genom att ändra A.