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)
Dockerizza Flask
Un esempio semplice per inserire in un container Docker una applicazione Flask Partiamo da una semplice applicazione che ha un file app.py ...
-
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