Calcolare angolo inclinazione giroscopio per filtro complementare -MPU6050

Ciao a tutti!!
Da qualche giorno sto provando ad usare il filtro complementare per avere una maggiore precisione dell’inclinazione di un oggetto.

Sono riuscito a calcolare l’inclinazione dell’accelerometro, ma ho problemi a calcolare quella del giroscopio.
Ho provato diversi metodi ma nessuno ha funzionato.

Potreste darmi qualche consiglio su come calcolare l’inclinazione del giroscopio?

Allego il codice

// Complementary Filter: angle = (0.98 * (angle + gyroData * delta_time)) + (0.02 * accData)

#include <Wire.h>

#define MPU 0x68 
#define GYRO_SENSITIVITY 65.536
#define DT 0.05

void Calculation_IMU_Angle(float[], float[], float[], float[]);
void Complementary_Filter(float[], float[], float[]);
void Time_Delay();

void setup() {
  // put your setup code here, to run once:

  Serial.begin(9600);
  
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B); 
  Wire.write(0);    
  Wire.endTransmission();
  Serial.begin(9600);
}

void loop() {
  // put your main code here, to run repeatedly:

  static float AccDataRaw[3], GyroDataRaw[3], AccAngle[2] = {0, 0}, GyroAngle[2] = {0, 0}, Angle[2] = {0, 0};
  
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);
  Wire.endTransmission();
  
  Wire.requestFrom(MPU, 12);
  
  AccDataRaw[0] = Wire.read() << 8 | Wire.read();
  AccDataRaw[1] = Wire.read() << 8 | Wire.read();
  AccDataRaw[2] = Wire.read() << 8 | Wire.read();
  
  GyroDataRaw[0] = Wire.read() << 8 | Wire.read();
  GyroDataRaw[1] = Wire.read() << 8 | Wire.read();
  GyroDataRaw[2] = Wire.read() << 8 | Wire.read();

  Calculation_IMU_Angle(AccDataRaw, AccAngle, GyroDataRaw, GyroAngle);
  
  Serial.print("Accelerometer: ");
  Serial.print("X = "); Serial.print(AccAngle[0]);
  Serial.print(" | Y = "); Serial.println(AccAngle[1]);

  Serial.print("Gyroscope: ");
  Serial.print("X = "); Serial.print(GyroAngle[0]);
  Serial.print(" | Y = "); Serial.println(GyroAngle[1]);
  
  Complementary_Filter(AccAngle, GyroAngle, Angle);
   
  Serial.print("Complementary Filter: "); 
  Serial.print("X = "); Serial.print(Angle[0]);
  Serial.print(" | Y = "); Serial.println(Angle[1]);
  
  Serial.println();

  Time_Delay();
}

void Complementary_Filter(float acc_angle[], float gyro_angle[], float angle[]){

  angle[0] = (0.98 * (angle[0] + gyro_angle[0] * 0.02)) + (0.02 * acc_angle[0]);  // roll X

  angle[1] = (0.98 * (angle[1] + gyro_angle[1] * 0.02)) + (0.02 * acc_angle[1]);  // pitch Y
}

void Calculation_IMU_Angle(float acc_data_raw[], float acc_angle[], float gyro_data_raw[], float gyro_angle[]){
  
  acc_angle[0] = atan2(acc_data_raw[1], sqrt((acc_data_raw[0] * acc_data_raw[0]) + (acc_data_raw[2] * acc_data_raw[2]))) * (180 / PI);  // roll X 
  acc_angle[1] = atan2(acc_data_raw[0], sqrt((acc_data_raw[1] * acc_data_raw[1]) + (acc_data_raw[2] * acc_data_raw[2]))) * (180 / PI);  // pitch Y

  gyro_angle[0] += (gyro_data_raw[0] / GYRO_SENSITIVITY) * DT;  
  gyro_angle[1] += (gyro_data_raw[1] / GYRO_SENSITIVITY) * DT;
}

void Time_Delay(){

  static long last_cycle = 0;
  
  while((millis() - last_cycle) < 50)
     delay(1);

  last_cycle = millis();
}

Con il giroscopio ottieni il valore della velocità angolare, non dell'inclinazione

Con l'MPU6050 puoi pensare anche di sfruttare il DMP interno per ottenere valori più stabili, vedi qui :wink:

Non ho mai usato quel filtro, però, prendendo per buono il codiche che hai postato, ti serve il valore raw della velocità angolare, e quella la ricavi direttamente dal giroscopio.

gyro_angle[0] += (gyro_data_raw[0] / GYRO_SENSITIVITY) * DT;

cosa secondo te non funziona di quel codice?

