martedì 19 marzo 2019

Complimentary filter con MPU6050 ed Arduino

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);
}

Nessun commento:

Posta un commento

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...