Accelerometro + Magnetometro

Salve a tutti, sto usando una MPU6050 per compensare l’inclinazione di un magnetometro QMC5883l, attualmente funziona bene quando lo inclino sull’asse Y, ma sull’asse X ha delle piccole variazioni che vanno da 1/9 gradi in base all’angolo. Il codice che uso:

#include <Wire.h>
#include <QMC5883L.h>

QMC5883L compass;

//Gyro x,y,z raw values
int gyro_x;
int gyro_y; 
int gyro_z;

//Acc x,y,z raw values and total acceleration
long acc_x;
long acc_y;
long acc_z;
long acc_total_vector;

//IMU temperature
int temperature;

//Gyro calibration values
long gyro_x_cal;
long gyro_y_cal;
long gyro_z_cal;

//Calculated angles on x,y,z
float angle_pitch;
float angle_roll;
float angle_yaw;

//"are gyro angle already set?" value
boolean set_gyro_angles;

//x,y acc angles
float angle_roll_acc;
float angle_pitch_acc;

//x,y output angles 
float angle_pitch_output;
float angle_roll_output;


unsigned long loop_timer = 0;

int16_t xhigh, xlow;
int16_t yhigh, ylow;
int16_t zhigh, zlow;


void setup()
{
 Wire.begin();
  TWBR = 12;
  Serial.begin(57600);

 compass.init();
 compass.setSamplingRate(50);

  IMU_setup();
  
 Serial.println("Turn compass in all directions to calibrate....");

  loop_timer = micros();
}



void loop()
{
  //give a 250Hz loop frequency
  while(micros() - loop_timer < 4000);                                      //wait until 4000us are passed.
  loop_timer = micros();

  IMU_calc();
  MAGN_calc();

}



void MAGN_calc(){
  
  if(compass.ready()){
    int cheading = readTiltCompensatedHeading();
    if(cheading == 0){
    }
    else{
      Serial.print(" ");
      Serial.println(cheading); 
    }
  }

}


int readTiltCompensatedHeading()
{
  int16_t x, y, z, t;
    
  if(!compass.readRaw(&x,&y,&z,&t)) return 0;
    
  /* Update the observed boundaries of the measurements */
    
  if(x<xlow) xlow = x;
  if(x>xhigh) xhigh = x;
  if(y<ylow) ylow = y;
  if(y>yhigh) yhigh = y;
  if(z<zlow) zlow = z;
  if(z>zhigh) zhigh = z;
    
  /* Bail out if not enough data is available. */
    
  if( xlow==xhigh || ylow==yhigh || zlow==zhigh) return 0;
    
  /* Recenter the measurement by subtracting the average */
    
  x -= (xhigh+xlow)/2;
  y -= (yhigh+ylow)/2;
  z -= (zhigh+zlow)/2;
    
  /* Rescale the measurement to the range observed. */
    
  float fx = (float)x/(xhigh-xlow);
  float fy = (float)y/(yhigh-ylow);
  float fz = (float)z/(zhigh-zlow);

  //#### VERIFY
  float Xh = (float)x * cos(angle_roll_output * 0.0174533) + (float)z * sin(angle_roll_output * 0.0174533);
  float Yh = (float)x * sin(angle_pitch_output * 0.0174533) * sin(angle_roll_output * 0.0174533) + (float)y * cos(angle_pitch_output * 0.0174533) - (float)z * sin(angle_pitch_output * 0.0174533) * cos(angle_roll_output * 0.0174533);
  //#### VERIFY
    
  int heading = atan2(Yh,Xh)*180/PI;
  if(heading<=0) heading += 360;
    
  return heading;
}



void IMU_setup(){

  setup_mpu_6050_registers();                                          //Setup the registers of the MPU-6050 (500dfs / +/-8g) and start the gyro
 
  Serial.println("Calibrating gyro");
                                                  
  for (int cal_int = 0; cal_int < 2000 ; cal_int ++){                  //Run this code 2000 times     
                             
    read_mpu_6050_data();                                              //Read the raw acc and gyro data from the MPU-6050
    gyro_x_cal += gyro_x;                                              //Add the gyro x-axis offset to the gyro_x_cal variable
    gyro_y_cal += gyro_y;                                              //Add the gyro y-axis offset to the gyro_y_cal variable
    gyro_z_cal += gyro_z;                                              //Add the gyro z-axis offset to the gyro_z_cal variable
    delay(3);                                                          //Delay 3ms to simulate the 250Hz program loop
  }
  
  gyro_x_cal /= 2000;                                                  //Divide the gyro_x_cal variable by 2000 to get the avarage offset
  gyro_y_cal /= 2000;                                                  //Divide the gyro_y_cal variable by 2000 to get the avarage offset
  gyro_z_cal /= 2000;                                                  //Divide the gyro_z_cal variable by 2000 to get the avarage offset
}


void IMU_calc(){

  read_mpu_6050_data();                                                                                 //Read the raw acc and gyro data from the MPU-6050

  gyro_x -= gyro_x_cal;                                                                                 //Subtract the offset calibration value from the raw gyro_x value
  gyro_y -= gyro_y_cal;                                                                                 //Subtract the offset calibration value from the raw gyro_y value
  gyro_z -= gyro_z_cal;                                                                                 //Subtract the offset calibration value from the raw gyro_z value
  
  //Gyro angle calculations
  //0.0000611 = 1 / (250Hz / 65.5)
  angle_pitch += gyro_x * 0.0000611;                                                                    //Calculate the traveled pitch angle and add this to the angle_pitch variable
  angle_roll += gyro_y * 0.0000611;                                                                     //Calculate the traveled roll angle and add this to the angle_roll variable
  angle_yaw = gyro_z / 65.5;                              
  
  //0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
  angle_pitch += angle_roll * sin(gyro_z * 0.000001066);                                                //If the IMU has yawed transfer the roll angle to the pitch angel
  angle_roll -= angle_pitch * sin(gyro_z * 0.000001066);                                                //If the IMU has yawed transfer the pitch angle to the roll angel
  
  //Accelerometer angle calculations
  acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z));                                   //Calculate the total accelerometer vector
  //57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
  angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296;                                        //Calculate the pitch angle
  angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296;                                        //Calculate the roll angle
  
  //Place the MPU-6050 spirit level and note the values in the following two lines for calibration
  angle_pitch_acc -= 0.0;                                                                               //Accelerometer calibration value for pitch
  angle_roll_acc -= 0.0;                                                                                //Accelerometer calibration value for roll

  if(set_gyro_angles){                                                                                  //If the IMU is already started
    angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004;                                      //Correct the drift of the gyro pitch angle with the accelerometer pitch angle
    angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004;                                         //Correct the drift of the gyro roll angle with the accelerometer roll angle
  }
  else{                                                                                                 //At first start
    angle_pitch = angle_pitch_acc;                                                                      //Set the gyro pitch angle equal to the accelerometer pitch angle 
    angle_roll = angle_roll_acc;                                                                        //Set the gyro roll angle equal to the accelerometer roll angle 
    set_gyro_angles = true;                                                                             //Set the IMU started flag
  }
  
  //To dampen the pitch and roll angles a complementary filter is used
  angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1;   //Take 90% of the output pitch value and add 10% of the raw pitch value
  angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1;      //Take 90% of the output roll value and add 10% of the raw roll value

  Serial.print(angle_pitch_output);
  Serial.print(" ");
  Serial.println(angle_roll_output);
}

Qualche idea sul motivo per cui sull’asse X non funzioni bene?

Ciao! su questo stesso sketch sarebbe possibile implementare la posizione nello "spazio" X Y Z oltre alla rotazione?