Per terminare la serie, dopo Kalman e Madgwick un esempio di Complementary Filter
Dal punto di vistra computazionale e' il piu' semplice perche' si tratta di una sola formula
senza ricorsione e matrici
angle = 0.98 *(angle+gyro*dt) + 0.02*acc
(i parametri possono essere ottimizzati)
Una breve statistica (per raffronto con le prove precedenti)
Nr campioni : 20121
Media : -11.97
Range : 0.41
Std : 0.02
Varianza : 0.0005
Skewness :0.62
Il codice e' stato adattato da qui
-----------------------------------------------------------------------
// Complementary filter
#include <Wire.h>
int16_t axRaw, ayRaw, azRaw, gxRaw, gyRaw, gzRaw, temperature;
float pitch=0;
float pitchAcc;
float P_CompCoeff= 0.98;
unsigned long delta_t,old_time;
void ComplementaryFilter(int ax,int ay,int az,int gy,int gz) {
long squaresum=(long)ay*ay+(long)az*az;
delta_t = micros()- old_time;
old_time = micros();
pitch+=((-gy/32.8f)*(delta_t/1000000.0f));
pitchAcc =atan(ax/sqrt(squaresum))*RAD_TO_DEG;
pitch =P_CompCoeff*pitch + (1.0f-P_CompCoeff)*pitchAcc;
}
void setup() {
Serial.begin(115200);
//Inizio configurazione clock interno
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x00);
Wire.endTransmission();
// Inizializzazione Accelerometro intervallo di 8g Fattore di scala 4096
Wire.beginTransmission(0x68);
Wire.write(0x1C);
Wire.write(0x10);
Wire.endTransmission();
// Inizializzazione Gyro Intervallo di 500 deg/sec LSB (fattore di scala) 65.5 deg/s
Wire.beginTransmission(0x68);
Wire.write(0x1B);
Wire.write(0x08);
Wire.endTransmission();
old_time = micros();
}
void loop() {
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68, 14);
while (Wire.available() < 14);
axRaw = Wire.read() << 8 | Wire.read();
ayRaw = Wire.read() << 8 | Wire.read();
azRaw = Wire.read() << 8 | Wire.read();
temperature = Wire.read() << 8 | Wire.read();
gxRaw = Wire.read() << 8 | Wire.read();
gyRaw = Wire.read() << 8 | Wire.read();
gzRaw = Wire.read() << 8 | Wire.read();
/*Serial.print(axRaw/4096.0); Serial.print(",");
Serial.print(ayRaw/4096.0); Serial.print(",");
Serial.print(azRaw/4096.0); Serial.print(",");
Serial.print(gxRaw/131.0); Serial.print(",");
Serial.print(gyRaw/131.0); Serial.print(",");
Serial.println(gzRaw/131.0);*/
ComplementaryFilter(axRaw,ayRaw,azRaw,gyRaw,gzRaw);
Serial.println(pitch);
delay(2);
}
Iscriviti a:
Commenti sul post (Atom)
Arduino logger low power
Alla fine ci sono riuscito La arduino MKR zero e' ormai da un mese che e' rimasta accesa sul tavolo...o meglio si attiva ogni minuto...
-
In questo post viene indicato come creare uno scatterplot dinamico basato da dati ripresi da un file csv (nel dettaglio il file csv e' c...
-
Questo post e' a seguito di quanto gia' visto nella precedente prova Lo scopo e' sempre il solito: creare un sistema che permet...
-
La scheda ESP32-2432S028R monta un Esp Dev Module con uno schermo TFT a driver ILI9341 di 320x240 pixels 16 bit colore.Il sito di riferiment...
Nessun commento:
Posta un commento