Non funziona cosa vuol dire? Valori sballati? Nulli?

Posta il link a dove hai preso lo sketch

L’ho provato al volo e ho cambiato

gyro_angle[0] += (gyro_data_raw[0] / GYRO_SENSITIVITY) * DT; 
gyro_angle[1] += (gyro_data_raw[1] / GYRO_SENSITIVITY) * DT;

in

gyro_angle[0] = (gyro_data_raw[0] / GYRO_SENSITIVITY) * DT; 
gyro_angle[1] = (gyro_data_raw[1] / GYRO_SENSITIVITY) * DT;

altrimenti il valore finale di Angle aumenta sempre.

Ho anche portato dt a 0.01 e tolto quel time_delay che non ho capito. Ho aggiunto un semplice delay(10) e stampo sulla seriale i valori solo ogni 200ms a 115200baud

// Complementary Filter: angle = (0.98 * (angle + gyroData * delta_time)) + (0.02 * accData)

#include <Wire.h>

#define MPU 0x68
#define GYRO_SENSITIVITY 65.536
#define DT 0.01 

void Calculation_IMU_Angle(float[], float[], float[], float[]);
void Complementary_Filter(float[], float[], float[]);
void Time_Delay();

unsigned long int previousmillis = 0;

void setup() {
  // put your setup code here, to run once:

  Serial.begin(115200);
 
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);
  Wire.write(0);   
  Wire.endTransmission();
}

void loop() {
  // put your main code here, to run repeatedly:

  static float AccDataRaw[3], GyroDataRaw[3], AccAngle[2] = {0, 0}, GyroAngle[2] = {0, 0}, Angle[2] = {0, 0};
 
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);
  Wire.endTransmission();
 
  Wire.requestFrom(MPU, 12);
 
  AccDataRaw[0] = Wire.read() << 8 | Wire.read();
  AccDataRaw[1] = Wire.read() << 8 | Wire.read();
  AccDataRaw[2] = Wire.read() << 8 | Wire.read();
 
  GyroDataRaw[0] = Wire.read() << 8 | Wire.read();
  GyroDataRaw[1] = Wire.read() << 8 | Wire.read();
  GyroDataRaw[2] = Wire.read() << 8 | Wire.read();

  Calculation_IMU_Angle(AccDataRaw, AccAngle, GyroDataRaw, GyroAngle);
  Complementary_Filter(AccAngle, GyroAngle, Angle);

  delay(10);

  if(millis() - previousmillis > 200){
 
  Serial.print("Accelerometer: ");
  Serial.print("X = "); Serial.print(AccAngle[0]);
  Serial.print(" | Y = "); Serial.println(AccAngle[1]);

  Serial.print("Gyroscope: ");
  Serial.print("X = "); Serial.print(GyroAngle[0]);
  Serial.print(" | Y = "); Serial.println(GyroAngle[1]);
 
//  Complementary_Filter(AccAngle, GyroAngle, Angle);
   
  Serial.print("Complementary Filter: ");
  Serial.print("X = "); Serial.print(Angle[0]);
  Serial.print(" | Y = "); Serial.println(Angle[1]);
 
  Serial.println();
previousmillis = millis();
  }
 // Time_Delay();
}

void Complementary_Filter(float acc_angle[], float gyro_angle[], float angle[]){

  angle[0] = (0.98 * (angle[0] + gyro_angle[0] * 0.02)) + (0.02 * acc_angle[0]);  // roll X

  angle[1] = (0.98 * (angle[1] + gyro_angle[1] * 0.02)) + (0.02 * acc_angle[1]);  // pitch Y
}

void Calculation_IMU_Angle(float acc_data_raw[], float acc_angle[], float gyro_data_raw[], float gyro_angle[]){
 
  acc_angle[0] = atan2(acc_data_raw[1], sqrt((acc_data_raw[0] * acc_data_raw[0]) + (acc_data_raw[2] * acc_data_raw[2]))) * (180 / PI);  // roll X
  acc_angle[1] = atan2(acc_data_raw[0], sqrt((acc_data_raw[1] * acc_data_raw[1]) + (acc_data_raw[2] * acc_data_raw[2]))) * (180 / PI);  // pitch Y

  gyro_angle[0] = (gyro_data_raw[0] / GYRO_SENSITIVITY) * DT; 
  gyro_angle[1] = (gyro_data_raw[1] / GYRO_SENSITIVITY) * DT;
}
/*
void Time_Delay(){

  static long last_cycle = 0;
 
  while((millis() - last_cycle) < 50)
     delay(1);

  last_cycle = millis();
}
*/

non dico che sia corretto ma almeno i valori sono stabili e allineati :wink:

You're welcome :smiley: